Update line to point distance to support points projected beyond the …

…segment Signed-off-by: Louise Poubel <[email protected]> Signed-off-by: Arjo Chakravarty <[email protected]>
gazebosim · Sep 20, 2021 · c835aee · c835aee
1 parent aed0c6b
commit c835aee
Show file tree

Hide file tree

Showing 2 changed files with 81 additions and 7 deletions.
diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh
@@ -230,12 +230,29 @@ namespace ignition
       }
 
       /// \brief Calculate shortest distance between line and point
-      /// \param[in] _pt point which we are measuring distance to.
+      /// \param[in] _pt Point which we are measuring distance to.
       /// \returns Distance from point to line.
       public: T Distance(const Vector3<T> &_pt)
       {
-        auto d = (_pt - this->pts[0]).Cross(this->pts[1] - this->pts[0]);
-        return d.Length() / (this->pts[1] - this->pts[0]).Length();
+        auto line = this->pts[1] - this->pts[0];
+        auto ptTo0 = _pt - this->pts[0];
+        auto ptTo1 = _pt - this->pts[1];
+
+        // Point is projected beyond pt0
+        if (ptTo0.Dot(line) <= 0.0)
+        {
+          return ptTo0.Length();
+        }
+
+        // Point is projected beyond pt1
+        if (ptTo1.Dot(line) >= 0.0)
+        {
+          return ptTo1.Length();
+        }
+
+        // Distance to point projected onto line
+        auto d = (ptTo0).Cross(line);
+        return d.Length() / (line).Length();
       }
 
       /// \brief Check if this line intersects the given line segment.

diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc
@@ -228,11 +228,68 @@ TEST(Line3Test, Distance)
   // Expect false when the first line is a point.
   line.Set(0, 0, 0, 0, 0, 0);
   EXPECT_FALSE(line.Distance(math::Line3d(2, 0, 0, 2, 1, 0), result));
+}
 
-  // Check when measured against a point.
-  line.Set(0, -1, 0, 0, 1, 0);
-  math::Vector3d point(5, 0, 0);
-  EXPECT_NEAR(line.Distance(point), 5, 1e-3);
+/////////////////////////////////////////////////
+TEST(Line3Test, DistanceToPoint)
+{
+  // Line on horizontal plane
+  math::Vector3d pointA{0, -1, 0};
+  math::Vector3d pointB{0, 1, 0};
+  math::Line3d line{pointA, pointB};
+
+  // Point on the line
+  {
+    math::Vector3d point(0, 0.5, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 0.0);
+  }
+
+  // Points projected onto the line
+  {
+    math::Vector3d point(5, 0, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 5);
+  }
+  {
+    math::Vector3d point(-1, -1, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 1);
+  }
+
+  // Points projected beyond the line's ends
+  {
+    math::Vector3d point(0, 2, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 1);
+  }
+  {
+    math::Vector3d point(2, -3, 0);
+    EXPECT_DOUBLE_EQ(line.Distance(point), sqrt(8));
+  }
+
+  // 3D line
+  pointA.Set(1, 1, 1);
+  pointB.Set(-1, -1, -1);
+  line.Set(pointA, pointB);
+
+  // Point on the line
+  {
+    math::Vector3d point(-0.5, -0.5, -0.5);
+    EXPECT_DOUBLE_EQ(line.Distance(point), 0.0);
+  }
+
+  // Point projected onto the line
+  {
+    math::Vector3d point(1, -1, 0);
+    EXPECT_NEAR(line.Distance(point), point.Length(), 1e-3);
+  }
+
+  // Points projected beyond the line's ends
+  {
+    math::Vector3d point(2, 2, 3);
+    EXPECT_NEAR(line.Distance(point), point.Distance(pointA), 1e-3);
+  }
+  {
+    math::Vector3d point(-5, -3, -8);
+    EXPECT_NEAR(line.Distance(point), point.Distance(pointB), 1e-3);
+  }
 }
 
 /////////////////////////////////////////////////