Distance

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;