model Distance "Measure the distance between the origins of two frame connectors"
import Modelica.Mechanics.MultiBody.Frames;
import Modelica.Mechanics.MultiBody.Types;
extends PlanarMechanics.Interfaces.PartialTwoFrames;
extends Modelica.Icons.RectangularSensor;
Modelica.Blocks.Interfaces.RealOutput distance(final quantity = "Position", final unit = "m", min = 0) "Distance between the origin of frame_a and the origin of frame_b"
annotation (Placement(transformation(
origin = {0, -110},
extent = {
{10, -10},
{-10, 10}},
rotation = 90)));
parameter Boolean animation = true "= true, if animation shall be enabled (show arrow)";
input SI.Diameter arrowDiameter = planarWorld.defaultArrowDiameter "Diameter of relative arrow from frame_a to frame_b"
annotation (Dialog(
group = "if animation = true",
enable = animation));
input Types.Color arrowColor = Modelica.Mechanics.MultiBody.Types.Defaults.SensorColor "Color of relative arrow from frame_a to frame_b"
annotation (
HideResult = true,
Dialog(
colorSelector = true,
group = "if animation = true",
enable = animation));
input Types.SpecularCoefficient specularCoefficient = planarWorld.defaultSpecularCoefficient "Reflection of ambient light (= 0: light is completely absorbed)"
annotation (
HideResult = true,
Dialog(
group = "if animation = true",
enable = animation));
input SI.Position s_small(min = sqrt(Modelica.Constants.small)) = 1e-10 "Prevent zero-division if distance between frame_a and frame_b is zero"
annotation (Dialog(tab = "Advanced"));
protected
PlanarMechanics.Visualizers.Advanced.Arrow arrow(r = {frame_a.x, frame_a.y, 0}, r_head = {frame_b.x, frame_b.y, 0} - {frame_a.x, frame_a.y, 0}, diameter = arrowDiameter, color = arrowColor, specularCoefficient = specularCoefficient) if planarWorld.enableAnimation and animation;
SI.Position r_rel_0[3] = {frame_b.x, frame_b.y, 0} - {frame_a.x, frame_a.y, 0} "Position vector from frame_a to frame_b resolved in world frame";
SI.Area L2 = r_rel_0 * r_rel_0;
SI.Area s_small2 = s_small ^ 2;
equation
distance = smooth(1, if noEvent(s_small2 < L2) then sqrt(L2) else L2 / (2 * s_small) * (3 - L2 / s_small2));
frame_a.t = 0;
frame_b.t = 0;
{frame_a.fx, frame_a.fy} = zeros(2);
{frame_b.fx, frame_b.fy} = zeros(2);
annotation (
Icon(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Line(
points = {
{0, -60},
{0, -100}},
color = {0, 0, 127}),
Line(points = {
{-70, 0},
{-101, 0}}),
Line(points = {
{70, 0},
{100, 0}}),
Text(
extent = {
{-150, 80},
{150, 40}},
textString = "%name",
textColor = {0, 0, 255})}),
Diagram(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Text(
extent = {
{-22, 70},
{20, 46}},
textString = "s",
textColor = {0, 0, 255}),
Line(
points = {
{-98, 40},
{88, 40}},
color = {0, 0, 255}),
Polygon(
points = {
{102, 40},
{87, 46},
{87, 34},
{102, 40}},
lineColor = {0, 0, 255},
fillColor = {0, 0, 255},
fillPattern = FillPattern.Solid)}),
Documentation(
revisions = "<html>\n<p>\n<img src=\"modelica://PlanarMechanics/Resources/Images/dlr_logo.png\" alt=\"DLR logo\">\n<strong>Developed 2010 at the DLR Institute of System Dynamics and Control</strong>\n</p>\n</html>",
info = "<html>\n<p>\nThe <strong>distance</strong> between the origins of <code>frame_a</code>\nand of <code>frame_b</code> are determined and provided at the\noutput signal connector <code>distance</code>. This\ndistance is always positive. <strong>Derivatives</strong> of this\nsignal can be easily obtained by connecting the\nblock\n<a href=\"modelica://Modelica.Blocks.Continuous.Der\">Modelica.Blocks.Continuous.Der</a>\nto <code>distance</code> (this block performs analytic differentiation\nof the input signal using the <code><strong>der</strong>(…)</code> operator).\n</p>\n<p>\nIn the following figure the animation of the\nsensor is shown. The light blue coordinate system is\n<code>frame_a</code>, the dark blue coordinate system is <code>frame_b</code>,\nand the yellow arrow is the animated sensor.\n</p>\n\n<div>\n<img src=\"modelica://Modelica/Resources/Images/Mechanics/MultiBody/Sensors/Distance.png\" alt=\"Distance animation\">\n</div>\n\n<p>\nIf the distance is smaller then the parameter <code>s_small</code>\n(in the "Advanced" menu), it is approximated such that its derivative is\nfinite for zero distance. Without such an approximation, the derivative would\nbe infinite and a division by zero would occur. The approximation is performed\nin the following way: If distance > <var>s</var><sub>small</sub>, it is\ncomputed as sqrt(<var>r</var>*<var>r</var>) where <var>r</var> is the\nposition vector from the origin of <code>frame_a</code> to the origin of <code>frame_b</code>.\nIf the distance becomes smaller then <var>s</var><sub>small</sub>, the "sqrt()"\nfunction is approximated by a second order polynomial, such that the function\nvalue and its first derivative are identical for sqrt() and the polynomial at\n<var>s</var><sub>small</sub>.\nFurthermore, the polynomial passes through zero. The effect is, that the distance\nfunction is continuous and differentiable everywhere. The derivative at zero distance\nis 3/(2*<var>s</var><sub>small</sub>).\n</p>\n</html>"));
end Distance;