From 56b214ef459c55e28052a47760793d826607c80f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 3 Aug 2021 14:44:04 +0800 Subject: [PATCH 01/81] Add volume below primitives Signed-off-by: Arjo Chakravarty --- include/ignition/math/Box.hh | 12 ++ include/ignition/math/Plane.hh | 69 +++++++++ include/ignition/math/Sphere.hh | 7 + include/ignition/math/detail/Box.hh | 194 +++++++++++++++++++++++++ include/ignition/math/detail/Sphere.hh | 23 +++ src/Box_TEST.cc | 31 ++++ src/Plane_TEST.cc | 20 +++ src/Sphere_TEST.cc | 22 +++ 8 files changed, 378 insertions(+) diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index 57670f4f7..d0db64319 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -21,6 +21,9 @@ #include #include #include +#include + +#include namespace ignition { @@ -128,6 +131,11 @@ namespace ignition /// \return Volume of the box in m^3. public: Precision Volume() const; + /// \brief Get the volume of the box below a plane. + /// \param[in] _plane The plane which cuts the box. + /// \return Volume below the plane in m^3. + public: Precision VolumeBelow(const Plane &_plane) const; + /// \brief Compute the box's density given a mass value. The /// box is assumed to be solid with uniform density. This /// function requires the box's size to be set to @@ -166,6 +174,10 @@ namespace ignition /// \brief The box's material. private: ignition::math::Material material; + + /// \brief Get intersection between a plane and the box. + private: std::vector> GetIntersections( + const Plane &_plane) const; }; /// \typedef Box Boxi diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index b7ae61a7a..305514ac8 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -21,6 +21,8 @@ #include #include #include +#include +#include namespace ignition { @@ -119,6 +121,73 @@ namespace ignition return this->normal.Dot(_point) - this->d; } + /// \brief Given x,y point find corresponding Z point on plane. + /// \param[in] x - 2d x coordinate. + /// \param[in] y - 2d y coordinate. + /// \return coincident point. + public: Vector3 GetPointOnPlane(const T x, const T y) const + { + auto z_val = (this->Normal().Z() != 0) ? + (this->Offset() - (this->Normal().Dot({x,y,0})))/this->Normal().Z() + : 0; + auto coincidentPoint = Vector3{x,y,z_val}; + return coincidentPoint; + } + + /// \brief Get the intersection of a line with the plane + /// given the line's gradient and a point in parametrized space. + /// \param[in] _point A point that lies on a line. + /// \param[in] _gradient The gradient of the line. + /// \return The point of intersection. std::nullopt if the line is + /// parrallel to the plane. + public: std::optional> Intersect( + const Vector3 &_point, + const Vector3 &_gradient) const + { + if(abs(this->Normal().Dot(_gradient)) < 1e-6) + { + return std::nullopt; + } + auto constant = this->Offset() - this->Normal().Dot(_point); + auto param = constant/this->Normal().Dot(_gradient); + auto intersection = _point + _gradient*param; + return intersection; + } + + /// \brief Get the volume under a plane. + /// \requires The plane is not parallel to the z axis. And the slopes are + /// not vertical. + /// \param[in] _line1 A line that lies on the plane and bounds the x-axis + /// \param[in] _line2 A line that lies on the plane and bounds the x-axis + /// \param[in] _ylowerlimit The lower limit of the y-axis + /// \param[in] _yupperlimit The upper limit of the y-axis + /// \return The volume under the plane. + public: T Volume(const Line2 &_line1, + const Line2 &_line2, + const T _ylowerlimit, + const T _yupperlimit) const + { + auto l = _ylowerlimit; + auto m = _yupperlimit; + auto k = this->Offset(); + auto j = this->Normal().X()/this->Normal().Z(); + auto n = this->Normal().Y()/this->Normal().Z(); + + auto a_1 = _line1.Slope(); + auto a_2 = _line2.Slope(); + auto b_1 = _line1[0].Y() - _line1[0].X() * a_1; + auto b_2 = _line2[0].Y() - _line2[0].X() * a_1; + + // computed using wolfram alpha: + // https://www.wolframalpha.com/input/?i=integral+from+m+to+l+of+%28integral+++%28k+-+jx-+ny%29++dy+from+a_1x+%2B+b_1+to+a_2x+%2Bb_2%29+dx + auto vol = 1/6 * + (-3 * (l*l - m*m) * (a_1 * (k - b_1 * n) + + a_2 * (b_2 * n - k) + (b_2 - b_1) * j) + + (a_1 - a_2) * (l*l*l - m*m*m) * (a_1 * n + a_2 * n + 2 * j) + + 3 * (b_1 - b_2) * (l - m) * (b_1 * n + b_2 * n - 2 * k)); + return vol; + } + /// \brief The side of the plane a point is on. /// \param[in] _point The 3D point to check. /// \return Plane::NEGATIVE_SIDE if the distance from the point to the diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh index d9bfe89aa..184958137 100644 --- a/include/ignition/math/Sphere.hh +++ b/include/ignition/math/Sphere.hh @@ -20,6 +20,7 @@ #include "ignition/math/MassMatrix3.hh" #include "ignition/math/Material.hh" #include "ignition/math/Quaternion.hh" +#include "ignition/math/Plane.hh" namespace ignition { @@ -91,6 +92,12 @@ namespace ignition /// \return Volume of the sphere in m^3. public: Precision Volume() const; + /// \brief Get the volume of sphere below a given plane in m^3. + /// It is assumed that the center of the sphere is on the origin + /// \param[in] plane - the plane which slices this sphere. + /// \return Volume below the sphere in m^3. + public: Precision VolumeBelow(const Planed &_plane) const; + /// \brief Compute the sphere's density given a mass value. The /// sphere is assumed to be solid with uniform density. This /// function requires the sphere's radius to be set to a diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index de1a521da..f7d13169a 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -16,6 +16,8 @@ */ #ifndef IGNITION_MATH_DETAIL_BOX_HH_ #define IGNITION_MATH_DETAIL_BOX_HH_ + +#include namespace ignition { namespace math @@ -113,6 +115,151 @@ T Box::Volume() const return this->size.X() * this->size.Y() * this->size.Z(); } +///////////////////////////////////////////////// +template +std::pair, Line2> GetIntersectionPairPerpendicularToXAxis( + const std::vector> &_points) +{ + for(int i = 0; i < _points.size(); i++) + { + for(int j = 0; j < _points.size(); j++) + { + if(i == j) continue; + + auto line1 = Line2(_points[i].X(), _points[i].Y(), + _points[j].X(), _points[j].Y()); + + std::unordered_set set{0,1,2,3}; + set.erase(i); + set.erase(j); + + auto it = set.begin(); + it++; + auto line2 = Line2(_points[*it].X(), _points[*it].Y(), + _points[*set.begin()].X(), _points[*set.begin()].Y()); + + if(line1.Intersect(line2)) + continue; + + if(!std::isnan(line1.Slope()) && !std::isnan(line2.Slope())) + return std::make_pair(line1, line2); + } + } +} + +///////////////////////////////////////////////// +template +T Box::VolumeBelow(const Plane &_plane) const +{ + // Get coordinates of all vertice of box + // TODO: Cache this for performance + std::vector > vertices { + Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, + Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} + }; + + auto numVerticesBelow = 0; + for(auto &v : vertices) + { + if(_plane.Distance(v) < 0) + { + numVerticesBelow++; + } + } + + if (numVerticesBelow == 0) + { + return 0; + } + else if (numVerticesBelow == 1) + { + // Get vertex which is below the plance + Vector3 vertex; + for(auto &v : vertices) + { + if(_plane.Distance(v) < 0) + { + vertex = v; + } + } + // Compute tetrahedron by getting points of intersection + auto intersections = GetIntersections(_plane); + auto area = (intersections[1] - intersections[0]).Cross(intersections[2] - intersections[0]).Length() / 2; + return area * abs(_plane.Distance(vertex)) / 3; + } + else if (numVerticesBelow == 2) + { + // Compute integral of area under plane bounded by box + auto intersections = GetIntersections(_plane); + if(_plane.Normal().Z() != 0) + { + //TODO: Determine direction of cut + // Get integral bounds + auto [line1, line2] = GetIntersectionPairPerpendicularToXAxis(intersections); + return _plane.Volume(line1, line2, -this->size.X()/2, this->size.X()/2); + } + else + { + // Create a a new plane and box with corrected axis. + } + } + else if (numVerticesBelow == 3) + { + + } + else if (numVerticesBelow == 4) + { + if(_plane.Normal().Z() == 0) + { + // Switch Axis + + //Plane newPlane(); + } + // Compute integral of area under plane bounded by box + // Setup bounds + Line2 line1(-this->size.X()/2, -this->size.Y()/2, + this->size.X()/2, -this->size.Y()/2); + + Line2 line2(-this->size.X()/2, this->size.Y()/2, + this->size.X()/2, this->size.Y()/2); + return _plane.Volume(line1, line2, -this->size.X()/2, this->size.X()/2); + } + else if (numVerticesBelow == 5) + { + // Compute ?? + } + else if (numVerticesBelow == 6) + { + // Compute + } + else if (numVerticesBelow == 7) + { + // Get vertex which is below the plance + Vector3 vertex; + for(auto &v : vertices) + { + if(_plane.Distance(v) > 0) + { + vertex = v; + } + } + // Compute tetrahedron by getting points of intersection + auto intersections = this->GetIntersections(_plane); + auto area = 0.5 * (intersections[1] - intersections[0]).Cross(intersections[2] - intersections[0]).Length(); + return this->Volume() - area * abs(_plane.Distance(vertex)) / 3; + } + else + { + return this->Volume(); + } +} + ///////////////////////////////////////////////// template T Box::DensityFromMass(const T _mass) const @@ -139,6 +286,53 @@ bool Box::MassMatrix(MassMatrix3 &_massMat) const { return _massMat.SetFromBox(this->material, this->size); } + + +////////////////////////////////////////////////// +template +std::vector> Box::GetIntersections( + const Plane &_plane) const +{ + std::vector> intersections; + // These are vertice via which we can describe edges. We only need 4 such + // vertices + std::vector > vertices { + Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, + Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2} + }; + + // Axises + std::vector> axes { + Vector3{1, 0, 0}, + Vector3{0, 1, 0}, + Vector3{0, 0, 1} + }; + + // There are 12 edges. However, we just need 4 points to describe the + // twelve edges. + for(auto &v : vertices) + { + for(auto &a : axes) + { + auto intersection = _plane.Intersect(v, a); + if(intersection.has_value() && + intersection->X() >= -this->size.X()/2 && + intersection->X() <= this->size.X()/2 && + intersection->Y() >= -this->size.Y()/2 && + intersection->Y() <= this->size.Y()/2 && + intersection->Z() >= -this->size.Z()/2 && + intersection->Z() <= this->size.Z()/2) + { + intersections.push_back(intersection.value()); + } + } + } + + return intersections; +} + } } #endif diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 6c85c197e..a8e3b180d 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -94,6 +94,29 @@ T Sphere::Volume() const { return (4.0/3.0) * IGN_PI * std::pow(this->radius, 3); } +#include +////////////////////////////////////////////////// +template +T Sphere::VolumeBelow(const Planed &_plane) const +{ + auto r = this->radius; + //get nearest point to center on plane + auto dist = _plane.Distance(Vector3d(0,0,0)); + + if(dist < -r) + { + //sphere is completely below plane + return Volume(); + } + else if(dist > r) + { + //sphere is completely above plane + return 0; + } + + auto h = r + dist; + return IGN_PI * h * h * (3 * r - h) / 3; +} ////////////////////////////////////////////////// template diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index c15452f7c..fc2a3d50f 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -113,6 +113,37 @@ TEST(BoxTest, VolumeAndDensity) EXPECT_GT(0.0, box2.DensityFromMass(mass)); } + +////////////////////////////////////////////////// +TEST(BoxTest, VolumeBelow) +{ + { + // Case 0: No vertices below the plane + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(1.0, 1.0, 1.0), -1.1); + EXPECT_DOUBLE_EQ(0.0, box.VolumeBelow(plane)); + } + { + // Case 1: only one vertex below the plane + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(1.0, 1.0, 1.0), -1.1); + EXPECT_LT(box.VolumeBelow(plane), box.Volume()); + } + { + // Case 2: two vertices below the plane + } + { + // Case 3: three vertices below the plane + } + { + // Case 4: four vertices below the plane + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0, 0, 1.0), 0); + + EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); + } +} + ////////////////////////////////////////////////// TEST(BoxTest, Mass) { diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 4defeb839..b56e1bcf2 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -89,6 +89,17 @@ TEST(PlaneTest, Plane) EXPECT_TRUE(plane.Normal() == Vector3d(0, 1, 0)); EXPECT_TRUE(plane.Size() == Vector2d(4, 4)); } + + { + auto plane = Planed(Vector3d(0, 1, 0), Vector2d(4, 4), 5.0); + auto point = plane.GetPointOnPlane(9, 5); + EXPECT_NEAR(plane.Normal().Dot(point), plane.Offset(), 1e-3); + } + { + auto plane = Planed(Vector3d(0, 0, 1), Vector2d(4, 4), 5.0); + auto point = plane.GetPointOnPlane(9, 7); + EXPECT_NEAR(plane.Normal().Dot(point), plane.Offset(), 1e-3); + } } ///////////////////////////////////////////////// @@ -150,3 +161,12 @@ TEST(PlaneTest, SideAxisAlignedBox) EXPECT_EQ(plane.Side(box), Planed::BOTH_SIDE); } } + +///////////////////////////////////////////////// +TEST(PlaneTest, Intersection) +{ + Planed plane(Vector3d(0.5, 0, 1), 1); + auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(1, 0, 1)); + EXPECT_TRUE(intersect.has_value()); + EXPECT_NEAR(intersect->Dot(plane.Normal()), plane.Offset(), 1e-6); +} \ No newline at end of file diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index aa9143eee..9a1d7ddb0 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -131,3 +131,25 @@ TEST(SphereTest, Mass) EXPECT_EQ(expectedMassMat, massMat); EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass()); } + +////////////////////////////////////////////////// +TEST(SphereTest, VolumeBelow) +{ + double r = 2; + math::Sphered sphere(r); + + { + math::Planed _plane(math::Vector3d{0,0,1}, math::Vector2d(4, 4), 2*r); + EXPECT_NEAR(sphere.Volume(), sphere.VolumeBelow(_plane), 1e-3); + } + + { + math::Planed _plane(math::Vector3d{0,0,1}, math::Vector2d(4, 4), -2*r); + EXPECT_NEAR(sphere.VolumeBelow(_plane), 0, 1e-3); + } + + { + math::Planed _plane(math::Vector3d{0,0,1}, 0); + EXPECT_NEAR(sphere.Volume()/2, sphere.VolumeBelow(_plane), 1e-3); + } +} \ No newline at end of file From 008aa1b808bf3513de3e21697b7c2652e1e1d9fc Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 5 Aug 2021 09:24:30 +0800 Subject: [PATCH 02/81] fix tests Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 8 ++++---- include/ignition/math/detail/Box.hh | 5 ++++- src/Box_TEST.cc | 4 ++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 305514ac8..c9ca63d5d 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -169,7 +169,7 @@ namespace ignition { auto l = _ylowerlimit; auto m = _yupperlimit; - auto k = this->Offset(); + auto k = this->Offset()/this->Normal().Z(); auto j = this->Normal().X()/this->Normal().Z(); auto n = this->Normal().Y()/this->Normal().Z(); @@ -180,11 +180,11 @@ namespace ignition // computed using wolfram alpha: // https://www.wolframalpha.com/input/?i=integral+from+m+to+l+of+%28integral+++%28k+-+jx-+ny%29++dy+from+a_1x+%2B+b_1+to+a_2x+%2Bb_2%29+dx - auto vol = 1/6 * - (-3 * (l*l - m*m) * (a_1 * (k - b_1 * n) + auto vol = 1.0/6.0 * + (-3.0 * (l*l - m*m) * (a_1 * (k - b_1 * n) + a_2 * (b_2 * n - k) + (b_2 - b_1) * j) + (a_1 - a_2) * (l*l*l - m*m*m) * (a_1 * n + a_2 * n + 2 * j) - + 3 * (b_1 - b_2) * (l - m) * (b_1 * n + b_2 * n - 2 * k)); + + 3.0 * (b_1 - b_2) * (l - m) * (b_1 * n + b_2 * n - 2 * k)); return vol; } diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index f7d13169a..41da1dcf7 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -223,12 +223,15 @@ T Box::VolumeBelow(const Plane &_plane) const } // Compute integral of area under plane bounded by box // Setup bounds + Plane newPlane( + _plane.Normal(), _plane.Offset() + (this->size.Z()/2)*_plane.Normal().Z()); Line2 line1(-this->size.X()/2, -this->size.Y()/2, this->size.X()/2, -this->size.Y()/2); Line2 line2(-this->size.X()/2, this->size.Y()/2, this->size.X()/2, this->size.Y()/2); - return _plane.Volume(line1, line2, -this->size.X()/2, this->size.X()/2); + + return newPlane.Volume(line2, line1, -this->size.X()/2, this->size.X()/2); } else if (numVerticesBelow == 5) { diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index fc2a3d50f..669be98c3 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -120,7 +120,7 @@ TEST(BoxTest, VolumeBelow) { // Case 0: No vertices below the plane math::Boxd box(2.0, 2.0, 2.0); - math::Planed plane(math::Vector3d(1.0, 1.0, 1.0), -1.1); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); EXPECT_DOUBLE_EQ(0.0, box.VolumeBelow(plane)); } { @@ -138,7 +138,7 @@ TEST(BoxTest, VolumeBelow) { // Case 4: four vertices below the plane math::Boxd box(2.0, 2.0, 2.0); - math::Planed plane(math::Vector3d(0, 0, 1.0), 0); + math::Planed plane(math::Vector3d(0, 0, 2.0), 0); EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); } From 0e40a5f1328d4c1e2d16537c691b03628e07549b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 5 Aug 2021 15:40:00 +0800 Subject: [PATCH 03/81] nearly there for boxes Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 8 +++++--- src/Box_TEST.cc | 6 ++++++ 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 41da1dcf7..adc918812 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -117,7 +117,7 @@ T Box::Volume() const ///////////////////////////////////////////////// template -std::pair, Line2> GetIntersectionPairPerpendicularToXAxis( +std::pair, Line2> GetIntersectionPairAlongXAxis( const std::vector> &_points) { for(int i = 0; i < _points.size(); i++) @@ -195,14 +195,16 @@ T Box::VolumeBelow(const Plane &_plane) const } else if (numVerticesBelow == 2) { + Plane newPlane( + _plane.Normal(), _plane.Offset() + (this->size.Z()/2)*_plane.Normal().Z()); // Compute integral of area under plane bounded by box auto intersections = GetIntersections(_plane); if(_plane.Normal().Z() != 0) { //TODO: Determine direction of cut // Get integral bounds - auto [line1, line2] = GetIntersectionPairPerpendicularToXAxis(intersections); - return _plane.Volume(line1, line2, -this->size.X()/2, this->size.X()/2); + auto [line1, line2] = GetIntersectionPairAlongXAxis(intersections); + return abs(newPlane.Volume(line1, line2, -this->size.X()/2, this->size.X()/2)); } else { diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 669be98c3..f1e338431 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -131,6 +131,12 @@ TEST(BoxTest, VolumeBelow) } { // Case 2: two vertices below the plane + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(1.0, 0.0, 1.0), -0.5); + EXPECT_DOUBLE_EQ(2.0, box.VolumeBelow(plane)); + + math::Planed plane2(math::Vector3d(0.0, 1.0, 1.0), -0.5); + EXPECT_DOUBLE_EQ(2.0, box.VolumeBelow(plane2)); } { // Case 3: three vertices below the plane From c18b73537374205e9ed8118455e27582817d012f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 6 Aug 2021 15:46:19 +0800 Subject: [PATCH 04/81] Fix boxes Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 162 +++++++++++++--------------- 1 file changed, 73 insertions(+), 89 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index adc918812..84bc19aed 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -149,120 +149,104 @@ std::pair, Line2> GetIntersectionPairAlongXAxis( ///////////////////////////////////////////////// template -T Box::VolumeBelow(const Plane &_plane) const +std::optional> GetLineOfEntryAlongZPlane( + const std::vector> &_points, + float _zplane_value, + Vector3 axis = Vector3(0,0,1) +) { - // Get coordinates of all vertice of box - // TODO: Cache this for performance - std::vector > vertices { - Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, - Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, - Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, - Vector3{-this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, - Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, - Vector3{-this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, - Vector3{this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, - Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} - }; - - auto numVerticesBelow = 0; - for(auto &v : vertices) + Vector3 points[2]; + int numPoints = 0; + for(auto v : _zplane_value) { - if(_plane.Distance(v) < 0) + if (v.Dot(axis) == _zplane_value) { - numVerticesBelow++; + points[numPoints] = v; + numPoints++; } - } - if (numVerticesBelow == 0) - { - return 0; + if (numPoints == 2) + break; } - else if (numVerticesBelow == 1) + if(numPoints != 2) + return std::nullopt; + return Line2(points[0], points[1]); +} + +///////////////////////////////////////////////// +template +Vector3 axisOfCut(const Box& box, std::vector>& intersections) +{ + int numXAxis = 0; + int numYAxis = 0; + int numZAxis = 0; + + for(auto point: intersections) { - // Get vertex which is below the plance - Vector3 vertex; - for(auto &v : vertices) + if(point.X() == -this->size.X()/2 || point.X() == this->size.X()/2) { - if(_plane.Distance(v) < 0) - { - vertex = v; - } + numXAxis++; } - // Compute tetrahedron by getting points of intersection - auto intersections = GetIntersections(_plane); - auto area = (intersections[1] - intersections[0]).Cross(intersections[2] - intersections[0]).Length() / 2; - return area * abs(_plane.Distance(vertex)) / 3; - } - else if (numVerticesBelow == 2) - { - Plane newPlane( - _plane.Normal(), _plane.Offset() + (this->size.Z()/2)*_plane.Normal().Z()); - // Compute integral of area under plane bounded by box - auto intersections = GetIntersections(_plane); - if(_plane.Normal().Z() != 0) + + if(point.Y() == -this->size.Y()/2 || point.Y() == this->size.Y()/2) { - //TODO: Determine direction of cut - // Get integral bounds - auto [line1, line2] = GetIntersectionPairAlongXAxis(intersections); - return abs(newPlane.Volume(line1, line2, -this->size.X()/2, this->size.X()/2)); + numYAxis++; } - else + + if(point.Z() == -this->size.Z()/2 || point.Z() == this->size.Z()/2) { - // Create a a new plane and box with corrected axis. + numZAxis++; } } - else if (numVerticesBelow == 3) - { - } - else if (numVerticesBelow == 4) + if(numXAxis >= numYAxis && numXAxis >= numZAxis) { - if(_plane.Normal().Z() == 0) - { - // Switch Axis - - //Plane newPlane(); - } - // Compute integral of area under plane bounded by box - // Setup bounds - Plane newPlane( - _plane.Normal(), _plane.Offset() + (this->size.Z()/2)*_plane.Normal().Z()); - Line2 line1(-this->size.X()/2, -this->size.Y()/2, - this->size.X()/2, -this->size.Y()/2); - - Line2 line2(-this->size.X()/2, this->size.Y()/2, - this->size.X()/2, this->size.Y()/2); - - return newPlane.Volume(line2, line1, -this->size.X()/2, this->size.X()/2); + return Vector3 (1,0,0); } - else if (numVerticesBelow == 5) + + if(numYAxis >= numXAxis && numYAxis >= numZAxis) { - // Compute ?? + return Vector3 (1,0,0); } - else if (numVerticesBelow == 6) + + if(numZAxis >= numYAxis && numZAxis >= numXAxis) { - // Compute + return Vector3 (1,0,0); } - else if (numVerticesBelow == 7) +} + +///////////////////////////////////////////////// +template +T Box::VolumeBelow(const Plane &_plane) const +{ + // Get coordinates of all vertice of box + // TODO: Cache this for performance + std::vector > vertices { + Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, + Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} + }; + + std::vector> verticesBelow; + for(auto &v : vertices) { - // Get vertex which is below the plance - Vector3 vertex; - for(auto &v : vertices) + if(_plane.Distance(v) < 0) { - if(_plane.Distance(v) > 0) - { - vertex = v; - } + verticesBelow.push_back(v); } - // Compute tetrahedron by getting points of intersection - auto intersections = this->GetIntersections(_plane); - auto area = 0.5 * (intersections[1] - intersections[0]).Cross(intersections[2] - intersections[0]).Length(); - return this->Volume() - area * abs(_plane.Distance(vertex)) / 3; - } - else - { - return this->Volume(); } + + auto intersectionPoints = GetIntersections(_plane); + + // std::vector> polytopeEdges = + + // Construct a convex hull. Use the Gift-Wrapping method for simplicity + // Intersection Points will form the first face. } ///////////////////////////////////////////////// From 40fa5b3b0b5cca30f737d5f6f9dfe4ae01fa386a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 9 Aug 2021 15:45:58 +0800 Subject: [PATCH 05/81] use convex hull algo Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 197 ++++++++++++++++------------ 1 file changed, 112 insertions(+), 85 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 84bc19aed..244a562bf 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -17,6 +17,9 @@ #ifndef IGNITION_MATH_DETAIL_BOX_HH_ #define IGNITION_MATH_DETAIL_BOX_HH_ +#include "ignition/math/Triangle3.hh" + +#include #include namespace ignition { @@ -117,103 +120,65 @@ T Box::Volume() const ///////////////////////////////////////////////// template -std::pair, Line2> GetIntersectionPairAlongXAxis( - const std::vector> &_points) -{ - for(int i = 0; i < _points.size(); i++) - { - for(int j = 0; j < _points.size(); j++) - { - if(i == j) continue; - - auto line1 = Line2(_points[i].X(), _points[i].Y(), - _points[j].X(), _points[j].Y()); - - std::unordered_set set{0,1,2,3}; - set.erase(i); - set.erase(j); - - auto it = set.begin(); - it++; - auto line2 = Line2(_points[*it].X(), _points[*it].Y(), - _points[*set.begin()].X(), _points[*set.begin()].Y()); - - if(line1.Intersect(line2)) - continue; - - if(!std::isnan(line1.Slope()) && !std::isnan(line2.Slope())) - return std::make_pair(line1, line2); - } - } -} - +struct TriangleFace { + Triangle3 triangle; + std::size_t indices[3]; +}; ///////////////////////////////////////////////// template -std::optional> GetLineOfEntryAlongZPlane( - const std::vector> &_points, - float _zplane_value, - Vector3 axis = Vector3(0,0,1) -) +bool isConvexOuterFace( + const TriangleFace &_face, const std::vector> &_vertices) { - Vector3 points[2]; - int numPoints = 0; - for(auto v : _zplane_value) - { - if (v.Dot(axis) == _zplane_value) - { - points[numPoints] = v; - numPoints++; - } + Plane plane(_face.triangle.Normal(), + _face.triangle.Normal().Dot(_vertices[_face.indices[0]])); - if (numPoints == 2) - break; - } - if(numPoints != 2) - return std::nullopt; - return Line2(points[0], points[1]); -} + std::optional signPositive = std::nullopt; -///////////////////////////////////////////////// -template -Vector3 axisOfCut(const Box& box, std::vector>& intersections) -{ - int numXAxis = 0; - int numYAxis = 0; - int numZAxis = 0; - - for(auto point: intersections) + for (auto v: _vertices) { - if(point.X() == -this->size.X()/2 || point.X() == this->size.X()/2) - { - numXAxis++; - } + auto dist = plane.Distance(v); + if (dist == 0) continue; - if(point.Y() == -this->size.Y()/2 || point.Y() == this->size.Y()/2) + if (dist > 0) { - numYAxis++; + if (signPositive.has_value()) + { + if(!signPositive.value()) return false; + } + signPositive = true; } - - if(point.Z() == -this->size.Z()/2 || point.Z() == this->size.Z()/2) + else { - numZAxis++; + if (signPositive.has_value()) + { + if(signPositive.value()) return false; + } + signPositive = false; } } - if(numXAxis >= numYAxis && numXAxis >= numZAxis) - { - return Vector3 (1,0,0); - } + return true; +} +///////////////////////////////////////////////// +struct Edge +{ + std::size_t a, b; - if(numYAxis >= numXAxis && numYAxis >= numZAxis) - { - return Vector3 (1,0,0); - } + Edge(std::size_t _a, std::size_t _b): a(_a), b(_b) {} - if(numZAxis >= numYAxis && numZAxis >= numXAxis) + bool operator==(const Edge& other) const { - return Vector3 (1,0,0); + return (a == other.a && b == other.b) || + (a == other.b && b == other.a); } -} +}; +///////////////////////////////////////////////// +struct EdgeHash { + std::size_t operator()(const Edge &_e) const + { + return std::hash()(_e.a) ^ std::hash()(_e.b); + }; +}; ///////////////////////////////////////////////// template @@ -232,7 +197,7 @@ T Box::VolumeBelow(const Plane &_plane) const Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} }; - std::vector> verticesBelow; + std::vector > verticesBelow; for(auto &v : vertices) { if(_plane.Distance(v) < 0) @@ -241,12 +206,74 @@ T Box::VolumeBelow(const Plane &_plane) const } } - auto intersectionPoints = GetIntersections(_plane); + if(verticesBelow.size() == 0) + return 0; + //if(verticesBelow.size() == 8) + // return Volume(); + + auto intersections = GetIntersections(_plane); + verticesBelow.insert(verticesBelow.end(), + intersections.begin(), intersections.end()); + + /// Get the convex hull triangle mesh of the vertices in the shape. + /// This uses the so called Gift-Wrapping algorithm. It isn't the most + /// efficient but given the smalll number of points it should be fast enough. + std::queue activeEdges; + + std::vector> triangles; + triangles.push_back(TriangleFace{ + Triangle3{ + verticesBelow[verticesBelow.size() - 1], + verticesBelow[verticesBelow.size() - 2], + verticesBelow[verticesBelow.size() - 3]}, + {verticesBelow.size() - 1, verticesBelow.size() - 2, verticesBelow.size() - 3} + }); + activeEdges.emplace(verticesBelow.size() - 1, verticesBelow.size() - 2); + activeEdges.emplace(verticesBelow.size() - 2, verticesBelow.size() - 3); + activeEdges.emplace(verticesBelow.size() - 1, verticesBelow.size() - 3); + + std::unordered_set exploredEdges; + while(!activeEdges.empty()) + { + auto edge = activeEdges.front(); + activeEdges.pop(); + if(exploredEdges.count(edge) != 0) + continue; + exploredEdges.insert(edge); + for (std::size_t i = 0; i < verticesBelow.size(); ++i) + { + if (i == edge.a || i == edge.b) continue; + // Attempt to create a triangle + TriangleFace newTriangle{ + Triangle3( + verticesBelow[edge.a], + verticesBelow[edge.b], + verticesBelow[i]), + {edge.a, edge.b, i} + }; + + if(!isConvexOuterFace(newTriangle, verticesBelow)) continue; + + triangles.push_back(newTriangle); + + if(exploredEdges.count(edge) == 0) + activeEdges.emplace(i, edge.a); + if(exploredEdges.count(edge) == 0) + activeEdges.emplace(i, edge.b); + } + } - // std::vector> polytopeEdges = + // Calculate the volume of the triangles + // https://n-e-r-v-o-u-s.com/blog/?p=4415 + T volume = 0; + for(auto triangle : triangles) + { + auto crossProduct = verticesBelow[triangle.indices[0]] + .Cross(verticesBelow[triangle.indices[1]]); + volume += crossProduct.Dot(verticesBelow[triangle.indices[2]]); + } - // Construct a convex hull. Use the Gift-Wrapping method for simplicity - // Intersection Points will form the first face. + return abs(volume)/6; } ///////////////////////////////////////////////// From e275ff8667c4ef59bbb470a97981678002b09bd9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 10 Aug 2021 18:06:32 +0800 Subject: [PATCH 06/81] Add overlap method Signed-off-by: Arjo Chakravarty --- include/ignition/math/Triangle3.hh | 35 +++++++++++++ include/ignition/math/detail/Box.hh | 80 ++++++++++++++++++++++++++--- src/Box_TEST.cc | 8 +-- src/Triangle3_TEST.cc | 21 ++++++++ 4 files changed, 132 insertions(+), 12 deletions(-) diff --git a/include/ignition/math/Triangle3.hh b/include/ignition/math/Triangle3.hh index 5835de3fa..5c565b619 100644 --- a/include/ignition/math/Triangle3.hh +++ b/include/ignition/math/Triangle3.hh @@ -234,6 +234,41 @@ namespace ignition return false; } + /// \brief Get whether a triangle overlaps with another triangle. + /// \return boolean indicating whether the triangles overlap. + public: bool Overlaps(const Triangle3 &_triangle) const + { + int sideCount = 0; + for(unsigned int i = 0; i < 3; ++i) + { + for(unsigned int j = i; j < 3; ++j) + { + Vector3d unused; + if(this->Side(i).Intersect(_triangle.Side(j), unused)) + { + sideCount++; + } + } + } + /// If the triangles are adjacent to each other they may share an + /// intersecting edge. However if two triangles overlap they must have + /// at least two intersecting edges. + if(sideCount > 1) + { + return true; + } + + // Check if any of the points of the triangles are inside the other + for(unsigned int i = 0; i < 3; ++i) + { + if(_triangle.Contains(this->pts[i])) + return true; + if(this->Contains(_triangle.pts[i])) + return true; + } + return false; + } + /// \brief Get the length of the triangle's perimeter. /// \return Sum of the triangle's line segments. public: T Perimeter() const diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 244a562bf..d54d48d19 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -123,6 +123,20 @@ template struct TriangleFace { Triangle3 triangle; std::size_t indices[3]; + + bool operator==(const TriangleFace &_t) const + { + std::unordered_set indices1, indices2; + indices1.insert(this->indices[0]); + indices1.insert(this->indices[1]); + indices1.insert(this->indices[2]); + + indices2.insert(_t.indices[0]); + indices2.insert(_t.indices[1]); + indices2.insert(_t.indices[2]); + + return indices1 == indices2; + } }; ///////////////////////////////////////////////// template @@ -180,6 +194,31 @@ struct EdgeHash { }; }; +///////////////////////////////////////////////// +template +struct TriangleFaceHash { + std::size_t operator()(const TriangleFace &_t) const + { + return std::hash()(_t.indices[0]) + ^ std::hash()(_t.indices[1]) + ^ std::hash()(_t.indices[2]); + } +}; +///////////////////////////////////////////////// +template +bool HasOverlap( + const TriangleFace& face, + const std::unordered_set, TriangleFaceHash> &_faces) +{ + for (auto f: _faces) + { + if(f.triangle.Overlaps(face.triangle)) + { + return true; + } + } + return false; +} ///////////////////////////////////////////////// template T Box::VolumeBelow(const Plane &_plane) const @@ -220,8 +259,8 @@ T Box::VolumeBelow(const Plane &_plane) const /// efficient but given the smalll number of points it should be fast enough. std::queue activeEdges; - std::vector> triangles; - triangles.push_back(TriangleFace{ + std::unordered_set, TriangleFaceHash> triangles; + triangles.insert(TriangleFace{ Triangle3{ verticesBelow[verticesBelow.size() - 1], verticesBelow[verticesBelow.size() - 2], @@ -252,27 +291,52 @@ T Box::VolumeBelow(const Plane &_plane) const {edge.a, edge.b, i} }; - if(!isConvexOuterFace(newTriangle, verticesBelow)) continue; + std::cout << "Testing edge " << verticesBelow[edge.a] + << "--" << verticesBelow[edge.b] << std::endl; + std::cout << "Point: " << verticesBelow[i] << std::endl; + + if(!isConvexOuterFace(newTriangle, verticesBelow) + || HasOverlap(newTriangle, triangles)) continue; - triangles.push_back(newTriangle); + std::cout << "Accepted" << std::endl; - if(exploredEdges.count(edge) == 0) - activeEdges.emplace(i, edge.a); - if(exploredEdges.count(edge) == 0) - activeEdges.emplace(i, edge.b); + bool triangleIsNew = false; + Edge newEdge1{edge.a, i}; + if(exploredEdges.count(newEdge1) == 0) + { + triangleIsNew = true; + activeEdges.push(newEdge1); + } + Edge newEdge2{edge.b, i}; + if(exploredEdges.count(newEdge2) == 0) + { + triangleIsNew = true; + activeEdges.push(newEdge2); + } + if(triangleIsNew) + { + triangles.insert(newTriangle); + } } } // Calculate the volume of the triangles // https://n-e-r-v-o-u-s.com/blog/?p=4415 T volume = 0; + std::cout << "Triangle Mesh" << std::endl; for(auto triangle : triangles) { + std::cout << "\tT: " << std::endl; + std::cout << "\t\t" << verticesBelow[triangle.indices[0]] << std::endl; + std::cout << "\t\t" << verticesBelow[triangle.indices[1]] << std::endl; + std::cout << "\t\t" << verticesBelow[triangle.indices[2]] << std::endl; auto crossProduct = verticesBelow[triangle.indices[0]] .Cross(verticesBelow[triangle.indices[1]]); volume += crossProduct.Dot(verticesBelow[triangle.indices[2]]); } + std::cout << "Triangles " << triangles.size() < Date: Wed, 11 Aug 2021 17:46:12 +0800 Subject: [PATCH 07/81] Finally `Box::VolumeBelow()` works :tada: Signed-off-by: Arjo Chakravarty --- include/ignition/math/Triangle3.hh | 23 +++-- include/ignition/math/Vector3.hh | 8 ++ include/ignition/math/detail/Box.hh | 144 ++++++++++++++-------------- 3 files changed, 98 insertions(+), 77 deletions(-) diff --git a/include/ignition/math/Triangle3.hh b/include/ignition/math/Triangle3.hh index 5c565b619..633c5695c 100644 --- a/include/ignition/math/Triangle3.hh +++ b/include/ignition/math/Triangle3.hh @@ -236,17 +236,27 @@ namespace ignition /// \brief Get whether a triangle overlaps with another triangle. /// \return boolean indicating whether the triangles overlap. - public: bool Overlaps(const Triangle3 &_triangle) const + public: bool Overlaps(const Triangle3 &_triangle, T thresh=1e-6) const { int sideCount = 0; for(unsigned int i = 0; i < 3; ++i) { for(unsigned int j = i; j < 3; ++j) { - Vector3d unused; - if(this->Side(i).Intersect(_triangle.Side(j), unused)) + Vector3d intersection; + if(this->Side(i).Intersect(_triangle.Side(j), intersection)) { - sideCount++; + bool countSide = true; + for(unsigned int k = 0; k < 3; ++k) + { + if(this->pts[k].Distance(intersection) < thresh|| + _triangle.pts[k].Distance(intersection) < thresh) + { + countSide = false; + } + } + if(countSide) + sideCount++; } } } @@ -255,17 +265,18 @@ namespace ignition /// at least two intersecting edges. if(sideCount > 1) { + std::cout<< "Sides: " << sideCount << std::endl; return true; } // Check if any of the points of the triangles are inside the other - for(unsigned int i = 0; i < 3; ++i) + /*for(unsigned int i = 0; i < 3; ++i) { if(_triangle.Contains(this->pts[i])) return true; if(this->Contains(_triangle.pts[i])) return true; - } + }*/ return false; } diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index 4d5622374..300fd3e86 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -260,6 +260,14 @@ namespace ignition return n.Normalize(); } + /// \brief Get Projection of another onto this vector + /// \param[in] _v the vector + /// \return the projection + public: T Project(const Vector3 &_v) const + { + return this->Dot(_v) / this->Length(); + } + /// \brief Get distance to a line /// \param[in] _pt1 first point on the line /// \param[in] _pt2 second point on the line diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index d54d48d19..d9520f7cd 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -219,6 +219,53 @@ bool HasOverlap( } return false; } +////////////////////////////////////////////////// +template +std::vector> TrianglesInPlane(Plane &_plane, + std::vector> _vertices) +{ + std::vector> triangles; + std::vector> pointsInPlane; + Vector3 centroid; + for(auto pt: _vertices) + { + if(_plane.Side(pt) == Plane::NO_SIDE) + { + pointsInPlane.push_back(pt); + centroid += pt; + } + } + centroid /= pointsInPlane.size(); + + if(pointsInPlane.size() < 3) + return {}; + + auto axis1 = (pointsInPlane[0] - centroid).Normalize(); + auto axis2 = axis1.Cross(_plane.Normal()).Normalize(); + + std::sort(pointsInPlane.begin(), pointsInPlane.end(), + [centroid, axis1, axis2] (const Vector3 &a, const Vector3 &b) + { + auto aDisplacement = a - centroid; + auto bDisplacement = b - centroid; + + auto aX = axis1.Project(aDisplacement); + auto aY = axis2.Project(aDisplacement); + + auto bX = axis1.Project(bDisplacement); + auto bY = axis2.Project(bDisplacement); + + return atan2(aY, aX) < atan2(bY, bX); + }); + for(std::size_t i = 0; i < pointsInPlane.size(); ++i) + { + triangles.push_back( + Triangle3(pointsInPlane[i], + pointsInPlane[(i+1) % pointsInPlane.size()], centroid)); + } + + return triangles; +} ///////////////////////////////////////////////// template T Box::VolumeBelow(const Plane &_plane) const @@ -254,70 +301,25 @@ T Box::VolumeBelow(const Plane &_plane) const verticesBelow.insert(verticesBelow.end(), intersections.begin(), intersections.end()); - /// Get the convex hull triangle mesh of the vertices in the shape. - /// This uses the so called Gift-Wrapping algorithm. It isn't the most - /// efficient but given the smalll number of points it should be fast enough. - std::queue activeEdges; - - std::unordered_set, TriangleFaceHash> triangles; - triangles.insert(TriangleFace{ - Triangle3{ - verticesBelow[verticesBelow.size() - 1], - verticesBelow[verticesBelow.size() - 2], - verticesBelow[verticesBelow.size() - 3]}, - {verticesBelow.size() - 1, verticesBelow.size() - 2, verticesBelow.size() - 3} - }); - activeEdges.emplace(verticesBelow.size() - 1, verticesBelow.size() - 2); - activeEdges.emplace(verticesBelow.size() - 2, verticesBelow.size() - 3); - activeEdges.emplace(verticesBelow.size() - 1, verticesBelow.size() - 3); + // Fit six planes to the vertices in the shape. + std::vector> triangles; + + std::vector> planes { + Plane{Vector3{0, 0, 1}, this->Size().Z()/2}, + Plane{Vector3{0, 0, 1}, -this->Size().Z()/2}, + Plane{Vector3{1, 0, 0}, this->Size().X()/2}, + Plane{Vector3{1, 0, 0}, -this->Size().X()/2}, + Plane{Vector3{0, 1, 0}, this->Size().Y()/2}, + Plane{Vector3{0, 1, 0}, -this->Size().Y()/2}, + _plane + }; - std::unordered_set exploredEdges; - while(!activeEdges.empty()) + for(auto &p : planes) { - auto edge = activeEdges.front(); - activeEdges.pop(); - if(exploredEdges.count(edge) != 0) - continue; - exploredEdges.insert(edge); - for (std::size_t i = 0; i < verticesBelow.size(); ++i) - { - if (i == edge.a || i == edge.b) continue; - // Attempt to create a triangle - TriangleFace newTriangle{ - Triangle3( - verticesBelow[edge.a], - verticesBelow[edge.b], - verticesBelow[i]), - {edge.a, edge.b, i} - }; - - std::cout << "Testing edge " << verticesBelow[edge.a] - << "--" << verticesBelow[edge.b] << std::endl; - std::cout << "Point: " << verticesBelow[i] << std::endl; - - if(!isConvexOuterFace(newTriangle, verticesBelow) - || HasOverlap(newTriangle, triangles)) continue; - - std::cout << "Accepted" << std::endl; - - bool triangleIsNew = false; - Edge newEdge1{edge.a, i}; - if(exploredEdges.count(newEdge1) == 0) - { - triangleIsNew = true; - activeEdges.push(newEdge1); - } - Edge newEdge2{edge.b, i}; - if(exploredEdges.count(newEdge2) == 0) - { - triangleIsNew = true; - activeEdges.push(newEdge2); - } - if(triangleIsNew) - { - triangles.insert(newTriangle); - } - } + auto new_triangles = TrianglesInPlane(p, verticesBelow); + triangles.insert(triangles.end(), + new_triangles.begin(), + new_triangles.end()); } // Calculate the volume of the triangles @@ -327,19 +329,19 @@ T Box::VolumeBelow(const Plane &_plane) const for(auto triangle : triangles) { std::cout << "\tT: " << std::endl; - std::cout << "\t\t" << verticesBelow[triangle.indices[0]] << std::endl; - std::cout << "\t\t" << verticesBelow[triangle.indices[1]] << std::endl; - std::cout << "\t\t" << verticesBelow[triangle.indices[2]] << std::endl; - auto crossProduct = verticesBelow[triangle.indices[0]] - .Cross(verticesBelow[triangle.indices[1]]); - volume += crossProduct.Dot(verticesBelow[triangle.indices[2]]); + std::cout << "\t\t" << triangle[0] << std::endl; + std::cout << "\t\t" << triangle[1] << std::endl; + std::cout << "\t\t" << triangle[2] << std::endl; + + auto crossProduct = triangle[2].Cross(triangle[1]); + volume += std::fabs(crossProduct.Dot(triangle[0])); } - std::cout << "Triangles " << triangles.size() < T Box::DensityFromMass(const T _mass) const From 13f0adef463b437cd86344ee4260bc2f9bea3dce Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 12 Aug 2021 12:31:10 +0800 Subject: [PATCH 08/81] signed volume Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 135 +++------------------------- src/Box_TEST.cc | 26 ++---- 2 files changed, 23 insertions(+), 138 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index d9520f7cd..abd36a083 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -19,8 +19,6 @@ #include "ignition/math/Triangle3.hh" -#include -#include namespace ignition { namespace math @@ -118,113 +116,12 @@ T Box::Volume() const return this->size.X() * this->size.Y() * this->size.Z(); } -///////////////////////////////////////////////// -template -struct TriangleFace { - Triangle3 triangle; - std::size_t indices[3]; - - bool operator==(const TriangleFace &_t) const - { - std::unordered_set indices1, indices2; - indices1.insert(this->indices[0]); - indices1.insert(this->indices[1]); - indices1.insert(this->indices[2]); - - indices2.insert(_t.indices[0]); - indices2.insert(_t.indices[1]); - indices2.insert(_t.indices[2]); - - return indices1 == indices2; - } -}; -///////////////////////////////////////////////// -template -bool isConvexOuterFace( - const TriangleFace &_face, const std::vector> &_vertices) -{ - Plane plane(_face.triangle.Normal(), - _face.triangle.Normal().Dot(_vertices[_face.indices[0]])); - - std::optional signPositive = std::nullopt; - - for (auto v: _vertices) - { - auto dist = plane.Distance(v); - if (dist == 0) continue; - - if (dist > 0) - { - if (signPositive.has_value()) - { - if(!signPositive.value()) return false; - } - signPositive = true; - } - else - { - if (signPositive.has_value()) - { - if(signPositive.value()) return false; - } - signPositive = false; - } - } - - return true; -} -///////////////////////////////////////////////// -struct Edge -{ - std::size_t a, b; - - Edge(std::size_t _a, std::size_t _b): a(_a), b(_b) {} - - bool operator==(const Edge& other) const - { - return (a == other.a && b == other.b) || - (a == other.b && b == other.a); - } -}; -///////////////////////////////////////////////// -struct EdgeHash { - std::size_t operator()(const Edge &_e) const - { - return std::hash()(_e.a) ^ std::hash()(_e.b); - }; -}; - -///////////////////////////////////////////////// -template -struct TriangleFaceHash { - std::size_t operator()(const TriangleFace &_t) const - { - return std::hash()(_t.indices[0]) - ^ std::hash()(_t.indices[1]) - ^ std::hash()(_t.indices[2]); - } -}; -///////////////////////////////////////////////// -template -bool HasOverlap( - const TriangleFace& face, - const std::unordered_set, TriangleFaceHash> &_faces) -{ - for (auto f: _faces) - { - if(f.triangle.Overlaps(face.triangle)) - { - return true; - } - } - return false; -} ////////////////////////////////////////////////// template -std::vector> TrianglesInPlane(Plane &_plane, - std::vector> _vertices) +std::vector, T>> TrianglesInPlane(Plane &_plane, + std::vector> &_vertices) { - std::vector> triangles; + std::vector, T>> triangles; std::vector> pointsInPlane; Vector3 centroid; for(auto pt: _vertices) @@ -259,9 +156,10 @@ std::vector> TrianglesInPlane(Plane &_plane, }); for(std::size_t i = 0; i < pointsInPlane.size(); ++i) { - triangles.push_back( + triangles.emplace_back( Triangle3(pointsInPlane[i], - pointsInPlane[(i+1) % pointsInPlane.size()], centroid)); + pointsInPlane[(i+1) % pointsInPlane.size()], centroid), + (_plane.Side({0,0,0}) == Plane::POSITIVE_SIDE) ? -1 : 1); } return triangles; @@ -302,15 +200,15 @@ T Box::VolumeBelow(const Plane &_plane) const intersections.begin(), intersections.end()); // Fit six planes to the vertices in the shape. - std::vector> triangles; + std::vector, T>> triangles; std::vector> planes { Plane{Vector3{0, 0, 1}, this->Size().Z()/2}, - Plane{Vector3{0, 0, 1}, -this->Size().Z()/2}, + Plane{Vector3{0, 0, -1}, this->Size().Z()/2}, Plane{Vector3{1, 0, 0}, this->Size().X()/2}, - Plane{Vector3{1, 0, 0}, -this->Size().X()/2}, + Plane{Vector3{-1, 0, 0}, this->Size().X()/2}, Plane{Vector3{0, 1, 0}, this->Size().Y()/2}, - Plane{Vector3{0, 1, 0}, -this->Size().Y()/2}, + Plane{Vector3{0, -1, 0}, this->Size().Y()/2}, _plane }; @@ -325,19 +223,14 @@ T Box::VolumeBelow(const Plane &_plane) const // Calculate the volume of the triangles // https://n-e-r-v-o-u-s.com/blog/?p=4415 T volume = 0; - std::cout << "Triangle Mesh" << std::endl; for(auto triangle : triangles) { - std::cout << "\tT: " << std::endl; - std::cout << "\t\t" << triangle[0] << std::endl; - std::cout << "\t\t" << triangle[1] << std::endl; - std::cout << "\t\t" << triangle[2] << std::endl; - - auto crossProduct = triangle[2].Cross(triangle[1]); - volume += std::fabs(crossProduct.Dot(triangle[0])); + auto crossProduct = (triangle.first[2]).Cross(triangle.first[1]); + auto meshVolume = std::fabs(crossProduct.Dot(triangle.first[0])); + volume += triangle.second * meshVolume; } - return volume/6; + return std::fabs(volume)/6; } diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 5a0888fa5..45ba6cc6e 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -118,35 +118,27 @@ TEST(BoxTest, VolumeAndDensity) TEST(BoxTest, VolumeBelow) { { - // Case 0: No vertices below the plane math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); - //EXPECT_DOUBLE_EQ(0.0, box.VolumeBelow(plane)); + EXPECT_DOUBLE_EQ(0.0, box.VolumeBelow(plane)); } { - // Case 1: only one vertex below the plane math::Boxd box(2.0, 2.0, 2.0); - math::Planed plane(math::Vector3d(1.0, 1.0, 1.0), -1.1); - //EXPECT_LT(box.VolumeBelow(plane), box.Volume()); + math::Planed plane(math::Vector3d(0, 0, 2.0), 0); + + EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); } + { - // Case 2: two vertices below the plane math::Boxd box(2.0, 2.0, 2.0); - math::Planed plane(math::Vector3d(1.0, 0.0, 1.0), -0.5); - //EXPECT_DOUBLE_EQ(2.0, box.VolumeBelow(plane)); + math::Planed plane(math::Vector3d(0, 0, 1.0), 0.5); - math::Planed plane2(math::Vector3d(0.0, 1.0, 1.0), -0.5); - //EXPECT_DOUBLE_EQ(2.0, box.VolumeBelow(plane2)); + EXPECT_DOUBLE_EQ(3*box.Volume()/4, box.VolumeBelow(plane)); } { - // Case 3: three vertices below the plane - } - { - // Case 4: four vertices below the plane math::Boxd box(2.0, 2.0, 2.0); - math::Planed plane(math::Vector3d(0, 0, 2.0), 0); - - EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 20.0); + EXPECT_DOUBLE_EQ(box.Volume(), box.VolumeBelow(plane)); } } From 5cfe42c660132f922f6baccedbd75701284bb7c0 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 12 Aug 2021 12:48:01 +0800 Subject: [PATCH 09/81] codecheck Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 4 ++-- include/ignition/math/Triangle3.hh | 20 +++++--------------- include/ignition/math/detail/Box.hh | 14 ++++++++------ include/ignition/math/detail/Sphere.hh | 8 ++++---- src/Plane_TEST.cc | 2 +- src/Sphere_TEST.cc | 8 ++++---- 6 files changed, 24 insertions(+), 32 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index c9ca63d5d..a43a19208 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -128,9 +128,9 @@ namespace ignition public: Vector3 GetPointOnPlane(const T x, const T y) const { auto z_val = (this->Normal().Z() != 0) ? - (this->Offset() - (this->Normal().Dot({x,y,0})))/this->Normal().Z() + (this->Offset() - (this->Normal().Dot({x, y, 0})))/this->Normal().Z() : 0; - auto coincidentPoint = Vector3{x,y,z_val}; + auto coincidentPoint = Vector3{x, y, z_val}; return coincidentPoint; } diff --git a/include/ignition/math/Triangle3.hh b/include/ignition/math/Triangle3.hh index 633c5695c..d78978a9a 100644 --- a/include/ignition/math/Triangle3.hh +++ b/include/ignition/math/Triangle3.hh @@ -236,7 +236,8 @@ namespace ignition /// \brief Get whether a triangle overlaps with another triangle. /// \return boolean indicating whether the triangles overlap. - public: bool Overlaps(const Triangle3 &_triangle, T thresh=1e-6) const + public: bool Overlaps(const Triangle3 &_triangle, + T thresh = 1e-6) const { int sideCount = 0; for(unsigned int i = 0; i < 3; ++i) @@ -246,17 +247,7 @@ namespace ignition Vector3d intersection; if(this->Side(i).Intersect(_triangle.Side(j), intersection)) { - bool countSide = true; - for(unsigned int k = 0; k < 3; ++k) - { - if(this->pts[k].Distance(intersection) < thresh|| - _triangle.pts[k].Distance(intersection) < thresh) - { - countSide = false; - } - } - if(countSide) - sideCount++; + sideCount++; } } } @@ -265,18 +256,17 @@ namespace ignition /// at least two intersecting edges. if(sideCount > 1) { - std::cout<< "Sides: " << sideCount << std::endl; return true; } // Check if any of the points of the triangles are inside the other - /*for(unsigned int i = 0; i < 3; ++i) + for(unsigned int i = 0; i < 3; ++i) { if(_triangle.Contains(this->pts[i])) return true; if(this->Contains(_triangle.pts[i])) return true; - }*/ + } return false; } diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index abd36a083..d848dcabf 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -19,6 +19,10 @@ #include "ignition/math/Triangle3.hh" +#include +#include +#include + namespace ignition { namespace math @@ -124,7 +128,7 @@ std::vector, T>> TrianglesInPlane(Plane &_plane, std::vector, T>> triangles; std::vector> pointsInPlane; Vector3 centroid; - for(auto pt: _vertices) + for(auto &pt : _vertices) { if(_plane.Side(pt) == Plane::NO_SIDE) { @@ -158,8 +162,8 @@ std::vector, T>> TrianglesInPlane(Plane &_plane, { triangles.emplace_back( Triangle3(pointsInPlane[i], - pointsInPlane[(i+1) % pointsInPlane.size()], centroid), - (_plane.Side({0,0,0}) == Plane::POSITIVE_SIDE) ? -1 : 1); + pointsInPlane[(i + 1) % pointsInPlane.size()], centroid), + (_plane.Side({0, 0, 0}) == Plane::POSITIVE_SIDE) ? -1 : 1); } return triangles; @@ -169,7 +173,7 @@ template T Box::VolumeBelow(const Plane &_plane) const { // Get coordinates of all vertice of box - // TODO: Cache this for performance + // TODO(arjo): Cache this for performance std::vector > vertices { Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, @@ -192,8 +196,6 @@ T Box::VolumeBelow(const Plane &_plane) const if(verticesBelow.size() == 0) return 0; - //if(verticesBelow.size() == 8) - // return Volume(); auto intersections = GetIntersections(_plane); verticesBelow.insert(verticesBelow.end(), diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index a8e3b180d..51a7b3121 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -100,17 +100,17 @@ template T Sphere::VolumeBelow(const Planed &_plane) const { auto r = this->radius; - //get nearest point to center on plane - auto dist = _plane.Distance(Vector3d(0,0,0)); + // get nearest point to center on plane + auto dist = _plane.Distance(Vector3d(0, 0, 0)); if(dist < -r) { - //sphere is completely below plane + // sphere is completely below plane return Volume(); } else if(dist > r) { - //sphere is completely above plane + // sphere is completely above plane return 0; } diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index b56e1bcf2..5bf3f6557 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -169,4 +169,4 @@ TEST(PlaneTest, Intersection) auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(1, 0, 1)); EXPECT_TRUE(intersect.has_value()); EXPECT_NEAR(intersect->Dot(plane.Normal()), plane.Offset(), 1e-6); -} \ No newline at end of file +} diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 9a1d7ddb0..47c71e9b0 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -139,17 +139,17 @@ TEST(SphereTest, VolumeBelow) math::Sphered sphere(r); { - math::Planed _plane(math::Vector3d{0,0,1}, math::Vector2d(4, 4), 2*r); + math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), 2*r); EXPECT_NEAR(sphere.Volume(), sphere.VolumeBelow(_plane), 1e-3); } { - math::Planed _plane(math::Vector3d{0,0,1}, math::Vector2d(4, 4), -2*r); + math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), -2*r); EXPECT_NEAR(sphere.VolumeBelow(_plane), 0, 1e-3); } { - math::Planed _plane(math::Vector3d{0,0,1}, 0); + math::Planed _plane(math::Vector3d{0, 0, 1}, 0); EXPECT_NEAR(sphere.Volume()/2, sphere.VolumeBelow(_plane), 1e-3); } -} \ No newline at end of file +} From d82a6ba4c49404467f6d9c5b5d2eafd23596fdfe Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 14 Aug 2021 17:21:07 +0800 Subject: [PATCH 10/81] Add center of volume recalculation Signed-off-by: Arjo Chakravarty --- include/ignition/math/Sphere.hh | 6 ++++++ include/ignition/math/detail/Sphere.hh | 29 ++++++++++++++++++++++++++ 2 files changed, 35 insertions(+) diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh index 184958137..e1febb197 100644 --- a/include/ignition/math/Sphere.hh +++ b/include/ignition/math/Sphere.hh @@ -98,6 +98,12 @@ namespace ignition /// \return Volume below the sphere in m^3. public: Precision VolumeBelow(const Planed &_plane) const; + /// \brief Center of volume below the plane. This is useful when + /// calculating where the buoyancy should be applied. + /// \param[in] plane - the plane which slices the sphere. + public: std::optional> + CenterOfVolumeBelow(const Planed &_plane) const; + /// \brief Compute the sphere's density given a mass value. The /// sphere is assumed to be solid with uniform density. This /// function requires the sphere's radius to be set to a diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 51a7b3121..32650fe64 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -118,6 +118,35 @@ T Sphere::VolumeBelow(const Planed &_plane) const return IGN_PI * h * h * (3 * r - h) / 3; } +////////////////////////////////////////////////// +template +std::optional> + Sphere::CenterOfVolumeBelow(const Planed &_plane) const +{ + auto r = this->radius; + // get nearest point to center on plane + auto dist = _plane.Distance(Vector3d(0, 0, 0)); + + if(dist < -r) + { + // sphere is completely below plane + return Vector3{0,0,0}; + } + else if(dist > r) + { + // sphere is completely above plane + return std::nullopt; + } + + auto h = r + dist; + + // Formula for geometric centorid: + // https://mathworld.wolfram.com/SphericalCap.html + auto numerator = 2 * r - h; + + return 3 * numerator * numerator / (4 * (3 * r - h)); +} + ////////////////////////////////////////////////// template bool Sphere::SetDensityFromMass(const T _mass) From 27287dc52c083e39134e8d191b3d46029e62ada3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 14 Aug 2021 17:38:46 +0800 Subject: [PATCH 11/81] Add center of volume functions Signed-off-by: Arjo Chakravarty --- include/ignition/math/Box.hh | 6 +++++ include/ignition/math/detail/Box.hh | 40 +++++++++++++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index d0db64319..7ed2cee42 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -136,6 +136,12 @@ namespace ignition /// \return Volume below the plane in m^3. public: Precision VolumeBelow(const Plane &_plane) const; + /// \brief Center of volume below the plane. This is useful when + /// calculating where the buoyancy should be applied. + /// \param[in] plane - the plane which slices the sphere. + public: std::optional> + CenterOfVolumeBelow(const Plane &_plane) const; + /// \brief Compute the box's density given a mass value. The /// box is assumed to be solid with uniform density. This /// function requires the box's size to be set to diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index d848dcabf..a5d72b3b7 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -235,8 +235,48 @@ T Box::VolumeBelow(const Plane &_plane) const return std::fabs(volume)/6; } +///////////////////////////////////////////////// +template +std::optional> + Box::CenterOfVolumeBelow(const Plane &_plane) const +{ + // Get coordinates of all vertice of box + // TODO(arjo): Cache this for performance + std::vector > vertices { + Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, + Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} + }; + + std::vector > verticesBelow; + for(auto &v : vertices) + { + if(_plane.Distance(v) < 0) + { + verticesBelow.push_back(v); + } + } + if(verticesBelow.size() == 0) + return std::nullopt; + auto intersections = GetIntersections(_plane); + verticesBelow.insert(verticesBelow.end(), + intersections.begin(), intersections.end()); + + Vector3 centroid; + for(auto v: verticesBelow) + { + centroid += v; + } + + return centroid; +} ///////////////////////////////////////////////// template T Box::DensityFromMass(const T _mass) const From f241019f5e47e7f3a7c48702279e69e0193a225c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 14 Aug 2021 17:44:03 +0800 Subject: [PATCH 12/81] codecheck Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 2 +- include/ignition/math/detail/Sphere.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index a5d72b3b7..f45295cf7 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -270,7 +270,7 @@ std::optional> intersections.begin(), intersections.end()); Vector3 centroid; - for(auto v: verticesBelow) + for(auto &v : verticesBelow) { centroid += v; } diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 32650fe64..b67c652c4 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -130,7 +130,7 @@ std::optional> if(dist < -r) { // sphere is completely below plane - return Vector3{0,0,0}; + return Vector3{0, 0, 0}; } else if(dist > r) { From 61258cdf9180fa62522e71997bd1460639257b35 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 14 Aug 2021 18:12:40 +0800 Subject: [PATCH 13/81] Trying to get windows CI to pass Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index a43a19208..69748fbc7 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -23,6 +23,7 @@ #include #include #include +#include namespace ignition { From 69af27589825d717eb6869de6d7bcc57762eba71 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 16 Aug 2021 13:34:18 +0800 Subject: [PATCH 14/81] Add preliminary cylinder logic. Signed-off-by: Arjo Chakravarty --- include/ignition/math/Cylinder.hh | 7 ++ include/ignition/math/detail/Cylinder.hh | 88 ++++++++++++++++++++++++ src/Cylinder_TEST.cc | 20 ++++++ 3 files changed, 115 insertions(+) diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 31a2eacca..927ce5aae 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -19,6 +19,7 @@ #include "ignition/math/MassMatrix3.hh" #include "ignition/math/Material.hh" +#include "ignition/math/Plane.hh" #include "ignition/math/Quaternion.hh" namespace ignition @@ -131,6 +132,10 @@ namespace ignition /// \return Volume of the cylinder in m^3. public: Precision Volume() const; + /// \brief Get the volume of the cylinder in m^3. + /// \return Volume of the cylinder in m^3. + public: Precision VolumeBelow(const Plane &_plane) const; + /// \brief Compute the cylinder's density given a mass value. The /// cylinder is assumed to be solid with uniform density. This /// function requires the cylinder's radius and length to be set to @@ -155,6 +160,8 @@ namespace ignition /// \sa Precision DensityFromMass(const Precision _mass) const public: bool SetDensityFromMass(const Precision _mass); + private: Precision CircleSegmentSliceArea(Precision _distance) const; + /// \brief Radius of the cylinder. private: Precision radius = 0.0; diff --git a/include/ignition/math/detail/Cylinder.hh b/include/ignition/math/detail/Cylinder.hh index ac4a96722..28667ac08 100644 --- a/include/ignition/math/detail/Cylinder.hh +++ b/include/ignition/math/detail/Cylinder.hh @@ -124,6 +124,83 @@ T Cylinder::Volume() const this->length; } +////////////////////////////////////////////////// +template +T Cylinder::VolumeBelow(const Plane &_plane) const +{ + auto length = this->Length(); + auto radius = this->Radius(); + + if(_plane.Normal().Dot(Vector3{0, 0, 1}) == T(0)) + { + // If the plane is parallel to the cylinder's axis + auto dist = _plane.Distance(Vector3(0,0,0)); + + if(abs(dist) >= radius) + { + if(dist < 0) + { + return Volume(); + } + else + { + return T(0); + } + } + else + { + auto volume = CircleSegmentSliceArea(abs(dist)) * this->Length(); + if(dist < 0) + { + return volume; + } + else + { + return this->Volume() - volume; + } + } + } + + //Compute intersection point of plane + auto theta = atan2(_plane.Normal().Y(), _plane.Normal().X()); + auto x = radius * cos(theta); + auto y = radius * sin(theta); + auto point_max = _plane.GetPointOnPlane(x, y); + x = radius * cos(theta + IGN_PI); + y = radius * sin(theta + IGN_PI); + auto point_min = _plane.GetPointOnPlane(x, y); + + //Get case type + if(abs(point_max.Z()) > length/2) + { + if(abs(point_min.Z()) > length/2) + { + // Plane cuts through both flat faces + + } + else + { + // Cuts through one flat face + // Point Min will be the point where it cuts through + // Next we need to determine which way is up. + + } + } + else if(abs(point_min.Z()) > length/2) + { + // Cuts through one flat face + } + else + { + // Plane Cuts through no flat faces. + auto a = abs(point_max.Z()) + length/2; + auto b = abs(point_min.Z()) + length/2; + auto avg_height = (a + b)/2; + return avg_height * IGN_PI * radius * radius; + } + +} + ////////////////////////////////////////////////// template bool Cylinder::SetDensityFromMass(const T _mass) @@ -144,6 +221,17 @@ T Cylinder::DensityFromMass(const T _mass) const return _mass / this->Volume(); } +////////////////////////////////////////////////// +template +T Cylinder::CircleSegmentSliceArea(T _distance) const +{ + auto r = this->Radius(); + auto theta = (_distance != T(0)) ? + 2 * acos(r/_distance) : + IGN_PI; + return r * r * (theta - sin(theta)) / 2; +} + } } #endif diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index c0ad31de2..0032a129b 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -138,3 +138,23 @@ TEST(CylinderTest, Mass) EXPECT_EQ(expectedMassMat, massMat); EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass()); } + +//////////////////////////////////////////////// +TEST(CylinderTest, VolumeBelow) +{ + math::Cylinderd cylinder(1.0, 1.0); + { + math::Planed plane(math::Vector3d(0, 1, 0), 0); + EXPECT_DOUBLE_EQ(cylinder.Volume()/2, cylinder.VolumeBelow(plane)); + } + + { + math::Planed plane(math::Vector3d(0, 1, 0), -100); + EXPECT_DOUBLE_EQ(0.0, cylinder.VolumeBelow(plane)); + } + + { + math::Planed plane(math::Vector3d(0, 1, 0), 100); + EXPECT_DOUBLE_EQ(cylinder.Volume(), cylinder.VolumeBelow(plane)); + } +} \ No newline at end of file From 3095359130e3a4c1f03544692a479e86a0a6c34c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 19 Aug 2021 10:27:27 +0800 Subject: [PATCH 15/81] Add plane transformation Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 9 +++++++++ include/ignition/math/Plane.hh | 12 +++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index f8d73b53f..a85b752da 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -229,6 +229,15 @@ namespace ignition return true; } + /// \brief Calculate distance between line and slope + /// \param[in] _pt point which we are measuring distance to. + /// \returns Distance from point to line. + public: T Distance(const Vector3 _pt) + { + auto d = (_pt - this->pts[0]).Cross(this->pts[1] - this->pts[0]); + return d.Length() / (this->pts[1] - this->pts[0]).Length(); + } + /// \brief Check if this line intersects the given line segment. /// \param[in] _line The line to check for intersection. /// \param[in] _epsilon The error bounds within which the intersection diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 69748fbc7..a69773369 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -21,8 +21,8 @@ #include #include #include -#include #include +#include #include namespace ignition @@ -189,6 +189,16 @@ namespace ignition return vol; } + /// \brief Transform a plane into another plane according to a quaternion + public: Plane Transform(const Matrix4 &_matrix) + { + auto newNormal = _matrix.Rotation() * this->Normal(); // See what I did there + auto newPoint = _matrix * this->GetPointOnPlane(T(0), T(0)); + auto offset = newNormal.Dot(newPoint); + + return Plane{newPoint, offset}; + } + /// \brief The side of the plane a point is on. /// \param[in] _point The 3D point to check. /// \return Plane::NEGATIVE_SIDE if the distance from the point to the From 37b3842b580bca3ae96952a67a96c6cb73b33b45 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 26 Aug 2021 16:59:28 +0800 Subject: [PATCH 16/81] fix function return value Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Sphere.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index b67c652c4..4fbba25b4 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -144,7 +144,7 @@ std::optional> // https://mathworld.wolfram.com/SphericalCap.html auto numerator = 2 * r - h; - return 3 * numerator * numerator / (4 * (3 * r - h)); + return {3 * numerator * numerator / (4 * (3 * r - h))}; } ////////////////////////////////////////////////// From bbed80700c862b66481810f8ee275b203d582efc Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 26 Aug 2021 21:13:13 +0800 Subject: [PATCH 17/81] fix types Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Sphere.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 4fbba25b4..a2c220d06 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -144,7 +144,7 @@ std::optional> // https://mathworld.wolfram.com/SphericalCap.html auto numerator = 2 * r - h; - return {3 * numerator * numerator / (4 * (3 * r - h))}; + return Vector3{0, 0, 3 * numerator * numerator / (4 * (3 * r - h))}; } ////////////////////////////////////////////////// From c6c7e00738922a002f4c9c1603d6708381fba71f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sun, 29 Aug 2021 16:31:50 +0800 Subject: [PATCH 18/81] Preliminary work for cylinders Signed-off-by: Arjo Chakravarty --- include/ignition/math/Cylinder.hh | 14 ++++- include/ignition/math/detail/Cylinder.hh | 75 +++++++++++++++++++----- 2 files changed, 73 insertions(+), 16 deletions(-) diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 927ce5aae..26e5d7f02 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -160,7 +160,19 @@ namespace ignition /// \sa Precision DensityFromMass(const Precision _mass) const public: bool SetDensityFromMass(const Precision _mass); - private: Precision CircleSegmentSliceArea(Precision _distance) const; + private: Precision + CircleSegmentSliceArea(const Precision _distance) const; + + /// \brief Get volume of a wedge cut from this cylinder. + private: Precision CylindricalWedgeVolume( + const Precision _b, + const Precision _a, + const Precision _h); + + private: std::pair, Vector3> + GetCylinderIntersectionsAtZ( + const Plane &_plane, + const Precision z) const; /// \brief Radius of the cylinder. private: Precision radius = 0.0; diff --git a/include/ignition/math/detail/Cylinder.hh b/include/ignition/math/detail/Cylinder.hh index 28667ac08..cc4d1fb06 100644 --- a/include/ignition/math/detail/Cylinder.hh +++ b/include/ignition/math/detail/Cylinder.hh @@ -171,24 +171,27 @@ T Cylinder::VolumeBelow(const Plane &_plane) const auto point_min = _plane.GetPointOnPlane(x, y); //Get case type - if(abs(point_max.Z()) > length/2) + if(point_max.Z() > length/2 && point_min.Z() < -length/2) { - if(abs(point_min.Z()) > length/2) - { - // Plane cuts through both flat faces - - } - else - { - // Cuts through one flat face - // Point Min will be the point where it cuts through - // Next we need to determine which way is up. + // Plane cuts through both faces + auto topPoints = + this->GetCylinderIntersectionsAtZ(_plane, length/2); + auto bottomPoints = + this->GetCylinderIntersectionsAtZ(_plane, -length/2); - } + auto topChord = topPoints.first - topPoints.second; } - else if(abs(point_min.Z()) > length/2) + else if(point_max.Z() > length/2 && point_min.Z() >= -length/2) { - // Cuts through one flat face + // Plane cuts through only top face + auto topPoints = + GetCylinderIntersectionsAtZ(_plane, length/2); + Line3 chord(topPoints.first, topPoints.second); + auto a = chord.Length()/2; + auto side = _plane.Distance(Vector3{0, 0, length/2}); + //auto b = (side < 0) ? + // this->radius - chord.Distance(Vector3{0, 0, length/2}): + // this->radius + chord.Distance(Vector3{0, 0, length/2}); } else { @@ -223,7 +226,7 @@ T Cylinder::DensityFromMass(const T _mass) const ////////////////////////////////////////////////// template -T Cylinder::CircleSegmentSliceArea(T _distance) const +T Cylinder::CircleSegmentSliceArea(const T _distance) const { auto r = this->Radius(); auto theta = (_distance != T(0)) ? @@ -232,6 +235,48 @@ T Cylinder::CircleSegmentSliceArea(T _distance) const return r * r * (theta - sin(theta)) / 2; } +////////////////////////////////////////////////// +template +T Cylinder::CylindricalWedgeVolume( + const T _b, const T _a, const T _h) +{ + auto r = this->Radius(); + auto psi = IGN_PI_2 + atan((_b - r)/_a); + return _h/(3*_b)*(_a*(3*r*r - _a*_a) + 3*r*r*(_b-r)*psi); +} + +////////////////////////////////////////////////// +template +std::pair, Vector3> + Cylinder::GetCylinderIntersectionsAtZ( + const Plane &_plane, + const T z) const +{ + auto k = (_plane.Offset() - _plane.Normal().Z() * z) + / this->Radius(); + auto a = _plane.Normal().X(); + auto b = _plane.Normal().Y(); + + auto internal = (b - sqrt(a*a + b*b - k*k))/(a+k); + auto theta1 = 2*(atan(internal)); + auto theta2 = 2*(atan(-internal)); + + math::Vector3d intersect1 + { + this->Radius() * cos(theta1), + this->Radius() * sin(theta1), + z + }; + + math::Vector3d intersect2 + { + this->Radius() * cos(theta2), + this->Radius() * sin(theta2), + z + }; + + return {intersect1, intersect2}; +} } } #endif From 278dc07456fb1b36a935d72f9141440763e4e124 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 08:22:06 +0800 Subject: [PATCH 19/81] remove unessecary include Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Sphere.hh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index a2c220d06..1c7c075da 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -94,7 +94,7 @@ T Sphere::Volume() const { return (4.0/3.0) * IGN_PI * std::pow(this->radius, 3); } -#include + ////////////////////////////////////////////////// template T Sphere::VolumeBelow(const Planed &_plane) const @@ -103,6 +103,8 @@ T Sphere::VolumeBelow(const Planed &_plane) const // get nearest point to center on plane auto dist = _plane.Distance(Vector3d(0, 0, 0)); + std::cout << dist << "\n"; + if(dist < -r) { // sphere is completely below plane From f4e70c76c39eeb9ec1120a854340d436de13d913 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 08:26:22 +0800 Subject: [PATCH 20/81] remove more useless logging Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Sphere.hh | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 1c7c075da..4c91a1b2f 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -103,8 +103,6 @@ T Sphere::VolumeBelow(const Planed &_plane) const // get nearest point to center on plane auto dist = _plane.Distance(Vector3d(0, 0, 0)); - std::cout << dist << "\n"; - if(dist < -r) { // sphere is completely below plane From 11180eefd2dc015f8466564f667bf1cd3e140bab Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 12:57:36 +0800 Subject: [PATCH 21/81] remove unused transform function. Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 10 ---------- src/Cylinder_TEST.cc | 2 +- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index a69773369..0e9c6c6b8 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -189,16 +189,6 @@ namespace ignition return vol; } - /// \brief Transform a plane into another plane according to a quaternion - public: Plane Transform(const Matrix4 &_matrix) - { - auto newNormal = _matrix.Rotation() * this->Normal(); // See what I did there - auto newPoint = _matrix * this->GetPointOnPlane(T(0), T(0)); - auto offset = newNormal.Dot(newPoint); - - return Plane{newPoint, offset}; - } - /// \brief The side of the plane a point is on. /// \param[in] _point The 3D point to check. /// \return Plane::NEGATIVE_SIDE if the distance from the point to the diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index 0032a129b..37370395c 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -157,4 +157,4 @@ TEST(CylinderTest, VolumeBelow) math::Planed plane(math::Vector3d(0, 1, 0), 100); EXPECT_DOUBLE_EQ(cylinder.Volume(), cylinder.VolumeBelow(plane)); } -} \ No newline at end of file +} From 6521336d34df603faebb487df56b74014dd5cb83 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 13:00:12 +0800 Subject: [PATCH 22/81] style fixes Signed-off-by: Arjo Chakravarty --- include/ignition/math/Cylinder.hh | 2 ++ include/ignition/math/detail/Cylinder.hh | 8 ++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 26e5d7f02..abf356d6f 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -22,6 +22,8 @@ #include "ignition/math/Plane.hh" #include "ignition/math/Quaternion.hh" +#include + namespace ignition { namespace math diff --git a/include/ignition/math/detail/Cylinder.hh b/include/ignition/math/detail/Cylinder.hh index cc4d1fb06..d4c025ada 100644 --- a/include/ignition/math/detail/Cylinder.hh +++ b/include/ignition/math/detail/Cylinder.hh @@ -134,7 +134,7 @@ T Cylinder::VolumeBelow(const Plane &_plane) const if(_plane.Normal().Dot(Vector3{0, 0, 1}) == T(0)) { // If the plane is parallel to the cylinder's axis - auto dist = _plane.Distance(Vector3(0,0,0)); + auto dist = _plane.Distance(Vector3(0, 0, 0)); if(abs(dist) >= radius) { @@ -161,7 +161,7 @@ T Cylinder::VolumeBelow(const Plane &_plane) const } } - //Compute intersection point of plane + // Compute intersection point of plane auto theta = atan2(_plane.Normal().Y(), _plane.Normal().X()); auto x = radius * cos(theta); auto y = radius * sin(theta); @@ -170,7 +170,7 @@ T Cylinder::VolumeBelow(const Plane &_plane) const y = radius * sin(theta + IGN_PI); auto point_min = _plane.GetPointOnPlane(x, y); - //Get case type + // Get case type if(point_max.Z() > length/2 && point_min.Z() < -length/2) { // Plane cuts through both faces @@ -189,7 +189,7 @@ T Cylinder::VolumeBelow(const Plane &_plane) const Line3 chord(topPoints.first, topPoints.second); auto a = chord.Length()/2; auto side = _plane.Distance(Vector3{0, 0, length/2}); - //auto b = (side < 0) ? + // auto b = (side < 0) ? // this->radius - chord.Distance(Vector3{0, 0, length/2}): // this->radius + chord.Distance(Vector3{0, 0, length/2}); } From 532775b36611a25ea62af45f6a28e7400f10ae45 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 13:05:14 +0800 Subject: [PATCH 23/81] :facepalm: compilation error Signed-off-by: Arjo Chakravarty --- include/ignition/math/Cylinder.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index abf356d6f..a223b8ba1 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -22,7 +22,7 @@ #include "ignition/math/Plane.hh" #include "ignition/math/Quaternion.hh" -#include +#include namespace ignition { From a90d11a75733538610ca22fdf8247d53d89a4b36 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 13:12:46 +0800 Subject: [PATCH 24/81] more style fixes. Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Cylinder.hh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/ignition/math/detail/Cylinder.hh b/include/ignition/math/detail/Cylinder.hh index d4c025ada..9fcb0cc3b 100644 --- a/include/ignition/math/detail/Cylinder.hh +++ b/include/ignition/math/detail/Cylinder.hh @@ -16,6 +16,9 @@ */ #ifndef IGNITION_MATH_DETAIL_CYLINDER_HH_ #define IGNITION_MATH_DETAIL_CYLINDER_HH_ + +#include + namespace ignition { namespace math From 4d6675dc0539580ac7859722c9c046e63e67f03b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 17:49:22 +0800 Subject: [PATCH 25/81] Fix docstring Signed-off-by: Arjo Chakravarty --- include/ignition/math/Box.hh | 2 +- include/ignition/math/Plane.hh | 4 ++-- include/ignition/math/Sphere.hh | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index 7ed2cee42..f9702d37b 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -138,7 +138,7 @@ namespace ignition /// \brief Center of volume below the plane. This is useful when /// calculating where the buoyancy should be applied. - /// \param[in] plane - the plane which slices the sphere. + /// \param[in] _plane - the plane which slices the sphere. public: std::optional> CenterOfVolumeBelow(const Plane &_plane) const; diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 0e9c6c6b8..76822228f 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -156,8 +156,8 @@ namespace ignition } /// \brief Get the volume under a plane. - /// \requires The plane is not parallel to the z axis. And the slopes are - /// not vertical. + /// Requires that the plane is not parallel to the z axis. And the slopes + /// are not vertical. /// \param[in] _line1 A line that lies on the plane and bounds the x-axis /// \param[in] _line2 A line that lies on the plane and bounds the x-axis /// \param[in] _ylowerlimit The lower limit of the y-axis diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh index e1febb197..2f602e455 100644 --- a/include/ignition/math/Sphere.hh +++ b/include/ignition/math/Sphere.hh @@ -94,13 +94,13 @@ namespace ignition /// \brief Get the volume of sphere below a given plane in m^3. /// It is assumed that the center of the sphere is on the origin - /// \param[in] plane - the plane which slices this sphere. + /// \param[in] _plane - the plane which slices this sphere. /// \return Volume below the sphere in m^3. public: Precision VolumeBelow(const Planed &_plane) const; /// \brief Center of volume below the plane. This is useful when /// calculating where the buoyancy should be applied. - /// \param[in] plane - the plane which slices the sphere. + /// \param[in] _plane - the plane which slices the sphere. public: std::optional> CenterOfVolumeBelow(const Planed &_plane) const; From 2578996aeafa9e4348e127b1ce63382ce07e7b0f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 18:22:53 +0800 Subject: [PATCH 26/81] Revert changes to cylinder since those are not ready yet. Cylinders are actually a good model for many maritime vehicles. Moved it to a seperate branch. Will make the PR later. Signed-off-by: Arjo Chakravarty --- include/ignition/math/Cylinder.hh | 14 --- include/ignition/math/detail/Cylinder.hh | 133 ----------------------- 2 files changed, 147 deletions(-) diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 1a32ba9ad..141eeea65 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -161,20 +161,6 @@ namespace ignition /// \sa Precision DensityFromMass(const Precision _mass) const public: bool SetDensityFromMass(const Precision _mass); - private: Precision - CircleSegmentSliceArea(const Precision _distance) const; - - /// \brief Get volume of a wedge cut from this cylinder. - private: Precision CylindricalWedgeVolume( - const Precision _b, - const Precision _a, - const Precision _h); - - private: std::pair, Vector3> - GetCylinderIntersectionsAtZ( - const Plane &_plane, - const Precision z) const; - /// \brief Radius of the cylinder. private: Precision radius = 0.0; diff --git a/include/ignition/math/detail/Cylinder.hh b/include/ignition/math/detail/Cylinder.hh index 9fcb0cc3b..570a8df99 100644 --- a/include/ignition/math/detail/Cylinder.hh +++ b/include/ignition/math/detail/Cylinder.hh @@ -127,86 +127,6 @@ T Cylinder::Volume() const this->length; } -////////////////////////////////////////////////// -template -T Cylinder::VolumeBelow(const Plane &_plane) const -{ - auto length = this->Length(); - auto radius = this->Radius(); - - if(_plane.Normal().Dot(Vector3{0, 0, 1}) == T(0)) - { - // If the plane is parallel to the cylinder's axis - auto dist = _plane.Distance(Vector3(0, 0, 0)); - - if(abs(dist) >= radius) - { - if(dist < 0) - { - return Volume(); - } - else - { - return T(0); - } - } - else - { - auto volume = CircleSegmentSliceArea(abs(dist)) * this->Length(); - if(dist < 0) - { - return volume; - } - else - { - return this->Volume() - volume; - } - } - } - - // Compute intersection point of plane - auto theta = atan2(_plane.Normal().Y(), _plane.Normal().X()); - auto x = radius * cos(theta); - auto y = radius * sin(theta); - auto point_max = _plane.GetPointOnPlane(x, y); - x = radius * cos(theta + IGN_PI); - y = radius * sin(theta + IGN_PI); - auto point_min = _plane.GetPointOnPlane(x, y); - - // Get case type - if(point_max.Z() > length/2 && point_min.Z() < -length/2) - { - // Plane cuts through both faces - auto topPoints = - this->GetCylinderIntersectionsAtZ(_plane, length/2); - auto bottomPoints = - this->GetCylinderIntersectionsAtZ(_plane, -length/2); - - auto topChord = topPoints.first - topPoints.second; - } - else if(point_max.Z() > length/2 && point_min.Z() >= -length/2) - { - // Plane cuts through only top face - auto topPoints = - GetCylinderIntersectionsAtZ(_plane, length/2); - Line3 chord(topPoints.first, topPoints.second); - auto a = chord.Length()/2; - auto side = _plane.Distance(Vector3{0, 0, length/2}); - // auto b = (side < 0) ? - // this->radius - chord.Distance(Vector3{0, 0, length/2}): - // this->radius + chord.Distance(Vector3{0, 0, length/2}); - } - else - { - // Plane Cuts through no flat faces. - auto a = abs(point_max.Z()) + length/2; - auto b = abs(point_min.Z()) + length/2; - auto avg_height = (a + b)/2; - return avg_height * IGN_PI * radius * radius; - } - -} - ////////////////////////////////////////////////// template bool Cylinder::SetDensityFromMass(const T _mass) @@ -227,59 +147,6 @@ T Cylinder::DensityFromMass(const T _mass) const return _mass / this->Volume(); } -////////////////////////////////////////////////// -template -T Cylinder::CircleSegmentSliceArea(const T _distance) const -{ - auto r = this->Radius(); - auto theta = (_distance != T(0)) ? - 2 * acos(r/_distance) : - IGN_PI; - return r * r * (theta - sin(theta)) / 2; -} - -////////////////////////////////////////////////// -template -T Cylinder::CylindricalWedgeVolume( - const T _b, const T _a, const T _h) -{ - auto r = this->Radius(); - auto psi = IGN_PI_2 + atan((_b - r)/_a); - return _h/(3*_b)*(_a*(3*r*r - _a*_a) + 3*r*r*(_b-r)*psi); -} - -////////////////////////////////////////////////// -template -std::pair, Vector3> - Cylinder::GetCylinderIntersectionsAtZ( - const Plane &_plane, - const T z) const -{ - auto k = (_plane.Offset() - _plane.Normal().Z() * z) - / this->Radius(); - auto a = _plane.Normal().X(); - auto b = _plane.Normal().Y(); - - auto internal = (b - sqrt(a*a + b*b - k*k))/(a+k); - auto theta1 = 2*(atan(internal)); - auto theta2 = 2*(atan(-internal)); - - math::Vector3d intersect1 - { - this->Radius() * cos(theta1), - this->Radius() * sin(theta1), - z - }; - - math::Vector3d intersect2 - { - this->Radius() * cos(theta2), - this->Radius() * sin(theta2), - z - }; - - return {intersect1, intersect2}; -} } } #endif From 482f5825f00fef2ad526f29a33812e268569d84f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 18:34:28 +0800 Subject: [PATCH 27/81] more cleanups Signed-off-by: Arjo Chakravarty --- src/Cylinder_TEST.cc | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index 37370395c..c0ad31de2 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -138,23 +138,3 @@ TEST(CylinderTest, Mass) EXPECT_EQ(expectedMassMat, massMat); EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass()); } - -//////////////////////////////////////////////// -TEST(CylinderTest, VolumeBelow) -{ - math::Cylinderd cylinder(1.0, 1.0); - { - math::Planed plane(math::Vector3d(0, 1, 0), 0); - EXPECT_DOUBLE_EQ(cylinder.Volume()/2, cylinder.VolumeBelow(plane)); - } - - { - math::Planed plane(math::Vector3d(0, 1, 0), -100); - EXPECT_DOUBLE_EQ(0.0, cylinder.VolumeBelow(plane)); - } - - { - math::Planed plane(math::Vector3d(0, 1, 0), 100); - EXPECT_DOUBLE_EQ(cylinder.Volume(), cylinder.VolumeBelow(plane)); - } -} From 4888d998cb4ab420c808b4eb00ea4125199beb37 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 19:05:09 +0800 Subject: [PATCH 28/81] Add triangle test Signed-off-by: Arjo Chakravarty --- src/Triangle3_TEST.cc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/Triangle3_TEST.cc b/src/Triangle3_TEST.cc index eeef07d9f..8fd203b12 100644 --- a/src/Triangle3_TEST.cc +++ b/src/Triangle3_TEST.cc @@ -221,6 +221,15 @@ TEST(Triangle3Test, Overlaps) Vector3d(100, 101, 100), Vector3d(101, 100, 100)); EXPECT_FALSE(tri.Overlaps(tri2)); + + tri.Set(Vector3d(0, 0, 5), + Vector3d(0, 5, 0), + Vector3d(5, 0, 0)); + tri2.Set(Vector3d(0, 0, 1), + Vector3d(0, 1, 0), + Vector3d(1, 0, 0)); + EXPECT_TRUE(tri.Overlaps(tri2)); + EXPECT_TRUE(tri2.Overlaps(tri)); } ///////////////////////////////////////////////// From e1985b3cb83a954eb7d76020cab7e8c4ac7e74ad Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 19:34:00 +0800 Subject: [PATCH 29/81] Remove redundant function Signed-off-by: Arjo Chakravarty --- include/ignition/math/Triangle3.hh | 37 +----------------------------- src/Triangle3_TEST.cc | 30 ------------------------ 2 files changed, 1 insertion(+), 66 deletions(-) diff --git a/include/ignition/math/Triangle3.hh b/include/ignition/math/Triangle3.hh index d78978a9a..7cb43b5eb 100644 --- a/include/ignition/math/Triangle3.hh +++ b/include/ignition/math/Triangle3.hh @@ -179,6 +179,7 @@ namespace ignition /// \param[out] _ipt1 Return value of the first intersection point, /// only valid if the return value of the function is true. /// \return True if the given line intersects this triangle. + #include public: bool Intersects(const Line3 &_line, Vector3 &_ipt1) const { // Triangle normal @@ -234,42 +235,6 @@ namespace ignition return false; } - /// \brief Get whether a triangle overlaps with another triangle. - /// \return boolean indicating whether the triangles overlap. - public: bool Overlaps(const Triangle3 &_triangle, - T thresh = 1e-6) const - { - int sideCount = 0; - for(unsigned int i = 0; i < 3; ++i) - { - for(unsigned int j = i; j < 3; ++j) - { - Vector3d intersection; - if(this->Side(i).Intersect(_triangle.Side(j), intersection)) - { - sideCount++; - } - } - } - /// If the triangles are adjacent to each other they may share an - /// intersecting edge. However if two triangles overlap they must have - /// at least two intersecting edges. - if(sideCount > 1) - { - return true; - } - - // Check if any of the points of the triangles are inside the other - for(unsigned int i = 0; i < 3; ++i) - { - if(_triangle.Contains(this->pts[i])) - return true; - if(this->Contains(_triangle.pts[i])) - return true; - } - return false; - } - /// \brief Get the length of the triangle's perimeter. /// \return Sum of the triangle's line segments. public: T Perimeter() const diff --git a/src/Triangle3_TEST.cc b/src/Triangle3_TEST.cc index 8fd203b12..bcc798ede 100644 --- a/src/Triangle3_TEST.cc +++ b/src/Triangle3_TEST.cc @@ -202,36 +202,6 @@ TEST(Triangle3Test, Intersects) } } -TEST(Triangle3Test, Overlaps) -{ - Triangle3d tri(Vector3d(0, 0, 0), - Vector3d(0, 1, 0), - Vector3d(1, 0, 0)); - Triangle3d tri2(Vector3d(0, 0, 0), - Vector3d(0, 1, 0), - Vector3d(1, 0, 0)); - EXPECT_TRUE(tri.Overlaps(tri2)); - - tri2.Set(Vector3d(0, 1, 0), - Vector3d(0, 0, 0), - Vector3d(1, 0, 0)); - EXPECT_TRUE(tri.Overlaps(tri2)); - - tri2.Set(Vector3d(100, 100, 100), - Vector3d(100, 101, 100), - Vector3d(101, 100, 100)); - EXPECT_FALSE(tri.Overlaps(tri2)); - - tri.Set(Vector3d(0, 0, 5), - Vector3d(0, 5, 0), - Vector3d(5, 0, 0)); - tri2.Set(Vector3d(0, 0, 1), - Vector3d(0, 1, 0), - Vector3d(1, 0, 0)); - EXPECT_TRUE(tri.Overlaps(tri2)); - EXPECT_TRUE(tri2.Overlaps(tri)); -} - ///////////////////////////////////////////////// TEST(Triangle3Test, ContainsPt) { From 7f204d44b6e5de8454696d1597e66c5ef2ffae0c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 19:39:51 +0800 Subject: [PATCH 30/81] >.< Doh stray include Signed-off-by: Arjo Chakravarty --- include/ignition/math/Triangle3.hh | 1 - 1 file changed, 1 deletion(-) diff --git a/include/ignition/math/Triangle3.hh b/include/ignition/math/Triangle3.hh index 7cb43b5eb..5835de3fa 100644 --- a/include/ignition/math/Triangle3.hh +++ b/include/ignition/math/Triangle3.hh @@ -179,7 +179,6 @@ namespace ignition /// \param[out] _ipt1 Return value of the first intersection point, /// only valid if the return value of the function is true. /// \return True if the given line intersects this triangle. - #include public: bool Intersects(const Line3 &_line, Vector3 &_ipt1) const { // Triangle normal From 99589e466aa61dec966b0f3d1d2687055fc6312b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Aug 2021 19:44:59 +0800 Subject: [PATCH 31/81] remove unused include Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Cylinder.hh | 3 --- 1 file changed, 3 deletions(-) diff --git a/include/ignition/math/detail/Cylinder.hh b/include/ignition/math/detail/Cylinder.hh index 570a8df99..ac4a96722 100644 --- a/include/ignition/math/detail/Cylinder.hh +++ b/include/ignition/math/detail/Cylinder.hh @@ -16,9 +16,6 @@ */ #ifndef IGNITION_MATH_DETAIL_CYLINDER_HH_ #define IGNITION_MATH_DETAIL_CYLINDER_HH_ - -#include - namespace ignition { namespace math From 5ee6f3bfb7dae01120fdcf27eba9c272de4b6749 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 31 Aug 2021 14:17:27 +0800 Subject: [PATCH 32/81] typesafe-ify Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 2 +- include/ignition/math/detail/Box.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 76822228f..fdad5e0b0 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -128,7 +128,7 @@ namespace ignition /// \return coincident point. public: Vector3 GetPointOnPlane(const T x, const T y) const { - auto z_val = (this->Normal().Z() != 0) ? + auto z_val = (this->Normal().Z() != T(0)) ? (this->Offset() - (this->Normal().Dot({x, y, 0})))/this->Normal().Z() : 0; auto coincidentPoint = Vector3{x, y, z_val}; diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index f45295cf7..cafd0ded1 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -136,7 +136,7 @@ std::vector, T>> TrianglesInPlane(Plane &_plane, centroid += pt; } } - centroid /= pointsInPlane.size(); + centroid /= T(pointsInPlane.size()); if(pointsInPlane.size() < 3) return {}; From 1c896fafe20c7560d340006ed0d05c0da28537f5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 1 Sep 2021 16:28:23 +0800 Subject: [PATCH 33/81] silence warning Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index fdad5e0b0..62e7d86ca 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -125,10 +125,13 @@ namespace ignition /// \brief Given x,y point find corresponding Z point on plane. /// \param[in] x - 2d x coordinate. /// \param[in] y - 2d y coordinate. + /// \param[in] eps - The value to consider zero. Defaults to 1e-16 /// \return coincident point. - public: Vector3 GetPointOnPlane(const T x, const T y) const + public: Vector3 GetPointOnPlane(const T x, + const T y, + const T eps = 1e-16) const { - auto z_val = (this->Normal().Z() != T(0)) ? + auto z_val = (std::fabs(this->Normal().Z()) < eps) ? (this->Offset() - (this->Normal().Dot({x, y, 0})))/this->Normal().Z() : 0; auto coincidentPoint = Vector3{x, y, z_val}; From 646595f4a358482f04a281c6263b911b6a8ecca1 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 1 Sep 2021 23:21:32 +0800 Subject: [PATCH 34/81] Codecheck Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 62e7d86ca..d24d41334 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -127,7 +127,7 @@ namespace ignition /// \param[in] y - 2d y coordinate. /// \param[in] eps - The value to consider zero. Defaults to 1e-16 /// \return coincident point. - public: Vector3 GetPointOnPlane(const T x, + public: Vector3 GetPointOnPlane(const T x, const T y, const T eps = 1e-16) const { From e8fd7a2be627afb4e7a050129f9fe7e6f49c221e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 1 Sep 2021 23:35:05 +0800 Subject: [PATCH 35/81] Flip sign... :facepalm: Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index d24d41334..e7b280823 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -131,7 +131,7 @@ namespace ignition const T y, const T eps = 1e-16) const { - auto z_val = (std::fabs(this->Normal().Z()) < eps) ? + auto z_val = (std::fabs(this->Normal().Z()) > eps) ? (this->Offset() - (this->Normal().Dot({x, y, 0})))/this->Normal().Z() : 0; auto coincidentPoint = Vector3{x, y, z_val}; From 16289b0a69c05c51cea91298b69c578cff377a5c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Sep 2021 17:52:34 +0800 Subject: [PATCH 36/81] Rename `Box::GetIntersections`. Also removed redundant changes. Signed-off-by: Arjo Chakravarty --- include/ignition/math/Box.hh | 7 +++++-- include/ignition/math/Cylinder.hh | 4 ---- include/ignition/math/detail/Box.hh | 4 ++-- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index f9702d37b..88db6c194 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -138,7 +138,7 @@ namespace ignition /// \brief Center of volume below the plane. This is useful when /// calculating where the buoyancy should be applied. - /// \param[in] _plane - the plane which slices the sphere. + /// \param[in] _plane The plane which slices the sphere. public: std::optional> CenterOfVolumeBelow(const Plane &_plane) const; @@ -182,7 +182,10 @@ namespace ignition private: ignition::math::Material material; /// \brief Get intersection between a plane and the box. - private: std::vector> GetIntersections( + /// \param[in] _plane The plane against which we are testing intersection. + /// \returns a list of points along the edges of the box where the + /// intersection occurs. + private: std::vector> Intersections( const Plane &_plane) const; }; diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 141eeea65..429cd4aad 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -133,10 +133,6 @@ namespace ignition /// \return Volume of the cylinder in m^3. public: Precision Volume() const; - /// \brief Get the volume of the cylinder in m^3. - /// \return Volume of the cylinder in m^3. - public: Precision VolumeBelow(const Plane &_plane) const; - /// \brief Compute the cylinder's density given a mass value. The /// cylinder is assumed to be solid with uniform density. This /// function requires the cylinder's radius and length to be set to diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index cafd0ded1..d862f1ed2 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -197,7 +197,7 @@ T Box::VolumeBelow(const Plane &_plane) const if(verticesBelow.size() == 0) return 0; - auto intersections = GetIntersections(_plane); + auto intersections = Intersections(_plane); verticesBelow.insert(verticesBelow.end(), intersections.begin(), intersections.end()); @@ -307,7 +307,7 @@ bool Box::MassMatrix(MassMatrix3 &_massMat) const ////////////////////////////////////////////////// template -std::vector> Box::GetIntersections( +std::vector> Box::Intersections( const Plane &_plane) const { std::vector> intersections; From 1257acaf013854697753ce180fbe97c3275b24f6 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Sep 2021 17:57:53 +0800 Subject: [PATCH 37/81] Revert *all* changes to cylinder Signed-off-by: Arjo Chakravarty --- include/ignition/math/Cylinder.hh | 3 --- 1 file changed, 3 deletions(-) diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 429cd4aad..228f73f86 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -19,11 +19,8 @@ #include "ignition/math/MassMatrix3.hh" #include "ignition/math/Material.hh" -#include "ignition/math/Plane.hh" #include "ignition/math/Quaternion.hh" -#include - namespace ignition { namespace math From 7f1f1c872b38aab6f37e00815ef799cbc8bec639 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Sep 2021 18:08:44 +0800 Subject: [PATCH 38/81] fix docs and signature Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index a85b752da..874b5deb0 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -229,10 +229,10 @@ namespace ignition return true; } - /// \brief Calculate distance between line and slope + /// \brief Calculate distance between line and point /// \param[in] _pt point which we are measuring distance to. /// \returns Distance from point to line. - public: T Distance(const Vector3 _pt) + public: T Distance(const Vector3 &_pt) { auto d = (_pt - this->pts[0]).Cross(this->pts[1] - this->pts[0]); return d.Length() / (this->pts[1] - this->pts[0]).Length(); From 6ff1b4902294c8100b455e1a5752a81432de29a3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Sep 2021 18:22:51 +0800 Subject: [PATCH 39/81] Add line test Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 2 +- src/Line3_TEST.cc | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index 874b5deb0..08edfadb7 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -229,7 +229,7 @@ namespace ignition return true; } - /// \brief Calculate distance between line and point + /// \brief Calculate shortest distance between line and point /// \param[in] _pt point which we are measuring distance to. /// \returns Distance from point to line. public: T Distance(const Vector3 &_pt) diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc index 05a060573..3bbcc07ca 100644 --- a/src/Line3_TEST.cc +++ b/src/Line3_TEST.cc @@ -228,6 +228,11 @@ 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_EQ(line.Distance(point), 5); } ///////////////////////////////////////////////// @@ -291,3 +296,9 @@ 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, DistanceVec3) +{ + +} \ No newline at end of file From 753b49d666dad01660bf6d1ca4142382492194b0 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Sep 2021 18:25:39 +0800 Subject: [PATCH 40/81] style fix Signed-off-by: Arjo Chakravarty --- src/Line3_TEST.cc | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc index 3bbcc07ca..fc83f85a6 100644 --- a/src/Line3_TEST.cc +++ b/src/Line3_TEST.cc @@ -296,9 +296,3 @@ 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, DistanceVec3) -{ - -} \ No newline at end of file From 633dfeea0ad12fffe7fe3f4881e08a165a4a4da2 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Sep 2021 18:38:57 +0800 Subject: [PATCH 41/81] Documentation Signed-off-by: Arjo Chakravarty --- include/ignition/math/Sphere.hh | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh index 2f602e455..dfadeceb0 100644 --- a/include/ignition/math/Sphere.hh +++ b/include/ignition/math/Sphere.hh @@ -94,13 +94,15 @@ namespace ignition /// \brief Get the volume of sphere below a given plane in m^3. /// It is assumed that the center of the sphere is on the origin - /// \param[in] _plane - the plane which slices this sphere. + /// \param[in] _plane The plane which slices this sphere. /// \return Volume below the sphere in m^3. public: Precision VolumeBelow(const Planed &_plane) const; /// \brief Center of volume below the plane. This is useful when /// calculating where the buoyancy should be applied. - /// \param[in] _plane - the plane which slices the sphere. + /// \param[in] _plane The plane which slices the sphere. + /// \return The center of volume if there is anything under the plane, + /// otherwise return a std::nullopt. public: std::optional> CenterOfVolumeBelow(const Planed &_plane) const; From 165a7242f25f7403a3465468ae3624e5525a1c5d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Sep 2021 18:39:51 +0800 Subject: [PATCH 42/81] Fix compiler :warning: Signed-off-by: Arjo Chakravarty --- src/Line3_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc index fc83f85a6..d6c1cd59a 100644 --- a/src/Line3_TEST.cc +++ b/src/Line3_TEST.cc @@ -232,7 +232,7 @@ TEST(Line3Test, Distance) // Check when measured against a point. line.Set(0, -1, 0, 0, 1, 0); math::Vector3d point(5, 0, 0); - EXPECT_EQ(line.Distance(point), 5); + EXPECT_NEAR(line.Distance(point), 5, 1e-3); } ///////////////////////////////////////////////// From f5a45fa4bbbbbbe4ee8da88e05ccefff41b7c788 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 16 Sep 2021 15:01:06 +0800 Subject: [PATCH 43/81] Added test for `Vector3d::Project()` Signed-off-by: Arjo Chakravarty --- src/Vector3_TEST.cc | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc index c6207a8b2..103627dd3 100644 --- a/src/Vector3_TEST.cc +++ b/src/Vector3_TEST.cc @@ -421,6 +421,18 @@ TEST(Vector3dTest, Multiply) EXPECT_EQ(v*v, math::Vector3d(0.01, 0.04, 0.09)); } +///////////////////////////////////////////////// +TEST(Vector3dTest, Projection) +{ + math::Vector3d v1(0, 0, 2); + math::Vector3d v2(0, 1, 0); + math::Vector3d v3(2, 3, 2); + + EXPECT_NEAR(v1.Project(v3), 2, 1e-15); + EXPECT_NEAR(v2.Project(v3), 3, 1e-15); + EXPECT_NEAR(v1.Project(v2), 0, 1e-15); +} + ///////////////////////////////////////////////// TEST(Vector3dTest, NotEqual) { From e7eaad69cdc284f671229599fadec6d56aee0f9b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 16 Sep 2021 17:04:49 +0800 Subject: [PATCH 44/81] Add test for CenterOfVolumeBelow() Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 34 ----------------------------- include/ignition/math/detail/Box.hh | 2 +- src/Box_TEST.cc | 17 +++++++++++++++ 3 files changed, 18 insertions(+), 35 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index e7b280823..68bfa9b11 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -158,40 +158,6 @@ namespace ignition return intersection; } - /// \brief Get the volume under a plane. - /// Requires that the plane is not parallel to the z axis. And the slopes - /// are not vertical. - /// \param[in] _line1 A line that lies on the plane and bounds the x-axis - /// \param[in] _line2 A line that lies on the plane and bounds the x-axis - /// \param[in] _ylowerlimit The lower limit of the y-axis - /// \param[in] _yupperlimit The upper limit of the y-axis - /// \return The volume under the plane. - public: T Volume(const Line2 &_line1, - const Line2 &_line2, - const T _ylowerlimit, - const T _yupperlimit) const - { - auto l = _ylowerlimit; - auto m = _yupperlimit; - auto k = this->Offset()/this->Normal().Z(); - auto j = this->Normal().X()/this->Normal().Z(); - auto n = this->Normal().Y()/this->Normal().Z(); - - auto a_1 = _line1.Slope(); - auto a_2 = _line2.Slope(); - auto b_1 = _line1[0].Y() - _line1[0].X() * a_1; - auto b_2 = _line2[0].Y() - _line2[0].X() * a_1; - - // computed using wolfram alpha: - // https://www.wolframalpha.com/input/?i=integral+from+m+to+l+of+%28integral+++%28k+-+jx-+ny%29++dy+from+a_1x+%2B+b_1+to+a_2x+%2Bb_2%29+dx - auto vol = 1.0/6.0 * - (-3.0 * (l*l - m*m) * (a_1 * (k - b_1 * n) - + a_2 * (b_2 * n - k) + (b_2 - b_1) * j) - + (a_1 - a_2) * (l*l*l - m*m*m) * (a_1 * n + a_2 * n + 2 * j) - + 3.0 * (b_1 - b_2) * (l - m) * (b_1 * n + b_2 * n - 2 * k)); - return vol; - } - /// \brief The side of the plane a point is on. /// \param[in] _point The 3D point to check. /// \return Plane::NEGATIVE_SIDE if the distance from the point to the diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index d862f1ed2..497c69df4 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -265,7 +265,7 @@ std::optional> if(verticesBelow.size() == 0) return std::nullopt; - auto intersections = GetIntersections(_plane); + auto intersections = Intersections(_plane); verticesBelow.insert(verticesBelow.end(), intersections.begin(), intersections.end()); diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 45ba6cc6e..e433d0dbc 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -142,6 +142,23 @@ TEST(BoxTest, VolumeBelow) } } +////////////////////////////////////////////////// +TEST(BoxTest, CenterOfVolumeBelow) +{ + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); + EXPECT_FALSE(box.CenterOfVolumeBelow(plane).has_value()); + } + + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 5.0); + EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value()); + EXPECT_EQ(box.CenterOfVolumeBelow(plane), math::Vector3d(0,0,0)); + } +} + ////////////////////////////////////////////////// TEST(BoxTest, Mass) { From 9739419f9c225f8187a3bcc238016eeb488a7d77 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 15 Sep 2021 16:57:45 -0700 Subject: [PATCH 45/81] Add Vector(2|3|4)::NaN to easily create invalid vectors (#222) Signed-off-by: Louise Poubel Signed-off-by: Arjo Chakravarty --- include/ignition/math/Vector2.hh | 9 ++++++ include/ignition/math/Vector3.hh | 14 ++++++++-- include/ignition/math/Vector4.hh | 10 +++++++ src/Vector2_TEST.cc | 22 +++++++++++++++ src/Vector3_TEST.cc | 47 ++++++++++++++++++++++++++++---- src/Vector4_TEST.cc | 25 +++++++++++++++++ src/python/Vector2.i | 1 + src/python/Vector2_TEST.py | 19 +++++++++++++ src/python/Vector3.i | 1 + src/python/Vector3_TEST.py | 21 ++++++++++++++ src/python/Vector4.i | 2 ++ src/python/Vector4_TEST.py | 23 ++++++++++++++++ src/ruby/Vector2.i | 1 + src/ruby/Vector2_TEST.rb | 22 +++++++++++++++ src/ruby/Vector3.i | 1 + src/ruby/Vector3_TEST.rb | 23 ++++++++++++++++ src/ruby/Vector4.i | 2 ++ src/ruby/Vector4_TEST.rb | 26 ++++++++++++++++++ 18 files changed, 261 insertions(+), 8 deletions(-) diff --git a/include/ignition/math/Vector2.hh b/include/ignition/math/Vector2.hh index dc8530a92..717fba2eb 100644 --- a/include/ignition/math/Vector2.hh +++ b/include/ignition/math/Vector2.hh @@ -18,6 +18,7 @@ #define IGNITION_MATH_VECTOR2_HH_ #include +#include #include #include @@ -40,6 +41,9 @@ namespace ignition /// \brief math::Vector2(1, 1) public: static const Vector2 One; + /// \brief math::Vector2(NaN, NaN, NaN) + public: static const Vector2 NaN; + /// \brief Default Constructor public: Vector2() { @@ -579,6 +583,11 @@ namespace ignition template const Vector2 Vector2::One(1, 1); + template + const Vector2 Vector2::NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + typedef Vector2 Vector2i; typedef Vector2 Vector2d; typedef Vector2 Vector2f; diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index 300fd3e86..b981730c9 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -17,10 +17,11 @@ #ifndef IGNITION_MATH_VECTOR3_HH_ #define IGNITION_MATH_VECTOR3_HH_ -#include -#include -#include #include +#include +#include +#include +#include #include #include @@ -54,6 +55,9 @@ namespace ignition /// \brief math::Vector3(0, 0, 1) public: static const Vector3 UnitZ; + /// \brief math::Vector3(NaN, NaN, NaN) + public: static const Vector3 NaN; + /// \brief Constructor public: Vector3() { @@ -765,6 +769,10 @@ namespace ignition template const Vector3 Vector3::UnitX(1, 0, 0); template const Vector3 Vector3::UnitY(0, 1, 0); template const Vector3 Vector3::UnitZ(0, 0, 1); + template const Vector3 Vector3::NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); typedef Vector3 Vector3i; typedef Vector3 Vector3d; diff --git a/include/ignition/math/Vector4.hh b/include/ignition/math/Vector4.hh index 0403f1cca..9d27b4b87 100644 --- a/include/ignition/math/Vector4.hh +++ b/include/ignition/math/Vector4.hh @@ -18,6 +18,7 @@ #define IGNITION_MATH_VECTOR4_HH_ #include +#include #include #include @@ -41,6 +42,9 @@ namespace ignition /// \brief math::Vector4(1, 1, 1, 1) public: static const Vector4 One; + /// \brief math::Vector4(NaN, NaN, NaN, NaN) + public: static const Vector4 NaN; + /// \brief Constructor public: Vector4() { @@ -735,6 +739,12 @@ namespace ignition template const Vector4 Vector4::One(1, 1, 1, 1); + template const Vector4 Vector4::NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + typedef Vector4 Vector4i; typedef Vector4 Vector4d; typedef Vector4 Vector4f; diff --git a/src/Vector2_TEST.cc b/src/Vector2_TEST.cc index 7a289fe97..3e6e95f75 100644 --- a/src/Vector2_TEST.cc +++ b/src/Vector2_TEST.cc @@ -443,3 +443,25 @@ TEST(Vector2Test, Length) EXPECT_EQ(vi.SquaredLength(), 25); } +///////////////////////////////////////////////// +TEST(Vector2Test, NaN) +{ + auto nanVec = math::Vector2d::NaN; + EXPECT_FALSE(nanVec.IsFinite()); + EXPECT_TRUE(math::isnan(nanVec.X())); + EXPECT_TRUE(math::isnan(nanVec.Y())); + + nanVec.Correct(); + EXPECT_EQ(math::Vector2d::Zero, nanVec); + EXPECT_TRUE(nanVec.IsFinite()); + + auto nanVecF = math::Vector2f::NaN; + EXPECT_FALSE(nanVecF.IsFinite()); + EXPECT_TRUE(math::isnan(nanVecF.X())); + EXPECT_TRUE(math::isnan(nanVecF.Y())); + + nanVecF.Correct(); + EXPECT_EQ(math::Vector2f::Zero, nanVecF); + EXPECT_TRUE(nanVecF.IsFinite()); +} + diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc index 103627dd3..7a37d3f33 100644 --- a/src/Vector3_TEST.cc +++ b/src/Vector3_TEST.cc @@ -125,11 +125,25 @@ TEST(Vector3dTest, Vector3d) EXPECT_TRUE(v.Equal(math::Vector3d(2, 1, .4))); // Test the static defines. - EXPECT_TRUE(math::Vector3d::Zero == math::Vector3d(0, 0, 0)); - EXPECT_TRUE(math::Vector3d::One == math::Vector3d(1, 1, 1)); - EXPECT_TRUE(math::Vector3d::UnitX == math::Vector3d(1, 0, 0)); - EXPECT_TRUE(math::Vector3d::UnitY == math::Vector3d(0, 1, 0)); - EXPECT_TRUE(math::Vector3d::UnitZ == math::Vector3d(0, 0, 1)); + EXPECT_EQ(math::Vector3d::Zero, math::Vector3d(0, 0, 0)); + EXPECT_EQ(math::Vector3f::Zero, math::Vector3f(0, 0, 0)); + EXPECT_EQ(math::Vector3i::Zero, math::Vector3i(0, 0, 0)); + + EXPECT_EQ(math::Vector3d::One, math::Vector3d(1, 1, 1)); + EXPECT_EQ(math::Vector3f::One, math::Vector3f(1, 1, 1)); + EXPECT_EQ(math::Vector3i::One, math::Vector3i(1, 1, 1)); + + EXPECT_EQ(math::Vector3d::UnitX, math::Vector3d(1, 0, 0)); + EXPECT_EQ(math::Vector3f::UnitX, math::Vector3f(1, 0, 0)); + EXPECT_EQ(math::Vector3i::UnitX, math::Vector3i(1, 0, 0)); + + EXPECT_EQ(math::Vector3d::UnitY, math::Vector3d(0, 1, 0)); + EXPECT_EQ(math::Vector3f::UnitY, math::Vector3f(0, 1, 0)); + EXPECT_EQ(math::Vector3i::UnitY, math::Vector3i(0, 1, 0)); + + EXPECT_EQ(math::Vector3d::UnitZ, math::Vector3d(0, 0, 1)); + EXPECT_EQ(math::Vector3f::UnitZ, math::Vector3f(0, 0, 1)); + EXPECT_EQ(math::Vector3i::UnitZ, math::Vector3i(0, 0, 1)); } ///////////////////////////////////////////////// @@ -479,3 +493,26 @@ TEST(Vector3dTest, NoException) EXPECT_NO_THROW(ss << vInf); } +///////////////////////////////////////////////// +TEST(Vector3dTest, NaN) +{ + auto nanVec = math::Vector3d::NaN; + EXPECT_FALSE(nanVec.IsFinite()); + EXPECT_TRUE(math::isnan(nanVec.X())); + EXPECT_TRUE(math::isnan(nanVec.Y())); + EXPECT_TRUE(math::isnan(nanVec.Z())); + + nanVec.Correct(); + EXPECT_EQ(math::Vector3d::Zero, nanVec); + EXPECT_TRUE(nanVec.IsFinite()); + + auto nanVecF = math::Vector3f::NaN; + EXPECT_FALSE(nanVecF.IsFinite()); + EXPECT_TRUE(math::isnan(nanVecF.X())); + EXPECT_TRUE(math::isnan(nanVecF.Y())); + EXPECT_TRUE(math::isnan(nanVecF.Z())); + + nanVecF.Correct(); + EXPECT_EQ(math::Vector3f::Zero, nanVecF); + EXPECT_TRUE(nanVecF.IsFinite()); +} diff --git a/src/Vector4_TEST.cc b/src/Vector4_TEST.cc index 5d99f2402..400a1f90c 100644 --- a/src/Vector4_TEST.cc +++ b/src/Vector4_TEST.cc @@ -443,3 +443,28 @@ TEST(Vector4dTest, Length) EXPECT_EQ(math::Vector4i::One.SquaredLength(), 4); } +///////////////////////////////////////////////// +TEST(Vector4Test, NaN) +{ + auto nanVec = math::Vector4d::NaN; + EXPECT_FALSE(nanVec.IsFinite()); + EXPECT_TRUE(math::isnan(nanVec.X())); + EXPECT_TRUE(math::isnan(nanVec.Y())); + EXPECT_TRUE(math::isnan(nanVec.Z())); + EXPECT_TRUE(math::isnan(nanVec.W())); + + nanVec.Correct(); + EXPECT_EQ(math::Vector4d::Zero, nanVec); + EXPECT_TRUE(nanVec.IsFinite()); + + auto nanVecF = math::Vector4f::NaN; + EXPECT_FALSE(nanVecF.IsFinite()); + EXPECT_TRUE(math::isnan(nanVecF.X())); + EXPECT_TRUE(math::isnan(nanVecF.Y())); + EXPECT_TRUE(math::isnan(nanVecF.Z())); + EXPECT_TRUE(math::isnan(nanVecF.W())); + + nanVecF.Correct(); + EXPECT_EQ(math::Vector4f::Zero, nanVecF); + EXPECT_TRUE(nanVecF.IsFinite()); +} diff --git a/src/python/Vector2.i b/src/python/Vector2.i index ff2f80098..6ff486814 100644 --- a/src/python/Vector2.i +++ b/src/python/Vector2.i @@ -37,6 +37,7 @@ namespace ignition %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; public: static const Vector2 Zero; public: static const Vector2 One; + public: static const Vector2 NaN; public: Vector2(); public: Vector2(const T &_x, const T &_y); diff --git a/src/python/Vector2_TEST.py b/src/python/Vector2_TEST.py index 7bcac078f..6043f0f0c 100644 --- a/src/python/Vector2_TEST.py +++ b/src/python/Vector2_TEST.py @@ -15,6 +15,7 @@ import unittest import math from ignition.math import Vector2d +from ignition.math import Vector2f class TestVector2(unittest.TestCase): @@ -295,6 +296,24 @@ def test_lenght(self): self.assertAlmostEqual(v.length(), 5) self.assertAlmostEqual(v.squared_length(), 25) + def test_nan(self): + nanVec = Vector2d.NAN + self.assertFalse(nanVec.is_finite()) + self.assertTrue(math.isnan(nanVec.x())) + self.assertTrue(math.isnan(nanVec.y())) + + nanVec.correct() + self.assertEqual(Vector2d.ZERO, nanVec) + self.assertTrue(nanVec.is_finite()) + + nanVecF = Vector2f.NAN + self.assertFalse(nanVecF.is_finite()) + self.assertTrue(math.isnan(nanVecF.x())) + self.assertTrue(math.isnan(nanVecF.y())) + + nanVecF.correct() + self.assertEqual(Vector2f.ZERO, nanVecF) + self.assertTrue(nanVecF.is_finite()) if __name__ == '__main__': unittest.main() diff --git a/src/python/Vector3.i b/src/python/Vector3.i index 12b48ced9..751350fb7 100644 --- a/src/python/Vector3.i +++ b/src/python/Vector3.i @@ -43,6 +43,7 @@ namespace ignition public: static const Vector3 UnitY; %rename(UNIT_Z) UnitZ; public: static const Vector3 UnitZ; + public: static const Vector3 NaN; public: Vector3(); public: Vector3(const T &_x, const T &_y, const T &_z); public: Vector3(const Vector3 &_v); diff --git a/src/python/Vector3_TEST.py b/src/python/Vector3_TEST.py index 2ac6ab18e..0c3b7e610 100644 --- a/src/python/Vector3_TEST.py +++ b/src/python/Vector3_TEST.py @@ -15,6 +15,7 @@ import unittest import math from ignition.math import Vector3d +from ignition.math import Vector3f class TestVector3(unittest.TestCase): @@ -350,6 +351,26 @@ def test_finite(self): self.assertTrue(vec1.is_finite()) + def test_nan(self): + nanVec = Vector3d.NAN + self.assertFalse(nanVec.is_finite()) + self.assertTrue(math.isnan(nanVec.x())) + self.assertTrue(math.isnan(nanVec.y())) + self.assertTrue(math.isnan(nanVec.z())) + + nanVec.correct() + self.assertEqual(Vector3d.ZERO, nanVec) + self.assertTrue(nanVec.is_finite()) + + nanVecF = Vector3f.NAN + self.assertFalse(nanVecF.is_finite()) + self.assertTrue(math.isnan(nanVecF.x())) + self.assertTrue(math.isnan(nanVecF.y())) + self.assertTrue(math.isnan(nanVecF.z())) + + nanVecF.correct() + self.assertEqual(Vector3f.ZERO, nanVecF) + self.assertTrue(nanVecF.is_finite()) if __name__ == '__main__': unittest.main() diff --git a/src/python/Vector4.i b/src/python/Vector4.i index 1608bce03..aaea4dbc6 100644 --- a/src/python/Vector4.i +++ b/src/python/Vector4.i @@ -37,6 +37,7 @@ namespace ignition %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; public: static const Vector4 Zero; public: static const Vector4 One; + public: static const Vector4 NaN; public: Vector4(); public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w); public: Vector4(const Vector4 &_v); @@ -58,6 +59,7 @@ namespace ignition public: bool operator==(const Vector4 &_v) const; public: bool Equal(const Vector4 &_v, const T &_tol) const; public: bool IsFinite() const; + public: inline void Correct(); public: inline T X() const; public: inline T Y() const; public: inline T Z() const; diff --git a/src/python/Vector4_TEST.py b/src/python/Vector4_TEST.py index b85ecf9cd..854050277 100644 --- a/src/python/Vector4_TEST.py +++ b/src/python/Vector4_TEST.py @@ -16,6 +16,7 @@ import unittest import math from ignition.math import Vector4d +from ignition.math import Vector4f class TestVector4(unittest.TestCase): @@ -256,6 +257,28 @@ def test_finite(self): vec1 = Vector4d(0.1, 0.2, 0.3, 0.4) self.assertTrue(vec1.is_finite()) + def test_nan(self): + nanVec = Vector4d.NAN + self.assertFalse(nanVec.is_finite()) + self.assertTrue(math.isnan(nanVec.x())) + self.assertTrue(math.isnan(nanVec.y())) + self.assertTrue(math.isnan(nanVec.z())) + self.assertTrue(math.isnan(nanVec.w())) + + nanVec.correct() + self.assertEqual(Vector4d.ZERO, nanVec) + self.assertTrue(nanVec.is_finite()) + + nanVecF = Vector4f.NAN + self.assertFalse(nanVecF.is_finite()) + self.assertTrue(math.isnan(nanVecF.x())) + self.assertTrue(math.isnan(nanVecF.y())) + self.assertTrue(math.isnan(nanVecF.z())) + self.assertTrue(math.isnan(nanVecF.w())) + + nanVecF.correct() + self.assertEqual(Vector4f.ZERO, nanVecF) + self.assertTrue(nanVecF.is_finite()) if __name__ == '__main__': unittest.main() diff --git a/src/ruby/Vector2.i b/src/ruby/Vector2.i index 63ae8adee..cad4128ea 100644 --- a/src/ruby/Vector2.i +++ b/src/ruby/Vector2.i @@ -35,6 +35,7 @@ namespace ignition { public: static const Vector2 Zero; public: static const Vector2 One; + public: static const Vector2 NaN; public: Vector2(); public: Vector2(const T &_x, const T &_y); diff --git a/src/ruby/Vector2_TEST.rb b/src/ruby/Vector2_TEST.rb index 653a47d86..cdc350e2c 100644 --- a/src/ruby/Vector2_TEST.rb +++ b/src/ruby/Vector2_TEST.rb @@ -257,6 +257,28 @@ def test_length assert((v.SquaredLength() - 17.65).abs < 1e-8, "Squared length of v should be near 17.65") end + + def test_nan + nanVec = Ignition::Math::Vector2d.NaN + assert(!nanVec.IsFinite(), + "NaN vector shouldn't be finite") + assert(nanVec.X().nan?, "X should be NaN") + assert(nanVec.Y().nan?, "Y should be NaN") + + nanVec.Correct() + assert(Ignition::Math::Vector2d.Zero == nanVec, + "Corrected vector should equal zero") + + nanVecF = Ignition::Math::Vector2f.NaN + assert(!nanVecF.IsFinite(), + "NaN vector shouldn't be finite") + assert(nanVecF.X().nan?, "X should be NaN") + assert(nanVecF.Y().nan?, "Y should be NaN") + + nanVecF.Correct() + assert(Ignition::Math::Vector2f.Zero == nanVecF, + "Corrected vector should equal zero") + end end exit Test::Unit::UI::Console::TestRunner.run(Vector2_TEST).passed? ? 0 : -1 diff --git a/src/ruby/Vector3.i b/src/ruby/Vector3.i index 0d8508d3c..0acf12da5 100644 --- a/src/ruby/Vector3.i +++ b/src/ruby/Vector3.i @@ -38,6 +38,7 @@ namespace ignition public: static const Vector3 UnitX; public: static const Vector3 UnitY; public: static const Vector3 UnitZ; + public: static const Vector3 NaN; public: Vector3(); public: Vector3(const T &_x, const T &_y, const T &_z); public: Vector3(const Vector3 &_v); diff --git a/src/ruby/Vector3_TEST.rb b/src/ruby/Vector3_TEST.rb index 1448cdf4e..d698b06d7 100755 --- a/src/ruby/Vector3_TEST.rb +++ b/src/ruby/Vector3_TEST.rb @@ -425,6 +425,29 @@ def test_finite assert(vec1.IsFinite(), "Vector3 vec1 should be be finite") end + def test_nan + nanVec = Ignition::Math::Vector3d.NaN + assert(!nanVec.IsFinite(), + "NaN vector shouldn't be finite") + assert(nanVec.X().nan?, "X should be NaN") + assert(nanVec.Y().nan?, "Y should be NaN") + assert(nanVec.Z().nan?, "Z should be NaN") + + nanVec.Correct() + assert(Ignition::Math::Vector3d.Zero == nanVec, + "Corrected vector should equal zero") + + nanVecF = Ignition::Math::Vector3f.NaN + assert(!nanVecF.IsFinite(), + "NaN vector shouldn't be finite") + assert(nanVecF.X().nan?, "X should be NaN") + assert(nanVecF.Y().nan?, "Y should be NaN") + assert(nanVecF.Z().nan?, "Z should be NaN") + + nanVecF.Correct() + assert(Ignition::Math::Vector3f.Zero == nanVecF, + "Corrected vector should equal zero") + end end exit Test::Unit::UI::Console::TestRunner.run(Vector3_TEST).passed? ? 0 : -1 diff --git a/src/ruby/Vector4.i b/src/ruby/Vector4.i index d5f2bbf7b..a9235cae3 100644 --- a/src/ruby/Vector4.i +++ b/src/ruby/Vector4.i @@ -35,6 +35,7 @@ namespace ignition { public: static const Vector4 Zero; public: static const Vector4 One; + public: static const Vector4 NaN; public: Vector4(); public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w); public: Vector4(const Vector4 &_v); @@ -56,6 +57,7 @@ namespace ignition public: bool operator==(const Vector4 &_v) const; public: bool Equal(const Vector4 &_v, const T &_tol) const; public: bool IsFinite() const; + public: inline void Correct(); public: inline T X() const; public: inline T Y() const; public: inline T Z() const; diff --git a/src/ruby/Vector4_TEST.rb b/src/ruby/Vector4_TEST.rb index 1d750133d..c594bcc16 100644 --- a/src/ruby/Vector4_TEST.rb +++ b/src/ruby/Vector4_TEST.rb @@ -295,6 +295,32 @@ def test_finite assert(vec1.IsFinite(), "Vector4 vec1 should be be finite") end + + def test_nan + nanVec = Ignition::Math::Vector4d.NaN + assert(!nanVec.IsFinite(), + "NaN vector shouldn't be finite") + assert(nanVec.X().nan?, "X should be NaN") + assert(nanVec.Y().nan?, "Y should be NaN") + assert(nanVec.Z().nan?, "Z should be NaN") + assert(nanVec.W().nan?, "W should be NaN") + + nanVec.Correct() + assert(Ignition::Math::Vector4d.Zero == nanVec, + "Corrected vector should equal zero") + + nanVecF = Ignition::Math::Vector4f.NaN + assert(!nanVecF.IsFinite(), + "NaN vector shouldn't be finite") + assert(nanVecF.X().nan?, "X should be NaN") + assert(nanVecF.Y().nan?, "Y should be NaN") + assert(nanVecF.Z().nan?, "Z should be NaN") + assert(nanVecF.W().nan?, "W should be NaN") + + nanVecF.Correct() + assert(Ignition::Math::Vector4f.Zero == nanVecF, + "Corrected vector should equal zero") + end end exit Test::Unit::UI::Console::TestRunner.run(Vector4_TEST).passed? ? 0 : -1 From 9fef0d995a85a2d3a54e4c45fec7863fab781999 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 16 Sep 2021 17:20:02 +0800 Subject: [PATCH 46/81] style Signed-off-by: Arjo Chakravarty --- src/Box_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index e433d0dbc..d2107498d 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -155,7 +155,7 @@ TEST(BoxTest, CenterOfVolumeBelow) math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 5.0); EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value()); - EXPECT_EQ(box.CenterOfVolumeBelow(plane), math::Vector3d(0,0,0)); + EXPECT_EQ(box.CenterOfVolumeBelow(plane), math::Vector3d(0, 0, 0)); } } From 5a20d68c80cdcf7c72f2e675f4f69fe2ce257d95 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 16 Sep 2021 17:44:38 +0800 Subject: [PATCH 47/81] update intersections test Signed-off-by: Arjo Chakravarty --- include/ignition/math/Box.hh | 14 +++++++------- src/Box_TEST.cc | 15 +++++++++++++++ 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index 88db6c194..3537b3cc8 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -175,18 +175,18 @@ namespace ignition /// could be due to an invalid size (<=0) or density (<=0). public: bool MassMatrix(MassMatrix3 &_massMat) const; - /// \brief Size of the box. - private: Vector3 size = Vector3::Zero; - - /// \brief The box's material. - private: ignition::math::Material material; - /// \brief Get intersection between a plane and the box. /// \param[in] _plane The plane against which we are testing intersection. /// \returns a list of points along the edges of the box where the /// intersection occurs. - private: std::vector> Intersections( + public: std::vector> Intersections( const Plane &_plane) const; + + /// \brief Size of the box. + private: Vector3 size = Vector3::Zero; + + /// \brief The box's material. + private: ignition::math::Material material; }; /// \typedef Box Boxi diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index d2107498d..16a2d9b34 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -113,6 +113,21 @@ TEST(BoxTest, VolumeAndDensity) EXPECT_GT(0.0, box2.DensityFromMass(mass)); } +////////////////////////////////////////////////// +TEST(BoxTest, Intersects) +{ + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); + EXPECT_EQ(box.Intersections(plane).size(), 0); + } + + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 0); + EXPECT_EQ(box.Intersections(plane).size(), 4); + } +} ////////////////////////////////////////////////// TEST(BoxTest, VolumeBelow) From e6e6e65592052cd97631e89e6401f56ebf518265 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 16 Sep 2021 17:47:56 +0800 Subject: [PATCH 48/81] Removed `GetPointOnPlane` as its unessescary Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 16 ---------------- src/Plane_TEST.cc | 11 ----------- 2 files changed, 27 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 68bfa9b11..af94bccef 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -122,22 +122,6 @@ namespace ignition return this->normal.Dot(_point) - this->d; } - /// \brief Given x,y point find corresponding Z point on plane. - /// \param[in] x - 2d x coordinate. - /// \param[in] y - 2d y coordinate. - /// \param[in] eps - The value to consider zero. Defaults to 1e-16 - /// \return coincident point. - public: Vector3 GetPointOnPlane(const T x, - const T y, - const T eps = 1e-16) const - { - auto z_val = (std::fabs(this->Normal().Z()) > eps) ? - (this->Offset() - (this->Normal().Dot({x, y, 0})))/this->Normal().Z() - : 0; - auto coincidentPoint = Vector3{x, y, z_val}; - return coincidentPoint; - } - /// \brief Get the intersection of a line with the plane /// given the line's gradient and a point in parametrized space. /// \param[in] _point A point that lies on a line. diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 5bf3f6557..8df17f8f0 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -89,17 +89,6 @@ TEST(PlaneTest, Plane) EXPECT_TRUE(plane.Normal() == Vector3d(0, 1, 0)); EXPECT_TRUE(plane.Size() == Vector2d(4, 4)); } - - { - auto plane = Planed(Vector3d(0, 1, 0), Vector2d(4, 4), 5.0); - auto point = plane.GetPointOnPlane(9, 5); - EXPECT_NEAR(plane.Normal().Dot(point), plane.Offset(), 1e-3); - } - { - auto plane = Planed(Vector3d(0, 0, 1), Vector2d(4, 4), 5.0); - auto point = plane.GetPointOnPlane(9, 7); - EXPECT_NEAR(plane.Normal().Dot(point), plane.Offset(), 1e-3); - } } ///////////////////////////////////////////////// From 7189fe01e910d022a6debdbb1e07592cb63dfb06 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 16 Sep 2021 21:58:27 +0800 Subject: [PATCH 49/81] fix :warning: about signess Signed-off-by: Arjo Chakravarty --- src/Box_TEST.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 16a2d9b34..9cbb46ea2 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -119,13 +119,13 @@ TEST(BoxTest, Intersects) { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); - EXPECT_EQ(box.Intersections(plane).size(), 0); + EXPECT_EQ(box.Intersections(plane).size(), 0UL); } { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 0); - EXPECT_EQ(box.Intersections(plane).size(), 4); + EXPECT_EQ(box.Intersections(plane).size(), 4UL); } } From d98acb72d805a071e487b56511f1b123ca4da1d4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 17 Sep 2021 07:58:40 +0800 Subject: [PATCH 50/81] Update include/ignition/math/Box.hh Co-authored-by: Louise Poubel Signed-off-by: Arjo Chakravarty --- include/ignition/math/Box.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index 3537b3cc8..fe225c7f2 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -138,7 +138,7 @@ namespace ignition /// \brief Center of volume below the plane. This is useful when /// calculating where the buoyancy should be applied. - /// \param[in] _plane The plane which slices the sphere. + /// \param[in] _plane The plane which slices the box. public: std::optional> CenterOfVolumeBelow(const Plane &_plane) const; From 8cfbb7d2ec98d334e64436bb5ddf665eb2754d51 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 17 Sep 2021 08:16:34 +0800 Subject: [PATCH 51/81] Rename intersections Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 2 +- include/ignition/math/detail/Box.hh | 2 +- src/Plane_TEST.cc | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index af94bccef..785b5b3f5 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -128,7 +128,7 @@ namespace ignition /// \param[in] _gradient The gradient of the line. /// \return The point of intersection. std::nullopt if the line is /// parrallel to the plane. - public: std::optional> Intersect( + public: std::optional> Intersection( const Vector3 &_point, const Vector3 &_gradient) const { diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 497c69df4..78c62785c 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -333,7 +333,7 @@ std::vector> Box::Intersections( { for(auto &a : axes) { - auto intersection = _plane.Intersect(v, a); + auto intersection = _plane.Intersection(v, a); if(intersection.has_value() && intersection->X() >= -this->size.X()/2 && intersection->X() <= this->size.X()/2 && diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 8df17f8f0..8d93fd98d 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -155,7 +155,7 @@ TEST(PlaneTest, SideAxisAlignedBox) TEST(PlaneTest, Intersection) { Planed plane(Vector3d(0.5, 0, 1), 1); - auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(1, 0, 1)); + auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 1)); EXPECT_TRUE(intersect.has_value()); EXPECT_NEAR(intersect->Dot(plane.Normal()), plane.Offset(), 1e-6); } From cf438da99be2e46a8b7f7bb2b17c1cd5c303ac4e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 17 Sep 2021 09:09:47 +0800 Subject: [PATCH 52/81] Rename Vector projection Signed-off-by: Arjo Chakravarty --- include/ignition/math/Vector3.hh | 4 ++-- include/ignition/math/detail/Box.hh | 12 ++++++++---- src/Vector3_TEST.cc | 6 +++--- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index b981730c9..8eef397cd 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -264,10 +264,10 @@ namespace ignition return n.Normalize(); } - /// \brief Get Projection of another onto this vector + /// \brief Get Length of a Vector Projection of another onto this vector /// \param[in] _v the vector /// \return the projection - public: T Project(const Vector3 &_v) const + public: T VectorProjectionLength(const Vector3 &_v) const { return this->Dot(_v) / this->Length(); } diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 78c62785c..6ffc77f3f 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -141,20 +141,24 @@ std::vector, T>> TrianglesInPlane(Plane &_plane, if(pointsInPlane.size() < 3) return {}; + + // Choose a basis in the plane of the triangle auto axis1 = (pointsInPlane[0] - centroid).Normalize(); auto axis2 = axis1.Cross(_plane.Normal()).Normalize(); + // Since the polygon is always convex, we can try to create a fan of triangles + // by sorting the points by their angle in the plane basis. std::sort(pointsInPlane.begin(), pointsInPlane.end(), [centroid, axis1, axis2] (const Vector3 &a, const Vector3 &b) { auto aDisplacement = a - centroid; auto bDisplacement = b - centroid; - auto aX = axis1.Project(aDisplacement); - auto aY = axis2.Project(aDisplacement); + auto aX = axis1.VectorProjectionLength(aDisplacement); + auto aY = axis2.VectorProjectionLength(aDisplacement); - auto bX = axis1.Project(bDisplacement); - auto bY = axis2.Project(bDisplacement); + auto bX = axis1.VectorProjectionLength(bDisplacement); + auto bY = axis2.VectorProjectionLength(bDisplacement); return atan2(aY, aX) < atan2(bY, bX); }); diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc index 7a37d3f33..e92a30817 100644 --- a/src/Vector3_TEST.cc +++ b/src/Vector3_TEST.cc @@ -442,9 +442,9 @@ TEST(Vector3dTest, Projection) math::Vector3d v2(0, 1, 0); math::Vector3d v3(2, 3, 2); - EXPECT_NEAR(v1.Project(v3), 2, 1e-15); - EXPECT_NEAR(v2.Project(v3), 3, 1e-15); - EXPECT_NEAR(v1.Project(v2), 0, 1e-15); + EXPECT_NEAR(v1.VectorProjectionLength(v3), 2, 1e-15); + EXPECT_NEAR(v2.VectorProjectionLength(v3), 3, 1e-15); + EXPECT_NEAR(v1.VectorProjectionLength(v2), 0, 1e-15); } ///////////////////////////////////////////////// From aed0c6b2acacbffe3329c41fc137abd2275bf524 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 17 Sep 2021 13:53:30 +0800 Subject: [PATCH 53/81] Add tolerance argument Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 785b5b3f5..69e2c6059 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -126,13 +126,16 @@ namespace ignition /// given the line's gradient and a point in parametrized space. /// \param[in] _point A point that lies on a line. /// \param[in] _gradient The gradient of the line. + /// \param[in] _tolerance The tolerance for determining a line is + /// parallel to the plane. Optional, default=10^-16 /// \return The point of intersection. std::nullopt if the line is /// parrallel to the plane. public: std::optional> Intersection( const Vector3 &_point, - const Vector3 &_gradient) const + const Vector3 &_gradient, + const double &_tolerance = 1e-6) const { - if(abs(this->Normal().Dot(_gradient)) < 1e-6) + if(abs(this->Normal().Dot(_gradient)) < _tolerance) { return std::nullopt; } From c835aeec5d93c0e89467dd34c5566d764653f1bf Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 16 Sep 2021 13:31:29 -0700 Subject: [PATCH 54/81] Update line to point distance to support points projected beyond the segment Signed-off-by: Louise Poubel Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 23 ++++++++++-- src/Line3_TEST.cc | 65 +++++++++++++++++++++++++++++++--- 2 files changed, 81 insertions(+), 7 deletions(-) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index 08edfadb7..ad2a6143f 100644 --- 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 &_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 index d6c1cd59a..104e7bfe2 100644 --- 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); + } } ///////////////////////////////////////////////// From b1a172fe45655263bc25bf98a46bfd430a490bc8 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 16 Sep 2021 16:12:18 -0700 Subject: [PATCH 55/81] Suggestions to #219 Signed-off-by: Louise Poubel Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 6 ++-- include/ignition/math/Sphere.hh | 5 +-- include/ignition/math/Vector3.hh | 2 +- include/ignition/math/detail/Sphere.hh | 14 +++++--- src/Line3_TEST.cc | 6 ++-- src/Plane_TEST.cc | 24 ++++++++++++-- src/Sphere_TEST.cc | 45 +++++++++++++++++++++++++- src/Vector3_TEST.cc | 30 +++++++++++++++++ 8 files changed, 115 insertions(+), 17 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 69e2c6059..20672fda2 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -122,9 +122,9 @@ namespace ignition return this->normal.Dot(_point) - this->d; } - /// \brief Get the intersection of a line with the plane + /// \brief Get the intersection of an infinite line with the plane, /// given the line's gradient and a point in parametrized space. - /// \param[in] _point A point that lies on a line. + /// \param[in] _point A point that lies on the line. /// \param[in] _gradient The gradient of the line. /// \param[in] _tolerance The tolerance for determining a line is /// parallel to the plane. Optional, default=10^-16 @@ -140,7 +140,7 @@ namespace ignition return std::nullopt; } auto constant = this->Offset() - this->Normal().Dot(_point); - auto param = constant/this->Normal().Dot(_gradient); + auto param = constant / this->Normal().Dot(_gradient); auto intersection = _point + _gradient*param; return intersection; } diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh index dfadeceb0..e01c8cf8f 100644 --- a/include/ignition/math/Sphere.hh +++ b/include/ignition/math/Sphere.hh @@ -94,12 +94,13 @@ namespace ignition /// \brief Get the volume of sphere below a given plane in m^3. /// It is assumed that the center of the sphere is on the origin + /// The plane's orientation is not considered. /// \param[in] _plane The plane which slices this sphere. /// \return Volume below the sphere in m^3. public: Precision VolumeBelow(const Planed &_plane) const; - /// \brief Center of volume below the plane. This is useful when - /// calculating where the buoyancy should be applied. + /// \brief Center of volume below the plane. This is useful for example + /// when calculating where buoyancy should be applied. /// \param[in] _plane The plane which slices the sphere. /// \return The center of volume if there is anything under the plane, /// otherwise return a std::nullopt. diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index 8eef397cd..8801a6a35 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -272,7 +272,7 @@ namespace ignition return this->Dot(_v) / this->Length(); } - /// \brief Get distance to a line + /// \brief Get distance to an infinite line defined by 2 points. /// \param[in] _pt1 first point on the line /// \param[in] _pt2 second point on the line /// \return the minimum distance from this point to the line diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 4c91a1b2f..02711c512 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -103,18 +103,24 @@ T Sphere::VolumeBelow(const Planed &_plane) const // get nearest point to center on plane auto dist = _plane.Distance(Vector3d(0, 0, 0)); - if(dist < -r) + if (dist < -r) { // sphere is completely below plane return Volume(); } - else if(dist > r) + else if (dist > r) { // sphere is completely above plane - return 0; + return 0.0; } - auto h = r + dist; + // Vertical plane + if (_plane.Normal().Z() < 1e-6) + { + return 0.0; + } + + auto h = r - dist; return IGN_PI * h * h * (3 * r - h) / 3; } diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc index 104e7bfe2..de2c53178 100644 --- a/src/Line3_TEST.cc +++ b/src/Line3_TEST.cc @@ -278,17 +278,17 @@ TEST(Line3Test, DistanceToPoint) // Point projected onto the line { math::Vector3d point(1, -1, 0); - EXPECT_NEAR(line.Distance(point), point.Length(), 1e-3); + EXPECT_DOUBLE_EQ(line.Distance(point), point.Length()); } // Points projected beyond the line's ends { math::Vector3d point(2, 2, 3); - EXPECT_NEAR(line.Distance(point), point.Distance(pointA), 1e-3); + EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointA)); } { math::Vector3d point(-5, -3, -8); - EXPECT_NEAR(line.Distance(point), point.Distance(pointB), 1e-3); + EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointB)); } } diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 8d93fd98d..2a41467b1 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -155,7 +155,25 @@ TEST(PlaneTest, SideAxisAlignedBox) TEST(PlaneTest, Intersection) { Planed plane(Vector3d(0.5, 0, 1), 1); - auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 1)); - EXPECT_TRUE(intersect.has_value()); - EXPECT_NEAR(intersect->Dot(plane.Normal()), plane.Offset(), 1e-6); + { + auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(1, 0, 1)); + EXPECT_TRUE(intersect.has_value()); + EXPECT_NEAR(intersect->Dot(plane.Normal()), plane.Offset(), 1e-6); + } + + plane.Set(Vector3d(1, 0, 0), 2); + { + auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(1, 0, 0)); + EXPECT_TRUE(intersect.has_value()); + EXPECT_EQ(intersect.value(), Vector3d(2, 0, 0)); + } + { + auto intersect = plane.Intersect(Vector3d(1, 1, 0), Vector3d(-1, -1, 0)); + EXPECT_TRUE(intersect.has_value()); + EXPECT_EQ(intersect.value(), Vector3d(2, 2, 0)); + } + { + auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(0, 1, 0)); + EXPECT_FALSE(intersect.has_value()); + } } diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 47c71e9b0..59454a534 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -138,18 +138,61 @@ TEST(SphereTest, VolumeBelow) double r = 2; math::Sphered sphere(r); + // Fully below { math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), 2*r); EXPECT_NEAR(sphere.Volume(), sphere.VolumeBelow(_plane), 1e-3); } + { + math::Planed _plane(math::Vector3d{0, 0, -1}, math::Vector2d(4, 4), 2*r); + EXPECT_NEAR(sphere.Volume(), sphere.VolumeBelow(_plane), 1e-3); + } + // Fully above { math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), -2*r); EXPECT_NEAR(sphere.VolumeBelow(_plane), 0, 1e-3); } + // Hemisphere { math::Planed _plane(math::Vector3d{0, 0, 1}, 0); - EXPECT_NEAR(sphere.Volume()/2, sphere.VolumeBelow(_plane), 1e-3); + EXPECT_NEAR(sphere.Volume() / 2, sphere.VolumeBelow(_plane), 1e-3); + } + + // Vertical plane + { + math::Planed _plane(math::Vector3d{1, 0, 0}, 0); + EXPECT_NEAR(0.0, sphere.VolumeBelow(_plane), 1e-3); + } + + // Expectations from https://planetcalc.com/283/ + { + math::Planed _plane(math::Vector3d{0, 0, 1}, 0.5); + EXPECT_NEAR(22.90745, sphere.VolumeBelow(_plane), 1e-3); } + + { + math::Planed _plane(math::Vector3d{0, 0, 1}, -0.5); + EXPECT_NEAR(10.60288, sphere.VolumeBelow(_plane), 1e-3); + } +} + +////////////////////////////////////////////////// +TEST(SphereTest, CenterOfVolumeBelow) +{ + double r = 2; + math::Sphered sphere(r); + + { + math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), 2*r); + EXPECT_EQ(Vector3d(0, 0, 0), sphere.CenterOfVolumeBelow(_plane)); + } + + { + math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), -2*r); + EXPECT_FALSE(sphere.CenterOfVolumeBelow(_plane)); + } + + // TODO: test more cases } diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc index e92a30817..81d875340 100644 --- a/src/Vector3_TEST.cc +++ b/src/Vector3_TEST.cc @@ -516,3 +516,33 @@ TEST(Vector3dTest, NaN) EXPECT_EQ(math::Vector3f::Zero, nanVecF); EXPECT_TRUE(nanVecF.IsFinite()); } + +///////////////////////////////////////////////// +TEST(Vector3dTest, DistToLine) +{ + // Line on horizontal plane + math::Vector3d pointA{0, -1, 0}; + math::Vector3d pointB{0, 1, 0}; + + // Point on the line + { + math::Vector3d point(0, 0.5, 0); + EXPECT_DOUBLE_EQ(point.DistToLine(pointA, pointB), 0.0); + } + + // Points projected onto the line + { + math::Vector3d point(5, 0, 0); + EXPECT_DOUBLE_EQ(point.DistToLine(pointA, pointB), 5); + } + { + math::Vector3d point(-1, -1, 0); + EXPECT_DOUBLE_EQ(point.DistToLine(pointA, pointB), 1); + } + + // Point projected beyond the segment's ends + { + math::Vector3d point(0, 2, 0); + EXPECT_DOUBLE_EQ(point.DistToLine(pointA, pointB), 0); + } +} From 9979f83dc1101f53b33aef0e29aeb94f54aa8598 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 16 Sep 2021 11:34:49 -0700 Subject: [PATCH 56/81] =?UTF-8?q?=F0=9F=8C=90=20Spherical=20coordinates:?= =?UTF-8?q?=20bug=20fix,=20docs=20and=20sanity=20checks=20(#235)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Migration guide and LOCAL2 Signed-off-by: Louise Poubel Co-authored-by: Carlos Agüero Co-authored-by: Steve Peters Signed-off-by: Arjo Chakravarty --- Migration.md | 7 + include/ignition/math/SphericalCoordinates.hh | 46 ++- src/SphericalCoordinates.cc | 39 ++- src/SphericalCoordinates_TEST.cc | 294 +++++++++++++++++- 4 files changed, 370 insertions(+), 16 deletions(-) diff --git a/Migration.md b/Migration.md index 4a5cfb2ef..ff607372e 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,13 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Ignition Math 6.8 to 6.9 + +1. **SphericalCoordinates**: A bug related to the LOCAL frame was fixed. To + preserve behaviour, the `LOCAL` frame was left with the bug, and a new + `LOCAL2` frame was introduced, which can be used to get the correct + calculations. + ## Ignition Math 4.X to 5.X ### Additions diff --git a/include/ignition/math/SphericalCoordinates.hh b/include/ignition/math/SphericalCoordinates.hh index 8ee48ccc4..120407ab6 100644 --- a/include/ignition/math/SphericalCoordinates.hh +++ b/include/ignition/math/SphericalCoordinates.hh @@ -34,7 +34,6 @@ namespace ignition // class SphericalCoordinatesPrivate; - /// \class SphericalCoordinates SphericalCoordinates.hh commmon/common.hh /// \brief Convert spherical coordinates for planetary surfaces. class IGNITION_MATH_VISIBLE SphericalCoordinates { @@ -61,7 +60,12 @@ namespace ignition GLOBAL = 3, /// \brief Heading-adjusted tangent plane (X, Y, Z) - LOCAL = 4 + /// This has kept a bug for backwards compatibility, use + /// LOCAL2 for the correct behaviour. + LOCAL = 4, + + /// \brief Heading-adjusted tangent plane (X, Y, Z) + LOCAL2 = 5 }; /// \brief Constructor. @@ -91,7 +95,16 @@ namespace ignition public: ~SphericalCoordinates(); /// \brief Convert a Cartesian position vector to geodetic coordinates. - /// \param[in] _xyz Cartesian position vector in the world frame. + /// This performs a `PositionTransform` from LOCAL to SPHERICAL. + /// + /// There's a known bug with this computation that can't be fixed on + /// version 6 to avoid behaviour changes. Directly call + /// `PositionTransform(_xyz, LOCAL2, SPHERICAL)` for correct results. + /// Note that `PositionTransform` returns spherical coordinates in + /// radians. + /// + /// \param[in] _xyz Cartesian position vector in the heading-adjusted + /// world frame. /// \return Cooordinates: geodetic latitude (deg), longitude (deg), /// altitude above sea level (m). public: ignition::math::Vector3d SphericalFromLocalPosition( @@ -99,7 +112,14 @@ namespace ignition /// \brief Convert a Cartesian velocity vector in the local frame /// to a global Cartesian frame with components East, North, Up. - /// \param[in] _xyz Cartesian velocity vector in the world frame. + /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)` + /// + /// There's a known bug with this computation that can't be fixed on + /// version 6 to avoid behaviour changes. Directly call + /// `VelocityTransform(_xyz, LOCAL2, GLOBAL)` for correct results. + /// + /// \param[in] _xyz Cartesian velocity vector in the heading-adjusted + /// world frame. /// \return Rotated vector with components (x,y,z): (East, North, Up). public: ignition::math::Vector3d GlobalFromLocalVelocity( const ignition::math::Vector3d &_xyz) const; @@ -110,6 +130,11 @@ namespace ignition /// \return Conversion to SurfaceType. public: static SurfaceType Convert(const std::string &_str); + /// \brief Convert a SurfaceType to a string. + /// \param[in] _type Surface type + /// \return Type as string + public: static std::string Convert(SurfaceType _type); + /// \brief Get the distance between two points expressed in geographic /// latitude and longitude. It assumes that both points are at sea level. /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents @@ -167,13 +192,16 @@ namespace ignition public: void SetHeadingOffset(const ignition::math::Angle &_angle); /// \brief Convert a geodetic position vector to Cartesian coordinates. - /// \param[in] _xyz Geodetic position in the planetary frame of reference - /// \return Cartesian position vector in the world frame + /// This performs a `PositionTransform` from SPHERICAL to LOCAL. + /// \param[in] _latLonEle Geodetic position in the planetary frame of + /// reference. X: latitude (deg), Y: longitude (deg), X: altitude. + /// \return Cartesian position vector in the heading-adjusted world frame. public: ignition::math::Vector3d LocalFromSphericalPosition( - const ignition::math::Vector3d &_xyz) const; + const ignition::math::Vector3d &_latLonEle) const; /// \brief Convert a Cartesian velocity vector with components East, /// North, Up to a local cartesian frame vector XYZ. + /// This is a wrapper around `VelocityTransform(_xyz, GLOBAL, LOCAL)` /// \param[in] _xyz Vector with components (x,y,z): (East, North, Up). /// \return Cartesian vector in the world frame. public: ignition::math::Vector3d LocalFromGlobalVelocity( @@ -183,15 +211,17 @@ namespace ignition public: void UpdateTransformationMatrix(); /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame + /// Spherical coordinates use radians, while the other frames use meters. /// \param[in] _pos Position vector in frame defined by parameter _in /// \param[in] _in CoordinateType for input /// \param[in] _out CoordinateType for output - /// \return Transformed coordinate using cached orgin + /// \return Transformed coordinate using cached origin. public: ignition::math::Vector3d PositionTransform(const ignition::math::Vector3d &_pos, const CoordinateType &_in, const CoordinateType &_out) const; /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame + /// Spherical coordinates use radians, while the other frames use meters. /// \param[in] _vel Velocity vector in frame defined by parameter _in /// \param[in] _in CoordinateType for input /// \param[in] _out CoordinateType for output diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index d528b68e0..b73e7b94f 100644 --- a/src/SphericalCoordinates.cc +++ b/src/SphericalCoordinates.cc @@ -99,6 +99,18 @@ SphericalCoordinates::SurfaceType SphericalCoordinates::Convert( return EARTH_WGS84; } +////////////////////////////////////////////////// +std::string SphericalCoordinates::Convert( + SphericalCoordinates::SurfaceType _type) +{ + if (_type == EARTH_WGS84) + return "EARTH_WGS84"; + + std::cerr << "SurfaceType not recognized, " + << "EARTH_WGS84 returned by default" << std::endl; + return "EARTH_WGS84"; +} + ////////////////////////////////////////////////// SphericalCoordinates::SphericalCoordinates() : dataPtr(new SphericalCoordinatesPrivate) @@ -372,8 +384,19 @@ ignition::math::Vector3d SphericalCoordinates::PositionTransform( this->dataPtr->sinHea); tmp.Y(-_pos.X() * this->dataPtr->sinHea - _pos.Y() * this->dataPtr->cosHea); + tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp; + break; + } + + case LOCAL2: + { + tmp.X(_pos.X() * this->dataPtr->cosHea + _pos.Y() * + this->dataPtr->sinHea); + tmp.Y(-_pos.X() * this->dataPtr->sinHea + _pos.Y() * + this->dataPtr->cosHea); + tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp; + break; } - /* Falls through. */ case GLOBAL: { @@ -440,6 +463,7 @@ ignition::math::Vector3d SphericalCoordinates::PositionTransform( // Convert from ECEF TO LOCAL case LOCAL: + case LOCAL2: tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin); tmp = ignition::math::Vector3d( @@ -477,13 +501,21 @@ ignition::math::Vector3d SphericalCoordinates::VelocityTransform( // First, convert to an ECEF vector switch (_in) { - // ENU (note no break at end of case) + // ENU case LOCAL: tmp.X(-_vel.X() * this->dataPtr->cosHea + _vel.Y() * this->dataPtr->sinHea); tmp.Y(-_vel.X() * this->dataPtr->sinHea - _vel.Y() * this->dataPtr->cosHea); - /* Falls through. */ + tmp = this->dataPtr->rotGlobalToECEF * tmp; + break; + case LOCAL2: + tmp.X(_vel.X() * this->dataPtr->cosHea + _vel.Y() * + this->dataPtr->sinHea); + tmp.Y(-_vel.X() * this->dataPtr->sinHea + _vel.Y() * + this->dataPtr->cosHea); + tmp = this->dataPtr->rotGlobalToECEF * tmp; + break; // spherical case GLOBAL: tmp = this->dataPtr->rotGlobalToECEF * tmp; @@ -511,6 +543,7 @@ ignition::math::Vector3d SphericalCoordinates::VelocityTransform( // Convert from ECEF to local case LOCAL: + case LOCAL2: tmp = this->dataPtr->rotECEFToGlobal * tmp; tmp = ignition::math::Vector3d( tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea, diff --git a/src/SphericalCoordinates_TEST.cc b/src/SphericalCoordinates_TEST.cc index b7b115991..9af116ec9 100644 --- a/src/SphericalCoordinates_TEST.cc +++ b/src/SphericalCoordinates_TEST.cc @@ -77,6 +77,10 @@ TEST(SphericalCoordinatesTest, Convert) EXPECT_EQ(math::SphericalCoordinates::EARTH_WGS84, math::SphericalCoordinates::Convert("OTHER-COORD")); + + EXPECT_EQ("EARTH_WGS84", math::SphericalCoordinates::Convert(st)); + EXPECT_EQ("EARTH_WGS84", math::SphericalCoordinates::Convert( + static_cast(2))); } ////////////////////////////////////////////////// @@ -128,6 +132,8 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) math::SphericalCoordinates sc(st, lat, lon, elev, heading); // Check GlobalFromLocal with heading offset of 90 degrees + // Heading 0: X == East, Y == North, Z == Up + // Heading 90: X == North, Y == West , Z == Up { // local frame ignition::math::Vector3d xyz; @@ -136,8 +142,8 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) xyz.Set(1, 0, 0); enu = sc.GlobalFromLocalVelocity(xyz); - EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); - EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); + EXPECT_NEAR(enu.Y()/*North*/, xyz.X(), 1e-6); + EXPECT_NEAR(enu.X()/*East*/, -xyz.Y(), 1e-6); EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); xyz.Set(0, 1, 0); @@ -253,6 +259,49 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) EXPECT_NEAR(tmp.Z(), goog_s.Z(), 1e-2); } } + + // Give no heading offset to confirm ENU frame + { + ignition::math::Angle lat(0.3), lon(-1.2), heading(0.0); + double elev = 354.1; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // Check GlobalFromLocal with no heading offset + { + // local frame + ignition::math::Vector3d xyz; + // east, north, up + ignition::math::Vector3d enu; + + xyz.Set(1, 0, 0); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(0, 1, 0); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(1, -1, 0); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(2243.52334, 556.35, 435.6553); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + } + } } ////////////////////////////////////////////////// @@ -306,7 +355,7 @@ TEST(SphericalCoordinatesTest, BadCoordinateType) math::SphericalCoordinates sc; math::Vector3d pos(1, 2, -4); math::Vector3d result = sc.PositionTransform(pos, - static_cast(5), + static_cast(7), static_cast(6)); EXPECT_EQ(result, pos); @@ -328,13 +377,13 @@ TEST(SphericalCoordinatesTest, BadCoordinateType) EXPECT_EQ(result, pos); result = sc.VelocityTransform(pos, - static_cast(5), + static_cast(7), math::SphericalCoordinates::ECEF); EXPECT_EQ(result, pos); result = sc.VelocityTransform(pos, math::SphericalCoordinates::ECEF, - static_cast(5)); + static_cast(7)); EXPECT_EQ(result, pos); } @@ -383,3 +432,238 @@ TEST(SphericalCoordinatesTest, AssignmentOp) math::SphericalCoordinates sc2 = sc1; EXPECT_EQ(sc1, sc2); } + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, NoHeading) +{ + // Default heading + auto st = math::SphericalCoordinates::EARTH_WGS84; + math::Angle lat(IGN_DTOR(-22.9)); + math::Angle lon(IGN_DTOR(-43.2)); + math::Angle heading(0.0); + double elev = 0; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // Origin matches input + auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); + EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); + EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt.Y()); + EXPECT_DOUBLE_EQ(elev, latLonAlt.Z()); + + auto xyzOrigin = sc.LocalFromSphericalPosition(latLonAlt); + EXPECT_EQ(math::Vector3d::Zero, xyzOrigin); + + // Check how different lat/lon affect the local position + + // Increase latitude == go North == go +Y + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree() + 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_LT(xyzOrigin.Y(), xyz.Y()); + } + + // Decrease latitude == go South == go -Y + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree() - 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Increase longitude == go East == go +X + // Also move a bit -Y because this is the Southern Hemisphere + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree() + 1.0, elev}); + EXPECT_LT(xyzOrigin.X(), xyz.X()); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Decrease longitude == go West == go -X + // Also move a bit -Y because this is the Southern Hemisphere + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree() - 1.0, elev}); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Increase altitude + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree(), elev + 10.0}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_NEAR(xyzOrigin.Z() + 10.0, xyz.Z(), 1e-6); + } + + // Decrease altitude + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree(), elev - 10.0}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_NEAR(xyzOrigin.Z() - 10.0, xyz.Z(), 1e-6); + } + + // Check how global and local velocities are connected + + // Velocity in + // +X (East), +Y (North), -X (West), -Y (South), +Z (up), -Z (down) + for (auto global : { + math::Vector3d::UnitX, + math::Vector3d::UnitY, + math::Vector3d::UnitZ, + -math::Vector3d::UnitX, + -math::Vector3d::UnitY, + -math::Vector3d::UnitZ}) + { + auto local = sc.LocalFromGlobalVelocity(global); + EXPECT_EQ(global, local); + + // This function is broken for horizontal velocities + global = sc.GlobalFromLocalVelocity(local); + if (abs(global.Z()) < 0.1) + { + EXPECT_NE(global, local); + } + else + { + EXPECT_EQ(global, local); + } + + // Directly call fixed version + global = sc.VelocityTransform(local, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(global, local); + } +} + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, WithHeading) +{ + // Heading 90 deg: X == North, Y == West , Z == Up + auto st = math::SphericalCoordinates::EARTH_WGS84; + math::Angle lat(IGN_DTOR(-22.9)); + math::Angle lon(IGN_DTOR(-43.2)); + math::Angle heading(IGN_DTOR(90.0)); + double elev = 0; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // Origin matches input + auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); + EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); + EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt.Y()); + EXPECT_DOUBLE_EQ(elev, latLonAlt.Z()); + + auto xyzOrigin = sc.LocalFromSphericalPosition(latLonAlt); + EXPECT_EQ(math::Vector3d::Zero, xyzOrigin); + + // Check how different lat/lon affect the local position + + // Increase latitude == go North == go +X + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree() + 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_LT(xyzOrigin.X(), xyz.X()); + } + + // Decrease latitude == go South == go -X + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree() - 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Increase longitude == go East == go -Y (and a bit -X) + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree() + 1.0, elev}); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Decrease longitude == go West == go +Y (and a bit -X) + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree() - 1.0, elev}); + EXPECT_LT(xyzOrigin.Y(), xyz.Y()); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Check how global and local velocities are connected + + // Global | Local + // ---------- | ------ + // +X (East) | -Y + // -X (West) | +Y + // +Y (North) | +X + // -Y (South) | -X + std::vector> globalLocal = + {{math::Vector3d::UnitX, -math::Vector3d::UnitY}, + {-math::Vector3d::UnitX, math::Vector3d::UnitY}, + {math::Vector3d::UnitY, math::Vector3d::UnitX}, + {-math::Vector3d::UnitY, -math::Vector3d::UnitX}}; + for (auto [global, local] : globalLocal) + { + auto localRes = sc.LocalFromGlobalVelocity(global); + EXPECT_EQ(local, localRes); + + // Directly call fixed version + auto globalRes = sc.VelocityTransform(local, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(global, globalRes); + } +} + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, Inverse) +{ + auto st = math::SphericalCoordinates::EARTH_WGS84; + ignition::math::Angle lat(0.3), lon(-1.2), heading(0.5); + double elev = 354.1; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // GLOBAL <-> LOCAL2 + { + math::Vector3d in(1, 2, -4); + auto out = sc.VelocityTransform(in, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_NE(in, out); + auto reverse = sc.VelocityTransform(out, + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::LOCAL2); + EXPECT_EQ(in, reverse); + } + + { + math::Vector3d in(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_NE(in, out); + auto reverse = sc.PositionTransform(out, + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::LOCAL2); + EXPECT_EQ(in, reverse); + } + + // SPHERICAL <-> LOCAL2 + { + math::Vector3d in(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::SPHERICAL); + EXPECT_NE(in, out); + auto reverse = sc.PositionTransform(out, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL2); + EXPECT_EQ(in, reverse); + } +} From 5c9343fe17e3b073d6d1f46c9d371df7f9631860 Mon Sep 17 00:00:00 2001 From: LolaSegura <48759425+LolaSegura@users.noreply.github.com> Date: Thu, 16 Sep 2021 22:19:02 -0300 Subject: [PATCH 57/81] Adds python interface to Quaternion, Pose3, Matrix3 and Matrix4 (#221) * Adds scripting interface to Quaternion and a python test * Adds scripting interface to Matrix3 and a python test * Adds scripting interface to Pose3 and a python test * Solves bug in the Reset() method inside Pose3 * Adds scripting interface to Matrix4 and a python test * Solves bug in the Construct test for Matrix4 class * Adds %rename tag to interface files in order to match pep-8 naiming style. * Adds a python method to convert from a Matrix3 to a Quaternion. * Adds to_quaternion() method to Matrix3. Signed-off-by: LolaSegura * Adds python binding for Quaternion::ToAxis method. Signed-off-by: Franco Cipollone * Matrix3_TEST: improve multiplication test This changes the test matrices that are multiplied togther so that they aren't scalar multiples of each other. This confirms non-commutativity in the test. * Matrix3_TEST.py: add stream out test Signed-off-by: Steve Peters Co-authored-by: Franco Cipollone Co-authored-by: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Co-authored-by: Steve Peters Signed-off-by: Arjo Chakravarty --- include/ignition/math/Pose3.hh | 2 +- src/Matrix3_TEST.cc | 24 +- src/Matrix4_TEST.cc | 78 ++--- src/python/CMakeLists.txt | 4 + src/python/Matrix3.i | 97 ++++++ src/python/Matrix3_TEST.py | 337 +++++++++++++++++++++ src/python/Matrix4.i | 102 +++++++ src/python/Matrix4_TEST.py | 520 ++++++++++++++++++++++++++++++++ src/python/Pose3.i | 102 +++++++ src/python/Pose3_TEST.py | 183 +++++++++++ src/python/Quaternion.i | 171 +++++++++++ src/python/Quaternion_TEST.py | 536 +++++++++++++++++++++++++++++++++ src/python/python.i | 4 + 13 files changed, 2109 insertions(+), 51 deletions(-) create mode 100644 src/python/Matrix3.i create mode 100644 src/python/Matrix3_TEST.py create mode 100644 src/python/Matrix4.i create mode 100644 src/python/Matrix4_TEST.py create mode 100644 src/python/Pose3.i create mode 100644 src/python/Pose3_TEST.py create mode 100644 src/python/Quaternion.i create mode 100644 src/python/Quaternion_TEST.py diff --git a/include/ignition/math/Pose3.hh b/include/ignition/math/Pose3.hh index e6e75f33a..b971cee8b 100644 --- a/include/ignition/math/Pose3.hh +++ b/include/ignition/math/Pose3.hh @@ -327,7 +327,7 @@ namespace ignition { // set the position to zero this->p.Set(); - this->q = Quaterniond::Identity; + this->q = Quaternion::Identity; } /// \brief Rotate vector part of a pose about the origin diff --git a/src/Matrix3_TEST.cc b/src/Matrix3_TEST.cc index ddcdbf6d6..1cd0044cd 100644 --- a/src/Matrix3_TEST.cc +++ b/src/Matrix3_TEST.cc @@ -131,24 +131,24 @@ TEST(Matrix3dTest, OperatorMul) 4, 5, 6, 7, 8, 9); - math::Matrix3d matB(10, 20, 30, - 40, 50, 60, - 70, 80, 90); + math::Matrix3d matB(11, 21, 31, + 41, 51, 61, + 71, 81, 91); mat = matA * matB; - EXPECT_EQ(mat, math::Matrix3d(300, 360, 420, - 660, 810, 960, - 1020, 1260, 1500)); + EXPECT_EQ(mat, math::Matrix3d(306, 366, 426, + 675, 825, 975, + 1044, 1284, 1524)); mat = matB * matA; - EXPECT_EQ(mat, math::Matrix3d(300, 360, 420, - 660, 810, 960, - 1020, 1260, 1500)); + EXPECT_EQ(mat, math::Matrix3d(312, 375, 438, + 672, 825, 978, + 1032, 1275, 1518)); mat = mat * 2.0; - EXPECT_EQ(mat, math::Matrix3d(600, 720, 840, - 1320, 1620, 1920, - 2040, 2520, 3000)); + EXPECT_EQ(mat, math::Matrix3d(624, 750, 876, + 1344, 1650, 1956, + 2064, 2550, 3036)); } ///////////////////////////////////////////////// diff --git a/src/Matrix4_TEST.cc b/src/Matrix4_TEST.cc index a1f7e62f9..d1459f87a 100644 --- a/src/Matrix4_TEST.cc +++ b/src/Matrix4_TEST.cc @@ -32,7 +32,7 @@ TEST(Matrix4dTest, Construct) { for (int j = 0; j < 4; ++j) { - EXPECT_DOUBLE_EQ(mat(i, i), 0.0); + EXPECT_DOUBLE_EQ(mat(i, j), 0.0); } } @@ -41,17 +41,16 @@ TEST(Matrix4dTest, Construct) { for (int j = 0; j < 4; ++j) { - EXPECT_DOUBLE_EQ(mat2(i, i), 0.0); + EXPECT_DOUBLE_EQ(mat2(i, j), 0.0); } } EXPECT_TRUE(mat2 == mat); - // Set individual values. math::Matrix4d mat3(0.0, 1.0, 2.0, 3.0, - 4.0, 5.0, 6.0, 7.0, - 8.0, 9.0, 10.0, 11.0, - 12.0, 13.0, 14.0, 15.0); + 4.0, 5.0, 6.0, 7.0, + 8.0, 9.0, 10.0, 11.0, + 12.0, 13.0, 14.0, 15.0); math::Matrix4d mat4; mat4 = mat3; @@ -126,7 +125,7 @@ TEST(Matrix4dTest, ConstructFromPose3d) // Rotate pitch by pi/2 so yaw coincides with roll causing a gimbal lock { math::Vector3d trans(3, 2, 1); - math::Quaterniond qt(0, IGN_PI/2, 0); + math::Quaterniond qt(0, IGN_PI / 2, 0); math::Pose3d pose(trans, qt); math::Matrix4d mat(pose); @@ -138,9 +137,9 @@ TEST(Matrix4dTest, ConstructFromPose3d) { // setup a ZXZ rotation to ensure non-commutative rotations - math::Pose3d pose1(1, -2, 3, 0, 0, IGN_PI/4); - math::Pose3d pose2(0, 1, -1, -IGN_PI/4, 0, 0); - math::Pose3d pose3(-1, 0, 0, 0, 0, -IGN_PI/4); + math::Pose3d pose1(1, -2, 3, 0, 0, IGN_PI / 4); + math::Pose3d pose2(0, 1, -1, -IGN_PI / 4, 0, 0); + math::Pose3d pose3(-1, 0, 0, 0, 0, -IGN_PI / 4); math::Matrix4d m1(pose1); math::Matrix4d m2(pose2); @@ -238,7 +237,7 @@ TEST(Matrix4dTest, MultiplyV) { for (int j = 0; j < 4; ++j) { - mat(i, j) = i-j; + mat(i, j) = i - j; } } @@ -254,8 +253,8 @@ TEST(Matrix4dTest, Multiply4) { for (int j = 0; j < 4; ++j) { - mat(i, j) = i-j; - mat1(j, i) = i+j; + mat(i, j) = i - j; + mat1(j, i) = i + j; } } @@ -277,28 +276,28 @@ TEST(Matrix4dTest, Multiply4) TEST(Matrix4dTest, Inverse) { math::Matrix4d mat(2, 3, 1, 5, - 1, 0, 3, 1, - 0, 2, -3, 2, - 0, 2, 3, 1); + 1, 0, 3, 1, + 0, 2, -3, 2, + 0, 2, 3, 1); math::Matrix4d mat1 = mat.Inverse(); EXPECT_EQ(mat1, math::Matrix4d(18, -35, -28, 1, - 9, -18, -14, 1, - -2, 4, 3, 0, - -12, 24, 19, -1)); + 9, -18, -14, 1, + -2, 4, 3, 0, + -12, 24, 19, -1)); } ///////////////////////////////////////////////// TEST(Matrix4dTest, GetAsPose3d) { math::Matrix4d mat(2, 3, 1, 5, - 1, 0, 3, 1, - 0, 2, -3, 2, - 0, 2, 3, 1); + 1, 0, 3, 1, + 0, 2, -3, 2, + 0, 2, 3, 1); math::Pose3d pose = mat.Pose(); EXPECT_EQ(pose, - math::Pose3d(5, 1, 2, -0.204124, 1.22474, 0.816497, 0.204124)); + math::Pose3d(5, 1, 2, -0.204124, 1.22474, 0.816497, 0.204124)); } ///////////////////////////////////////////////// @@ -408,7 +407,6 @@ TEST(Matrix4dTest, RotationDiagLessThanZero) EXPECT_EQ(euler, math::Vector3d(-1.5708, 4.26136, -1.3734)); } - { mat(0, 0) = -0.1; mat(1, 1) = -0.2; @@ -428,7 +426,6 @@ TEST(Matrix4dTest, RotationDiagLessThanZero) } } - ///////////////////////////////////////////////// TEST(Matrix4dTest, Rotation) { @@ -632,14 +629,14 @@ TEST(Matrix4dTest, Transpose) EXPECT_EQ(math::Matrix4d::Identity, math::Matrix4d::Identity.Transposed()); // Matrix and expected transpose - math::Matrix4d m(-2, 4, 0, -3.5, - 0.1, 9, 55, 1.2, + math::Matrix4d m(-2, 4, 0, -3.5, + 0.1, 9, 55, 1.2, -7, 1, 26, 11.5, .2, 3, -5, -0.1); - math::Matrix4d mT(-2, 0.1, -7, .2, - 4, 9, 1, 3, - 0, 55, 26, -5, - -3.5, 1.2, 11.5, -0.1); + math::Matrix4d mT(-2, 0.1, -7, .2, + 4, 9, 1, 3, + 0, 55, 26, -5, + -3.5, 1.2, 11.5, -0.1); EXPECT_NE(m, mT); EXPECT_EQ(m.Transposed(), mT); EXPECT_DOUBLE_EQ(m.Determinant(), m.Transposed().Determinant()); @@ -652,19 +649,23 @@ TEST(Matrix4dTest, Transpose) TEST(Matrix4dTest, LookAt) { EXPECT_EQ(math::Matrix4d::LookAt(-math::Vector3d::UnitX, - math::Vector3d::Zero).Pose(), + math::Vector3d::Zero) + .Pose(), math::Pose3d(-1, 0, 0, 0, 0, 0)); EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d(3, 2, 0), - math::Vector3d(0, 2, 0)).Pose(), + math::Vector3d(0, 2, 0)) + .Pose(), math::Pose3d(3, 2, 0, 0, 0, IGN_PI)); EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d(1, 6, 1), - math::Vector3d::One).Pose(), + math::Vector3d::One) + .Pose(), math::Pose3d(1, 6, 1, 0, 0, -IGN_PI_2)); EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d(-1, -1, 0), - math::Vector3d(1, 1, 0)).Pose(), + math::Vector3d(1, 1, 0)) + .Pose(), math::Pose3d(-1, -1, 0, 0, 0, IGN_PI_4)); // Default up is Z @@ -711,12 +712,13 @@ TEST(Matrix4dTest, LookAt) // Different ups EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d::One, math::Vector3d(0, 1, 1), - math::Vector3d::UnitY).Pose(), + math::Vector3d::UnitY) + .Pose(), math::Pose3d(1, 1, 1, IGN_PI_2, 0, IGN_PI)); EXPECT_EQ(math::Matrix4d::LookAt(math::Vector3d::One, math::Vector3d(0, 1, 1), - math::Vector3d(0, 1, 1)).Pose(), + math::Vector3d(0, 1, 1)) + .Pose(), math::Pose3d(1, 1, 1, IGN_PI_4, 0, IGN_PI)); } - diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 0df5117cd..390ed1e84 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -93,8 +93,12 @@ if (PYTHONLIBS_FOUND) Kmeans_TEST Line2_TEST Line3_TEST + Matrix3_TEST + Matrix4_TEST PID_TEST + Pose3_TEST python_TEST + Quaternion_TEST Rand_TEST RollingMean_TEST SemanticVersion_TEST diff --git a/src/python/Matrix3.i b/src/python/Matrix3.i new file mode 100644 index 000000000..5aa049134 --- /dev/null +++ b/src/python/Matrix3.i @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +%module matrix3 +%{ +#include +#include +#include +#include +#include +%} + +%include "std_string.i" +%include Quaternion.i + +namespace ignition +{ + namespace math + { + template + class Matrix3 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; + + public: static const Matrix3 Identity; + public: static const Matrix3 Zero; + public: Matrix3(); + public: Matrix3(const Matrix3 &_m); + public: Matrix3(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22); + public: explicit Matrix3(const Quaternion &_q); + public: virtual ~Matrix3() {} + public: void Set(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22); + public: void Axes(const Vector3 &_xAxis, + const Vector3 &_yAxis, + const Vector3 &_zAxis); + public: void Axis(const Vector3 &_axis, T _angle); + %rename(from_2_axes) From2Axes; + public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2); + public: void Col(unsigned int _c, const Vector3 &_v); + public: Matrix3 operator-(const Matrix3 &_m) const; + public: Matrix3 operator+(const Matrix3 &_m) const; + public: Matrix3 operator*(const T &_s) const; + public: Matrix3 operator*(const Matrix3 &_m) const; + public: Vector3 operator*(const Vector3 &_vec) const; + public: bool Equal(const Matrix3 &_m, const T &_tol) const; + public: bool operator==(const Matrix3 &_m) const; + public: bool operator!=(const Matrix3 &_m) const;; + public: T Determinant() const; + public: Matrix3 Inverse() const; + public: void Transpose(); + public: Matrix3 Transposed() const; + }; + + %extend Matrix3{ + T __call__(size_t row, size_t col) const { + return (*$self)(row, col); + } + } + + %extend Matrix3 { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %extend Matrix3 { + ignition::math::Quaternion to_quaternion() { + return ignition::math::Quaternion(*$self); + } + } + + %template(Matrix3i) Matrix3; + %template(Matrix3d) Matrix3; + %template(Matrix3f) Matrix3; + } +} diff --git a/src/python/Matrix3_TEST.py b/src/python/Matrix3_TEST.py new file mode 100644 index 000000000..22941264f --- /dev/null +++ b/src/python/Matrix3_TEST.py @@ -0,0 +1,337 @@ +# Copyright (C) 2021 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import unittest +from ignition.math import Matrix3d +from ignition.math import Quaterniond +from ignition.math import Vector3d + + +class TestMatrix3(unittest.TestCase): + + def test_matrix3d(self): + matrix = Matrix3d() + self.assertAlmostEqual(matrix, Matrix3d(0, 0, 0, 0, 0, 0, 0, 0, 0)) + + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + self.assertAlmostEqual(matrix, Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9)) + + matrix1 = Matrix3d(matrix) + self.assertAlmostEqual(matrix1, Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9)) + + matrix = Matrix3d() + matrix.axes(Vector3d(1, 1, 1), Vector3d(2, 2, 2), + Vector3d(3, 3, 3)) + self.assertAlmostEqual(matrix, Matrix3d(1, 2, 3, 1, 2, 3, 1, 2, 3)) + + matrix.axis(Vector3d(1, 1, 1), math.pi) + self.assertAlmostEqual(matrix, Matrix3d(1, 2, 2, 2, 1, 2, 2, 2, 1)) + + matrix.col(0, Vector3d(3, 4, 5)) + self.assertAlmostEqual(matrix, Matrix3d(3, 2, 2, 4, 1, 2, 5, 2, 1)) + + matrix.col(3, Vector3d(1, 1, 1)) + self.assertAlmostEqual(matrix, Matrix3d(3, 2, 1, 4, 1, 1, 5, 2, 1)) + + def test_sub(self): + matZero = Matrix3d.ZERO + matIdent = Matrix3d.IDENTITY + + mat = matIdent - matZero + self.assertAlmostEqual(mat, matIdent) + + matA = Matrix3d(1, 2, 3, + 4, 5, 6, + 7, 8, 9) + matB = Matrix3d(10, 20, 30, + 40, 50, 60, + 70, 80, 90) + + mat = matB - matA + self.assertAlmostEqual(mat, Matrix3d(9, 18, 27, + 36, 45, 54, + 63, 72, 81)) + + def test_add(self): + + matZero = Matrix3d.ZERO + matIdent = Matrix3d.IDENTITY + + mat = matIdent + matZero + self.assertAlmostEqual(mat, matIdent) + + matA = Matrix3d(1, 2, 3, + 4, 5, 6, + 7, 8, 9) + matB = Matrix3d(10, 20, 30, + 40, 50, 60, + 70, 80, 90) + + mat = matB + matA + self.assertAlmostEqual(mat, Matrix3d(11, 22, 33, + 44, 55, 66, + 77, 88, 99)) + + def test_mul(self): + matZero = Matrix3d.ZERO + matIdent = Matrix3d.IDENTITY + + mat = matIdent * matZero + self.assertAlmostEqual(mat, matZero) + + matA = Matrix3d(1, 2, 3, + 4, 5, 6, + 7, 8, 9) + matB = Matrix3d(11, 21, 31, + 41, 51, 61, + 71, 81, 91) + + mat = matA * matB + self.assertAlmostEqual(mat, Matrix3d(306, 366, 426, + 675, 825, 975, + 1044, 1284, 1524)) + + mat = matB * matA + self.assertAlmostEqual(mat, Matrix3d(312, 375, 438, + 672, 825, 978, + 1032, 1275, 1518)) + + mat = mat * 2.0 + self.assertAlmostEqual(mat, Matrix3d(624, 750, 876, + 1344, 1650, 1956, + 2064, 2550, 3036)) + + def test_stream_out(self): + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + + self.assertEqual(str(matrix), "1 2 3 4 5 6 7 8 9") + + def test_vector3_mul(self): + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + + # Scalar + self.assertAlmostEqual(Matrix3d.ZERO, matrix * 0) + + # Vector3.ZERO + self.assertAlmostEqual(Vector3d.ZERO, matrix * Vector3d.ZERO) + + # Matrix3.ZERO + self.assertAlmostEqual(Matrix3d.ZERO, matrix * Matrix3d.ZERO) + self.assertAlmostEqual(Matrix3d.ZERO, Matrix3d.ZERO * matrix) + + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + + # scalar 1.0 + self.assertAlmostEqual(matrix, matrix * 1.0) + + # Vector3.Unit + # right multiply + self.assertAlmostEqual(Vector3d(matrix(0, 0), matrix(1, 0), + matrix(2, 0)), + matrix * Vector3d.UNIT_X) + self.assertAlmostEqual(Vector3d(matrix(0, 1), matrix(1, 1), + matrix(2, 1)), + matrix * Vector3d.UNIT_Y) + self.assertAlmostEqual(Vector3d(matrix(0, 2), matrix(1, 2), + matrix(2, 2)), + matrix * Vector3d.UNIT_Z) + + # Matrix3.IDENTITY + self.assertAlmostEqual(matrix, matrix * Matrix3d.IDENTITY) + self.assertAlmostEqual(matrix, Matrix3d.IDENTITY * matrix) + + # Multiply arbitrary matrix by itself + matrix = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + matrix2 = Matrix3d(30, 36, 42, + 66, 81, 96, + 102, 126, 150) + + self.assertAlmostEqual(matrix * matrix, matrix2) + + def test_not_equal(self): + matrix1 = Matrix3d() + matrix2 = Matrix3d() + + self.assertTrue(matrix1 == matrix2) + self.assertFalse(matrix1 != matrix2) + + matrix1 = Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9) + matrix2 = Matrix3d(matrix1) + + self.assertFalse(matrix1 != matrix1) + + matrix2 = Matrix3d(1.00001, 2, 3, 4, 5, 6, 7, 8, 9) + self.assertTrue(matrix1 != matrix2) + + matrix2 = Matrix3d(1.000001, 2, 3, 4, 5, 6, 7, 8, 9) + self.assertFalse(matrix1 != matrix2) + + def test_equal_tolerance(self): + self.assertFalse(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1e-6)) + self.assertFalse(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1e-3)) + self.assertFalse(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1e-1)) + self.assertTrue(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1)) + self.assertTrue(Matrix3d.ZERO.equal(Matrix3d.IDENTITY, 1.1)) + + def test_inverse(self): + self.assertAlmostEqual(Matrix3d.IDENTITY, Matrix3d.IDENTITY.inverse()) + + # Matrix multiplied by its inverse results in the identity matrix + matrix1 = Matrix3d(-2, 4, 0, 0.1, 9, 55, -7, 1, 26) + matrix2 = matrix1.inverse() + self.assertAlmostEqual(matrix1 * matrix2, Matrix3d.IDENTITY) + self.assertAlmostEqual(matrix2 * matrix1, Matrix3d.IDENTITY) + + # Inverse of inverse results in the same matrix + self.assertAlmostEqual((matrix1.inverse()).inverse(), matrix1) + + # Invert multiplication by scalar + scalar = 2.5 + self.assertAlmostEqual((matrix1 * scalar).inverse(), + matrix1.inverse() * (1.0/scalar)) + + def test_determinant(self): + # |ZERO matrix| = 0.0 + self.assertAlmostEqual(0.0, Matrix3d.ZERO.determinant()) + + # |IDENTITY matrix| = 1.0 + self.assertAlmostEqual(1.0, Matrix3d.IDENTITY.determinant()) + + # Determinant of arbitrary matrix + m = Matrix3d(-2, 4, 0, 0.1, 9, 55, -7, 1, 26) + self.assertAlmostEqual(-1908.4, m.determinant()) + + def test_transpose(self): + # transpose of zero matrix is itself + self.assertAlmostEqual(Matrix3d.ZERO, Matrix3d.ZERO.transposed()) + + # transpose of identity matrix is itself + self.assertAlmostEqual(Matrix3d.IDENTITY, + Matrix3d.IDENTITY.transposed()) + + # Matrix and expected transpose + m = Matrix3d(-2, 4, 0, + 0.1, 9, 55, + -7, 1, 26) + mT = Matrix3d(-2, 0.1, -7, + 4, 9, 1, + 0, 55, 26) + self.assertNotEqual(m, mT) + self.assertAlmostEqual(m.transposed(), mT) + self.assertAlmostEqual(m.determinant(), m.transposed().determinant()) + + mT.transpose() + self.assertAlmostEqual(m, mT) + + def test_from_2axes(self): + v1 = Vector3d(1.0, 0.0, 0.0) + v2 = Vector3d(0.0, 1.0, 0.0) + + m1 = Matrix3d() + m1.from_2_axes(v1, v2) + + m2 = Matrix3d() + m2.from_2_axes(v2, v1) + + m1Correct = Matrix3d(0, -1, 0, + 1, 0, 0, + 0, 0, 1) + m2Correct = Matrix3d(m1Correct) + m2Correct.transpose() + + self.assertNotEqual(m1, m2) + self.assertAlmostEqual(m1Correct, m1) + self.assertAlmostEqual(m2Correct, m2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1 * m2) + self.assertAlmostEqual(v2, m1 * v1) + self.assertAlmostEqual(v1, m2 * v2) + + # rotation about 45 degrees + v1.set(1.0, 0.0, 0.0) + v2.set(1.0, 1.0, 0.0) + m2.from_2_axes(v1, v2) + # m1 is 90 degrees rotation + self.assertAlmostEqual(m1, m2*m2) + + # with non-unit vectors + v1.set(0.5, 0.5, 0) + v2.set(-0.5, 0.5, 0) + + m1.from_2_axes(v1, v2) + m2.from_2_axes(v2, v1) + + self.assertNotEqual(m1, m2) + self.assertAlmostEqual(m1Correct, m1) + self.assertAlmostEqual(m2Correct, m2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1 * m2) + self.assertAlmostEqual(v2, m1 * v1) + self.assertAlmostEqual(v1, m2 * v2) + + # For zero-length vectors, a unit matrix is returned + v1.set(0, 0, 0) + v2.set(-0.5, 0.5, 0) + m1.from_2_axes(v1, v2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1) + + # For zero-length vectors, a unit matrix is returned + v1.set(-0.5, 0.5, 0) + v2.set(0, 0, 0) + m1.from_2_axes(v1, v2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1) + + # Parallel vectors + v1.set(1, 0, 0) + v2.set(2, 0, 0) + m1.from_2_axes(v1, v2) + self.assertAlmostEqual(Matrix3d.IDENTITY, m1) + + # Opposite vectors + v1.set(1, 0, 0) + v2.set(-2, 0, 0) + m1.from_2_axes(v1, v2) + self.assertAlmostEqual(Matrix3d.ZERO - Matrix3d.IDENTITY, m1) + + def test_to_quaternion(self): + q = Quaterniond(math.pi/2.0, math.pi/2.0, 0) + matFromQuat = Matrix3d(q) + quatFromMat = matFromQuat.to_quaternion() + self.assertTrue(q == quatFromMat) + + # test the cases where matrix trace is negative + # (requires special handling) + q = Quaterniond(0, 0, 0, 1) + mat = Matrix3d(-1, 0, 0, + 0, -1, 0, + 0, 0, 1) + q2 = mat.to_quaternion() + self.assertTrue(q == q2) + + q = Quaterniond(0, 0, 1, 0) + mat = Matrix3d(-1, 0, 0, + 0, 1, 0, + 0, 0, -1) + q2 = mat.to_quaternion() + self.assertTrue(q == q2) + + q = Quaterniond(0, 1, 0, 0) + mat = Matrix3d(1, 0, 0, + 0, -1, 0, + 0, 0, -1) + q2 = mat.to_quaternion() + self.assertTrue(q == q2) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/Matrix4.i b/src/python/Matrix4.i new file mode 100644 index 000000000..35f93b2e6 --- /dev/null +++ b/src/python/Matrix4.i @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +%module matrix4 +%{ +#include +#include +#include +#include +#include +#include +%} + +%include "std_string.i" + +namespace ignition +{ + namespace math + { + template + class Matrix4 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; + + public: static const Matrix4 Identity; + public: static const Matrix4 Zero; + + public: Matrix4(); + public: Matrix4(const Matrix4 &_m); + public: Matrix4(T _v00, T _v01, T _v02, T _v03, + T _v10, T _v11, T _v12, T _v13, + T _v20, T _v21, T _v22, T _v23, + T _v30, T _v31, T _v32, T _v33); + public: explicit Matrix4(const Quaternion &_q); + public: explicit Matrix4(const Pose3 &_pose) : Matrix4(_pose.Rot()); + public: virtual ~Matrix4() {}; + public: void Set( + T _v00, T _v01, T _v02, T _v03, + T _v10, T _v11, T _v12, T _v13, + T _v20, T _v21, T _v22, T _v23, + T _v30, T _v31, T _v32, T _v33); + public: void Axis(const Vector3 &_axis, T _angle); + public: void SetTranslation(const Vector3 &_t); + public: void SetTranslation(T _x, T _y, T _z); + public: Vector3 Translation() const; + public: Vector3 Scale() const; + public: Quaternion Rotation() const; + public: Vector3 EulerRotation(bool _firstSolution) const; + public: Pose3 Pose() const; + public: void Scale(const Vector3 &_s); + public: void Scale(T _x, T _y, T _z); + public: bool IsAffine() const; + public: bool TransformAffine(const Vector3 &_v, + Vector3 &_result) const; + public: T Determinant() const; + public: Matrix4 Inverse() const; + public: void Transpose(); + public: Matrix4 Transposed() const; + public: Matrix4 operator*=(const Matrix4 &_m2); + public: Matrix4 operator*(const Matrix4 &_m2) const; + public: Vector3 operator*(const Vector3 &_vec) const; + public: bool Equal(const Matrix4 &_m, const T &_tol) const; + public: bool operator==(const Matrix4 &_m) const; + public: bool operator!=(const Matrix4 &_m) const; + public: static Matrix4 LookAt(const Vector3 &_eye, + const Vector3 &_target, const Vector3 &_up = Vector3::UnitZ); + }; + + %extend Matrix4{ + T __call__(size_t row, size_t col) const { + return (*$self)(row, col); + } + } + + %extend Matrix4 { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Matrix4i) Matrix4; + %template(Matrix4d) Matrix4; + %template(Matrix4f) Matrix4; + } +} diff --git a/src/python/Matrix4_TEST.py b/src/python/Matrix4_TEST.py new file mode 100644 index 000000000..44fe82dca --- /dev/null +++ b/src/python/Matrix4_TEST.py @@ -0,0 +1,520 @@ +# Copyright (C) 2021 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import unittest +from ignition.math import Matrix4d +from ignition.math import Pose3d +from ignition.math import Quaterniond +from ignition.math import Vector3d + + +class TestMatrix4(unittest.TestCase): + def test_construct(self): + mat = Matrix4d() + + for i in range(5): + for j in range(5): + self.assertAlmostEqual(mat(i, j), 0.0) + + mat2 = Matrix4d(mat) + for i in range(5): + for j in range(5): + self.assertAlmostEqual(mat2(i, j), 0.0) + + self.assertTrue(mat2 == mat) + + mat3 = Matrix4d(0.0, 1.0, 2.0, 3.0, + 4.0, 5.0, 6.0, 7.0, + 8.0, 9.0, 10.0, 11.0, + 12.0, 13.0, 14.0, 15.0) + + mat4 = Matrix4d(mat3) + self.assertAlmostEqual(mat4, mat3) + + self.assertAlmostEqual(mat3(0, 0), 0.0) + self.assertAlmostEqual(mat3(0, 1), 1.0) + self.assertAlmostEqual(mat3(0, 2), 2.0) + self.assertAlmostEqual(mat3(0, 3), 3.0) + self.assertAlmostEqual(mat3(1, 0), 4.0) + self.assertAlmostEqual(mat3(1, 1), 5.0) + self.assertAlmostEqual(mat3(1, 2), 6.0) + self.assertAlmostEqual(mat3(1, 3), 7.0) + self.assertAlmostEqual(mat3(2, 0), 8.0) + self.assertAlmostEqual(mat3(2, 1), 9.0) + self.assertAlmostEqual(mat3(2, 2), 10.0) + self.assertAlmostEqual(mat3(2, 3), 11.0) + self.assertAlmostEqual(mat3(3, 0), 12.0) + self.assertAlmostEqual(mat3(3, 1), 13.0) + self.assertAlmostEqual(mat3(3, 2), 14.0) + self.assertAlmostEqual(mat3(3, 3), 15.0) + + def test_construct_from_pose3(self): + trans = Vector3d(1, 2, 3) + qt = Quaterniond(0.1, 0.2, 0.3) + pose = Pose3d(trans, qt) + mat = Matrix4d(pose) + + self.assertAlmostEqual(pose, mat.pose()) + self.assertAlmostEqual(trans, mat.translation()) + self.assertAlmostEqual(qt, mat.rotation()) + self.assertAlmostEqual(pose.inverse(), mat.inverse().pose()) + # Ensure inverses multiply to identity + self.assertAlmostEqual(mat.inverse() * mat, Matrix4d.IDENTITY) + self.assertAlmostEqual(mat * mat.inverse(), Matrix4d.IDENTITY) + self.assertAlmostEqual(pose.inverse() * pose, Pose3d.ZERO) + self.assertAlmostEqual(pose * pose.inverse(), Pose3d.ZERO) + + # Repeat test with *= + m = Matrix4d(Matrix4d.IDENTITY) + m *= mat + self.assertAlmostEqual(m, mat) + m *= mat.inverse() + self.assertAlmostEqual(m, Matrix4d.IDENTITY) + + p = Pose3d() + p *= pose + self.assertAlmostEqual(p, pose) + p *= pose.inverse() + self.assertAlmostEqual(p, Pose3d.ZERO) + + # ZERO values + trans = Vector3d(0, 0, 0) + qt = Quaterniond(0, 0, 0) + pose = Pose3d(trans, qt) + mat = Matrix4d(pose) + + self.assertAlmostEqual(pose, mat.pose()) + self.assertAlmostEqual(trans, mat.translation()) + self.assertAlmostEqual(qt, mat.rotation()) + self.assertAlmostEqual(pose.inverse(), mat.inverse().pose()) + + # Rotate pitch by pi/2 so yaw coincides with roll causing a gimbal lock + trans = Vector3d(3, 2, 1) + qt = Quaterniond(0, math.pi/2, 0) + pose = Pose3d(trans, qt) + mat = Matrix4d(pose) + + self.assertAlmostEqual(pose, mat.pose()) + self.assertAlmostEqual(trans, mat.translation()) + self.assertAlmostEqual(qt, mat.rotation()) + self.assertAlmostEqual(pose.inverse(), mat.inverse().pose()) + + # Setup a ZXZ rotation to ensure non-commutative rotations + pose1 = Pose3d(1, -2, 3, 0, 0, math.pi/4) + pose2 = Pose3d(0, 1, -1, -math.pi/4, 0, 0) + pose3 = Pose3d(-1, 0, 0, 0, 0, -math.pi/4) + + m1 = Matrix4d(pose1) + m2 = Matrix4d(pose2) + m3 = Matrix4d(pose3) + + # Ensure rotations are not commutative + self.assertNotEqual(m1 * m2 * m3, m3 * m2 * m1) + + # Ensure pose multiplication order matches matrix order + self.assertAlmostEqual(m1 * m2 * m3, Matrix4d(pose1 * pose2 * pose3)) + self.assertAlmostEqual(m3 * m2 * m1, Matrix4d(pose3 * pose2 * pose1)) + + # Repeat test with *= + m = Matrix4d(Matrix4d.IDENTITY) + p = Pose3d() + + m *= m1 + p *= pose1 + self.assertAlmostEqual(m, m1) + self.assertAlmostEqual(p, pose1) + self.assertAlmostEqual(m, Matrix4d(p)) + + m *= m2 + p *= pose2 + self.assertAlmostEqual(m, m1 * m2) + self.assertAlmostEqual(p, pose1 * pose2) + self.assertAlmostEqual(m, Matrix4d(p)) + + m *= m3 + p *= pose3 + self.assertAlmostEqual(m, m1 * m2 * m3) + self.assertAlmostEqual(p, pose1 * pose2 * pose3) + self.assertAlmostEqual(m, Matrix4d(p)) + + def test_scale(self): + mat = Matrix4d() + mat2 = Matrix4d() + + mat.scale(Vector3d(1, 2, 3)) + mat2.scale(1, 2, 3) + + self.assertAlmostEqual(mat, mat2) + + self.assertAlmostEqual(mat(0, 0), 1.0) + self.assertAlmostEqual(mat(1, 1), 2.0) + self.assertAlmostEqual(mat(2, 2), 3.0) + self.assertAlmostEqual(mat(3, 3), 1.0) + + self.assertAlmostEqual(mat2(0, 0), 1.0) + self.assertAlmostEqual(mat2(1, 1), 2.0) + self.assertAlmostEqual(mat2(2, 2), 3.0) + self.assertAlmostEqual(mat2(3, 3), 1.0) + + self.assertAlmostEqual(mat.scale(), mat2.scale()) + self.assertAlmostEqual(mat.scale(), Vector3d(1, 2, 3)) + + for i in range(0, 4): + for j in range(0, 4): + if i != j: + self.assertAlmostEqual(mat(i, j), 0.0) + self.assertAlmostEqual(mat2(i, j), 0.0) + elif i == 3 and j == 3: + self.assertAlmostEqual(mat(i, j), 1.0) + self.assertAlmostEqual(mat2(i, j), 1.0) + + def test_multiply_vect(self): + mat = Matrix4d() + vec = Vector3d(-1.2, 2.3, 10.5) + + self.assertAlmostEqual(mat * vec, Vector3d(0.0, 0.0, 0.0)) + + mat = Matrix4d(Matrix4d.IDENTITY) + self.assertAlmostEqual(mat * vec, vec) + + def test_multiply_mat(self): + mat = Matrix4d(0, -1, -2, -3, + 1, 0, -1, -2, + 2, 1, 0, -1, + 3, 2, 1, 0) + mat1 = Matrix4d(0, 1, 2, 3, + 1, 2, 3, 4, + 2, 3, 4, 5, + 3, 4, 5, 6) + + mat3 = Matrix4d(-14, -20, -26, -32, + -8, -10, -12, -14, + -2, 0, 2, 4, + 4, 10, 16, 22) + + mat2 = mat * mat1 + + self.assertAlmostEqual(mat2, mat3) + + mat4 = mat + mat4 *= mat1 + self.assertAlmostEqual(mat2, mat4) + + def test_inverse(self): + mat = Matrix4d(2, 3, 1, 5, + 1, 0, 3, 1, + 0, 2, -3, 2, + 0, 2, 3, 1) + + mat1 = mat.inverse() + self.assertAlmostEqual(mat1, Matrix4d(18, -35, -28, 1, + 9, -18, -14, 1, + -2, 4, 3, 0, + -12, 24, 19, -1)) + + def test_get_pose3(self): + mat = Matrix4d(2, 3, 1, 5, + 1, 0, 3, 1, + 0, 2, -3, 2, + 0, 2, 3, 1) + pose = mat.pose() + + self.assertAlmostEqual(pose, Pose3d(5, 1, 2, + -0.204124, 1.22474, 0.816497, 0.204124)) + + def test_translation(self): + mat = Matrix4d() + mat2 = Matrix4d() + + mat.set_translation(Vector3d(1, 2, 3)) + mat2.set_translation(1, 2, 3) + + self.assertEqual(mat, mat2) + + self.assertAlmostEqual(mat(0, 3), 1.0) + self.assertAlmostEqual(mat(1, 3), 2.0) + self.assertAlmostEqual(mat(2, 3), 3.0) + + self.assertAlmostEqual(mat2(0, 3), 1.0) + self.assertAlmostEqual(mat2(1, 3), 2.0) + self.assertAlmostEqual(mat2(2, 3), 3.0) + + self.assertEqual(mat.translation(), mat2.translation()) + self.assertEqual(mat.translation(), Vector3d(1, 2, 3)) + + for i in range(0, 4): + for j in range(0, 2): + self.assertAlmostEqual(mat(i, j), 0.0) + self.assertAlmostEqual(mat2(i, j), 0.0) + + self.assertAlmostEqual(mat(3, 3), 0.0) + self.assertAlmostEqual(mat2(3, 3), 0.0) + + def test_rotation_diag_zero(self): + mat = Matrix4d(0, 0.2, 0.3, 0.4, + 0.5, 0, 0.7, 0.8, + 0.9, 1.0, 0, 1.2, + 1.3, 1.4, 1.5, 1.0) + + quat = mat.rotation() + self.assertAlmostEqual(quat.x(), 0.5, delta=1e-6) + self.assertAlmostEqual(quat.y(), 0.35, delta=1e-6) + self.assertAlmostEqual(quat.z(), 0.6, delta=1e-6) + self.assertAlmostEqual(quat.w(), 0.15, delta=1e-6) + + euler = mat.euler_rotation(True) + self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.5708)) + + euler = mat.euler_rotation(False) + self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.5708)) + + def test_rotation_diag_less_zero(self): + mat = Matrix4d(-0.1, 0.2, 0.3, 0.4, + 0.5, 0, 0.7, 0.8, + 0.9, 1.0, 0, 1.2, + 1.3, 1.4, 1.5, 1.0) + + quat = mat.rotation() + self.assertAlmostEqual(quat.x(), 0.333712, delta=1e-6) + self.assertAlmostEqual(quat.y(), 0.524404, delta=1e-6) + self.assertAlmostEqual(quat.z(), 0.810443, delta=1e-6) + self.assertAlmostEqual(quat.w(), -0.286039, delta=1e-6) + + euler = mat.euler_rotation(True) + self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.76819)) + + euler = mat.euler_rotation(False) + self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.3734)) + + mat = Matrix4d(-0.1, 0.2, 0.3, 0.4, + 0.5, -0.2, 0.7, 0.8, + 0.9, 1.0, 0.0, 1.2, + 1.3, 1.4, 1.5, 1.0) + + quat = mat.rotation() + self.assertAlmostEqual(quat.x(), 0.526235, delta=1e-6) + self.assertAlmostEqual(quat.y(), 0.745499, delta=1e-6) + self.assertAlmostEqual(quat.z(), 0.570088, delta=1e-6) + self.assertAlmostEqual(quat.w(), 0.131559, delta=1e-6) + + euler = mat.euler_rotation(True) + self.assertAlmostEqual(euler, Vector3d(1.5708, -1.11977, 1.76819)) + + euler = mat.euler_rotation(False) + self.assertAlmostEqual(euler, Vector3d(-1.5708, 4.26136, -1.3734)) + + def test_rotation(self): + mat = Matrix4d(0.1, 0.2, 0.3, 0.4, + 0.5, 0.6, 0.7, 0.8, + 0.9, 1.0, 1.1, 1.2, + 1.3, 1.4, 1.5, 1.6) + + quat = mat.rotation() + self.assertAlmostEqual(quat.x(), 0.0896421, delta=1e-6) + self.assertAlmostEqual(quat.y(), -0.179284, delta=1e-6) + self.assertAlmostEqual(quat.z(), 0.0896421, delta=1e-6) + self.assertAlmostEqual(quat.w(), 0.83666, delta=1e-6) + + euler = mat.euler_rotation(True) + self.assertAlmostEqual(euler, Vector3d(0.737815, -1.11977, 1.3734)) + + euler = mat.euler_rotation(False) + self.assertAlmostEqual(euler, Vector3d(-2.40378, 4.26136, -1.76819)) + + def test_euler_rotation2(self): + mat = Matrix4d(0.1, 0.2, 0.3, 0.4, + 0.5, 0.6, 0.7, 0.8, + 1.9, 1.2, 1.1, 1.2, + 1.3, 1.4, 1.5, 1.6) + + euler = mat.euler_rotation(True) + self.assertAlmostEqual(euler, Vector3d(-2.55359, -1.5708, 0)) + + euler = mat.euler_rotation(False) + self.assertAlmostEqual(euler, Vector3d(-2.55359, -1.5708, 0)) + + mat = Matrix4d(0.1, 0.2, 0.3, 0.4, + 0.5, 0.6, 0.7, 0.8, + -1.2, 1.2, 1.1, 1.2, + 1.3, 1.4, 1.5, 1.6) + + euler = mat.euler_rotation(True) + self.assertAlmostEqual(euler, Vector3d(0.588003, 1.5708, 0)) + + euler = mat.euler_rotation(False) + self.assertAlmostEqual(euler, Vector3d(0.588003, 1.5708, 0)) + + def test_affine_transform(self): + mat = Matrix4d(Matrix4d.ZERO) + vec = Vector3d(1, 2, 3) + + v = Vector3d() + + self.assertFalse(mat.transform_affine(vec, v)) + + mat = Matrix4d(Matrix4d.IDENTITY) + self.assertTrue(mat.transform_affine(vec, v)) + + def test_stream_out(self): + matA = Matrix4d(1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16) + + self.assertEqual(str(matA), "1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16") + + def test_not_equal(self): + matrix1 = Matrix4d() + matrix2 = Matrix4d() + self.assertTrue(matrix1 == matrix2) + self.assertFalse(matrix1 != matrix2) + + matrix1 = Matrix4d(1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16) + matrix2 = Matrix4d(matrix1) + + self.assertFalse(matrix1 != matrix2) + + matrix2 = Matrix4d(1.00001, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16) + self.assertTrue(matrix1 != matrix2) + + matrix2 = Matrix4d(1.0000001, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16) + self.assertFalse(matrix1 != matrix2) + + def test_equal_tolerance(self): + self.assertFalse(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1e-6)) + self.assertFalse(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1e-3)) + self.assertFalse(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1e-1)) + self.assertTrue(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1)) + self.assertTrue(Matrix4d.ZERO.equal(Matrix4d.IDENTITY, 1.1)) + + def test_determinant(self): + # |ZERO matrix| = 0.0 + self.assertAlmostEqual(0.0, Matrix4d.ZERO.determinant()) + + # |IDENTITY matrix| = 1.0 + self.assertAlmostEqual(1.0, Matrix4d.IDENTITY.determinant()) + + # Determinant of arbitrary matrix + m = Matrix4d(2, 3, 0.1, -5, + 1, 0, 3.2, 1, + 0, 2, -3, 2.1, + 0, 2, 3.2, 1) + self.assertAlmostEqual(129.82, m.determinant()) + + def test_transpose(self): + # Transpose of zero matrix is itself + self.assertAlmostEqual(Matrix4d.ZERO, Matrix4d.ZERO.transposed()) + + # Transpose of identity matrix is itself + self.assertAlmostEqual(Matrix4d.IDENTITY, + Matrix4d.IDENTITY.transposed()) + + # Matrix and expected transpose + m = Matrix4d(-2, 4, 0, -3.5, + 0.1, 9, 55, 1.2, + -7, 1, 26, 11.5, + .2, 3, -5, -0.1) + mT = Matrix4d(-2, 0.1, -7, .2, + 4, 9, 1, 3, + 0, 55, 26, -5, + -3.5, 1.2, 11.5, -0.1) + self.assertNotEqual(m, mT) + self.assertEqual(m.transposed(), mT) + self.assertAlmostEqual(m.determinant(), m.transposed().determinant()) + + mT.transpose() + self.assertEqual(m, mT) + + def test_look_at(self): + self.assertAlmostEqual(Matrix4d.look_at(-Vector3d.UNIT_X, + Vector3d.ZERO).pose(), + Pose3d(-1, 0, 0, 0, 0, 0)) + + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(3, 2, 0), + Vector3d(0, 2, 0)).pose(), + Pose3d(3, 2, 0, 0, 0, math.pi)) + + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(1, 6, 1), + Vector3d.ONE).pose(), + Pose3d(1, 6, 1, 0, 0, -math.pi/2)) + + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(-1, -1, 0), + Vector3d(1, 1, 0)).pose(), + Pose3d(-1, -1, 0, 0, 0, math.pi/4)) + + # Default up is z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(0.1, -5, 222), + Vector3d(999, -0.6, 0)), + Matrix4d.look_at(Vector3d(0.1, -5, 222), + Vector3d(999, -0.6, 0), + Vector3d.UNIT_Z)) + + # up == zero, default up = +z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(1.23, 456, 0.7), + Vector3d(0, 8.9, -10), + Vector3d.ZERO), + Matrix4d.look_at(Vector3d(1.23, 456, 0.7), + Vector3d(0, 8.9, -10))) + + # up == +x, default up = +z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(0.25, 9, -5), + Vector3d(-6, 0, 0.4), + Vector3d.UNIT_X), + Matrix4d.look_at(Vector3d(0.25, 9, -5), + Vector3d(-6, 0, 0.4))) + + # up == -x, default up = +z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(0, 0, 0.2), + Vector3d(-8, 0, -6), + -Vector3d.UNIT_X), + Matrix4d.look_at(Vector3d(0, 0, 0.2), + Vector3d(-8, 0, -6))) + + # eye == target, default direction = +x + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.ONE, + Vector3d.ONE), + Matrix4d.look_at(Vector3d.ONE, + Vector3d(1.0001, 1, 1))) + + # Not possible to keep _up on +z + self.assertAlmostEqual(Matrix4d.look_at(Vector3d(-1, 0, 10), + Vector3d(-1, 0, 0)), + Matrix4d.look_at(Vector3d(-1, 0, 10), + Vector3d(-1, 0, 0), + -Vector3d.UNIT_X)) + + # Different ups + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.ONE, + Vector3d(0, 1, 1), + Vector3d.UNIT_Y).pose(), + Pose3d(1, 1, 1, math.pi/2, 0, math.pi)) + + self.assertAlmostEqual(Matrix4d.look_at(Vector3d.ONE, + Vector3d(0, 1, 1), + Vector3d(0, 1, 1)).pose(), + Pose3d(1, 1, 1, math.pi/4, 0, math.pi)) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/Pose3.i b/src/python/Pose3.i new file mode 100644 index 000000000..5b94579ba --- /dev/null +++ b/src/python/Pose3.i @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +%module quaternion +%{ + #include + #include + #include + #include +%} + +%include "std_string.i" +%include "Quaternion.i" + + +namespace ignition +{ + namespace math + { + template + class Pose3 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; + + public: static const Pose3 Zero; + + public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0); + public: Pose3(const Vector3 &_pos, const Quaternion &_rot) + : p(_pos), q(_rot); + public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw) + : p(_x, _y, _z), q(_roll, _pitch, _yaw); + public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz) + : p(_x, _y, _z), q(_qw, _qx, _qy, _qz); + public: Pose3(const Pose3 &_pose) + : p(_pose.p), q(_pose.q); + public: virtual ~Pose3(); + public: void Set(const Vector3 &_pos, const Quaternion &_rot); + public: void Set(const Vector3 &_pos, const Vector3 &_rpy); + public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw); + public: bool IsFinite() const; + public: inline void Correct(); + public: Pose3 Inverse() const; + public: Pose3 operator+(const Pose3 &_pose) const; + public: const Pose3 &operator+=(const Pose3 &_pose); + public: inline Pose3 operator-() const; + public: inline Pose3 operator-(const Pose3 &_pose) const; + public: const Pose3 &operator-=(const Pose3 &_pose); + public: bool operator==(const Pose3 &_pose) const; + public: bool operator!=(const Pose3 &_pose) const; + public: Pose3 operator*(const Pose3 &_pose) const; + public: const Pose3 &operator*=(const Pose3 &_pose); + public: Vector3 CoordPositionAdd(const Vector3 &_pos) const; + public: Vector3 CoordPositionAdd(const Pose3 &_pose) const; + public: inline Vector3 CoordPositionSub(const Pose3 &_pose) const; + public: Quaternion CoordRotationAdd(const Quaternion &_rot) const; + public: inline Quaternion CoordRotationSub( + const Quaternion &_rot) const; + public: Pose3 CoordPoseSolve(const Pose3 &_b) const; + public: void Reset(); + public: Pose3 RotatePositionAboutOrigin(const Quaternion &_q) const; + public: void Round(int _precision); + public: inline Vector3 &Pos(); + public: inline const T X() const; + public: inline void SetX(T x); + public: inline const T Y() const; + public: inline void SetY(T y); + public: inline const T Z() const; + public: inline void SetZ(T z); + public: inline Quaternion &Rot(); + public: inline const T Roll() const; + public: inline const T Pitch() const; + public: inline const T Yaw() const; + }; + + %extend Pose3 { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Pose3i) Pose3; + %template(Pose3d) Pose3; + %template(Pose3f) Pose3; + } +} diff --git a/src/python/Pose3_TEST.py b/src/python/Pose3_TEST.py new file mode 100644 index 000000000..7a8451b69 --- /dev/null +++ b/src/python/Pose3_TEST.py @@ -0,0 +1,183 @@ +# Copyright (C) 2021 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import unittest +from ignition.math import Pose3d +from ignition.math import Quaterniond +from ignition.math import Vector3d + + +class TestPose3(unittest.TestCase): + def test_construction(self): + pose = Pose3d(1, 0, 0, 0, 0, 0) + + # Copy + pose2 = Pose3d(pose) + self.assertAlmostEqual(pose2, pose) + + # Inequality + pose3 = Pose3d() + self.assertNotEqual(pose3, pose) + + def test_pose(self): + A = Pose3d(Vector3d(1, 0, 0), + Quaterniond(0, 0, math.pi/4.0)) + B = Pose3d(Vector3d(1, 0, 0), + Quaterniond(0, 0, math.pi/2.0)) + + # test hypothesis that if + # A is the transform from O to P specified in frame O + # B is the transform from P to Q specified in frame P + # then, B + A is the transform from O to Q specified in frame O + self.assertAlmostEqual((B + A).pos().x(), 1.0 + 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).pos().y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).pos().z(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().x(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().y(), 0.0) + self.assertAlmostEqual((B + A).rot().euler().z(), 3.0*math.pi/4.0) + + # If: + # A is the transform from O to P in frame O + # B is the transform from O to Q in frame O + # then -A is transform from P to O specified in frame P + self.assertAlmostEqual((Pose3d() - A).pos().x(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).pos().y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).pos().z(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().x(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().y(), 0.0) + self.assertAlmostEqual((Pose3d() - A).rot().euler().z(), -math.pi/4) + + # test negation operator + self.assertAlmostEqual((-A).pos().x(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).pos().y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).pos().z(), 0.0) + self.assertAlmostEqual((-A).rot().euler().x(), 0.0) + self.assertAlmostEqual((-A).rot().euler().y(), 0.0) + self.assertAlmostEqual((-A).rot().euler().z(), -math.pi/4.0) + + # If: + # A is the transform from O to P in frame O + # B is the transform from O to Q in frame O + # B - A is the transform from P to Q in frame P + B = Pose3d(Vector3d(1, 1, 0), + Quaterniond(0, 0, math.pi/2.0)) + self.assertAlmostEqual((B - A).pos().x(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).pos().y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).pos().z(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().x(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().y(), 0.0) + self.assertAlmostEqual((B - A).rot().euler().z(), math.pi/4.0) + + pose = Pose3d() + self.assertTrue(pose.pos() == Vector3d(0, 0, 0)) + self.assertTrue(pose.rot() == Quaterniond(0, 0, 0)) + + pose = Pose3d(Vector3d(1, 2, 3), Quaterniond(.1, .2, .3)) + self.assertTrue(pose.pos() == Vector3d(1, 2, 3)) + self.assertTrue(pose.rot() == Quaterniond(.1, .2, .3)) + + pose1 = Pose3d(pose) + self.assertTrue(pose1 == pose) + + pose.set(Vector3d(2, 3, 4), Quaterniond(.3, .4, .5)) + self.assertTrue(pose.pos() == Vector3d(2, 3, 4)) + self.assertTrue(pose.rot() == Quaterniond(.3, .4, .5)) + self.assertTrue(pose.is_finite()) + + pose1 = pose.inverse() + self.assertTrue(pose1.pos() == Vector3d(-1.38368, -3.05541, -4.21306)) + self.assertTrue(pose1.rot() == Quaterniond(0.946281, -0.0933066, + -0.226566, -0.210984)) + + pose = Pose3d(1, 2, 3, .1, .2, .3) + Pose3d(4, 5, 6, .4, .5, .6) + self.assertTrue(pose == Pose3d(5.74534, 7.01053, 8.62899, + 0.675732, 0.535753, 1.01174)) + + pose += pose + self.assertTrue(pose == Pose3d(11.314, 16.0487, 15.2559, + 1.49463, 0.184295, 2.13932)) + + pose -= Pose3d(pose) + self.assertTrue(pose == Pose3d(0, 0, 0, 0, 0, 0)) + + pose.pos().set(5, 6, 7) + pose.rot().euler(Vector3d(.4, .6, 0)) + + self.assertTrue(pose.coord_position_add(Vector3d(1, 2, 3)) == + Vector3d(7.82531, 6.67387, 9.35871)) + + self.assertTrue(pose.coord_position_add(pose1) == + Vector3d(2.58141, 2.4262, 3.8013)) + self.assertTrue(pose.coord_rotation_add(Quaterniond(0.1, 0, 0.2)) == + Quaterniond(0.520975, 0.596586, 0.268194)) + self.assertTrue(pose.coord_pose_solve(pose1) == + Pose3d(-0.130957, -11.552, -10.2329, + -0.462955, -1.15624, -0.00158047)) + + self.assertTrue(pose.rotate_position_about_origin( + Quaterniond(0.1, 0, 0.2)) == + Pose3d(6.09235, 5.56147, 6.47714, 0.4, 0.6, 0)) + + pose.reset() + self.assertTrue(pose.pos() == Vector3d(0, 0, 0)) + self.assertTrue(pose.rot() == Quaterniond(0, 0, 0)) + + def test_pose_atributes(self): + pose = Pose3d(0, 1, 2, 1, 0, 0) + + self.assertTrue(pose.pos() == Vector3d(0, 1, 2)) + self.assertTrue(pose.rot() == Quaterniond(1, 0, 0)) + + def test_stream_out(self): + p = Pose3d(0.1, 1.2, 2.3, 0.0, 0.1, 1.0) + self.assertAlmostEqual(str(p), "0.1 1.2 2.3 0 0.1 1") + + def test_mutable_pose(self): + pose = Pose3d(0, 1, 2, 0, 0, 0) + + self.assertTrue(pose.pos() == Vector3d(0, 1, 2)) + self.assertTrue(pose.rot() == Quaterniond(0, 0, 0)) + + pose = Pose3d(Vector3d(10, 20, 30), Quaterniond(1, 2, 1)) + + self.assertTrue(pose.pos() == Vector3d(10, 20, 30)) + self.assertTrue(pose.rot() == Quaterniond(1, 2, 1)) + + def test_pose_elements(self): + pose = Pose3d(0, 1, 2, 1, 1, 2) + self.assertAlmostEqual(pose.x(), 0) + self.assertAlmostEqual(pose.y(), 1) + self.assertAlmostEqual(pose.z(), 2) + self.assertAlmostEqual(pose.roll(), 1) + self.assertAlmostEqual(pose.pitch(), 1) + self.assertAlmostEqual(pose.yaw(), 2) + + def test_set_pose_element(self): + pose = Pose3d(1, 2, 3, 1.57, 1, 2) + self.assertAlmostEqual(pose.x(), 1) + self.assertAlmostEqual(pose.y(), 2) + self.assertAlmostEqual(pose.z(), 3) + + pose.set_x(10) + pose.set_y(12) + pose.set_z(13) + + self.assertAlmostEqual(pose.x(), 10) + self.assertAlmostEqual(pose.y(), 12) + self.assertAlmostEqual(pose.z(), 13) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/Quaternion.i b/src/python/Quaternion.i new file mode 100644 index 000000000..0bc6d70bf --- /dev/null +++ b/src/python/Quaternion.i @@ -0,0 +1,171 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +%module quaternion +%{ +#include +#include +#include +#include +%} + +%include "std_string.i" + +%inline %{ + template + struct ToAxisOutput { + ignition::math::Vector3 axis; + D angle; + }; +%} + +namespace ignition +{ + namespace math + { + template + class Quaternion + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + %rename("%(uppercase)s", %$isstatic, %$isvariable) ""; + + public: static const Quaternion Identity; + public: static const Quaternion Zero; + + public: Quaternion(); + public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z); + public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw); + public: Quaternion(const Vector3 &_axis, const T &_angle); + public: explicit Quaternion(const Vector3 &_rpy); + public: Quaternion(const Quaternion &_qt); + public: ~Quaternion(); + public: void Invert(); + public: inline Quaternion Inverse() const; + public: Quaternion Log() const; + public: Quaternion Exp() const; + public: void Normalize(); + public: Quaternion Normalized() const; + public: void Axis(T _ax, T _ay, T _az, T _aa); + public: void Axis(const Vector3 &_axis, T _a); + public: void Set(T _w, T _x, T _y, T _z); + public: void Euler(const Vector3 &_vec); + public: void Euler(T _roll, T _pitch, T _yaw); + public: Vector3 Euler() const; + public: static Quaternion EulerToQuaternion(const Vector3 &_vec); + public: static Quaternion EulerToQuaternion(T _x, T _y, T _z); + public: T Roll() const; + public: T Pitch() const; + public: T Yaw() const; + %rename(from_2_axes) From2Axes; + public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2); + public: void Scale(T _scale); + public: Quaternion operator+(const Quaternion &_qt) const; + public: Quaternion operator+=(const Quaternion &_qt); + public: Quaternion operator-(const Quaternion &_qt) const; + public: Quaternion operator-=(const Quaternion &_qt); + public: inline Quaternion operator*(const Quaternion &_q) const; + public: Quaternion operator*(const T &_f) const; + public: Quaternion operator*=(const Quaternion &_qt); + public: Vector3 operator*(const Vector3 &_v) const; + public: bool Equal(const Quaternion &_qt, const T &_tol) const; + public: bool operator==(const Quaternion &_qt) const; + public: bool operator!=(const Quaternion &_qt) const; + public: Quaternion operator-() const; + public: inline Vector3 RotateVector(const Vector3 &_vec) const; + public: Vector3 RotateVectorReverse(const Vector3 &_vec) const; + public: bool IsFinite() const; + public: inline void Correct(); + %rename(x_axis) XAxis; + public: Vector3 XAxis() const; + %rename(y_axis) YAxis; + public: Vector3 YAxis() const; + %rename(z_axis) ZAxis; + public: Vector3 ZAxis() const; + public: void Round(int _precision); + public: T Dot(const Quaternion &_q) const; + public: static Quaternion Squad(T _fT, + const Quaternion &_rkP, const Quaternion &_rkA, + const Quaternion &_rkB, const Quaternion &_rkQ, + bool _shortestPath = false); + public: static Quaternion Slerp(T _fT, + const Quaternion &_rkP, const Quaternion &_rkQ, + bool _shortestPath = false); + public: Quaternion Integrate(const Vector3 &_angularVelocity, + const T _deltaT) const; + + public: inline void X(T _v); + public: inline void Y(T _v); + public: inline void Z(T _v); + public: inline void W(T _v); + + %pythoncode %{ + def to_axis(self): + to_axis_output = self._to_axis() + return [to_axis_output.axis, to_axis_output.angle] + %} + }; + + %extend Quaternion{ + inline ToAxisOutput _to_axis() { + ignition::math::Vector3 axis; + T angle; + (*$self).ToAxis(axis, angle); + ToAxisOutput output; + output.axis = axis; + output.angle = angle; + return output; + } + } + + %extend Quaternion{ + inline T W() const + { + return (*$self).W(); + } + + inline T X() const + { + return (*$self).X(); + } + + inline T Y() const + { + return (*$self).Y(); + } + + inline T Z() const + { + return (*$self).Z(); + } + } + + %extend Quaternion { + std::string __str__() const { + std::ostringstream out; + out << *$self; + return out.str(); + } + } + + %template(Quaternioni) Quaternion; + %template(Quaterniond) Quaternion; + %template(Quaternionf) Quaternion; + } +} + %template(ToAxisOutputi) ToAxisOutput; + %template(ToAxisOutputd) ToAxisOutput; + %template(ToAxisOutputf) ToAxisOutput; diff --git a/src/python/Quaternion_TEST.py b/src/python/Quaternion_TEST.py new file mode 100644 index 000000000..c99ffc07f --- /dev/null +++ b/src/python/Quaternion_TEST.py @@ -0,0 +1,536 @@ +# Copyright (C) 2021 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:#www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import unittest +from ignition.math import Matrix3d +from ignition.math import Matrix4d +from ignition.math import Quaterniond +from ignition.math import Quaternionf +from ignition.math import Quaternioni +from ignition.math import Vector3d + + +class TestQuaternion(unittest.TestCase): + def test_construction(self): + q = Quaterniond(0, 0, 0, 1) + + q2 = Quaterniond(q) + self.assertEqual(q2, q) + + q3 = q + self.assertEqual(q3, q) + + q4 = Quaterniond(q) + self.assertEqual(q4, q2) + q = q4 + self.assertEqual(q, q2) + + q5 = q2 + self.assertEqual(q5, q3) + q2 = q5 + self.assertEqual(q2, q3) + + q6 = Quaterniond() + self.assertNotEqual(q6, q3) + + def test_unit(self): + q = Quaterniond() + self.assertAlmostEqual(q.w(), 1.0) + self.assertAlmostEqual(q.x(), 0.0) + self.assertAlmostEqual(q.y(), 0.0) + self.assertAlmostEqual(q.z(), 0.0) + + def test_construct_values(self): + q = Quaterniond(1.0, 2.0, 3.0, 4.0) + self.assertAlmostEqual(q.w(), 1.0) + self.assertAlmostEqual(q.x(), 2.0) + self.assertAlmostEqual(q.y(), 3.0) + self.assertAlmostEqual(q.z(), 4.0) + + def test_construct_zero(self): + q = Quaterniond(0.0, 0.0, 0.0, 0.0) + self.assertAlmostEqual(q.w(), 0.0) + self.assertAlmostEqual(q.x(), 0.0) + self.assertAlmostEqual(q.y(), 0.0) + self.assertAlmostEqual(q.z(), 0.0) + + qI = q.inverse() + self.assertAlmostEqual(qI.w(), 1.0) + self.assertAlmostEqual(qI.x(), 0.0) + self.assertAlmostEqual(qI.y(), 0.0) + self.assertAlmostEqual(qI.z(), 0.0) + + def test_construct_euler(self): + q = Quaterniond(0, 1, 2) + self.assertAlmostEqual(q, Quaterniond(Vector3d(0, 1, 2))) + + def test_construct_axis_angle(self): + q1 = Quaterniond(Vector3d(0, 0, 1), math.pi) + + self.assertAlmostEqual(q1.x(), 0.0) + self.assertAlmostEqual(q1.y(), 0.0) + self.assertAlmostEqual(q1.z(), 1.0) + self.assertAlmostEqual(q1.w(), 0.0) + + q = Quaterniond(q1) + self.assertTrue(q == q1) + + def test_equal(self): + # double + q = Quaterniond(1, 2, 3, 4) + q2 = Quaterniond(1.01, 2.015, 3.002, 4.007) + self.assertTrue(q.equal(q2, 0.02)) + self.assertFalse(q.equal(q2, 0.01)) + + # floats + q3 = Quaternionf(1, 2, 3, 4) + q4 = Quaternionf(1.05, 2.1, 3.03, 4.04) + self.assertTrue(q3.equal(q4, 0.2)) + self.assertFalse(q3.equal(q4, 0.04)) + + # ints + q5 = Quaternioni(3, 5, -1, 9) + q6 = Quaternioni(3, 6, 1, 12) + self.assertTrue(q5.equal(q6, 3)) + self.assertFalse(q5.equal(q6, 2)) + + def test_identity(self): + q = Quaterniond.IDENTITY + self.assertAlmostEqual(q.w(), 1.0) + self.assertAlmostEqual(q.x(), 0.0) + self.assertAlmostEqual(q.y(), 0.0) + self.assertAlmostEqual(q.z(), 0.0) + + def test_mathlog(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + self.assertAlmostEqual(q.log(), + Quaterniond(0, -1.02593, 0.162491, 1.02593)) + + q1 = Quaterniond(q) + q1.w(2.0) + self.assertAlmostEqual(q1.log(), + Quaterniond(0, -0.698401, 0.110616, 0.698401)) + + def test_math_exp(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + self.assertAlmostEqual(q.exp(), Quaterniond(0.545456, -0.588972, + 0.093284, 0.588972)) + + q1 = Quaterniond(q) + q1.x(0.000000001) + q1.y(0.0) + q1.z(0.0) + q1.w(0.0) + self.assertAlmostEqual(q1.exp(), Quaterniond(1, 0, 0, 0)) + + def test_math_invert(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + + q.invert() + self.assertAlmostEqual(q, Quaterniond(0.110616, 0.698401, + -0.110616, -0.698401)) + + def test_math_axis(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + + q.axis(0, 1, 0, math.pi) + self.assertAlmostEqual(q, Quaterniond(6.12303e-17, 0, 1, 0)) + + q.axis(Vector3d(1, 0, 0), math.pi) + self.assertAlmostEqual(q, Quaterniond(0, 1, 0, 0)) + + def test_math_set(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + + q.set(1, 2, 3, 4) + self.assertAlmostEqual(q.w(), 1.0) + self.assertAlmostEqual(q.x(), 2.0) + self.assertAlmostEqual(q.y(), 3.0) + self.assertAlmostEqual(q.z(), 4.0) + + def test_math_normalized(self): + q = Quaterniond(1, 2, 3, 4) + + q2 = q.normalized() + self.assertAlmostEqual(q2, Quaterniond(0.182574, 0.365148, + 0.547723, 0.730297)) + + def test_normalize(self): + q = Quaterniond(1, 2, 3, 4) + + q.normalize() + self.assertAlmostEqual(q, Quaterniond(0.182574, 0.365148, + 0.547723, 0.730297)) + + def test_math(self): + q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) + self.assertTrue(q == Quaterniond(0.110616, -0.698401, + 0.110616, 0.698401)) + + q.set(1, 2, 3, 4) + + q.normalize() + + self.assertAlmostEqual(q.roll(), 1.4289, delta=1e-3) + self.assertAlmostEqual(q.pitch(), -0.339837, delta=1e-3) + self.assertAlmostEqual(q.yaw(), 2.35619, delta=1e-3) + + axis, angle = q.to_axis() + self.assertAlmostEqual( + axis, Vector3d(0.371391, 0.557086, 0.742781), delta=1e-3) + self.assertAlmostEqual(angle, 2.77438, delta=1e-3) + + q.scale(0.1) + self.assertTrue(q == Quaterniond(0.990394, 0.051354, + 0.0770309, 0.102708)) + + q = q + Quaterniond(0, 1, 2) + self.assertTrue(q == Quaterniond(1.46455, -0.352069, + 0.336066, 0.841168)) + + q += q + self.assertTrue(q == Quaterniond(2.92911, -0.704137, + 0.672131, 1.68234)) + + q -= Quaterniond(.4, .2, .1) + self.assertTrue(q == Quaterniond(1.95416, -0.896677, 0.56453, 1.65341)) + + q = q - Quaterniond(0, 1, 2) + self.assertTrue(q == Quaterniond(1.48, -0.493254, + 0.305496, 0.914947)) + + q *= Quaterniond(.4, .1, .01) + self.assertTrue(q == Quaterniond(1.53584, -0.236801, + 0.551841, 0.802979)) + + q = q * 5.0 + self.assertTrue(q == Quaterniond(7.67918, -1.184, 2.7592, 4.0149)) + + self.assertTrue(q.rotate_vector_reverse(Vector3d(1, 2, 3)) == + Vector3d(-0.104115, 0.4975, 3.70697)) + + self.assertAlmostEqual(q.dot(Quaterniond(.4, .2, .1)), 7.67183, + delta=1e-3) + + self.assertTrue(Quaterniond.squad(1.1, Quaterniond(.1, 0, .2), + Quaterniond(0, .3, .4), Quaterniond(.5, .2, 1), + Quaterniond(0, 0, 2), True) == + Quaterniond(0.346807, -0.0511734, + -0.0494723, 0.935232)) + + self.assertTrue(Quaterniond.euler_to_quaternion( + Vector3d(.1, .2, .3)) == + Quaterniond(0.983347, 0.0342708, + 0.106021, 0.143572)) + + q.round(2) + self.assertAlmostEqual(-1.18, q.x()) + self.assertAlmostEqual(2.76, q.y()) + self.assertAlmostEqual(4.01, q.z()) + self.assertAlmostEqual(7.68, q.w()) + + q.x(0.0) + q.y(0.0) + q.z(0.0) + q.w(0.0) + q.normalize() + self.assertTrue(q == Quaterniond()) + + q.axis(0, 0, 0, 0) + self.assertTrue(q == Quaterniond()) + + self.assertTrue(Quaterniond.euler_to_quaternion(0.1, 0.2, 0.3) == + Quaterniond(0.983347, 0.0342708, 0.106021, 0.143572)) + + # to_axis() method + q.x(0.0) + q.y(0.0) + q.z(0.0) + q.w(0.0) + axis, angle = q.to_axis() + self.assertAlmostEqual(axis, Vector3d(1., 0., 0.), delta=1e-3) + self.assertAlmostEqual(angle, 0., delta=1e-3) + + # simple 180 rotation about yaw, + # should result in x and y flipping signs + q = Quaterniond(0, 0, math.pi) + v = Vector3d(1, 2, 3) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(-1, -2, 3)) + self.assertTrue(r2 == Vector3d(-1, -2, 3)) + + # simple 90 rotation about yaw, should map x to y, y to -x + # simple -90 rotation about yaw, should map x to -y, y to x + q = Quaterniond(0, 0, 0.5 * math.pi) + v = Vector3d(1, 2, 3) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(-2, 1, 3)) + self.assertTrue(r2 == Vector3d(2, -1, 3)) + self.assertTrue(q.inverse().x_axis() == Vector3d(0, -1, 0)) + self.assertTrue(q.inverse().y_axis() == Vector3d(1, 0, 0)) + self.assertTrue(q.inverse().z_axis() == Vector3d(0, 0, 1)) + + # Test RPY fixed-body-frame convention: + # Rotate each unit vector in roll and pitch + q = Quaterniond(math.pi/2.0, math.pi/2.0, 0) + v1 = Vector3d(1, 0, 0) + r1 = q.rotate_vector(v1) + # 90 degrees about x does nothing, + # 90 degrees about y sends point down to -z + self.assertAlmostEqual(r1, Vector3d(0, 0, -1)) + + v2 = Vector3d(0, 1, 0) + r2 = q.rotate_vector(v2) + # 90 degrees about x sends point to +z + # 90 degrees about y sends point to +x + self.assertAlmostEqual(r2, Vector3d(1, 0, 0)) + + v3 = Vector3d(0, 0, 1) + r3 = q.rotate_vector(v3) + # 90 degrees about x sends point to -y + # 90 degrees about y does nothing + self.assertAlmostEqual(r3, Vector3d(0, -1, 0)) + + # now try a harder case (axis[1,2,3], rotation[0.3*pi]) + # verified with octave + q.axis(Vector3d(1, 2, 3), 0.3*math.pi) + self.assertTrue(q.inverse().x_axis() == + Vector3d(0.617229, -0.589769, 0.520770)) + self.assertTrue(q.inverse().y_axis() == + Vector3d(0.707544, 0.705561, -0.039555)) + self.assertTrue(q.inverse().z_axis() == + Vector3d(-0.344106, 0.392882, 0.852780)) + + # rotate about the axis of rotation should not change axis + v = Vector3d(1, 2, 3) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(1, 2, 3)) + self.assertTrue(r2 == Vector3d(1, 2, 3)) + + # rotate unit vectors + v = Vector3d(0, 0, 1) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(0.520770, -0.039555, 0.852780)) + self.assertTrue(r2 == Vector3d(-0.34411, 0.39288, 0.85278)) + v = Vector3d(0, 1, 0) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(-0.58977, 0.70556, 0.39288)) + self.assertTrue(r2 == Vector3d(0.707544, 0.705561, -0.039555)) + v = Vector3d(1, 0, 0) + r1 = q.rotate_vector(v) + r2 = q.rotate_vector_reverse(v) + self.assertTrue(r1 == Vector3d(0.61723, 0.70754, -0.34411)) + self.assertTrue(r2 == Vector3d(0.61723, -0.58977, 0.52077)) + + self.assertTrue(-q == Quaterniond(-0.891007, -0.121334, + -0.242668, -0.364002)) + + self.assertTrue(Matrix3d(q) == Matrix3d( + 0.617229, -0.589769, 0.52077, + 0.707544, 0.705561, -0.0395554, + -0.344106, 0.392882, 0.85278)) + + matFromQ = Matrix3d(q) + self.assertTrue(matFromQ == Matrix3d( + 0.617229, -0.589769, 0.52077, + 0.707544, 0.705561, -0.0395554, + -0.344106, 0.392882, 0.85278)) + + self.assertTrue(Matrix4d(q) == Matrix4d( + 0.617229, -0.589769, 0.52077, 0, + 0.707544, 0.705561, -0.0395554, 0, + -0.344106, 0.392882, 0.85278, 0, + 0, 0, 0, 1)) + + def test_stream_out(self): + q = Quaterniond(0.1, 1.2, 2.3) + self.assertEqual(str(q), "0.1 1.2 2.3") + + def test_slerp(self): + q1 = Quaterniond(0.1, 1.2, 2.3) + q2 = Quaterniond(1.2, 2.3, -3.4) + + q3 = Quaterniond.slerp(1.0, q1, q2, True) + self.assertAlmostEqual(q3, Quaterniond(0.554528, -0.717339, + 0.32579, 0.267925)) + + def test_from_2_axes(self): + v1 = Vector3d(1.0, 0.0, 0.0) + v2 = Vector3d(0.0, 1.0, 0.0) + + q1 = Quaterniond() + q1.from_2_axes(v1, v2) + + q2 = Quaterniond() + q2.from_2_axes(v2, v1) + + q2Correct = Quaterniond(math.sqrt(2)/2, 0, 0, -math.sqrt(2)/2) + q1Correct = Quaterniond(math.sqrt(2)/2, 0, 0, math.sqrt(2)/2) + + self.assertNotEqual(q1, q2) + self.assertAlmostEqual(q1Correct, q1) + self.assertAlmostEqual(q2Correct, q2) + self.assertAlmostEqual(Quaterniond.IDENTITY, q1 * q2) + self.assertAlmostEqual(v2, q1 * v1) + self.assertAlmostEqual(v1, q2 * v2) + + # still the same rotation, but with non-unit vectors + v1.set(0.5, 0.5, 0) + v2.set(-0.5, 0.5, 0) + + q1.from_2_axes(v1, v2) + q2.from_2_axes(v2, v1) + + self.assertNotEqual(q1, q2) + self.assertAlmostEqual(q1Correct, q1) + self.assertAlmostEqual(q2Correct, q2) + self.assertAlmostEqual(Quaterniond.IDENTITY, q1 * q2) + self.assertAlmostEqual(v2, q1 * v1) + self.assertAlmostEqual(v1, q2 * v2) + + # Test various settings of opposite vectors (which need special care) + tolerance = 1e-4 + + v1.set(1, 0, 0) + v2.set(-1, 0, 0) + q1.from_2_axes(v1, v2) + q2 = q1 * q1 + self.assertTrue(abs(q2.w()-1.0) <= tolerance or + abs(q2.w()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.x(), 0.0) + self.assertAlmostEqual(q2.y(), 0.0) + self.assertAlmostEqual(q2.z(), 0.0) + + v1.set(0, 1, 0) + v2.set(0, -1, 0) + q1.from_2_axes(v1, v2) + q2 = q1 * q1 + self.assertTrue(abs(q2.w()-1.0) <= tolerance or + abs(q2.w()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.x(), 0.0) + self.assertAlmostEqual(q2.y(), 0.0) + self.assertAlmostEqual(q2.z(), 0.0) + + v1.set(0, 0, 1) + v2.set(0, 0, -1) + q1.from_2_axes(v1, v2) + q2 = q1 * q1 + self.assertTrue(abs(q2.w()-1.0) <= tolerance or + abs(q2.w()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.x(), 0.0) + self.assertAlmostEqual(q2.y(), 0.0) + self.assertAlmostEqual(q2.z(), 0.0) + + v1.set(0, 1, 1) + v2.set(0, -1, -1) + q1.from_2_axes(v1, v2) + q2 = q1 * q1 + self.assertTrue(abs(q2.w()-1.0) <= tolerance or + abs(q2.w()-(-1.0)) <= tolerance) + self.assertAlmostEqual(q2.x(), 0.0) + self.assertAlmostEqual(q2.y(), 0.0) + self.assertAlmostEqual(q2.z(), 0.0) + + def test_integrate(self): + # integrate by zero, expect no change + q = Quaterniond(0.5, 0.5, 0.5, 0.5) + self.assertAlmostEqual(q, q.integrate(Vector3d.ZERO, 1.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UNIT_X, 0.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UNIT_Y, 0.0)) + self.assertAlmostEqual(q, q.integrate(Vector3d.UNIT_Z, 0.0)) + + # integrate along single axes, + # expect linear change in roll, pitch, yaw + q = Quaterniond(1, 0, 0, 0) + qRoll = q.integrate(Vector3d.UNIT_X, 1.0) + qPitch = q.integrate(Vector3d.UNIT_Y, 1.0) + qYaw = q.integrate(Vector3d.UNIT_Z, 1.0) + self.assertAlmostEqual(qRoll.euler(), Vector3d.UNIT_X) + self.assertAlmostEqual(qPitch.euler(), Vector3d.UNIT_Y) + self.assertAlmostEqual(qYaw.euler(), Vector3d.UNIT_Z) + + # integrate sequentially along single axes in order XYZ, + # expect rotations to match euler Angles + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qX = q.integrate(Vector3d.UNIT_X, angle) + qXY = qX.integrate(Vector3d.UNIT_Y, angle) + self.assertAlmostEqual(qXY.euler(), Vector3d(1, 1, 0)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qX = q.integrate(Vector3d.UNIT_X, angle) + qXZ = qX.integrate(Vector3d.UNIT_Z, angle) + self.assertAlmostEqual(qXZ.euler(), Vector3d(1, 0, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qY = q.integrate(Vector3d.UNIT_Y, angle) + qYZ = qY.integrate(Vector3d.UNIT_Z, angle) + self.assertAlmostEqual(qYZ.euler(), Vector3d(0, 1, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qX = q.integrate(Vector3d.UNIT_X, angle) + qXY = qX.integrate(Vector3d.UNIT_Y, angle) + qXYZ = qXY.integrate(Vector3d.UNIT_Z, angle) + self.assertAlmostEqual(qXYZ.euler(), Vector3d.ONE*angle) + + # integrate sequentially along single axes in order ZYX, + # expect rotations to not match euler Angles + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qZ = q.integrate(Vector3d.UNIT_Z, angle) + qZY = qZ.integrate(Vector3d.UNIT_Y, angle) + self.assertNotEqual(qZY.euler(), Vector3d(0, 1, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qZ = q.integrate(Vector3d.UNIT_Z, angle) + qZX = qZ.integrate(Vector3d.UNIT_X, angle) + self.assertNotEqual(qZX.euler(), Vector3d(1, 0, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qZ = q.integrate(Vector3d.UNIT_Z, angle) + qZY = qZ.integrate(Vector3d.UNIT_Y, angle) + qZYX = qZY.integrate(Vector3d.UNIT_X, angle) + self.assertNotEqual(qZYX.euler(), Vector3d(1, 1, 1)*angle) + + q = Quaterniond(1, 0, 0, 0) + angle = 0.5 + qY = q.integrate(Vector3d.UNIT_Y, angle) + qYX = qY.integrate(Vector3d.UNIT_X, angle) + self.assertNotEqual(qYX.euler(), Vector3d(1, 1, 0)*angle) + + # integrate a full rotation about different axes, + # expect no change. + q = Quaterniond(0.5, 0.5, 0.5, 0.5) + fourPi = 4 * math.pi + qX = q.integrate(Vector3d.UNIT_X, fourPi) + qY = q.integrate(Vector3d.UNIT_Y, fourPi) + qZ = q.integrate(Vector3d.UNIT_Z, fourPi) + self.assertAlmostEqual(q, qX) + self.assertAlmostEqual(q, qY) + self.assertAlmostEqual(q, qZ) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/python.i b/src/python/python.i index ae23a58e6..18a73da43 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -6,8 +6,12 @@ %include Vector3.i %include Vector4.i %include Color.i +%include Pose3.i +%include Quaternion.i %include Line2.i %include Line3.i +%include Matrix3.i +%include Matrix4.i %include PID.i %include RollingMean.i %include SemanticVersion.i From 495583da2f3abbcc3cee9893af2cf1b5fbf17366 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 14:24:59 +0800 Subject: [PATCH 58/81] fixes Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Sphere.hh | 12 +++------ src/Sphere_TEST.cc | 35 ++++++++++++++++++++++---- 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 02711c512..43759e324 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -105,7 +105,7 @@ T Sphere::VolumeBelow(const Planed &_plane) const if (dist < -r) { - // sphere is completely below plane + // sphere is below plane. return Volume(); } else if (dist > r) @@ -114,12 +114,6 @@ T Sphere::VolumeBelow(const Planed &_plane) const return 0.0; } - // Vertical plane - if (_plane.Normal().Z() < 1e-6) - { - return 0.0; - } - auto h = r - dist; return IGN_PI * h * h * (3 * r - h) / 3; } @@ -144,13 +138,15 @@ std::optional> return std::nullopt; } + // Get the height of the spherical cap auto h = r + dist; // Formula for geometric centorid: // https://mathworld.wolfram.com/SphericalCap.html auto numerator = 2 * r - h; - return Vector3{0, 0, 3 * numerator * numerator / (4 * (3 * r - h))}; + auto z = 3 * numerator * numerator / (4 * (3 * r - h)); + return z * _plane.Normal().Normalized(); } ////////////////////////////////////////////////// diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 59454a534..1eccffea6 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -143,6 +143,8 @@ TEST(SphereTest, VolumeBelow) math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), 2*r); EXPECT_NEAR(sphere.Volume(), sphere.VolumeBelow(_plane), 1e-3); } + + // Fully below (because plane is rotated down) { math::Planed _plane(math::Vector3d{0, 0, -1}, math::Vector2d(4, 4), 2*r); EXPECT_NEAR(sphere.Volume(), sphere.VolumeBelow(_plane), 1e-3); @@ -163,7 +165,7 @@ TEST(SphereTest, VolumeBelow) // Vertical plane { math::Planed _plane(math::Vector3d{1, 0, 0}, 0); - EXPECT_NEAR(0.0, sphere.VolumeBelow(_plane), 1e-3); + EXPECT_NEAR(sphere.Volume() / 2, sphere.VolumeBelow(_plane), 1e-3); } // Expectations from https://planetcalc.com/283/ @@ -185,14 +187,37 @@ TEST(SphereTest, CenterOfVolumeBelow) math::Sphered sphere(r); { - math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), 2*r); + math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(0, 0), 2 * r); EXPECT_EQ(Vector3d(0, 0, 0), sphere.CenterOfVolumeBelow(_plane)); } { - math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(4, 4), -2*r); - EXPECT_FALSE(sphere.CenterOfVolumeBelow(_plane)); + math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(0, 0), -2 * r); + EXPECT_FALSE(sphere.CenterOfVolumeBelow(_plane).has_value()); + } + + { + // Handcalculated value. + math::Planed _plane(math::Vector3d{0, 1, 0}, math::Vector2d(0, 0), 0.4 * r); + EXPECT_EQ( + Vector3d(0, 0.3375, 0), sphere.CenterOfVolumeBelow(_plane).value()); + } + + { + // Handcalculated value. + math::Planed _plane(math::Vector3d{0, -1, 0}, math::Vector2d(0, 0), -0.4 * r); + EXPECT_EQ( + Vector3d(0, 1.225, 0), sphere.CenterOfVolumeBelow(_plane).value()); } - // TODO: test more cases + { + // Weighted sums of the center of volume results in (0,0,0). + math::Planed _plane1(math::Vector3d{0, 0, 1}, -0.5); + math::Planed _plane2(math::Vector3d{0, 0, -1}, -0.5); + EXPECT_EQ( + sphere.CenterOfVolumeBelow(_plane1).value() * sphere.VolumeBelow(_plane1) + + sphere.CenterOfVolumeBelow(_plane2).value() * sphere.VolumeBelow(_plane2), + math::Vector3d(0, 0, 0) + ); + } } From 305d4d4ce2638eac2643c6c7c3be63e5bf44cfa2 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 15:30:02 +0800 Subject: [PATCH 59/81] Rigorous tests and bugfixes Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Sphere.hh | 4 +-- src/Sphere_TEST.cc | 47 ++++++++++++++++++++++++-- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 43759e324..b6b0600c3 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -139,14 +139,14 @@ std::optional> } // Get the height of the spherical cap - auto h = r + dist; + auto h = r - dist; // Formula for geometric centorid: // https://mathworld.wolfram.com/SphericalCap.html auto numerator = 2 * r - h; auto z = 3 * numerator * numerator / (4 * (3 * r - h)); - return z * _plane.Normal().Normalized(); + return - z * _plane.Normal().Normalized(); } ////////////////////////////////////////////////// diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 1eccffea6..2818dca6a 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -197,15 +197,56 @@ TEST(SphereTest, CenterOfVolumeBelow) } { + // Halfway point is a good spot to test. Center of Volume for a hemisphere + // is 3/8 its radius. In this case the point should fall below the z-plane + math::Planed _plane(math::Vector3d{0, 1, 0}, math::Vector2d(0, 0), 0); + EXPECT_EQ( + Vector3d(0, -0.75, 0), sphere.CenterOfVolumeBelow(_plane).value()); + } + + { + // Halfway point is a good spot to test. Center of Volume for a hemisphere + // is 3/8 its radius. In this case the point should fall below the z-plane + math::Planed _plane(math::Vector3d{0, -1, 0}, math::Vector2d(0, 0), 0); + EXPECT_EQ( + Vector3d(0, 0.75, 0), sphere.CenterOfVolumeBelow(_plane).value()); + } + + { // Handcalculated value. + // Plane at y = 0.8 pointing upwards + // Cap height is 2.8 + // Centroid should be at 0.3375. However, keep in mind this assumes an + // inverted cap. + // Center of volume below should be at -0.3375 math::Planed _plane(math::Vector3d{0, 1, 0}, math::Vector2d(0, 0), 0.4 * r); + EXPECT_EQ( + Vector3d(0, -0.3375, 0), sphere.CenterOfVolumeBelow(_plane).value()); + } + + { + // Handcalculated value. + math::Planed _plane(math::Vector3d{0, 1, 0}, + math::Vector2d(0, 0), -0.4 * r); + + EXPECT_EQ( + Vector3d(0, -1.225, 0), sphere.CenterOfVolumeBelow(_plane).value()); + } + + { + // Handcalculated value. + math::Planed _plane(math::Vector3d{0, -1, 0}, + math::Vector2d(0, 0), -0.4 * r); + EXPECT_EQ( Vector3d(0, 0.3375, 0), sphere.CenterOfVolumeBelow(_plane).value()); } { // Handcalculated value. - math::Planed _plane(math::Vector3d{0, -1, 0}, math::Vector2d(0, 0), -0.4 * r); + math::Planed _plane(math::Vector3d{0, -1, 0}, + math::Vector2d(0, 0), 0.4 * r); + EXPECT_EQ( Vector3d(0, 1.225, 0), sphere.CenterOfVolumeBelow(_plane).value()); } @@ -213,10 +254,12 @@ TEST(SphereTest, CenterOfVolumeBelow) { // Weighted sums of the center of volume results in (0,0,0). math::Planed _plane1(math::Vector3d{0, 0, 1}, -0.5); + // Flip plane1 axis math::Planed _plane2(math::Vector3d{0, 0, -1}, -0.5); EXPECT_EQ( sphere.CenterOfVolumeBelow(_plane1).value() * sphere.VolumeBelow(_plane1) - + sphere.CenterOfVolumeBelow(_plane2).value() * sphere.VolumeBelow(_plane2), + + sphere.CenterOfVolumeBelow(_plane2).value() + * sphere.VolumeBelow(_plane2), math::Vector3d(0, 0, 0) ); } From aba72c148af372b6f019c0821b5b02268597fb12 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 15:51:11 +0800 Subject: [PATCH 60/81] Style Signed-off-by: Arjo Chakravarty --- src/Sphere_TEST.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 2818dca6a..f28885738 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -197,7 +197,7 @@ TEST(SphereTest, CenterOfVolumeBelow) } { - // Halfway point is a good spot to test. Center of Volume for a hemisphere + // Halfway point is a good spot to test. Center of Volume for a hemisphere // is 3/8 its radius. In this case the point should fall below the z-plane math::Planed _plane(math::Vector3d{0, 1, 0}, math::Vector2d(0, 0), 0); EXPECT_EQ( @@ -205,7 +205,7 @@ TEST(SphereTest, CenterOfVolumeBelow) } { - // Halfway point is a good spot to test. Center of Volume for a hemisphere + // Halfway point is a good spot to test. Center of Volume for a hemisphere // is 3/8 its radius. In this case the point should fall below the z-plane math::Planed _plane(math::Vector3d{0, -1, 0}, math::Vector2d(0, 0), 0); EXPECT_EQ( @@ -258,7 +258,7 @@ TEST(SphereTest, CenterOfVolumeBelow) math::Planed _plane2(math::Vector3d{0, 0, -1}, -0.5); EXPECT_EQ( sphere.CenterOfVolumeBelow(_plane1).value() * sphere.VolumeBelow(_plane1) - + sphere.CenterOfVolumeBelow(_plane2).value() + + sphere.CenterOfVolumeBelow(_plane2).value() * sphere.VolumeBelow(_plane2), math::Vector3d(0, 0, 0) ); From 0b8c21846afacbf9f60b871075e5f094cfa22c25 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 15:55:45 +0800 Subject: [PATCH 61/81] more fixes Signed-off-by: Arjo Chakravarty --- src/Sphere_TEST.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index f28885738..fe5cd2aa3 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -198,7 +198,7 @@ TEST(SphereTest, CenterOfVolumeBelow) { // Halfway point is a good spot to test. Center of Volume for a hemisphere - // is 3/8 its radius. In this case the point should fall below the z-plane + // is 3/8 its radius. In this case the point should fall below the y-plane math::Planed _plane(math::Vector3d{0, 1, 0}, math::Vector2d(0, 0), 0); EXPECT_EQ( Vector3d(0, -0.75, 0), sphere.CenterOfVolumeBelow(_plane).value()); @@ -206,7 +206,8 @@ TEST(SphereTest, CenterOfVolumeBelow) { // Halfway point is a good spot to test. Center of Volume for a hemisphere - // is 3/8 its radius. In this case the point should fall below the z-plane + // is 3/8 its radius. In this case the point should fall above the y-plane + // thanks to flipped normal math::Planed _plane(math::Vector3d{0, -1, 0}, math::Vector2d(0, 0), 0); EXPECT_EQ( Vector3d(0, 0.75, 0), sphere.CenterOfVolumeBelow(_plane).value()); @@ -239,7 +240,7 @@ TEST(SphereTest, CenterOfVolumeBelow) math::Vector2d(0, 0), -0.4 * r); EXPECT_EQ( - Vector3d(0, 0.3375, 0), sphere.CenterOfVolumeBelow(_plane).value()); + Vector3d(0, 1.225, 0), sphere.CenterOfVolumeBelow(_plane).value()); } { @@ -248,7 +249,7 @@ TEST(SphereTest, CenterOfVolumeBelow) math::Vector2d(0, 0), 0.4 * r); EXPECT_EQ( - Vector3d(0, 1.225, 0), sphere.CenterOfVolumeBelow(_plane).value()); + Vector3d(0, 0.3375, 0), sphere.CenterOfVolumeBelow(_plane).value()); } { From acacbb90ba5d8c92ea4f392fc71d30d4e08549f1 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 15:59:50 +0800 Subject: [PATCH 62/81] fix compile issues caused by merge Signed-off-by: Arjo Chakravarty --- src/Plane_TEST.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 2a41467b1..0415fc415 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -156,24 +156,24 @@ TEST(PlaneTest, Intersection) { Planed plane(Vector3d(0.5, 0, 1), 1); { - auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(1, 0, 1)); + auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 1)); EXPECT_TRUE(intersect.has_value()); EXPECT_NEAR(intersect->Dot(plane.Normal()), plane.Offset(), 1e-6); } plane.Set(Vector3d(1, 0, 0), 2); { - auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(1, 0, 0)); + auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(1, 0, 0)); EXPECT_TRUE(intersect.has_value()); EXPECT_EQ(intersect.value(), Vector3d(2, 0, 0)); } { - auto intersect = plane.Intersect(Vector3d(1, 1, 0), Vector3d(-1, -1, 0)); + auto intersect = plane.Intersection(Vector3d(1, 1, 0), Vector3d(-1, -1, 0)); EXPECT_TRUE(intersect.has_value()); EXPECT_EQ(intersect.value(), Vector3d(2, 2, 0)); } { - auto intersect = plane.Intersect(Vector3d(0, 0, 0), Vector3d(0, 1, 0)); + auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 0)); EXPECT_FALSE(intersect.has_value()); } } From b1a36c0b0e053c4564861b29c824e448c6c5969d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 16:05:14 +0800 Subject: [PATCH 63/81] More comprehensive tests and bugfixes for box Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 4 ++-- src/Box_TEST.cc | 16 ++++++++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 6ffc77f3f..bbf0d077c 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -238,7 +238,7 @@ T Box::VolumeBelow(const Plane &_plane) const return std::fabs(volume)/6; } - +#include ///////////////////////////////////////////////// template std::optional> @@ -279,7 +279,7 @@ std::optional> centroid += v; } - return centroid; + return centroid / verticesBelow.size(); } ///////////////////////////////////////////////// template diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 9cbb46ea2..f69129413 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -172,6 +172,22 @@ TEST(BoxTest, CenterOfVolumeBelow) EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value()); EXPECT_EQ(box.CenterOfVolumeBelow(plane), math::Vector3d(0, 0, 0)); } + + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 0); + EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value()); + EXPECT_EQ(box.CenterOfVolumeBelow(plane).value(), + math::Vector3d(0, 0, -0.5)); + } + + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, -1.0), 0); + EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value()); + EXPECT_EQ(box.CenterOfVolumeBelow(plane).value(), + math::Vector3d(0, 0, 0.5)); + } } ////////////////////////////////////////////////// From 8e756284f30e7b6ca33064f5766210e65d591a84 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 17:08:03 +0800 Subject: [PATCH 64/81] fix windows warning (I think) Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index bbf0d077c..f06250335 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -279,7 +279,7 @@ std::optional> centroid += v; } - return centroid / verticesBelow.size(); + return centroid / static_cast(verticesBelow.size()); } ///////////////////////////////////////////////// template From 9a66163ba193245c8026d45d764adf22143a5555 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 20:56:04 +0800 Subject: [PATCH 65/81] Add plane intersection in bounds Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 27 +++++++++++++++++++++++++-- include/ignition/math/Vector3.hh | 3 ++- src/Plane_TEST.cc | 11 +++++++++++ 3 files changed, 38 insertions(+), 3 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 20672fda2..7218f3934 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -22,9 +22,11 @@ #include #include #include -#include +#include #include +#include + namespace ignition { namespace math @@ -142,7 +144,28 @@ namespace ignition auto constant = this->Offset() - this->Normal().Dot(_point); auto param = constant / this->Normal().Dot(_gradient); auto intersection = _point + _gradient*param; - return intersection; + + if(this->Size() == Vector2(0,0)) + return intersection; + + auto dotProduct = Vector3::UnitZ.Dot(this->Normal()); + auto angle = acos(dotProduct / this->Normal().Length()); + auto axis = Vector3::UnitZ.Cross(this->Normal().Normalized()); + Quaternion rotation(axis, angle); + + Vector3 rotatedXAxis = rotation * Vector3::UnitX; + Vector3 rotatedYAxis = rotation * Vector3::UnitY; + + auto xBasis = rotatedXAxis.VectorProjectionLength(intersection); + auto yBasis = rotatedYAxis.VectorProjectionLength(intersection); + + std::cout << "Basis " << xBasis << ", " << yBasis << "\n"; + if(fabs(xBasis) < this->Size().X() / 2 + && fabs(yBasis) < this->Size().Y() / 2) + { + return intersection; + } + return std::nullopt; } /// \brief The side of the plane a point is on. diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index 8801a6a35..8414ffcb1 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -266,7 +266,8 @@ namespace ignition /// \brief Get Length of a Vector Projection of another onto this vector /// \param[in] _v the vector - /// \return the projection + /// \return the projection. Note: If the length of the current vector is + /// zero this method will return a NaN public: T VectorProjectionLength(const Vector3 &_v) const { return this->Dot(_v) / this->Length(); diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 0415fc415..2d2419ad5 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -176,4 +176,15 @@ TEST(PlaneTest, Intersection) auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 0)); EXPECT_FALSE(intersect.has_value()); } + + { + Planed planeBounded(Vector3d(0, 0, 1), Vector2d(0.5, 0.5), 0); + auto intersect1 = + planeBounded.Intersection(Vector3d(0,0,0), Vector3d(0,0,1)); + EXPECT_TRUE(intersect1.has_value()); + EXPECT_EQ(intersect1.value(), Vector3d(0, 0, 0)); + auto intersect2 = + planeBounded.Intersection(Vector3d(20,20,0), Vector3d(0,0,1)); + EXPECT_FALSE(intersect2.has_value()); + } } From 4bf9cfb7ff576c80d0a0ab812ae74a79ae09ec20 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 21:07:33 +0800 Subject: [PATCH 66/81] style Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 8 ++++---- src/Plane_TEST.cc | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 7218f3934..29f9b189b 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -25,8 +25,6 @@ #include #include -#include - namespace ignition { namespace math @@ -145,9 +143,12 @@ namespace ignition auto param = constant / this->Normal().Dot(_gradient); auto intersection = _point + _gradient*param; - if(this->Size() == Vector2(0,0)) + if(this->Size() == Vector2(0, 0)) return intersection; + // Check if the point is within the size bounds + // To do this we create a Quaternion using Angle, Axis constructor and + // rotate the Y and X axis the same amount as the normal. auto dotProduct = Vector3::UnitZ.Dot(this->Normal()); auto angle = acos(dotProduct / this->Normal().Length()); auto axis = Vector3::UnitZ.Cross(this->Normal().Normalized()); @@ -159,7 +160,6 @@ namespace ignition auto xBasis = rotatedXAxis.VectorProjectionLength(intersection); auto yBasis = rotatedYAxis.VectorProjectionLength(intersection); - std::cout << "Basis " << xBasis << ", " << yBasis << "\n"; if(fabs(xBasis) < this->Size().X() / 2 && fabs(yBasis) < this->Size().Y() / 2) { diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 2d2419ad5..fcf3785fc 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -180,11 +180,11 @@ TEST(PlaneTest, Intersection) { Planed planeBounded(Vector3d(0, 0, 1), Vector2d(0.5, 0.5), 0); auto intersect1 = - planeBounded.Intersection(Vector3d(0,0,0), Vector3d(0,0,1)); + planeBounded.Intersection(Vector3d(0, 0, 0), Vector3d(0, 0, 1)); EXPECT_TRUE(intersect1.has_value()); EXPECT_EQ(intersect1.value(), Vector3d(0, 0, 0)); auto intersect2 = - planeBounded.Intersection(Vector3d(20,20,0), Vector3d(0,0,1)); + planeBounded.Intersection(Vector3d(20, 20, 0), Vector3d(0, 0, 1)); EXPECT_FALSE(intersect2.has_value()); } } From d0c605da13e52c4d9a630bd4f48d779732f2d857 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 20 Sep 2021 21:18:26 +0800 Subject: [PATCH 67/81] style Signed-off-by: Arjo Chakravarty --- src/Sphere_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index fe5cd2aa3..4714e3ee5 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -213,7 +213,7 @@ TEST(SphereTest, CenterOfVolumeBelow) Vector3d(0, 0.75, 0), sphere.CenterOfVolumeBelow(_plane).value()); } - { + { // Handcalculated value. // Plane at y = 0.8 pointing upwards // Cap height is 2.8 From d4968df7cdf7ab5f4fd02c3fb1f7c11da2cdc2f7 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 20 Sep 2021 21:14:45 -0700 Subject: [PATCH 68/81] More suggestions to #219 Signed-off-by: Louise Poubel --- include/ignition/math/Box.hh | 26 ++- include/ignition/math/Plane.hh | 10 +- include/ignition/math/Sphere.hh | 14 +- include/ignition/math/detail/Box.hh | 146 ++++++------- include/ignition/math/detail/Sphere.hh | 8 +- src/Box_TEST.cc | 292 ++++++++++++++++++++++++- src/Plane_TEST.cc | 23 ++ src/Sphere_TEST.cc | 2 + 8 files changed, 416 insertions(+), 105 deletions(-) diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index fe225c7f2..3f9f5259a 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -23,7 +23,7 @@ #include #include -#include +#include namespace ignition { @@ -132,16 +132,27 @@ namespace ignition public: Precision Volume() const; /// \brief Get the volume of the box below a plane. - /// \param[in] _plane The plane which cuts the box. + /// \param[in] _plane The plane which cuts the box, expressed in the box's + /// frame. /// \return Volume below the plane in m^3. public: Precision VolumeBelow(const Plane &_plane) const; /// \brief Center of volume below the plane. This is useful when - /// calculating where the buoyancy should be applied. - /// \param[in] _plane The plane which slices the box. + /// calculating where buoyancy should be applied, for example. + /// \param[in] _plane The plane which cuts the box, expressed in the box's + /// frame. + /// \return Center of volume, in box's frame. public: std::optional> CenterOfVolumeBelow(const Plane &_plane) const; + /// \brief All the vertices which are on or below the plane. + /// \param[in] _plane The plane which cuts the box, expressed in the box's + /// frame. + /// \return Box vertices which are below the plane, expressed in the box's + /// frame. + public: std::set> + VerticesBelow(const Plane &_plane) const; + /// \brief Compute the box's density given a mass value. The /// box is assumed to be solid with uniform density. This /// function requires the box's size to be set to @@ -175,11 +186,12 @@ namespace ignition /// could be due to an invalid size (<=0) or density (<=0). public: bool MassMatrix(MassMatrix3 &_massMat) const; - /// \brief Get intersection between a plane and the box. + /// \brief Get intersection between a plane and the box's edges. + /// Edges contained on the plane are ignored. /// \param[in] _plane The plane against which we are testing intersection. - /// \returns a list of points along the edges of the box where the + /// \returns A list of points along the edges of the box where the /// intersection occurs. - public: std::vector> Intersections( + public: std::set> Intersections( const Plane &_plane) const; /// \brief Size of the box. diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 29f9b189b..a248202c9 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -129,13 +129,13 @@ namespace ignition /// \param[in] _tolerance The tolerance for determining a line is /// parallel to the plane. Optional, default=10^-16 /// \return The point of intersection. std::nullopt if the line is - /// parrallel to the plane. + /// parallel to the plane (including lines on the plane). public: std::optional> Intersection( const Vector3 &_point, const Vector3 &_gradient, const double &_tolerance = 1e-6) const { - if(abs(this->Normal().Dot(_gradient)) < _tolerance) + if (std::abs(this->Normal().Dot(_gradient)) < _tolerance) { return std::nullopt; } @@ -143,7 +143,7 @@ namespace ignition auto param = constant / this->Normal().Dot(_gradient); auto intersection = _point + _gradient*param; - if(this->Size() == Vector2(0, 0)) + if (this->Size() == Vector2(0, 0)) return intersection; // Check if the point is within the size bounds @@ -160,8 +160,8 @@ namespace ignition auto xBasis = rotatedXAxis.VectorProjectionLength(intersection); auto yBasis = rotatedYAxis.VectorProjectionLength(intersection); - if(fabs(xBasis) < this->Size().X() / 2 - && fabs(yBasis) < this->Size().Y() / 2) + if (fabs(xBasis) < this->Size().X() / 2 && + fabs(yBasis) < this->Size().Y() / 2) { return intersection; } diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh index e01c8cf8f..537bd109f 100644 --- a/include/ignition/math/Sphere.hh +++ b/include/ignition/math/Sphere.hh @@ -94,18 +94,20 @@ namespace ignition /// \brief Get the volume of sphere below a given plane in m^3. /// It is assumed that the center of the sphere is on the origin - /// The plane's orientation is not considered. - /// \param[in] _plane The plane which slices this sphere. + /// \param[in] _plane The plane which slices this sphere, expressed + /// in the sphere's reference frame. /// \return Volume below the sphere in m^3. - public: Precision VolumeBelow(const Planed &_plane) const; + public: Precision VolumeBelow(const Plane &_plane) const; /// \brief Center of volume below the plane. This is useful for example /// when calculating where buoyancy should be applied. - /// \param[in] _plane The plane which slices the sphere. + /// \param[in] _plane The plane which slices this sphere, expressed + /// in the sphere's reference frame. /// \return The center of volume if there is anything under the plane, - /// otherwise return a std::nullopt. + /// otherwise return a std::nullopt. Expressed in the sphere's reference + /// frame. public: std::optional> - CenterOfVolumeBelow(const Planed &_plane) const; + CenterOfVolumeBelow(const Plane &_plane) const; /// \brief Compute the sphere's density given a mass value. The /// sphere is assumed to be solid with uniform density. This diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index f06250335..a6097a010 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -122,15 +122,15 @@ T Box::Volume() const ////////////////////////////////////////////////// template -std::vector, T>> TrianglesInPlane(Plane &_plane, - std::vector> &_vertices) +std::vector, T>> TrianglesInPlane( + const Plane &_plane, std::set> &_vertices) { std::vector, T>> triangles; std::vector> pointsInPlane; Vector3 centroid; - for(auto &pt : _vertices) + for (const auto &pt : _vertices) { - if(_plane.Side(pt) == Plane::NO_SIDE) + if (_plane.Side(pt) == Plane::NO_SIDE) { pointsInPlane.push_back(pt); centroid += pt; @@ -138,10 +138,9 @@ std::vector, T>> TrianglesInPlane(Plane &_plane, } centroid /= T(pointsInPlane.size()); - if(pointsInPlane.size() < 3) + if (pointsInPlane.size() < 3) return {}; - // Choose a basis in the plane of the triangle auto axis1 = (pointsInPlane[0] - centroid).Normalize(); auto axis2 = axis1.Cross(_plane.Normal()).Normalize(); @@ -149,10 +148,10 @@ std::vector, T>> TrianglesInPlane(Plane &_plane, // Since the polygon is always convex, we can try to create a fan of triangles // by sorting the points by their angle in the plane basis. std::sort(pointsInPlane.begin(), pointsInPlane.end(), - [centroid, axis1, axis2] (const Vector3 &a, const Vector3 &b) + [centroid, axis1, axis2] (const Vector3 &_a, const Vector3 &_b) { - auto aDisplacement = a - centroid; - auto bDisplacement = b - centroid; + auto aDisplacement = _a - centroid; + auto bDisplacement = _b - centroid; auto aX = axis1.VectorProjectionLength(aDisplacement); auto aY = axis2.VectorProjectionLength(aDisplacement); @@ -162,7 +161,7 @@ std::vector, T>> TrianglesInPlane(Plane &_plane, return atan2(aY, aX) < atan2(bY, bX); }); - for(std::size_t i = 0; i < pointsInPlane.size(); ++i) + for (std::size_t i = 0; i < pointsInPlane.size(); ++i) { triangles.emplace_back( Triangle3(pointsInPlane[i], @@ -172,43 +171,23 @@ std::vector, T>> TrianglesInPlane(Plane &_plane, return triangles; } + ///////////////////////////////////////////////// template T Box::VolumeBelow(const Plane &_plane) const { - // Get coordinates of all vertice of box - // TODO(arjo): Cache this for performance - std::vector > vertices { - Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, - Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, - Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, - Vector3{-this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, - Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, - Vector3{-this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, - Vector3{this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, - Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} - }; - - std::vector > verticesBelow; - for(auto &v : vertices) - { - if(_plane.Distance(v) < 0) - { - verticesBelow.push_back(v); - } - } - - if(verticesBelow.size() == 0) + auto verticesBelow = this->VerticesBelow(_plane); + if (verticesBelow.size() == 0) return 0; - auto intersections = Intersections(_plane); - verticesBelow.insert(verticesBelow.end(), - intersections.begin(), intersections.end()); + auto intersections = this->Intersections(_plane); + verticesBelow.merge(intersections); // Fit six planes to the vertices in the shape. std::vector, T>> triangles; - std::vector> planes { + std::vector> planes + { Plane{Vector3{0, 0, 1}, this->Size().Z()/2}, Plane{Vector3{0, 0, -1}, this->Size().Z()/2}, Plane{Vector3{1, 0, 0}, this->Size().X()/2}, @@ -218,35 +197,56 @@ T Box::VolumeBelow(const Plane &_plane) const _plane }; - for(auto &p : planes) + for (const auto &p : planes) { - auto new_triangles = TrianglesInPlane(p, verticesBelow); + auto newTriangles = TrianglesInPlane(p, verticesBelow); triangles.insert(triangles.end(), - new_triangles.begin(), - new_triangles.end()); + newTriangles.begin(), + newTriangles.end()); } // Calculate the volume of the triangles // https://n-e-r-v-o-u-s.com/blog/?p=4415 T volume = 0; - for(auto triangle : triangles) + for (const auto &triangle : triangles) { auto crossProduct = (triangle.first[2]).Cross(triangle.first[1]); auto meshVolume = std::fabs(crossProduct.Dot(triangle.first[0])); volume += triangle.second * meshVolume; } - return std::fabs(volume)/6; + return std::abs(volume)/6; } -#include + ///////////////////////////////////////////////// template std::optional> Box::CenterOfVolumeBelow(const Plane &_plane) const +{ + auto verticesBelow = this->VerticesBelow(_plane); + if (verticesBelow.size() == 0) + return std::nullopt; + + auto intersections = this->Intersections(_plane); + verticesBelow.merge(intersections); + + Vector3 centroid; + for (const auto &v : verticesBelow) + { + centroid += v; + } + + return centroid / static_cast(verticesBelow.size()); +} + +///////////////////////////////////////////////// +template +std::set> Box::VerticesBelow(const Plane &_plane) const { // Get coordinates of all vertice of box // TODO(arjo): Cache this for performance - std::vector > vertices { + std::set > vertices + { Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, @@ -257,30 +257,18 @@ std::optional> Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} }; - std::vector > verticesBelow; - for(auto &v : vertices) + std::set > verticesBelow; + for (const auto &v : vertices) { - if(_plane.Distance(v) < 0) + if (_plane.Distance(v) <= 0) { - verticesBelow.push_back(v); + verticesBelow.insert(v); } } - if(verticesBelow.size() == 0) - return std::nullopt; - - auto intersections = Intersections(_plane); - verticesBelow.insert(verticesBelow.end(), - intersections.begin(), intersections.end()); - - Vector3 centroid; - for(auto &v : verticesBelow) - { - centroid += v; - } - - return centroid / static_cast(verticesBelow.size()); + return verticesBelow; } + ///////////////////////////////////////////////// template T Box::DensityFromMass(const T _mass) const @@ -311,13 +299,14 @@ bool Box::MassMatrix(MassMatrix3 &_massMat) const ////////////////////////////////////////////////// template -std::vector> Box::Intersections( +std::set> Box::Intersections( const Plane &_plane) const { - std::vector> intersections; - // These are vertice via which we can describe edges. We only need 4 such + std::set> intersections; + // These are vertices via which we can describe edges. We only need 4 such // vertices - std::vector > vertices { + std::vector > vertices + { Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, @@ -325,7 +314,8 @@ std::vector> Box::Intersections( }; // Axises - std::vector> axes { + std::vector> axes + { Vector3{1, 0, 0}, Vector3{0, 1, 0}, Vector3{0, 0, 1} @@ -333,20 +323,20 @@ std::vector> Box::Intersections( // There are 12 edges. However, we just need 4 points to describe the // twelve edges. - for(auto &v : vertices) + for (auto &v : vertices) { - for(auto &a : axes) + for (auto &a : axes) { auto intersection = _plane.Intersection(v, a); - if(intersection.has_value() && - intersection->X() >= -this->size.X()/2 && - intersection->X() <= this->size.X()/2 && - intersection->Y() >= -this->size.Y()/2 && - intersection->Y() <= this->size.Y()/2 && - intersection->Z() >= -this->size.Z()/2 && - intersection->Z() <= this->size.Z()/2) + if (intersection.has_value() && + intersection->X() >= -this->size.X()/2 && + intersection->X() <= this->size.X()/2 && + intersection->Y() >= -this->size.Y()/2 && + intersection->Y() <= this->size.Y()/2 && + intersection->Z() >= -this->size.Z()/2 && + intersection->Z() <= this->size.Z()/2) { - intersections.push_back(intersection.value()); + intersections.insert(intersection.value()); } } } diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index b6b0600c3..740ee6741 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -97,7 +97,7 @@ T Sphere::Volume() const ////////////////////////////////////////////////// template -T Sphere::VolumeBelow(const Planed &_plane) const +T Sphere::VolumeBelow(const Plane &_plane) const { auto r = this->radius; // get nearest point to center on plane @@ -121,18 +121,18 @@ T Sphere::VolumeBelow(const Planed &_plane) const ////////////////////////////////////////////////// template std::optional> - Sphere::CenterOfVolumeBelow(const Planed &_plane) const + Sphere::CenterOfVolumeBelow(const Plane &_plane) const { auto r = this->radius; // get nearest point to center on plane auto dist = _plane.Distance(Vector3d(0, 0, 0)); - if(dist < -r) + if (dist < -r) { // sphere is completely below plane return Vector3{0, 0, 0}; } - else if(dist > r) + else if (dist > r) { // sphere is completely above plane return std::nullopt; diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index f69129413..c3d4edbeb 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -114,58 +114,192 @@ TEST(BoxTest, VolumeAndDensity) } ////////////////////////////////////////////////// -TEST(BoxTest, Intersects) +TEST(BoxTest, Intersections) { + // No intersections { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); EXPECT_EQ(box.Intersections(plane).size(), 0UL); } + // Plane crosses 4 edges { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 0); - EXPECT_EQ(box.Intersections(plane).size(), 4UL); + + auto intersections = box.Intersections(plane); + ASSERT_EQ(4UL, intersections.size()); + auto it = intersections.begin(); + EXPECT_EQ(math::Vector3d(-1.0, -1.0, 0.0), *it++); + EXPECT_EQ(math::Vector3d(-1.0, 1.0, 0.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, -1.0, 0.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, 1.0, 0.0), *it++); + } + + // Plane coincides with box's face + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 1.0); + + auto intersections = box.Intersections(plane); + ASSERT_EQ(4UL, intersections.size()); + auto it = intersections.begin(); + EXPECT_EQ(math::Vector3d(-1.0, -1.0, 1.0), *it++); + EXPECT_EQ(math::Vector3d(-1.0, 1.0, 1.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, -1.0, 1.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, 1.0, 1.0), *it++); + } + + // 3 intersections + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(1.0, 1.0, 1.0), 1.0); + + auto intersections = box.Intersections(plane); + ASSERT_EQ(3UL, intersections.size()); + auto it = intersections.begin(); + EXPECT_EQ(math::Vector3d(-1.0, 1.0, 1.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, -1.0, 1.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, 1.0, -1.0), *it++); + } + + // 6 intersections + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(1.0, 1.0, 1.0), 0.5); + + auto intersections = box.Intersections(plane); + ASSERT_EQ(6UL, intersections.size()); + auto it = intersections.begin(); + EXPECT_EQ(math::Vector3d(-1.0, 1.0, 0.5), *it++); + EXPECT_EQ(math::Vector3d(-1.0, 0.5, 1.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, -1.0, 0.5), *it++); + EXPECT_EQ(math::Vector3d(0.5, -1.0, 1.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, 0.5, -1.0), *it++); + EXPECT_EQ(math::Vector3d(0.5, 1.0, -1.0), *it++); + } + + // 5 intersections + // This is the plane above tilted further up + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(1.0, 1.0, 2.0), 0.5); + + auto intersections = box.Intersections(plane); + ASSERT_EQ(5UL, intersections.size()); + auto it = intersections.begin(); + EXPECT_EQ(math::Vector3d(-1.0, 1.0, 0.25), *it++); + EXPECT_EQ(math::Vector3d(-1.0, -0.5, 1.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, -1.0, 0.25), *it++); + EXPECT_EQ(math::Vector3d(-0.5, -1.0, 1.0), *it++); + EXPECT_EQ(math::Vector3d(1.0, 1.0, -0.75), *it++); } } ////////////////////////////////////////////////// TEST(BoxTest, VolumeBelow) { + // Fully above { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); EXPECT_DOUBLE_EQ(0.0, box.VolumeBelow(plane)); } + // Fully below + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 20.0); + EXPECT_DOUBLE_EQ(box.Volume(), box.VolumeBelow(plane)); + } + // Fully below (because plane is rotated down) + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0.0, 0.0, -1.0), 20.0); + EXPECT_DOUBLE_EQ(box.Volume(), box.VolumeBelow(plane)); + } + // Cut in half { math::Boxd box(2.0, 2.0, 2.0); - math::Planed plane(math::Vector3d(0, 0, 2.0), 0); + math::Planed plane(math::Vector3d(0, 0, 1.0), 0); EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); } + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0, 1, 0), 0); + + EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); + } + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(-1, 0, 0), 0); + + EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); + } + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(-1, -1, 0), 0); + + EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); + } + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(0, 1, 1), 0); + EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); + } + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed plane(math::Vector3d(1, 1, 1), 0); + + EXPECT_DOUBLE_EQ(box.Volume()/2, box.VolumeBelow(plane)); + } + // Cut in 3/4 { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0, 0, 1.0), 0.5); EXPECT_DOUBLE_EQ(3*box.Volume()/4, box.VolumeBelow(plane)); } + // Opposites add to the total volume { math::Boxd box(2.0, 2.0, 2.0); - math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 20.0); - EXPECT_DOUBLE_EQ(box.Volume(), box.VolumeBelow(plane)); + math::Planed planeA(math::Vector3d(0, 0, 1.0), 0.5); + math::Planed planeB(math::Vector3d(0, 0, 1.0), -0.5); + + EXPECT_DOUBLE_EQ(box.Volume(), + box.VolumeBelow(planeA) + box.VolumeBelow(planeB)); + } + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed planeA(math::Vector3d(0, 1.0, 1.0), 0.5); + math::Planed planeB(math::Vector3d(0, 1.0, 1.0), -0.5); + + EXPECT_DOUBLE_EQ(box.Volume(), + box.VolumeBelow(planeA) + box.VolumeBelow(planeB)); + } + { + math::Boxd box(2.0, 2.0, 2.0); + math::Planed planeA(math::Vector3d(-1, 1.0, 1.0), 0.5); + math::Planed planeB(math::Vector3d(-1, 1.0, 1.0), -0.5); + + EXPECT_DOUBLE_EQ(box.Volume(), + box.VolumeBelow(planeA) + box.VolumeBelow(planeB)); } } ////////////////////////////////////////////////// TEST(BoxTest, CenterOfVolumeBelow) { + // Fully above { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); EXPECT_FALSE(box.CenterOfVolumeBelow(plane).has_value()); } + // Fully below { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 5.0); @@ -173,6 +307,7 @@ TEST(BoxTest, CenterOfVolumeBelow) EXPECT_EQ(box.CenterOfVolumeBelow(plane), math::Vector3d(0, 0, 0)); } + // Cut in half { math::Boxd box(2.0, 2.0, 2.0); math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 0); @@ -190,6 +325,153 @@ TEST(BoxTest, CenterOfVolumeBelow) } } +////////////////////////////////////////////////// +TEST(BoxTest, VerticesBelow) +{ + math::Boxd box(2.0, 2.0, 2.0); + auto size = box.Size(); + + math::Vector3d pXpYpZ{ size.X()/2, size.Y()/2, size.Z()/2}; + math::Vector3d nXpYpZ{-size.X()/2, size.Y()/2, size.Z()/2}; + math::Vector3d pXnYpZ{ size.X()/2, -size.Y()/2, size.Z()/2}; + math::Vector3d nXnYpZ{-size.X()/2, -size.Y()/2, size.Z()/2}; + math::Vector3d pXpYnZ{ size.X()/2, size.Y()/2, -size.Z()/2}; + math::Vector3d nXpYnZ{-size.X()/2, size.Y()/2, -size.Z()/2}; + math::Vector3d pXnYnZ{ size.X()/2, -size.Y()/2, -size.Z()/2}; + math::Vector3d nXnYnZ{-size.X()/2, -size.Y()/2, -size.Z()/2}; + + // Fully above + { + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), -5.0); + EXPECT_TRUE(box.VerticesBelow(plane).empty()); + } + // Fully below + { + math::Planed plane(math::Vector3d(0.0, 0.0, 1.0), 20.0); + EXPECT_EQ(8u, box.VerticesBelow(plane).size()); + } + // Fully below (because plane is rotated down) + { + math::Planed plane(math::Vector3d(0.0, 0.0, -1.0), 20.0); + EXPECT_EQ(8u, box.VerticesBelow(plane).size()); + } + // 4 vertices + { + math::Planed plane(math::Vector3d(0, 0, 1.0), 0); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(4u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(nXnYnZ, *it++); + EXPECT_EQ(nXpYnZ, *it++); + EXPECT_EQ(pXnYnZ, *it++); + EXPECT_EQ(pXpYnZ, *it++); + } + { + math::Planed plane(math::Vector3d(0, 1, 0), 0.5); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(4u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(nXnYnZ, *it++); + EXPECT_EQ(nXnYpZ, *it++); + EXPECT_EQ(pXnYnZ, *it++); + EXPECT_EQ(pXnYpZ, *it++); + } + { + math::Planed plane(math::Vector3d(-1, 0, 0), -0.5); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(4u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(pXnYnZ, *it++); + EXPECT_EQ(pXnYpZ, *it++); + EXPECT_EQ(pXpYnZ, *it++); + EXPECT_EQ(pXpYpZ, *it++); + } + { + math::Planed plane(math::Vector3d(1, 1, 1), 0.0); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(4u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(nXnYnZ, *it++); + EXPECT_EQ(nXnYpZ, *it++); + EXPECT_EQ(nXpYnZ, *it++); + EXPECT_EQ(pXnYnZ, *it++); + } + // 6 vertices + { + math::Planed plane(math::Vector3d(-1, -1, 0), 0.3); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(6u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(nXpYnZ, *it++); + EXPECT_EQ(nXpYpZ, *it++); + EXPECT_EQ(pXnYnZ, *it++); + EXPECT_EQ(pXnYpZ, *it++); + EXPECT_EQ(pXpYnZ, *it++); + EXPECT_EQ(pXpYpZ, *it++); + } + { + math::Planed plane(math::Vector3d(0, 1, 1), 0.9); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(6u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(nXnYnZ, *it++); + EXPECT_EQ(nXnYpZ, *it++); + EXPECT_EQ(pXnYpZ, *it++); + EXPECT_EQ(nXpYnZ, *it++); + EXPECT_EQ(pXnYnZ, *it++); + EXPECT_EQ(pXpYnZ, *it++); + } + // 2 vertices + { + math::Planed plane(math::Vector3d(-1, -1, 0), -0.5); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(2u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(pXpYnZ, *it++); + EXPECT_EQ(pXpYpZ, *it++); + } + // 7 vertices + { + math::Planed plane(math::Vector3d(1, 1, 1), 1.0); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(7u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(nXnYnZ, *it++); + EXPECT_EQ(nXnYpZ, *it++); + EXPECT_EQ(pXnYpZ, *it++); + EXPECT_EQ(nXpYnZ, *it++); + EXPECT_EQ(nXpYpZ, *it++); + EXPECT_EQ(pXnYnZ, *it++); + EXPECT_EQ(pXpYnZ, *it++); + } + // 1 vertex + { + math::Planed plane(math::Vector3d(1, 1, 1), -1.2); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(1u, vertices.size()); + + auto it = vertices.begin(); + EXPECT_EQ(nXnYnZ, *it++); + } +} + ////////////////////////////////////////////////// TEST(BoxTest, Mass) { diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index fcf3785fc..e082d2c26 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -172,11 +172,34 @@ TEST(PlaneTest, Intersection) EXPECT_TRUE(intersect.has_value()); EXPECT_EQ(intersect.value(), Vector3d(2, 2, 0)); } + // Lines on plane + { + auto intersect = plane.Intersection(Vector3d(2, 0, 0), Vector3d(0, 1, 0)); + EXPECT_FALSE(intersect.has_value()); + } + { + auto intersect = plane.Intersection(Vector3d(2, 0, 0), Vector3d(0, 0, 1)); + EXPECT_FALSE(intersect.has_value()); + } + { + auto intersect = plane.Intersection(Vector3d(2, 0, 0), Vector3d(0, 1, 1)); + EXPECT_FALSE(intersect.has_value()); + } + // Lines parallel to plane { auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 0)); EXPECT_FALSE(intersect.has_value()); } + { + auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(0, 0, 1)); + EXPECT_FALSE(intersect.has_value()); + } + { + auto intersect = plane.Intersection(Vector3d(0, 0, 0), Vector3d(0, 1, 1)); + EXPECT_FALSE(intersect.has_value()); + } + // Bounded plane { Planed planeBounded(Vector3d(0, 0, 1), Vector2d(0.5, 0.5), 0); auto intersect1 = diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 4714e3ee5..cfb069c87 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -186,11 +186,13 @@ TEST(SphereTest, CenterOfVolumeBelow) double r = 2; math::Sphered sphere(r); + // Entire sphere below plane { math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(0, 0), 2 * r); EXPECT_EQ(Vector3d(0, 0, 0), sphere.CenterOfVolumeBelow(_plane)); } + // Entire sphere above plane { math::Planed _plane(math::Vector3d{0, 0, 1}, math::Vector2d(0, 0), -2 * r); EXPECT_FALSE(sphere.CenterOfVolumeBelow(_plane).has_value()); From 9a659fe07f8aaeab242f731c1de809ff2503341e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 20 Sep 2021 21:23:13 -0700 Subject: [PATCH 69/81] abs, include Signed-off-by: Louise Poubel --- include/ignition/math/Plane.hh | 4 ++-- include/ignition/math/detail/Box.hh | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index a248202c9..0da719218 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -160,8 +160,8 @@ namespace ignition auto xBasis = rotatedXAxis.VectorProjectionLength(intersection); auto yBasis = rotatedYAxis.VectorProjectionLength(intersection); - if (fabs(xBasis) < this->Size().X() / 2 && - fabs(yBasis) < this->Size().Y() / 2) + if (std::abs(xBasis) < this->Size().X() / 2 && + std::abs(yBasis) < this->Size().Y() / 2) { return intersection; } diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index a6097a010..f3e28faf5 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -20,6 +20,7 @@ #include "ignition/math/Triangle3.hh" #include +#include #include #include @@ -211,7 +212,7 @@ T Box::VolumeBelow(const Plane &_plane) const for (const auto &triangle : triangles) { auto crossProduct = (triangle.first[2]).Cross(triangle.first[1]); - auto meshVolume = std::fabs(crossProduct.Dot(triangle.first[0])); + auto meshVolume = std::abs(crossProduct.Dot(triangle.first[0])); volume += triangle.second * meshVolume; } From 95cd4ee4594c0fd4abdd1d8f53238868d041bdcf Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Sep 2021 16:51:57 +0800 Subject: [PATCH 70/81] Fixed header Signed-off-by: Arjo Chakravarty --- include/ignition/math/Box.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index 3f9f5259a..24b76449e 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -20,8 +20,8 @@ #include #include #include -#include #include +#include #include From 20d66b19c8e291093a1dea3e58b118bb301d1516 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Sep 2021 19:19:13 +0800 Subject: [PATCH 71/81] Workaround. Hopefully will make windows happy. Signed-off-by: Arjo Chakravarty --- include/ignition/math/Box.hh | 12 +- include/ignition/math/detail/Box.hh | 12 +- .../ignition/math/detail/WellOrderedVector.hh | 63 ++++++++ src/Box_TEST.cc | 134 ++++++++---------- 4 files changed, 138 insertions(+), 83 deletions(-) create mode 100644 include/ignition/math/detail/WellOrderedVector.hh diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index 24b76449e..09a55ec59 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -23,6 +23,8 @@ #include #include +#include "ignition/math/detail/WellOrderedVector.hh" + #include namespace ignition @@ -31,7 +33,11 @@ namespace ignition { // Inline bracket to help doxygen filtering. inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // + /// \brief This is the type used for deduplicating and returning the set of + /// intersections. + template + using IntersectionPoints = std::set, WellOrderedVectors>; + /// \class Box Box.hh ignition/math/Box.hh /// \brief A representation of a box. All units are in meters. /// @@ -150,7 +156,7 @@ namespace ignition /// frame. /// \return Box vertices which are below the plane, expressed in the box's /// frame. - public: std::set> + public: IntersectionPoints VerticesBelow(const Plane &_plane) const; /// \brief Compute the box's density given a mass value. The @@ -191,7 +197,7 @@ namespace ignition /// \param[in] _plane The plane against which we are testing intersection. /// \returns A list of points along the edges of the box where the /// intersection occurs. - public: std::set> Intersections( + public: IntersectionPoints Intersections( const Plane &_plane) const; /// \brief Size of the box. diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index f3e28faf5..6d7a4ae9b 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -124,7 +124,7 @@ T Box::Volume() const ////////////////////////////////////////////////// template std::vector, T>> TrianglesInPlane( - const Plane &_plane, std::set> &_vertices) + const Plane &_plane, IntersectionPoints &_vertices) { std::vector, T>> triangles; std::vector> pointsInPlane; @@ -242,11 +242,11 @@ std::optional> ///////////////////////////////////////////////// template -std::set> Box::VerticesBelow(const Plane &_plane) const +IntersectionPoints Box::VerticesBelow(const Plane &_plane) const { // Get coordinates of all vertice of box // TODO(arjo): Cache this for performance - std::set > vertices + IntersectionPoints vertices { Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, @@ -258,7 +258,7 @@ std::set> Box::VerticesBelow(const Plane &_plane) const Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} }; - std::set > verticesBelow; + IntersectionPoints verticesBelow; for (const auto &v : vertices) { if (_plane.Distance(v) <= 0) @@ -300,10 +300,10 @@ bool Box::MassMatrix(MassMatrix3 &_massMat) const ////////////////////////////////////////////////// template -std::set> Box::Intersections( +IntersectionPoints Box::Intersections( const Plane &_plane) const { - std::set> intersections; + IntersectionPoints intersections; // These are vertices via which we can describe edges. We only need 4 such // vertices std::vector > vertices diff --git a/include/ignition/math/detail/WellOrderedVector.hh b/include/ignition/math/detail/WellOrderedVector.hh new file mode 100644 index 000000000..1040a9bf5 --- /dev/null +++ b/include/ignition/math/detail/WellOrderedVector.hh @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_MATH_DETAIL_WELLORDERED_VECTOR_HH_ +#define IGNITION_MATH_DETAIL_WELLORDERED_VECTOR_HH_ +#include + +namespace ignition +{ + namespace math + { + /// \brief Comparator for well-ordering vectors. + template + struct WellOrderedVectors + { + /// \brief The normal Vector3::operator< is not actually properly ordered. + /// It does not form an ordinal set. This leads to various complications. + /// To solve this we introduce this function which orders vector3's by + /// their X value first, then their Y values and lastly their Z-values. + /// \param[in] _a - first vector + /// \param[in] _b - second vector + /// \return true if _a comes before _b. + bool operator() (const Vector3& _a, const Vector3& _b) const + { + if (_a[0] < _b[0]) + { + return true; + } + else if (equal(_a[0], _b[0], 1e-3)) + { + if (_a[1] < _b[1]) + { + return true; + } + else if (equal(_a[1], _b[1], 1e-3)) + { + return _a[2] < _b[2]; + } + else + { + return false; + } + } + return false; + } + }; + } +} + +#endif diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index c3d4edbeb..454abf4cb 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -130,11 +130,10 @@ TEST(BoxTest, Intersections) auto intersections = box.Intersections(plane); ASSERT_EQ(4UL, intersections.size()); - auto it = intersections.begin(); - EXPECT_EQ(math::Vector3d(-1.0, -1.0, 0.0), *it++); - EXPECT_EQ(math::Vector3d(-1.0, 1.0, 0.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, -1.0, 0.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, 1.0, 0.0), *it++); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, -1.0, 0.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 0.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 0.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, 1.0, 0.0)), 1UL); } // Plane coincides with box's face @@ -144,11 +143,10 @@ TEST(BoxTest, Intersections) auto intersections = box.Intersections(plane); ASSERT_EQ(4UL, intersections.size()); - auto it = intersections.begin(); - EXPECT_EQ(math::Vector3d(-1.0, -1.0, 1.0), *it++); - EXPECT_EQ(math::Vector3d(-1.0, 1.0, 1.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, -1.0, 1.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, 1.0, 1.0), *it++); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, -1.0, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, 1.0, 1.0)), 1UL); } // 3 intersections @@ -158,10 +156,9 @@ TEST(BoxTest, Intersections) auto intersections = box.Intersections(plane); ASSERT_EQ(3UL, intersections.size()); - auto it = intersections.begin(); - EXPECT_EQ(math::Vector3d(-1.0, 1.0, 1.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, -1.0, 1.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, 1.0, -1.0), *it++); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, 1.0, -1.0)), 1UL); } // 6 intersections @@ -171,13 +168,12 @@ TEST(BoxTest, Intersections) auto intersections = box.Intersections(plane); ASSERT_EQ(6UL, intersections.size()); - auto it = intersections.begin(); - EXPECT_EQ(math::Vector3d(-1.0, 1.0, 0.5), *it++); - EXPECT_EQ(math::Vector3d(-1.0, 0.5, 1.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, -1.0, 0.5), *it++); - EXPECT_EQ(math::Vector3d(0.5, -1.0, 1.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, 0.5, -1.0), *it++); - EXPECT_EQ(math::Vector3d(0.5, 1.0, -1.0), *it++); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 0.5)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 0.5, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 0.5)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(0.5, -1.0, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, 0.5, -1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(0.5, 1.0, -1.0)), 1UL); } // 5 intersections @@ -188,12 +184,11 @@ TEST(BoxTest, Intersections) auto intersections = box.Intersections(plane); ASSERT_EQ(5UL, intersections.size()); - auto it = intersections.begin(); - EXPECT_EQ(math::Vector3d(-1.0, 1.0, 0.25), *it++); - EXPECT_EQ(math::Vector3d(-1.0, -0.5, 1.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, -1.0, 0.25), *it++); - EXPECT_EQ(math::Vector3d(-0.5, -1.0, 1.0), *it++); - EXPECT_EQ(math::Vector3d(1.0, 1.0, -0.75), *it++); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, 1.0, 0.25)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(-1.0, -0.5, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, -1.0, 0.25)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(-0.5, -1.0, 1.0)), 1UL); + EXPECT_EQ(intersections.count(math::Vector3d(1.0, 1.0, -0.75)), 1UL); } } @@ -362,11 +357,10 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(4u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(nXnYnZ, *it++); - EXPECT_EQ(nXpYnZ, *it++); - EXPECT_EQ(pXnYnZ, *it++); - EXPECT_EQ(pXpYnZ, *it++); + EXPECT_EQ(vertices.count(nXnYnZ), 1UL); + EXPECT_EQ(vertices.count(nXpYnZ), 1UL); + EXPECT_EQ(vertices.count(pXnYnZ), 1UL); + EXPECT_EQ(vertices.count(pXpYnZ), 1UL); } { math::Planed plane(math::Vector3d(0, 1, 0), 0.5); @@ -374,11 +368,10 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(4u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(nXnYnZ, *it++); - EXPECT_EQ(nXnYpZ, *it++); - EXPECT_EQ(pXnYnZ, *it++); - EXPECT_EQ(pXnYpZ, *it++); + EXPECT_EQ(vertices.count(nXnYnZ), 1UL); + EXPECT_EQ(vertices.count(nXnYpZ), 1UL); + EXPECT_EQ(vertices.count(pXnYnZ), 1UL); + EXPECT_EQ(vertices.count(pXnYpZ), 1UL); } { math::Planed plane(math::Vector3d(-1, 0, 0), -0.5); @@ -386,11 +379,10 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(4u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(pXnYnZ, *it++); - EXPECT_EQ(pXnYpZ, *it++); - EXPECT_EQ(pXpYnZ, *it++); - EXPECT_EQ(pXpYpZ, *it++); + EXPECT_EQ(vertices.count(pXnYnZ), 1UL); + EXPECT_EQ(vertices.count(pXnYpZ), 1UL); + EXPECT_EQ(vertices.count(pXpYnZ), 1UL); + EXPECT_EQ(vertices.count(pXpYpZ), 1UL); } { math::Planed plane(math::Vector3d(1, 1, 1), 0.0); @@ -398,11 +390,10 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(4u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(nXnYnZ, *it++); - EXPECT_EQ(nXnYpZ, *it++); - EXPECT_EQ(nXpYnZ, *it++); - EXPECT_EQ(pXnYnZ, *it++); + EXPECT_EQ(vertices.count(nXnYnZ), 1UL); + EXPECT_EQ(vertices.count(nXnYpZ), 1UL); + EXPECT_EQ(vertices.count(nXpYnZ), 1UL); + EXPECT_EQ(vertices.count(pXnYnZ), 1UL); } // 6 vertices { @@ -411,13 +402,12 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(6u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(nXpYnZ, *it++); - EXPECT_EQ(nXpYpZ, *it++); - EXPECT_EQ(pXnYnZ, *it++); - EXPECT_EQ(pXnYpZ, *it++); - EXPECT_EQ(pXpYnZ, *it++); - EXPECT_EQ(pXpYpZ, *it++); + EXPECT_EQ(vertices.count(nXpYnZ), 1UL); + EXPECT_EQ(vertices.count(nXpYpZ), 1UL); + EXPECT_EQ(vertices.count(pXnYnZ), 1UL); + EXPECT_EQ(vertices.count(pXnYpZ), 1UL); + EXPECT_EQ(vertices.count(pXpYnZ), 1UL); + EXPECT_EQ(vertices.count(pXpYpZ), 1UL); } { math::Planed plane(math::Vector3d(0, 1, 1), 0.9); @@ -425,13 +415,12 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(6u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(nXnYnZ, *it++); - EXPECT_EQ(nXnYpZ, *it++); - EXPECT_EQ(pXnYpZ, *it++); - EXPECT_EQ(nXpYnZ, *it++); - EXPECT_EQ(pXnYnZ, *it++); - EXPECT_EQ(pXpYnZ, *it++); + EXPECT_EQ(vertices.count(nXnYnZ), 1UL); + EXPECT_EQ(vertices.count(nXnYpZ), 1UL); + EXPECT_EQ(vertices.count(pXnYpZ), 1UL); + EXPECT_EQ(vertices.count(nXpYnZ), 1UL); + EXPECT_EQ(vertices.count(pXnYnZ), 1UL); + EXPECT_EQ(vertices.count(pXpYnZ), 1UL); } // 2 vertices { @@ -440,9 +429,8 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(2u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(pXpYnZ, *it++); - EXPECT_EQ(pXpYpZ, *it++); + EXPECT_EQ(vertices.count(pXpYnZ), 1UL); + EXPECT_EQ(vertices.count(pXpYpZ), 1UL); } // 7 vertices { @@ -451,14 +439,13 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(7u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(nXnYnZ, *it++); - EXPECT_EQ(nXnYpZ, *it++); - EXPECT_EQ(pXnYpZ, *it++); - EXPECT_EQ(nXpYnZ, *it++); - EXPECT_EQ(nXpYpZ, *it++); - EXPECT_EQ(pXnYnZ, *it++); - EXPECT_EQ(pXpYnZ, *it++); + EXPECT_EQ(vertices.count(nXnYnZ), 1UL); + EXPECT_EQ(vertices.count(nXnYpZ), 1UL); + EXPECT_EQ(vertices.count(pXnYpZ), 1UL); + EXPECT_EQ(vertices.count(nXpYnZ), 1UL); + EXPECT_EQ(vertices.count(nXpYpZ), 1UL); + EXPECT_EQ(vertices.count(pXnYnZ), 1UL); + EXPECT_EQ(vertices.count(pXpYnZ), 1UL); } // 1 vertex { @@ -467,8 +454,7 @@ TEST(BoxTest, VerticesBelow) auto vertices = box.VerticesBelow(plane); ASSERT_EQ(1u, vertices.size()); - auto it = vertices.begin(); - EXPECT_EQ(nXnYnZ, *it++); + EXPECT_EQ(vertices.count(nXnYnZ), 1UL); } } From 149237577e14cf71a78a54d5a737cf85acb95596 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 05:43:55 +0800 Subject: [PATCH 72/81] Address @scpeter 's feedback Signed-off-by: Arjo Chakravarty --- include/ignition/math/Plane.hh | 4 ++-- include/ignition/math/Vector3.hh | 9 --------- include/ignition/math/detail/Box.hh | 18 +++++++++++------- src/Vector3_TEST.cc | 12 ------------ 4 files changed, 13 insertions(+), 30 deletions(-) diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 0da719218..4994f8128 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -157,8 +157,8 @@ namespace ignition Vector3 rotatedXAxis = rotation * Vector3::UnitX; Vector3 rotatedYAxis = rotation * Vector3::UnitY; - auto xBasis = rotatedXAxis.VectorProjectionLength(intersection); - auto yBasis = rotatedYAxis.VectorProjectionLength(intersection); + auto xBasis = rotatedXAxis.Dot(intersection); + auto yBasis = rotatedYAxis.Dot(intersection); if (std::abs(xBasis) < this->Size().X() / 2 && std::abs(yBasis) < this->Size().Y() / 2) diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index 8414ffcb1..6a3b5b1e9 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -264,15 +264,6 @@ namespace ignition return n.Normalize(); } - /// \brief Get Length of a Vector Projection of another onto this vector - /// \param[in] _v the vector - /// \return the projection. Note: If the length of the current vector is - /// zero this method will return a NaN - public: T VectorProjectionLength(const Vector3 &_v) const - { - return this->Dot(_v) / this->Length(); - } - /// \brief Get distance to an infinite line defined by 2 points. /// \param[in] _pt1 first point on the line /// \param[in] _pt2 second point on the line diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 6d7a4ae9b..88cd20b66 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -128,6 +128,7 @@ std::vector, T>> TrianglesInPlane( { std::vector, T>> triangles; std::vector> pointsInPlane; + Vector3 centroid; for (const auto &pt : _vertices) { @@ -154,11 +155,14 @@ std::vector, T>> TrianglesInPlane( auto aDisplacement = _a - centroid; auto bDisplacement = _b - centroid; - auto aX = axis1.VectorProjectionLength(aDisplacement); - auto aY = axis2.VectorProjectionLength(aDisplacement); + // project line onto the new basis vectors + // The axis length will never be zero as we have three different points + // and the centroid will be different. + auto aX = axis1.Dot(aDisplacement) / axis1.Length(); + auto aY = axis2.Dot(aDisplacement) / axis2.Length(); - auto bX = axis1.VectorProjectionLength(bDisplacement); - auto bY = axis2.VectorProjectionLength(bDisplacement); + auto bX = axis1.Dot(bDisplacement) / axis1.Length(); + auto bY = axis2.Dot(bDisplacement) / axis2.Length(); return atan2(aY, aX) < atan2(bY, bX); }); @@ -178,7 +182,7 @@ template T Box::VolumeBelow(const Plane &_plane) const { auto verticesBelow = this->VerticesBelow(_plane); - if (verticesBelow.size() == 0) + if (verticesBelow.empty()) return 0; auto intersections = this->Intersections(_plane); @@ -225,7 +229,7 @@ std::optional> Box::CenterOfVolumeBelow(const Plane &_plane) const { auto verticesBelow = this->VerticesBelow(_plane); - if (verticesBelow.size() == 0) + if (verticesBelow.empty()) return std::nullopt; auto intersections = this->Intersections(_plane); @@ -314,7 +318,7 @@ IntersectionPoints Box::Intersections( Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2} }; - // Axises + // Axes std::vector> axes { Vector3{1, 0, 0}, diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc index 81d875340..9a4615aa1 100644 --- a/src/Vector3_TEST.cc +++ b/src/Vector3_TEST.cc @@ -435,18 +435,6 @@ TEST(Vector3dTest, Multiply) EXPECT_EQ(v*v, math::Vector3d(0.01, 0.04, 0.09)); } -///////////////////////////////////////////////// -TEST(Vector3dTest, Projection) -{ - math::Vector3d v1(0, 0, 2); - math::Vector3d v2(0, 1, 0); - math::Vector3d v3(2, 3, 2); - - EXPECT_NEAR(v1.VectorProjectionLength(v3), 2, 1e-15); - EXPECT_NEAR(v2.VectorProjectionLength(v3), 3, 1e-15); - EXPECT_NEAR(v1.VectorProjectionLength(v2), 0, 1e-15); -} - ///////////////////////////////////////////////// TEST(Vector3dTest, NotEqual) { From 72d77795aa44d4ad30ddb3e7e44b554d71c14cfe Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 05:58:32 +0800 Subject: [PATCH 73/81] Address @scpeters feedback about Line3 Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index ad2a6143f..da1b2b1b4 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -231,8 +231,9 @@ namespace ignition /// \brief Calculate shortest distance between line and point /// \param[in] _pt Point which we are measuring distance to. + /// \param[in] _epsilon Tolerance below which assume this line is a point. /// \returns Distance from point to line. - public: T Distance(const Vector3 &_pt) + public: T Distance(const Vector3 &_pt, const T _epsilon = 1e-6) { auto line = this->pts[1] - this->pts[0]; auto ptTo0 = _pt - this->pts[0]; @@ -251,8 +252,13 @@ namespace ignition } // Distance to point projected onto line - auto d = (ptTo0).Cross(line); - return d.Length() / (line).Length(); + auto d = ptTo0.Cross(line); + + // If line has length zero assume its a point. + if(line.Length() <= _epsilon) + return ptTo0.Length(); + + return d.Length() / line.Length(); } /// \brief Check if this line intersects the given line segment. From e356258a8942fad3d6787ea54fc449e819d79e44 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 06:29:33 +0800 Subject: [PATCH 74/81] Reverting, because the dividee by zero will never occur Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index da1b2b1b4..b7158c130 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -231,9 +231,8 @@ namespace ignition /// \brief Calculate shortest distance between line and point /// \param[in] _pt Point which we are measuring distance to. - /// \param[in] _epsilon Tolerance below which assume this line is a point. /// \returns Distance from point to line. - public: T Distance(const Vector3 &_pt, const T _epsilon = 1e-6) + public: T Distance(const Vector3 &_pt) { auto line = this->pts[1] - this->pts[0]; auto ptTo0 = _pt - this->pts[0]; @@ -253,11 +252,6 @@ namespace ignition // Distance to point projected onto line auto d = ptTo0.Cross(line); - - // If line has length zero assume its a point. - if(line.Length() <= _epsilon) - return ptTo0.Length(); - return d.Length() / line.Length(); } From b06b483f12cc94f1da33fcbc1923606610fa2d51 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 06:39:15 +0800 Subject: [PATCH 75/81] Added comment Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index b7158c130..e98b18385 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -238,7 +238,7 @@ namespace ignition auto ptTo0 = _pt - this->pts[0]; auto ptTo1 = _pt - this->pts[1]; - // Point is projected beyond pt0 + // Point is projected beyond pt0 or the line has length 0 if (ptTo0.Dot(line) <= 0.0) { return ptTo0.Length(); @@ -251,6 +251,8 @@ namespace ignition } // Distance to point projected onto line + // line.Length() will have to be > 0 at this point otherwise it would + // return at line 244. auto d = ptTo0.Cross(line); return d.Length() / line.Length(); } From 91a6b43fb2ab9d385e49ad0b13a752c67a5dc64f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 06:42:44 +0800 Subject: [PATCH 76/81] Add test cases and assert Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 1 + src/Line3_TEST.cc | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index e98b18385..d53e7edb1 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -254,6 +254,7 @@ namespace ignition // line.Length() will have to be > 0 at this point otherwise it would // return at line 244. auto d = ptTo0.Cross(line); + assert(lineLength > 0); return d.Length() / line.Length(); } diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc index de2c53178..db50be5b6 100644 --- a/src/Line3_TEST.cc +++ b/src/Line3_TEST.cc @@ -290,6 +290,24 @@ TEST(Line3Test, DistanceToPoint) math::Vector3d point(-5, -3, -8); EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointB)); } + pointA.Set(0, 0, 0); + line.Set(pointA, pointA); + + // Point on the line + { + math::Vector3d point(0, 0, 0); + EXPECT_DOUBLE_EQ(line.Distance(point), 0.0); + } + + // Points projected beyond the line's ends + { + math::Vector3d point(2, 2, 3); + EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointA)); + } + { + math::Vector3d point(-5, -3, -8); + EXPECT_DOUBLE_EQ(line.Distance(point), point.Distance(pointA)); + } } ///////////////////////////////////////////////// From f547e7112905661fa03ffa5c5b874b5658a639ce Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 06:47:00 +0800 Subject: [PATCH 77/81] Whelps compile error. Signed-off-by: Arjo Chakravarty --- include/ignition/math/Line3.hh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index d53e7edb1..e927817b8 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -254,8 +254,9 @@ namespace ignition // line.Length() will have to be > 0 at this point otherwise it would // return at line 244. auto d = ptTo0.Cross(line); + auto lineLength = line.Length(); assert(lineLength > 0); - return d.Length() / line.Length(); + return d.Length() / lineLength; } /// \brief Check if this line intersects the given line segment. From 69b04481edfdfb16334116c876ffaca35f791c79 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 07:22:41 +0800 Subject: [PATCH 78/81] Add docs Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 88cd20b66..05f102ee1 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -122,10 +122,16 @@ T Box::Volume() const } ////////////////////////////////////////////////// +/// \brief Given a *convex* polygon described by the verices in a given plane, +/// compute the list of triangles which form this polygon. +/// \param[in] _plane The plane in which the vertices exist. +/// \param[in] _vertices The vertices of the polygon. +/// \return a list of triangles, or and empty vector if _vertices in the _plane +/// are less than 3. template std::vector, T>> TrianglesInPlane( const Plane &_plane, IntersectionPoints &_vertices) -{ +{ std::vector, T>> triangles; std::vector> pointsInPlane; @@ -188,7 +194,7 @@ T Box::VolumeBelow(const Plane &_plane) const auto intersections = this->Intersections(_plane); verticesBelow.merge(intersections); - // Fit six planes to the vertices in the shape. + // Reconstruct the cut-box as a triangle mesh by attempting to fit planes. std::vector, T>> triangles; std::vector> planes @@ -326,8 +332,8 @@ IntersectionPoints Box::Intersections( Vector3{0, 0, 1} }; - // There are 12 edges. However, we just need 4 points to describe the - // twelve edges. + // There are 12 edges, which are checked along 3 axes from 4 box corner + // points. for (auto &v : vertices) { for (auto &a : axes) From bacd9f9ca14b9166e9e4d94a0d8c2bd042f026ec Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 07:26:30 +0800 Subject: [PATCH 79/81] whitespaces Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 05f102ee1..803ecc1d6 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -123,7 +123,7 @@ T Box::Volume() const ////////////////////////////////////////////////// /// \brief Given a *convex* polygon described by the verices in a given plane, -/// compute the list of triangles which form this polygon. +/// compute the list of triangles which form this polygon. /// \param[in] _plane The plane in which the vertices exist. /// \param[in] _vertices The vertices of the polygon. /// \return a list of triangles, or and empty vector if _vertices in the _plane @@ -131,7 +131,7 @@ T Box::Volume() const template std::vector, T>> TrianglesInPlane( const Plane &_plane, IntersectionPoints &_vertices) -{ +{ std::vector, T>> triangles; std::vector> pointsInPlane; From 4c0bd0267e825b7e3251e74c5df7cc24d88cd77d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 07:35:10 +0800 Subject: [PATCH 80/81] More docs fixes Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 803ecc1d6..6801a5889 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -126,8 +126,8 @@ T Box::Volume() const /// compute the list of triangles which form this polygon. /// \param[in] _plane The plane in which the vertices exist. /// \param[in] _vertices The vertices of the polygon. -/// \return a list of triangles, or and empty vector if _vertices in the _plane -/// are less than 3. +/// \return A vector of triangles and their centroids, or an empty vector +/// if _vertices in the _plane are less than 3. template std::vector, T>> TrianglesInPlane( const Plane &_plane, IntersectionPoints &_vertices) @@ -192,6 +192,8 @@ T Box::VolumeBelow(const Plane &_plane) const return 0; auto intersections = this->Intersections(_plane); + // TODO(arjo): investigate the use of _epsilon tolerance as this method + // implicitly uses Vector3::operator==() verticesBelow.merge(intersections); // Reconstruct the cut-box as a triangle mesh by attempting to fit planes. From b4f9ddce0441145a3401581c5f707c43bf71e09e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 07:38:06 +0800 Subject: [PATCH 81/81] update scalar value Signed-off-by: Arjo Chakravarty --- include/ignition/math/detail/Box.hh | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 6801a5889..2ce5f0f9b 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -126,8 +126,9 @@ T Box::Volume() const /// compute the list of triangles which form this polygon. /// \param[in] _plane The plane in which the vertices exist. /// \param[in] _vertices The vertices of the polygon. -/// \return A vector of triangles and their centroids, or an empty vector -/// if _vertices in the _plane are less than 3. +/// \return A vector of triangles and their sign, or an empty vector +/// if _vertices in the _plane are less than 3. The sign will be +1 if the +/// triangle is outward facing, -1 otherwise. template std::vector, T>> TrianglesInPlane( const Plane &_plane, IntersectionPoints &_vertices)