Merge branch 'gz-math9' into releases/9.3.0

Author: scpeters

include/gz/math/Line2.hh

@@ -45,7 +45,7 @@ namespace gz::math
     /// \param[in] _y1 Y coordinate of the start point.
     /// \param[in] _x2 X coordinate of the end point.
     /// \param[in] _y2 Y coordinate of the end point.
-    public: Line2(double _x1, double _y1, double _x2, double _y2)
+    public: Line2(T _x1, T _y1, T _x2, T _y2)
     {
       this->Set(_x1, _y1, _x2, _y2);
     }
@@ -65,7 +65,7 @@ namespace gz::math
     /// \param[in] _y1 Y coordinate of the start point.
     /// \param[in] _x2 X coordinate of the end point.
     /// \param[in] _y2 Y coordinate of the end point.
-    public: void Set(double _x1, double _y1, double _x2, double _y2)
+    public: void Set(T _x1, T _y1, T _x2, T _y2)
     {
       this->pts[0].Set(_x1, _y1);
       this->pts[1].Set(_x2, _y2);
@@ -77,7 +77,7 @@ namespace gz::math
     /// (a.start.y - a.end.y) * (b.start.x - b.end.x)
     /// \param[in] _line Line for the cross product computation.
     /// \return Return the cross product of this line and the given line.
-    public: double CrossProduct(const Line2<T> &_line) const
+    public: T CrossProduct(const Line2<T> &_line) const
     {
       return (this->pts[0].X() - this->pts[1].X()) *
              (_line[0].Y() -_line[1].Y()) -
@@ -90,7 +90,7 @@ namespace gz::math
     //  (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
     /// \param[in] _pt Point for the cross product computation.
     /// \return Return the cross product of this line and the given point.
-    public: double CrossProduct(const Vector2<T> &_pt) const
+    public: T CrossProduct(const Vector2<T> &_pt) const
     {
       return (_pt.Y() - this->pts[0].Y()) *
              (this->pts[1].X() - this->pts[0].X()) -
@@ -108,7 +108,7 @@ namespace gz::math
                            double _epsilon = 1e-6) const
     {
       return math::equal(this->CrossProduct(_pt),
-          0., _epsilon);
+          static_cast<T>(0), static_cast<T>(_epsilon));
     }
 
     /// \brief Check if the given line is parallel with this line.
@@ -122,7 +122,7 @@ namespace gz::math
                           double _epsilon = 1e-6) const
     {
       return math::equal(this->CrossProduct(_line),
-          0., _epsilon);
+          static_cast<T>(0), static_cast<T>(_epsilon));
     }
 
     /// \brief Check if the given line is collinear with this line. This
@@ -160,14 +160,15 @@ namespace gz::math
     public: bool Within(const math::Vector2<T> &_pt,
                         double _epsilon = 1e-6) const
     {
+      auto eps = static_cast<T>(_epsilon);
       return _pt.X() <= std::max(this->pts[0].X(),
-                                 this->pts[1].X()) + _epsilon &&
+                                 this->pts[1].X()) + eps &&
              _pt.X() >= std::min(this->pts[0].X(),
-                                 this->pts[1].X()) - _epsilon &&
+                                 this->pts[1].X()) - eps &&
              _pt.Y() <= std::max(this->pts[0].Y(),
-                                 this->pts[1].Y()) + _epsilon &&
+                                 this->pts[1].Y()) + eps &&
              _pt.Y() >= std::min(this->pts[0].Y(),
-                                 this->pts[1].Y()) - _epsilon;
+                                 this->pts[1].Y()) - eps;
     }
 
     /// \brief Check if this line intersects the given line segment.
@@ -193,11 +194,11 @@ namespace gz::math
     public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
                            double _epsilon = 1e-6) const
     {
-      double d = this->CrossProduct(_line);
+      T d = this->CrossProduct(_line);
 
       // d is zero if the two line are collinear. Must check special
       // cases.
-      if (math::equal(d, 0.0, _epsilon))
+      if (math::equal(d, static_cast<T>(0), static_cast<T>(_epsilon)))
       {
         // Check if _line's starting point is on the line.
         if (this->Within(_line[0], _epsilon))
@@ -253,10 +254,7 @@ namespace gz::math
     /// \return The length of the line.
     public: T Length() const
     {
-      return sqrt((this->pts[0].X() - this->pts[1].X()) *
-                  (this->pts[0].X() - this->pts[1].X()) +
-                  (this->pts[0].Y() - this->pts[1].Y()) *
-                  (this->pts[0].Y() - this->pts[1].Y()));
+      return static_cast<T>(this->pts[0].Distance(this->pts[1]));
     }
 
     /// \brief Get the slope of the line

src/Line2_TEST.cc

@@ -40,6 +40,49 @@ TEST(Line2Test, Constructor)
   EXPECT_NO_THROW(lineB[2].X());
   EXPECT_DOUBLE_EQ(lineB[2].X(), lineB[1].X());
   EXPECT_NO_THROW(lineA[0].X());
+
+  // Test Set method with Vector2
+  lineA.Set(math::Vector2d(5, 6), math::Vector2d(7, 8));
+  EXPECT_EQ(lineA[0], math::Vector2d(5, 6));
+  EXPECT_EQ(lineA[1], math::Vector2d(7, 8));
+}
+
+/////////////////////////////////////////////////
+TEST(Line2Test, CrossProduct)
+{
+  math::Line2d lineA(0, 0, 10, 0);
+  math::Line2d lineB(0, 0, 0, 10);
+  EXPECT_DOUBLE_EQ(lineA.CrossProduct(lineB), 100.0);
+
+  math::Vector2d pt(5, 5);
+  EXPECT_DOUBLE_EQ(lineA.CrossProduct(pt), 50.0);
+}
+
+/////////////////////////////////////////////////
+TEST(Line2Test, OnSegmentAndWithin)
+{
+  math::Line2d line(0, 0, 10, 10);
+
+  // Point on segment
+  math::Vector2d pt1(5, 5);
+  EXPECT_TRUE(line.Within(pt1));
+  EXPECT_TRUE(line.OnSegment(pt1));
+
+  // Point collinear with line, but outside segment bounds
+  math::Vector2d pt2(15, 15);
+  EXPECT_FALSE(line.Within(pt2));
+  EXPECT_FALSE(line.OnSegment(pt2));
+
+  // Point within bounding box of segment, but not collinear
+  math::Vector2d pt3(5, 6);
+  EXPECT_TRUE(line.Within(pt3));
+  EXPECT_FALSE(line.OnSegment(pt3));
+
+  // Endpoints
+  EXPECT_TRUE(line.Within(line[0]));
+  EXPECT_TRUE(line.OnSegment(line[0]));
+  EXPECT_TRUE(line.Within(line[1]));
+  EXPECT_TRUE(line.OnSegment(line[1]));
 }
 
 /////////////////////////////////////////////////
@@ -83,29 +126,34 @@ TEST(Line2Test, ParallelLine)
     // Line is always parallel with itself
     math::Line2d line(0, 0, 10, 0);
     EXPECT_TRUE(line.Parallel(line, 1e-10));
+    EXPECT_DOUBLE_EQ(line.CrossProduct(line), 0.0);
   }
 
   {
     // Degenerate line segment
     // Still expect Line is parallel with itself
     math::Line2d line(0, 0, 0, 0);
     EXPECT_TRUE(line.Parallel(line, 1e-10));
+    EXPECT_DOUBLE_EQ(line.CrossProduct(line), 0.0);
   }
 
   math::Line2d lineA(0, 0, 10, 0);
   math::Line2d lineB(0, 0, 10, 0);
   EXPECT_TRUE(lineA.Parallel(lineB, 1e-10));
+  EXPECT_DOUBLE_EQ(lineA.CrossProduct(lineB), 0.0);
 
   lineB.Set(0, 0, 0, 10);
   EXPECT_FALSE(lineA.Parallel(lineB));
 
   lineB.Set(0, 10, 10, 10);
   EXPECT_TRUE(lineA.Parallel(lineB));
+  EXPECT_DOUBLE_EQ(lineA.CrossProduct(lineB), 0.0);
 
   lineB.Set(0, 10, 10, 10.00001);
   EXPECT_FALSE(lineA.Parallel(lineB, 1e-10));
   EXPECT_FALSE(lineA.Parallel(lineB));
   EXPECT_TRUE(lineA.Parallel(lineB, 1e-3));
+  EXPECT_NEAR(lineA.CrossProduct(lineB), 0.0, 1e-3);
 }
 
 /////////////////////////////////////////////////
@@ -234,6 +282,28 @@ TEST(Line2Test, Intersect)
   lineB.Set(0, 10, 10, 0);
   EXPECT_TRUE(lineA.Intersect(lineB, pt));
   EXPECT_EQ(pt, math::Vector2d(5, 5));
+
+  // Collinear parallel non-overlapping lines
+  lineA.Set(0, 0, 10, 0);
+  lineB.Set(20, 0, 30, 0);
+  EXPECT_FALSE(lineA.Intersect(lineB, pt));
+  EXPECT_FALSE(lineA.Intersect(lineB));
+
+  // Collinear lines where lineB end point is within lineA but start point isn't
+  lineA.Set(0, 0, 10, 0);
+  lineB.Set(-5, 0, 5, 0);
+  EXPECT_TRUE(lineA.Intersect(lineB, pt));
+  EXPECT_EQ(pt, math::Vector2d(5, 0));
+
+  // Lines whose extensions intersect, but intersection Y is outside bounds
+  lineA.Set(-10, 0, 10, 0);
+  lineB.Set(0, 2, 0, 10);
+  EXPECT_FALSE(lineA.Intersect(lineB, pt));
+
+  // Lines whose extensions intersect, but intersection X is outside bounds
+  lineA.Set(0, -10, 0, 10);
+  lineB.Set(2, 0, 10, 0);
+  EXPECT_FALSE(lineA.Intersect(lineB, pt));
 }
 
 /////////////////////////////////////////////////
@@ -243,7 +313,9 @@ TEST(Line2Test, Equality)
   math::Line2d lineB(1, 2, 2, 2);
 
   EXPECT_TRUE(lineA != lineB);
+  EXPECT_FALSE(lineA == lineB);
   EXPECT_TRUE(lineA == lineA);
+  EXPECT_FALSE(lineA != lineA);
 
   lineB.Set(1, 1, 2, 1.1);
   EXPECT_FALSE(lineA == lineB);
@@ -266,3 +338,18 @@ TEST(Line2Test, OperatorStreamOut)
   stream << line;
   EXPECT_EQ(stream.str(), "0 1 2 3");
 }
+
+/////////////////////////////////////////////////
+TEST(Line2Test, TemplateTypes)
+{
+  math::Line2i lineI(0, 0, 10, 10);
+  EXPECT_EQ(lineI[0], math::Vector2i(0, 0));
+  EXPECT_EQ(lineI[1], math::Vector2i(10, 10));
+  EXPECT_EQ(lineI.Length(), 14);
+
+  math::Line2f lineF(0.0f, 0.0f, 10.0f, 10.0f);
+  EXPECT_FLOAT_EQ(lineF[0].X(), 0.0f);
+  EXPECT_FLOAT_EQ(lineF[1].Y(), 10.0f);
+  EXPECT_NEAR(lineF.Length(), 14.1421356f, 1e-4f);
+}
+

src/python_pybind11/src/Line2.hh

@@ -56,15 +56,15 @@ void helpDefineMathLine2(py::module &m, const std::string &typestr)
                     py::dynamic_attr())
     .def(py::init<const gz::math::Vector2<T>&,
                   const gz::math::Vector2<T>&>())
-    .def(py::init<double, double, double, double>())
+    .def(py::init<T, T, T, T>())
     .def(py::self != py::self)
     .def(py::self == py::self)
     .def("set",
          py::overload_cast<const gz::math::Vector2<T>&,
                            const gz::math::Vector2<T>&>(&Class::Set),
          "Set the start and end point of the line segment")
     .def("set",
-         py::overload_cast<double, double, double, double>(&Class::Set),
+         py::overload_cast<T, T, T, T>(&Class::Set),
          "Set the start and end point of the line segment")
     .def("cross_product",
          py::overload_cast<const Class&>(&Class::CrossProduct, py::const_),