diff --git a/include/gz/math/Line3.hh b/include/gz/math/Line3.hh index 7ebecb83..770daf9d 100644 --- a/include/gz/math/Line3.hh +++ b/include/gz/math/Line3.hh @@ -104,8 +104,10 @@ namespace gz::math const double _x2, const double _y2, const double _z = 0) { - this->pts[0].Set(_x1, _y1, _z); - this->pts[1].Set(_x2, _y2, _z); + this->pts[0].Set( + static_cast(_x1), static_cast(_y1), static_cast(_z)); + this->pts[1].Set( + static_cast(_x2), static_cast(_y2), static_cast(_z)); } /// \brief Set the start and end point of the line segment @@ -119,8 +121,10 @@ namespace gz::math const double _z1, const double _x2, const double _y2, const double _z2) { - this->pts[0].Set(_x1, _y1, _z1); - this->pts[1].Set(_x2, _y2, _z2); + this->pts[0].Set( + static_cast(_x1), static_cast(_y1), static_cast(_z1)); + this->pts[1].Set( + static_cast(_x2), static_cast(_y2), static_cast(_z2)); } /// \brief Get the direction of the line @@ -134,7 +138,7 @@ namespace gz::math /// \return The length of the line. public: T Length() const { - return this->pts[0].Distance(this->pts[1]); + return static_cast(this->pts[0].Distance(this->pts[1])); } /// \brief Get the shortest line between this line and the @@ -214,7 +218,8 @@ namespace gz::math double mua = clamp(numer / denom, 0.0, 1.0); double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0); - _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub)); + _result.Set(this->pts[0] + (p21 * static_cast(mua)), + _line[0] + (p43 * static_cast(mub))); return true; } @@ -229,13 +234,13 @@ namespace gz::math auto ptTo1 = _pt - this->pts[1]; // Point is projected beyond pt0 or the line has length 0 - if (ptTo0.Dot(line) <= 0.0) + if (ptTo0.Dot(line) <= static_cast(0)) { return ptTo0.Length(); } // Point is projected beyond pt1 - if (ptTo1.Dot(line) >= 0.0) + if (ptTo1.Dot(line) >= static_cast(0)) { return ptTo1.Length(); } @@ -341,18 +346,19 @@ namespace gz::math public: bool Within(const math::Vector3 &_pt, double _epsilon = 1e-6) const { + auto eps = static_cast(_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 && _pt.Z() <= std::max(this->pts[0].Z(), - this->pts[1].Z()) + _epsilon && + this->pts[1].Z()) + eps && _pt.Z() >= std::min(this->pts[0].Z(), - this->pts[1].Z()) - _epsilon; + this->pts[1].Z()) - eps; } /// \brief Equality operator. diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc index 98fe4e95..7e730c25 100644 --- a/src/Line3_TEST.cc +++ b/src/Line3_TEST.cc @@ -25,6 +25,10 @@ using namespace gz; ///////////////////////////////////////////////// TEST(Line3Test, Constructor) { + math::Line3d lineDefault; + EXPECT_EQ(lineDefault[0], math::Vector3d::Zero); + EXPECT_EQ(lineDefault[1], math::Vector3d::Zero); + math::Line3d lineA(0, 0, 10, 10); EXPECT_DOUBLE_EQ(lineA[0].X(), 0.0); EXPECT_DOUBLE_EQ(lineA[0].Y(), 0.0); @@ -88,6 +92,10 @@ TEST(Line3Test, Set) EXPECT_DOUBLE_EQ(lineA[1].X(), 5.0); EXPECT_DOUBLE_EQ(lineA[1].Y(), 6.0); EXPECT_DOUBLE_EQ(lineA[1].Z(), 7.0); + + lineA.Set(math::Vector3d(20, 21, 22), math::Vector3d(23, 24, 25)); + EXPECT_EQ(lineA[0], math::Vector3d(20, 21, 22)); + EXPECT_EQ(lineA[1], math::Vector3d(23, 24, 25)); } ///////////////////////////////////////////////// @@ -344,6 +352,17 @@ TEST(Line3Test, Intersect) EXPECT_TRUE(line.Intersect(math::Line3d(0, -1, 0, 0, 0.1, 0))); EXPECT_TRUE(line.Intersect(math::Line3d(0, 1, 0, 0, 1.1, 0))); + + // Parallel non-overlapping lines + EXPECT_FALSE(line.Intersect(math::Line3d(0, 2, 0, 0, 3, 0))); + EXPECT_FALSE(line.Intersect(math::Line3d(0, 2, 0, 0, 3, 0), pt)); + + // Parallel overlapping line where _line[0] is within this line + EXPECT_TRUE(line.Intersect(math::Line3d(0, 0.5, 0, 0, 2, 0), pt)); + EXPECT_EQ(pt, math::Vector3d(0, 0.5, 0)); + + // Skew non-intersecting lines + EXPECT_FALSE(line.Intersect(math::Line3d(1, 0, 1, 1, 1, 1), pt)); } ///////////////////////////////////////////////// @@ -371,3 +390,17 @@ TEST(Line3Test, Coplanar) EXPECT_FALSE(line.Coplanar(math::Line3d(1, 0, 0, 1, 1, 1))); EXPECT_FALSE(line.Coplanar(math::Line3d(1, 0, 1, 2, 0, 0))); } + +///////////////////////////////////////////////// +TEST(Line3Test, TemplateTypes) +{ + math::Line3i lineI(0, 0, 0, 10, 10, 10); + EXPECT_EQ(lineI[0], math::Vector3i(0, 0, 0)); + EXPECT_EQ(lineI[1], math::Vector3i(10, 10, 10)); + + math::Line3f lineF(0.0f, 0.0f, 0.0f, 10.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(), 17.320508f, 1e-4f); +} + diff --git a/src/python_pybind11/test/Line3_TEST.py b/src/python_pybind11/test/Line3_TEST.py index 3f5fe475..6572dd11 100644 --- a/src/python_pybind11/test/Line3_TEST.py +++ b/src/python_pybind11/test/Line3_TEST.py @@ -14,8 +14,8 @@ import math import unittest -from gz.math8 import Line3d -from gz.math8 import Vector3d +from gz.math8 import Line3d, Line3f, Line3i +from gz.math8 import Vector3d, Vector3i class TestLine3d(unittest.TestCase): @@ -80,6 +80,10 @@ def test_set(self): self.assertAlmostEqual(line_a[1].y(), 6.0) self.assertAlmostEqual(line_a[1].z(), 7.0) + line_a.set(Vector3d(20, 21, 22), Vector3d(23, 24, 25)) + self.assertEqual(line_a[0], Vector3d(20, 21, 22)) + self.assertEqual(line_a[1], Vector3d(23, 24, 25)) + def test_length(self): line_a = Line3d(0, 0, 0, 10, 10, 10) self.assertAlmostEqual(line_a.length(), math.sqrt(300), delta=1e-10) @@ -225,6 +229,17 @@ def test_interesct(self): self.assertTrue(line.intersect(Line3d(0, -1, 0, 0, 0.1, 0))) self.assertTrue(line.intersect(Line3d(0, 1, 0, 0, 1.1, 0))) + # Parallel non-overlapping lines + self.assertFalse(line.intersect(Line3d(0, 2, 0, 0, 3, 0))) + self.assertFalse(line.intersect(Line3d(0, 2, 0, 0, 3, 0), pt)) + + # Parallel overlapping line where _line[0] is within this line + self.assertTrue(line.intersect(Line3d(0, 0.5, 0, 0, 2, 0), pt)) + self.assertEqual(pt, Vector3d(0, 0.5, 0)) + + # Skew non-intersecting lines + self.assertFalse(line.intersect(Line3d(1, 0, 1, 1, 1, 1), pt)) + def test_parallel(self): line = Line3d(0, 0, 0, 0, 1, 0) self.assertTrue(line.parallel(Line3d(1, 0, 0, 1, 1, 0))) @@ -245,6 +260,17 @@ def test_coplanar(self): self.assertFalse(line.coplanar(Line3d(1, 0, 0, 1, 1, 1))) self.assertFalse(line.coplanar(Line3d(1, 0, 1, 2, 0, 0))) + def test_template_types(self): + line_i = Line3i(0, 0, 0, 10, 10, 10) + self.assertEqual(line_i[0], Vector3i(0, 0, 0)) + self.assertEqual(line_i[1], Vector3i(10, 10, 10)) + + line_f = Line3f(0.0, 0.0, 0.0, 10.0, 10.0, 10.0) + self.assertAlmostEqual(line_f[0].x(), 0.0) + self.assertAlmostEqual(line_f[1].y(), 10.0) + self.assertAlmostEqual(line_f.length(), 17.320508, delta=1e-4) + if __name__ == '__main__': unittest.main() +