model Distance "Measure the distance between the origins of two frame connectors"
import Modelica.Mechanics.MultiBody.Frames;
import Modelica.Mechanics.MultiBody.Types;
extends Interfaces.PartialTwoFrames;
extends Modelica.Icons.TranslationalSensor;
Modelica.Blocks.Interfaces.RealOutput distance "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 = world.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 (Dialog(
colorSelector = true,
group = "if animation = true",
enable = animation));
input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient "Reflection of ambient light (= 0: light is completely absorbed)"
annotation (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
Modelica.Mechanics.MultiBody.Visualizers.Advanced.Arrow arrow(r = frame_a.r_0, r_head = frame_b.r_0 - frame_a.r_0, diameter = arrowDiameter, color = arrowColor, specularCoefficient = specularCoefficient) if world.enableAnimation and animation;
SI.Position r_rel_0[3] = frame_b.r_0 - frame_a.r_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.f = zeros(3);
frame_a.t = zeros(3);
frame_b.f = zeros(3);
frame_b.t = zeros(3);
annotation (
Icon(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Line(
points = {
{0, -60},
{0, -100}},
color = {0, 0, 255}),
Line(points = {
{-70, 0},
{-101, 0}}),
Line(points = {
{70, 0},
{100, 0}}),
Text(
extent = {
{-128, 30},
{133, 78}},
textString = "%name",
lineColor = {0, 0, 255})}),
Diagram(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Line(points = {
{-70, 0},
{-101, 0}}),
Line(points = {
{70, 0},
{100, 0}}),
Line(
points = {
{0, -60},
{0, -100}},
color = {0, 0, 255}),
Text(
extent = {
{-22, 70},
{20, 46}},
textString = "s",
lineColor = {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(info = "<html>\n<p>\nThe <strong>distance</strong> between the origins of frame_a\nand of frame_b are determined and provided at the\noutput signal connector <strong>distance</strong>. 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 \"distance\" (this block performs analytic differentiation\nof the input signal using the der(..) operator).\n</p>\n<p>\nIn the following figure the animation of a Distance\nsensor is shown. The light blue coordinate system is\nframe_a, the dark blue coordinate system is frame_b, and\nthe yellow arrow is the animated sensor.\n</p>\n\n<p>\n<img src=\"modelica://Modelica/Resources/Images/Mechanics/MultiBody/Sensors/Distance.png\">\n</p>\n\n<p>\nIf the distance is smaller as parameter <strong>s_small</strong> (in the \"advanced\" menu),\nit 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 > s_small, it is computed as sqrt(r*r) where\nr is the position vector from the origin of frame_a to the origin of frame_b.\nIf the distance becomes smaller as s_small, the \"sqrt()\" function is approximated\nby a second order polynomial, such that the function value and its first derivative\nare identical for sqrt() and the polynomial at s_small. Furthermore, the polynomial\npasses through zero. The effect is, that the distance function is continuous and\ndifferentiable everywhere. The derivative at zero distance is 3/(2*s_small).\n</p>\n</html>"));
end Distance;