From 46d5324c2053151702e59d7e36bf6982a1e3ad71 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 16 Sep 2021 11:34:49 -0700 Subject: [PATCH 01/11] =?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 --- 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 b54dce578d15ec24f2008b95d7b9d08f13ba5613 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 02/11] 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 --- 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 e311b3b704647f576b39249683991ddaa1be17d5 Mon Sep 17 00:00:00 2001 From: LolaSegura <48759425+LolaSegura@users.noreply.github.com> Date: Sat, 18 Sep 2021 02:09:45 -0300 Subject: [PATCH 03/11] Adds python interface to Filter, MovingWindowFilter, RotationSpline. (#230) * Adds python interface to Filter, MovingWindowFilter, RotationSpline. Signed-off-by: LolaSegura Co-authored-by: Steve Peters Co-authored-by: Franco Cipollone --- src/python/CMakeLists.txt | 3 + src/python/Filter.i | 104 ++++++++++++++++++++++++++ src/python/Filter_TEST.py | 98 ++++++++++++++++++++++++ src/python/MovingWindowFilter.i | 44 +++++++++++ src/python/MovingWindowFilter_TEST.py | 66 ++++++++++++++++ src/python/RotationSpline.i | 48 ++++++++++++ src/python/RotationSpline_TEST.py | 83 ++++++++++++++++++++ src/python/python.i | 3 + 8 files changed, 449 insertions(+) create mode 100644 src/python/Filter.i create mode 100644 src/python/Filter_TEST.py create mode 100644 src/python/MovingWindowFilter.i create mode 100644 src/python/MovingWindowFilter_TEST.py create mode 100644 src/python/RotationSpline.i create mode 100644 src/python/RotationSpline_TEST.py diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 390ed1e84..4081d9b77 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -89,18 +89,21 @@ if (PYTHONLIBS_FOUND) set(python_tests Angle_TEST Color_TEST + Filter_TEST GaussMarkovProcess_TEST Kmeans_TEST Line2_TEST Line3_TEST Matrix3_TEST Matrix4_TEST + MovingWindowFilter_TEST PID_TEST Pose3_TEST python_TEST Quaternion_TEST Rand_TEST RollingMean_TEST + RotationSpline_TEST SemanticVersion_TEST SignalStats_TEST Spline_TEST diff --git a/src/python/Filter.i b/src/python/Filter.i new file mode 100644 index 000000000..be9e70562 --- /dev/null +++ b/src/python/Filter.i @@ -0,0 +1,104 @@ +/* + * 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 filter +%{ +#include +#include +#include +#include +#include +%} + +%import Quaternion.i + +namespace ignition +{ +namespace math +{ + template + class Filter + { + public: virtual ~Filter(); + public: virtual void Set(const T &_val); + public: virtual void Fc(double _fc, double _fs) = 0; + public: virtual const T &Value() const; + }; + + %template(Filterd) Filter; + %template(Filterq) Filter>; + + template + class OnePole : public Filter + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: OnePole() = default; + public: OnePole(double _fc, double _fs); + public: virtual void Fc(double _fc, double _fs) override; + public: const T& Process(const T &_x); + }; + + + %template(OnePoled) OnePole; + + class OnePoleQuaternion + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual const Quaternion &Value() const; + public: OnePoleQuaternion(); + public: OnePoleQuaternion(double _fc, double _fs) + : OnePole>(_fc, _fs); + public: const Quaternion& Process( + const Quaternion &_x); + }; + + + class OnePoleVector3 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual const Vector3 &Value() const; + public: const Vector3& Process(const Vector3 &_x); + public: OnePoleVector3(); + public: OnePoleVector3(double _fc, double _fs) + : OnePole>(_fc, _fs); + }; + + template + class BiQuad : public Filter + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: BiQuad() = default; + public: BiQuad(double _fc, double _fs); + public: void Fc(double _fc, double _fs) override; + public: void Fc(double _fc, double _fs, double _q); + public: virtual void Set(const T &_val) override; + public: virtual const T& Process(const T &_x); + }; + + %template(BiQuadd) BiQuad; + + class BiQuadVector3 + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: virtual const Vector3& Process(const Vector3 &_x); + public: virtual const Vector3 &Value() const; + public: BiQuadVector3(); + public: BiQuadVector3(double _fc, double _fs) + : BiQuad>(_fc, _fs); + }; +} +} diff --git a/src/python/Filter_TEST.py b/src/python/Filter_TEST.py new file mode 100644 index 000000000..def7d9aa4 --- /dev/null +++ b/src/python/Filter_TEST.py @@ -0,0 +1,98 @@ +# 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 unittest +from ignition.math import BiQuadd +from ignition.math import BiQuadVector3 +from ignition.math import OnePoled +from ignition.math import OnePoleQuaternion +from ignition.math import OnePoleVector3 +from ignition.math import Quaterniond +from ignition.math import Vector3d + + +class TestFilter(unittest.TestCase): + + def test_one_pole(self): + filter_a = OnePoled() + self.assertAlmostEqual(filter_a.process(0.2), 0.0) + + filter_a.fc(0.6, 1.4) + self.assertAlmostEqual(filter_a.process(2.5), 2.3307710879153634) + + filter_b = OnePoled(0.1, 0.2) + self.assertAlmostEqual(filter_b.process(0.5), 0.47839304086811385) + + filter_b.set(5.4) + self.assertAlmostEqual(filter_b.value(), 5.4) + + def test_one_pole_quaternion(self): + filter_a = OnePoleQuaternion() + self.assertAlmostEqual(filter_a.value(), Quaterniond(1, 0, 0, 0)) + + filter_b = OnePoleQuaternion(0.4, 1.4) + self.assertAlmostEqual(filter_b.value(), Quaterniond(1, 0, 0, 0)) + + self.assertAlmostEqual(filter_a.process(Quaterniond(0.1, 0.2, 0.3)), + Quaterniond(1, 0, 0, 0)) + + self.assertAlmostEqual(filter_b.process(Quaterniond(0.1, 0.2, 0.3)), + Quaterniond(0.98841, 0.0286272, + 0.0885614, 0.119929)) + + def test_one_pole_vector3(self): + filter_a = OnePoleVector3() + self.assertAlmostEqual(filter_a.value(), Vector3d(0, 0, 0)) + + filter_b = OnePoleVector3(1.2, 3.4) + self.assertAlmostEqual(filter_b.value(), Vector3d(0, 0, 0)) + + self.assertAlmostEqual(filter_a.process(Vector3d(0.1, 0.2, 0.3)), + Vector3d(0, 0, 0)) + + self.assertAlmostEqual(filter_b.process(Vector3d(0.1, 0.2, 0.3)), + Vector3d(0.089113, 0.178226, 0.267339)) + + def test_biquad(self): + filter_a = BiQuadd() + self.assertAlmostEqual(filter_a.value(), 0.0, delta=1e-10) + self.assertAlmostEqual(filter_a.process(1.1), 0.0, delta=1e-10) + + filter_a.fc(0.3, 1.4) + self.assertAlmostEqual(filter_a.process(1.2), 0.66924691484768517) + + filter_a.fc(0.3, 1.4, 0.1) + self.assertAlmostEqual(filter_a.process(10.25), 0.96057152402651302) + + filter_b = BiQuadd(4.3, 10.6) + self.assertAlmostEqual(filter_b.value(), 0.0, delta=1e-10) + self.assertAlmostEqual(filter_b.process(0.1234), 0.072418159950486546) + + filter_b.set(4.5) + self.assertAlmostEqual(filter_b.value(), 4.5) + + def test_biquad_vector3(self): + filter_a = BiQuadVector3() + self.assertEqual(filter_a.value(), Vector3d(0, 0, 0)) + self.assertEqual(filter_a.process(Vector3d(1.1, 2.3, 3.4)), + Vector3d(0, 0, 0)) + + filter_b = BiQuadVector3(6.5, 22.4) + self.assertEqual(filter_b.value(), Vector3d(0, 0, 0)) + self.assertEqual(filter_b.process(Vector3d(0.1, 20.3, 33.45)), + Vector3d(0.031748, 6.44475, 10.6196)) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/MovingWindowFilter.i b/src/python/MovingWindowFilter.i new file mode 100644 index 000000000..ea6fc003d --- /dev/null +++ b/src/python/MovingWindowFilter.i @@ -0,0 +1,44 @@ +/* + * 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 movingwindowfilter +%{ +#include "ignition/math/MovingWindowFilter.hh" +#include "ignition/math/Vector3.hh" +%} + +namespace ignition +{ +namespace math +{ + template< typename T> + class MovingWindowFilter + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: MovingWindowFilter(); + public: virtual ~MovingWindowFilter(); + public: void Update(const T _val); + public: void SetWindowSize(const unsigned int _n); + public: unsigned int WindowSize() const; + public: bool WindowFilled() const; + public: T Value() const; + }; + + %template(MovingWindowFilteri) MovingWindowFilter; + %template(MovingWindowFilterd) MovingWindowFilter; + %template(MovingWindowFilterv3) MovingWindowFilter>; +} +} diff --git a/src/python/MovingWindowFilter_TEST.py b/src/python/MovingWindowFilter_TEST.py new file mode 100644 index 000000000..bf8bf691c --- /dev/null +++ b/src/python/MovingWindowFilter_TEST.py @@ -0,0 +1,66 @@ +# 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 unittest +from ignition.math import MovingWindowFilterd +from ignition.math import MovingWindowFilteri +from ignition.math import MovingWindowFilterv3 +from ignition.math import Vector3d + + +class TestFilter(unittest.TestCase): + + def test_set_window_size(self): + filter_int = MovingWindowFilteri() + + self.assertEqual(filter_int.window_size(), 4) + self.assertFalse(filter_int.window_filled()) + + filter_int.set_window_size(10) + self.assertEqual(filter_int.window_size(), 10) + self.assertFalse(filter_int.window_filled()) + + def test_filter_something(self): + double_mwf = MovingWindowFilterd() + double_mwf_2 = MovingWindowFilterd() + vector_mwf = MovingWindowFilterv3() + + double_mwf.set_window_size(10) + double_mwf_2.set_window_size(2) + vector_mwf.set_window_size(40) + + for i in range(20): + double_mwf.update(i) + double_mwf_2.update(i) + v = Vector3d(1.0*i, + 2.0*i, + 3.0*i) + vector_mwf.update(v) + + sum = 0 + for i in range(20-10, 20, 1): + sum += i + self.assertAlmostEqual(double_mwf.value(), sum/10.0) + self.assertAlmostEqual(double_mwf_2.value(), (18.0+19.0)/2.0) + + vsum = Vector3d() + for i in range(20): + vsum += Vector3d(1.0*i, + 2.0*i, + 3.0*i) + self.assertAlmostEqual(vector_mwf.value(), vsum / 20.0) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/RotationSpline.i b/src/python/RotationSpline.i new file mode 100644 index 000000000..7f62e815d --- /dev/null +++ b/src/python/RotationSpline.i @@ -0,0 +1,48 @@ +/* + * 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 rotationspline +%{ +#include +#include +#include +%} + +namespace ignition +{ +namespace math +{ + class RotationSpline + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: RotationSpline(); + public: ~RotationSpline(); + public: void AddPoint(const Quaternion &_p); + public: const Quaternion &Point(const unsigned int _index) const; + public: unsigned int PointCount() const; + public: void Clear(); + public: bool UpdatePoint(const unsigned int _index, + const Quaternion &_value); + public: Quaternion Interpolate(double _t, + const bool _useShortestPath = true); + public: Quaternion Interpolate(const unsigned int _fromIndex, + const double _t, const bool _useShortestPath = true); + public: void AutoCalculate(bool _autoCalc); + public: void RecalcTangents(); + }; +} +} diff --git a/src/python/RotationSpline_TEST.py b/src/python/RotationSpline_TEST.py new file mode 100644 index 000000000..598226157 --- /dev/null +++ b/src/python/RotationSpline_TEST.py @@ -0,0 +1,83 @@ +# 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 unittest +from ignition.math import Quaterniond +from ignition.math import RotationSpline +from ignition.math import Vector3d + + +class TestRotationSpline(unittest.TestCase): + def test_rotation_spline(self): + s = RotationSpline() + + s.add_point(Quaterniond(0, 0, 0)) + self.assertEqual(1, s.point_count()) + + s.clear() + self.assertEqual(0, s.point_count()) + + s.add_point(Quaterniond(0, 0, 0)) + self.assertTrue(s.point(0) == Quaterniond(0, 0, 0)) + s.add_point(Quaterniond(.1, .1, .1)) + self.assertTrue(s.point(1) == Quaterniond(.1, .1, .1)) + + # update_point + self.assertFalse(s.update_point(2, Quaterniond(.2, .2, .2))) + + self.assertTrue(s.update_point(1, Quaterniond(.2, .2, .2))) + s.auto_calculate(False) + self.assertTrue(s.update_point(0, Quaterniond( + Vector3d(-.1, -.1, -.1)))) + s.auto_calculate(True) + + # interpolate + self.assertTrue(s.interpolate(0.5) == + Quaterniond(0.998089, 0.0315333, 0.0427683, 0.0315333)) + + # interpolate + s.add_point(Quaterniond(.4, .4, .4)) + self.assertFalse(s.interpolate(4, 0.2).is_finite()) + + self.assertEqual(s.interpolate(s.point_count()-1, 0.2), + s.point(s.point_count()-1)) + self.assertTrue(s.interpolate(1, 0.2) == + Quaterniond(0.978787, 0.107618, 0.137159, 0.107618)) + self.assertEqual(s.interpolate(1, 0.0), s.point(1)) + self.assertEqual(s.interpolate(1, 1.0), s.point(2)) + + def test_get_point(self): + s = RotationSpline() + self.assertFalse(s.point(0).is_finite()) + self.assertFalse(s.point(1).is_finite()) + + s.add_point(Quaterniond(0, 0, 0)) + self.assertTrue(s.point(1).is_finite()) + + def test_recalc_tangents(self): + s = RotationSpline() + s.add_point(Quaterniond(0, 0, 0)) + s.add_point(Quaterniond(.4, .4, .4)) + s.add_point(Quaterniond(0, 0, 0)) + + s.recalc_tangents() + self.assertEqual(s.interpolate(0, 0.5), + Quaterniond(0.987225, 0.077057, 0.11624, 0.077057)) + + self.assertEqual(s.interpolate(1, 0.5), + Quaterniond(0.987225, 0.077057, 0.11624, 0.077057)) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/python.i b/src/python/python.i index 18a73da43..ff0046db6 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -12,8 +12,11 @@ %include Line3.i %include Matrix3.i %include Matrix4.i +%include Filter.i +%include MovingWindowFilter.i %include PID.i %include RollingMean.i +%include RotationSpline.i %include SemanticVersion.i %include SignalStats.i %include Spline.i From e431c4a8373313854b5100bb6e2b30654fff8384 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Sat, 18 Sep 2021 10:01:51 +0200 Subject: [PATCH 04/11] Use python executable variable instead of hardcode python3 (#239) * Use python interpreter instead of hardcode python3 * Use previous PythonInterp instead of Python3 Signed-off-by: Jose Luis Rivero * find python version 3 Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- CMakeLists.txt | 1 + src/python/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b2e302868..51af9b6ff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ if (SWIG_FOUND) ######################################## # Include python + find_package(PythonInterp 3 REQUIRED) # change to Python3 when Bionic is EOL find_package(PythonLibs QUIET) if (NOT PYTHONLIBS_FOUND) message (STATUS "Searching for Python - not found.") diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 4081d9b77..5fc25482a 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -117,7 +117,7 @@ if (PYTHONLIBS_FOUND) foreach (test ${python_tests}) add_test(NAME ${test}.py COMMAND - python3 ${CMAKE_SOURCE_DIR}/src/python/${test}.py) + "${PYTHON_EXECUTABLE}" "${CMAKE_SOURCE_DIR}/src/python/${test}.py") set(_env_vars) list(APPEND _env_vars "PYTHONPATH=${FAKE_INSTALL_PREFIX}/lib/python/") From 4af8a25cbd2553e939741d88a4c4e66f288fcfd3 Mon Sep 17 00:00:00 2001 From: Hill Ma Date: Mon, 20 Sep 2021 11:00:37 -0700 Subject: [PATCH 05/11] Fix Bazel build. (#242) Signed-off-by: Hill Ma --- eigen3/BUILD.bazel | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/eigen3/BUILD.bazel b/eigen3/BUILD.bazel index d4b1d043e..e9ffd8f70 100644 --- a/eigen3/BUILD.bazel +++ b/eigen3/BUILD.bazel @@ -10,9 +10,9 @@ package( licenses(["notice"]) -public_headers = [ - "include/ignition/math/eigen3/Conversions.hh", -] +public_headers = glob([ + "include/ignition/math/eigen3/*.hh", +]) cc_library( name = "eigen3", From 274378c7a763b0fc6b370629eb19043bca286839 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 22 Sep 2021 08:01:21 +0800 Subject: [PATCH 06/11] Volume below a plane for spheres and boxes (#219) Signed-off-by: Arjo Chakravarty Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel Co-authored-by: Steve Peters --- include/ignition/math/Box.hh | 41 ++- include/ignition/math/Line3.hh | 30 ++ include/ignition/math/Plane.hh | 49 +++ include/ignition/math/Sphere.hh | 18 + include/ignition/math/Vector3.hh | 2 +- include/ignition/math/detail/Box.hh | 219 +++++++++++ include/ignition/math/detail/Sphere.hh | 54 +++ .../ignition/math/detail/WellOrderedVector.hh | 63 ++++ src/Box_TEST.cc | 345 ++++++++++++++++++ src/Line3_TEST.cc | 80 ++++ src/Plane_TEST.cc | 61 ++++ src/Sphere_TEST.cc | 136 +++++++ src/Vector3_TEST.cc | 30 ++ 13 files changed, 1126 insertions(+), 2 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 57670f4f7..09a55ec59 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -20,15 +20,24 @@ #include #include #include +#include #include +#include "ignition/math/detail/WellOrderedVector.hh" + +#include + namespace ignition { namespace math { // 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. /// @@ -128,6 +137,28 @@ 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, 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 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: IntersectionPoints + 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 @@ -161,6 +192,14 @@ 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'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 + /// intersection occurs. + public: IntersectionPoints Intersections( + const Plane &_plane) const; + /// \brief Size of the box. private: Vector3 size = Vector3::Zero; diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index f8d73b53f..e927817b8 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -229,6 +229,36 @@ namespace ignition return true; } + /// \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) + { + 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 or the line has length 0 + 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 + // 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() / lineLength; + } + /// \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 b7ae61a7a..4994f8128 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -21,6 +21,9 @@ #include #include #include +#include +#include +#include namespace ignition { @@ -119,6 +122,52 @@ namespace ignition return this->normal.Dot(_point) - this->d; } + /// \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 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 + /// \return The point of intersection. std::nullopt if the line is + /// 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 (std::abs(this->Normal().Dot(_gradient)) < _tolerance) + { + return std::nullopt; + } + auto constant = this->Offset() - this->Normal().Dot(_point); + auto param = constant / this->Normal().Dot(_gradient); + auto intersection = _point + _gradient*param; + + 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()); + Quaternion rotation(axis, angle); + + Vector3 rotatedXAxis = rotation * Vector3::UnitX; + Vector3 rotatedYAxis = rotation * Vector3::UnitY; + + 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) + { + return intersection; + } + return std::nullopt; + } + /// \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..537bd109f 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,23 @@ 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, expressed + /// in the sphere's reference frame. + /// \return Volume below the sphere in m^3. + 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 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. Expressed in the sphere's reference + /// frame. + public: std::optional> + 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 /// function requires the sphere's radius to be set to a diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index af1dc71a0..6a3b5b1e9 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -264,7 +264,7 @@ namespace ignition return n.Normalize(); } - /// \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/Box.hh b/include/ignition/math/detail/Box.hh index de1a521da..2ce5f0f9b 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -16,6 +16,14 @@ */ #ifndef IGNITION_MATH_DETAIL_BOX_HH_ #define IGNITION_MATH_DETAIL_BOX_HH_ + +#include "ignition/math/Triangle3.hh" + +#include +#include +#include +#include + namespace ignition { namespace math @@ -113,6 +121,168 @@ T Box::Volume() const return this->size.X() * this->size.Y() * this->size.Z(); } +////////////////////////////////////////////////// +/// \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 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) +{ + std::vector, T>> triangles; + std::vector> pointsInPlane; + + Vector3 centroid; + for (const auto &pt : _vertices) + { + if (_plane.Side(pt) == Plane::NO_SIDE) + { + pointsInPlane.push_back(pt); + centroid += pt; + } + } + centroid /= T(pointsInPlane.size()); + + 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; + + // 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.Dot(bDisplacement) / axis1.Length(); + auto bY = axis2.Dot(bDisplacement) / axis2.Length(); + + return atan2(aY, aX) < atan2(bY, bX); + }); + for (std::size_t i = 0; i < pointsInPlane.size(); ++i) + { + triangles.emplace_back( + Triangle3(pointsInPlane[i], + pointsInPlane[(i + 1) % pointsInPlane.size()], centroid), + (_plane.Side({0, 0, 0}) == Plane::POSITIVE_SIDE) ? -1 : 1); + } + + return triangles; +} + +///////////////////////////////////////////////// +template +T Box::VolumeBelow(const Plane &_plane) const +{ + auto verticesBelow = this->VerticesBelow(_plane); + if (verticesBelow.empty()) + 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. + 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{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 + }; + + for (const auto &p : planes) + { + auto newTriangles = TrianglesInPlane(p, verticesBelow); + triangles.insert(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 (const auto &triangle : triangles) + { + auto crossProduct = (triangle.first[2]).Cross(triangle.first[1]); + auto meshVolume = std::abs(crossProduct.Dot(triangle.first[0])); + volume += triangle.second * meshVolume; + } + + return std::abs(volume)/6; +} + +///////////////////////////////////////////////// +template +std::optional> + Box::CenterOfVolumeBelow(const Plane &_plane) const +{ + auto verticesBelow = this->VerticesBelow(_plane); + if (verticesBelow.empty()) + 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 +IntersectionPoints Box::VerticesBelow(const Plane &_plane) const +{ + // Get coordinates of all vertice of box + // TODO(arjo): Cache this for performance + 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}, + 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} + }; + + IntersectionPoints verticesBelow; + for (const auto &v : vertices) + { + if (_plane.Distance(v) <= 0) + { + verticesBelow.insert(v); + } + } + + return verticesBelow; +} + ///////////////////////////////////////////////// template T Box::DensityFromMass(const T _mass) const @@ -139,6 +309,55 @@ bool Box::MassMatrix(MassMatrix3 &_massMat) const { return _massMat.SetFromBox(this->material, this->size); } + + +////////////////////////////////////////////////// +template +IntersectionPoints Box::Intersections( + const Plane &_plane) const +{ + IntersectionPoints intersections; + // These are vertices 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} + }; + + // Axes + std::vector> axes + { + Vector3{1, 0, 0}, + Vector3{0, 1, 0}, + Vector3{0, 0, 1} + }; + + // There are 12 edges, which are checked along 3 axes from 4 box corner + // points. + for (auto &v : vertices) + { + 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) + { + intersections.insert(intersection.value()); + } + } + } + + return intersections; +} + } } #endif diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 6c85c197e..740ee6741 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -95,6 +95,60 @@ T Sphere::Volume() const return (4.0/3.0) * IGN_PI * std::pow(this->radius, 3); } +////////////////////////////////////////////////// +template +T Sphere::VolumeBelow(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) + { + // sphere is below plane. + return Volume(); + } + else if (dist > r) + { + // sphere is completely above plane + return 0.0; + } + + auto h = r - dist; + return IGN_PI * h * h * (3 * r - h) / 3; +} + +////////////////////////////////////////////////// +template +std::optional> + 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) + { + // sphere is completely below plane + return Vector3{0, 0, 0}; + } + else if (dist > r) + { + // sphere is completely above plane + 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; + + auto z = 3 * numerator * numerator / (4 * (3 * r - h)); + return - z * _plane.Normal().Normalized(); +} + ////////////////////////////////////////////////// template bool Sphere::SetDensityFromMass(const T _mass) 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 c15452f7c..454abf4cb 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -113,6 +113,351 @@ TEST(BoxTest, VolumeAndDensity) EXPECT_GT(0.0, box2.DensityFromMass(mass)); } +////////////////////////////////////////////////// +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); + + auto intersections = box.Intersections(plane); + ASSERT_EQ(4UL, intersections.size()); + 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 + { + 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()); + 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 + { + 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()); + 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 + { + 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()); + 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 + // 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()); + 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); + } +} + +////////////////////////////////////////////////// +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, 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 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); + EXPECT_TRUE(box.CenterOfVolumeBelow(plane).has_value()); + 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); + 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)); + } +} + +////////////////////////////////////////////////// +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()); + + 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); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(4u, vertices.size()); + + 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); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(4u, vertices.size()); + + 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); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(4u, vertices.size()); + + 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 + { + math::Planed plane(math::Vector3d(-1, -1, 0), 0.3); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(6u, vertices.size()); + + 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); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(6u, vertices.size()); + + 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 + { + math::Planed plane(math::Vector3d(-1, -1, 0), -0.5); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(2u, vertices.size()); + + EXPECT_EQ(vertices.count(pXpYnZ), 1UL); + EXPECT_EQ(vertices.count(pXpYpZ), 1UL); + } + // 7 vertices + { + math::Planed plane(math::Vector3d(1, 1, 1), 1.0); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(7u, vertices.size()); + + 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 + { + math::Planed plane(math::Vector3d(1, 1, 1), -1.2); + + auto vertices = box.VerticesBelow(plane); + ASSERT_EQ(1u, vertices.size()); + + EXPECT_EQ(vertices.count(nXnYnZ), 1UL); + } +} + ////////////////////////////////////////////////// TEST(BoxTest, Mass) { diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc index 05a060573..db50be5b6 100644 --- a/src/Line3_TEST.cc +++ b/src/Line3_TEST.cc @@ -230,6 +230,86 @@ TEST(Line3Test, Distance) EXPECT_FALSE(line.Distance(math::Line3d(2, 0, 0, 2, 1, 0), result)); } +///////////////////////////////////////////////// +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_DOUBLE_EQ(line.Distance(point), point.Length()); + } + + // 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(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)); + } +} + ///////////////////////////////////////////////// TEST(Line3Test, Intersect) { diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index 4defeb839..e082d2c26 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -150,3 +150,64 @@ 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.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.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.Intersection(Vector3d(1, 1, 0), Vector3d(-1, -1, 0)); + 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 = + 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()); + } +} diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index aa9143eee..cfb069c87 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -131,3 +131,139 @@ TEST(SphereTest, Mass) EXPECT_EQ(expectedMassMat, massMat); EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass()); } + +////////////////////////////////////////////////// +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); + } + + // 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); + } + + // 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); + } + + // Vertical plane + { + math::Planed _plane(math::Vector3d{1, 0, 0}, 0); + EXPECT_NEAR(sphere.Volume() / 2, 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); + + // 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()); + } + + { + // 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 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()); + } + + { + // 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 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()); + } + + { + // 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, 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()); + } + + { + // 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), + math::Vector3d(0, 0, 0) + ); + } +} diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc index 9076b1099..9a4615aa1 100644 --- a/src/Vector3_TEST.cc +++ b/src/Vector3_TEST.cc @@ -504,3 +504,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 f3aeb2753745e464000655f8514b2d9f916b48c9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 21 Sep 2021 17:32:41 -0700 Subject: [PATCH 07/11] =?UTF-8?q?=F0=9F=8E=88=206.9.0~pre1=20(#245)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- CMakeLists.txt | 4 ++-- Changelog.md | 63 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 51af9b6ff..d938b1dba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-math6 VERSION 6.8.0) +project(ignition-math6 VERSION 6.9.0) #============================================================================ # Find ignition-cmake @@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.8.0 REQUIRED) # Configure the project #============================================================================ set (c++standard 17) -ign_configure_project(VERSION_SUFFIX) +ign_configure_project(VERSION_SUFFIX pre1) #============================================================================ # Set project-specific options diff --git a/Changelog.md b/Changelog.md index 8cad86da6..77386e3b9 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,69 @@ ## Ignition Math 6.x.x +## Ignition Math 6.9.0 (2021-09-XX) + +1. Volume below a plane for spheres and boxes + * [Pull request #219](https://github.com/ignitionrobotics/ign-math/pull/219) + +1. 🌐 Spherical coordinates: bug fix, docs and sanity checks + * [Pull request #235](https://github.com/ignitionrobotics/ign-math/pull/235) + +1. Add Vector(2|3|4)::NaN to easily create invalid vectors + * [Pull request #222](https://github.com/ignitionrobotics/ign-math/pull/222) + +1. Add options to install python/ruby in system standard paths + * [Pull request #236](https://github.com/ignitionrobotics/ign-math/pull/236) + +1. Add eigen utils to convert mesh 3d vertices to oriented box + * [Pull request #224](https://github.com/ignitionrobotics/ign-math/pull/224) + +1. Python interface + + 1. Adds python interface to RollingMean, Color and Spline + * [Pull request #218](https://github.com/ignitionrobotics/ign-math/pull/218) + + 1. Adds python interface for Kmeans and Vector3Stats + * [Pull request #232](https://github.com/ignitionrobotics/ign-math/pull/232) + + 1. Adds python interface to PID and SemanticVersion. + * [Pull request #229](https://github.com/ignitionrobotics/ign-math/pull/229) + + 1. Adds python interface to triangle. + * [Pull request #231](https://github.com/ignitionrobotics/ign-math/pull/231) + + 1. Adds Line2, Line3, SignalStats, Temperature python interface + * [Pull request #220](https://github.com/ignitionrobotics/ign-math/pull/220) + + 1. Python interface: Renames methods to match PEP8 style + * [Pull request #226](https://github.com/ignitionrobotics/ign-math/pull/226) + + 1. Adds python interface to Filter, MovingWindowFilter, RotationSpline. + * [Pull request #230](https://github.com/ignitionrobotics/ign-math/pull/230) + + 1. Adds python interface to Quaternion, Pose3, Matrix3 and Matrix4 + * [Pull request #221](https://github.com/ignitionrobotics/ign-math/pull/221) + + 1. Basic setup for Python interface using SWIG + * [Pull request #216](https://github.com/ignitionrobotics/ign-math/pull/216) + * [Pull request #223](https://github.com/ignitionrobotics/ign-math/pull/223) + * [Pull request #208](https://github.com/ignitionrobotics/ign-math/pull/208) + * [Pull request #239](https://github.com/ignitionrobotics/ign-math/pull/239) + +1. πŸ‘©β€πŸŒΎ Don't use std::pow with integers in Vectors and handle sqrt + * [Pull request #207](https://github.com/ignitionrobotics/ign-math/pull/207) + +1. Relax expectations about zero in SpeedLimiter_TEST to make ARM happy + * [Pull request #204](https://github.com/ignitionrobotics/ign-math/pull/204) + +1. Infrastructure + * [Pull request #242](https://github.com/ignitionrobotics/ign-math/pull/242) + * [Pull request #217](https://github.com/ignitionrobotics/ign-math/pull/217) + * [Pull request #211](https://github.com/ignitionrobotics/ign-math/pull/211) + * [Pull request #209](https://github.com/ignitionrobotics/ign-math/pull/209) + * [Pull request #227](https://github.com/ignitionrobotics/ign-math/pull/227) + * [Pull request #225](https://github.com/ignitionrobotics/ign-math/pull/225) + ## Ignition Math 6.8.0 (2021-03-30) 1. Add speed limiter class From e2f3870f9cf82d85bc70793519eb4484df1730d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Sat, 25 Sep 2021 00:14:48 +0200 Subject: [PATCH 08/11] Add MACOS install instructions (#252) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add MACOS install instructions Signed-off-by: Alejandro Hernández * Improve instructions Signed-off-by: Alejandro Hernández --- tutorials/installation.md | 47 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/tutorials/installation.md b/tutorials/installation.md index 323638601..6eab5e021 100644 --- a/tutorials/installation.md +++ b/tutorials/installation.md @@ -32,6 +32,22 @@ sudo apt install libignition-math<#>-dev Be sure to replace `<#>` with a number value, such as 1 or 2, depending on which version you need. +### macOS + +On macOS, add OSRF packages: + ``` + ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)" + brew tap osrf/simulation + ``` + +Install Ignition Math: + ``` + brew install ignition-math<#> + ``` + +Be sure to replace `<#>` with a number value, such as 1 or 2, depending on +which version you need. + ## Windows Install [Conda package management system](https://docs.conda.io/projects/conda/en/latest/user-guide/install/download.html). @@ -126,6 +142,36 @@ The optional Eigen component of Ignition Math requires: sudo make install ``` +### macOS + +1. Clone the repository + ``` + git clone https://github.com/ignitionrobotics/ign-math -b ign-math<#> + ``` + Be sure to replace `<#>` with a number value, such as 1 or 2, depending on + which version you need. + +2. Install dependencies + ``` + brew install --only-dependencies ignition-math<#> + ``` + Be sure to replace `<#>` with a number value, such as 1 or 2, depending on + which version you need. + +3. Configure and build + ``` + cd ign-math + mkdir build + cd build + cmake .. + make + ``` + +4. Optionally, install + ``` + sudo make install + ``` + ### Windows 1. Navigate to `condabin` if necessary to use the `conda` command (i.e., if Conda is not in your `PATH` environment variable. You can find the location of `condabin` in Anaconda Prompt, `where conda`). @@ -229,4 +275,3 @@ The interfaces and Ruby test codes are in the `src` folder. To use a C++ class i ``` ctest -R Ruby_TEST.rb ``` - From 65a7c249fb1d0c1a4b09880759b8a41ae9dae9b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 27 Sep 2021 19:31:38 +0200 Subject: [PATCH 09/11] Readme Folder Structure (#253) Signed-off-by: ahcorde --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index b393a03f0..613afd075 100644 --- a/README.md +++ b/README.md @@ -64,6 +64,8 @@ ign-math β”œβ”€β”€ include/ignition/math Header files. β”œβ”€β”€ src Source files and unit tests. β”‚Β Β  └── graph Source files for the graph classes. +β”‚Β Β  └── python SWIG Python interfaces. +β”‚Β Β  └── ruby SWIG Ruby interfaces. β”œβ”€β”€ eigen3 Files for Eigen component. β”œβ”€β”€ test β”‚ β”œβ”€β”€ integration Integration tests. From ac567b5fa6487c835a83e2fecb3ed765b7fa33d1 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 29 Sep 2021 14:19:46 -0700 Subject: [PATCH 10/11] =?UTF-8?q?=F0=9F=8E=88=206.9.0=20(#254)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d938b1dba..4e5012744 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.8.0 REQUIRED) # Configure the project #============================================================================ set (c++standard 17) -ign_configure_project(VERSION_SUFFIX pre1) +ign_configure_project(VERSION_SUFFIX) #============================================================================ # Set project-specific options diff --git a/Changelog.md b/Changelog.md index 77386e3b9..3bdbd14ea 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,7 +2,7 @@ ## Ignition Math 6.x.x -## Ignition Math 6.9.0 (2021-09-XX) +## Ignition Math 6.9.0 (2021-09-28) 1. Volume below a plane for spheres and boxes * [Pull request #219](https://github.com/ignitionrobotics/ign-math/pull/219) @@ -64,6 +64,8 @@ * [Pull request #209](https://github.com/ignitionrobotics/ign-math/pull/209) * [Pull request #227](https://github.com/ignitionrobotics/ign-math/pull/227) * [Pull request #225](https://github.com/ignitionrobotics/ign-math/pull/225) + * [Pull request #252](https://github.com/ignitionrobotics/ign-math/pull/252) + * [Pull request #253](https://github.com/ignitionrobotics/ign-math/pull/253) ## Ignition Math 6.8.0 (2021-03-30) From 237fab5dc7372c4f9ad98c1a5cdccafd64022a09 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 27 Dec 2021 17:08:56 -0800 Subject: [PATCH 11/11] Remove deprecated functions from python SWIG API (#344) Signed-off-by: Louise Poubel Signed-off-by: Steve Peters --- src/python/Matrix3.i | 10 ++--- src/python/Matrix3_TEST.py | 26 ++++++------ src/python/Pose3.i | 13 +++--- src/python/Pose3_TEST.py | 56 ++++++++++++++------------ src/python/Quaternion.i | 50 +++++++++++------------ src/python/Quaternion_TEST.py | 74 +++++++++++++++++------------------ src/python/Vector3.i | 4 +- 7 files changed, 120 insertions(+), 113 deletions(-) diff --git a/src/python/Matrix3.i b/src/python/Matrix3.i index 5aa049134..41ecc947e 100644 --- a/src/python/Matrix3.i +++ b/src/python/Matrix3.i @@ -49,13 +49,13 @@ namespace ignition 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, + public: void SetAxes(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: void SetFromAxisAngle(const Vector3 &_axis, T _angle); + %rename(set_from_2_axes) SetFrom2Axes; + public: void SetFrom2Axes(const Vector3 &_v1, const Vector3 &_v2); + public: void SetCol(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; diff --git a/src/python/Matrix3_TEST.py b/src/python/Matrix3_TEST.py index 22941264f..5c0ec279a 100644 --- a/src/python/Matrix3_TEST.py +++ b/src/python/Matrix3_TEST.py @@ -32,17 +32,17 @@ def test_matrix3d(self): 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), + matrix.set_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) + matrix.set_from_axis_angle(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)) + matrix.set_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)) + matrix.set_col(3, Vector3d(1, 1, 1)) self.assertAlmostEqual(matrix, Matrix3d(3, 2, 1, 4, 1, 1, 5, 2, 1)) def test_sub(self): @@ -240,10 +240,10 @@ def test_from_2axes(self): v2 = Vector3d(0.0, 1.0, 0.0) m1 = Matrix3d() - m1.from_2_axes(v1, v2) + m1.set_from_2_axes(v1, v2) m2 = Matrix3d() - m2.from_2_axes(v2, v1) + m2.set_from_2_axes(v2, v1) m1Correct = Matrix3d(0, -1, 0, 1, 0, 0, @@ -261,7 +261,7 @@ def test_from_2axes(self): # 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) + m2.set_from_2_axes(v1, v2) # m1 is 90 degrees rotation self.assertAlmostEqual(m1, m2*m2) @@ -269,8 +269,8 @@ def test_from_2axes(self): 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) + m1.set_from_2_axes(v1, v2) + m2.set_from_2_axes(v2, v1) self.assertNotEqual(m1, m2) self.assertAlmostEqual(m1Correct, m1) @@ -282,25 +282,25 @@ def test_from_2axes(self): # 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) + m1.set_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) + m1.set_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) + m1.set_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) + m1.set_from_2_axes(v1, v2) self.assertAlmostEqual(Matrix3d.ZERO - Matrix3d.IDENTITY, m1) def test_to_quaternion(self): diff --git a/src/python/Pose3.i b/src/python/Pose3.i index 5b94579ba..554926704 100644 --- a/src/python/Pose3.i +++ b/src/python/Pose3.i @@ -36,19 +36,18 @@ namespace ignition { %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() = default; 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: Pose3(const Pose3 &_pose) = default; + public: ~Pose3() = default; 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); @@ -74,6 +73,7 @@ namespace ignition public: void Reset(); public: Pose3 RotatePositionAboutOrigin(const Quaternion &_q) const; public: void Round(int _precision); + public: inline const Vector3 &Pos() const; public: inline Vector3 &Pos(); public: inline const T X() const; public: inline void SetX(T x); @@ -81,10 +81,12 @@ namespace ignition public: inline void SetY(T y); public: inline const T Z() const; public: inline void SetZ(T z); + public: inline const Quaternion &Rot() const; public: inline Quaternion &Rot(); public: inline const T Roll() const; public: inline const T Pitch() const; public: inline const T Yaw() const; + public: bool Equal(const Pose3 &_p, const T &_tol) const; }; %extend Pose3 { @@ -95,7 +97,6 @@ namespace ignition } } - %template(Pose3i) Pose3; %template(Pose3d) Pose3; %template(Pose3f) Pose3; } diff --git a/src/python/Pose3_TEST.py b/src/python/Pose3_TEST.py index 7a8451b69..2cb5eeed0 100644 --- a/src/python/Pose3_TEST.py +++ b/src/python/Pose3_TEST.py @@ -41,31 +41,31 @@ def test_pose(self): # 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) + self.assertAlmostEqual((B + A).x(), 1.0 + 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B + A).z(), 0.0) + self.assertAlmostEqual((B + A).roll(), 0.0) + self.assertAlmostEqual((B + A).pitch(), 0.0) + self.assertAlmostEqual((B + A).yaw(), 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) + self.assertAlmostEqual((Pose3d() - A).x(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((Pose3d() - A).z(), 0.0) + self.assertAlmostEqual((Pose3d() - A).roll(), 0.0) + self.assertAlmostEqual((Pose3d() - A).pitch(), 0.0) + self.assertAlmostEqual((Pose3d() - A).yaw(), -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) + self.assertAlmostEqual((-A).x(), -1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((-A).z(), 0.0) + self.assertAlmostEqual((-A).roll(), 0.0) + self.assertAlmostEqual((-A).pitch(), 0.0) + self.assertAlmostEqual((-A).yaw(), -math.pi/4.0) # If: # A is the transform from O to P in frame O @@ -73,12 +73,12 @@ def test_pose(self): # 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) + self.assertAlmostEqual((B - A).x(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).y(), 1.0/math.sqrt(2)) + self.assertAlmostEqual((B - A).z(), 0.0) + self.assertAlmostEqual((B - A).roll(), 0.0) + self.assertAlmostEqual((B - A).pitch(), 0.0) + self.assertAlmostEqual((B - A).yaw(), math.pi/4.0) pose = Pose3d() self.assertTrue(pose.pos() == Vector3d(0, 0, 0)) @@ -113,7 +113,7 @@ def test_pose(self): self.assertTrue(pose == Pose3d(0, 0, 0, 0, 0, 0)) pose.pos().set(5, 6, 7) - pose.rot().euler(Vector3d(.4, .6, 0)) + pose.rot().set_from_euler(Vector3d(.4, .6, 0)) self.assertTrue(pose.coord_position_add(Vector3d(1, 2, 3)) == Vector3d(7.82531, 6.67387, 9.35871)) @@ -158,11 +158,17 @@ def test_mutable_pose(self): def test_pose_elements(self): pose = Pose3d(0, 1, 2, 1, 1, 2) self.assertAlmostEqual(pose.x(), 0) + self.assertAlmostEqual(pose.x(), pose.pos().x()) self.assertAlmostEqual(pose.y(), 1) + self.assertAlmostEqual(pose.y(), pose.pos().y()) self.assertAlmostEqual(pose.z(), 2) + self.assertAlmostEqual(pose.z(), pose.pos().z()) self.assertAlmostEqual(pose.roll(), 1) + self.assertAlmostEqual(pose.roll(), pose.rot().euler().x()) self.assertAlmostEqual(pose.pitch(), 1) + self.assertAlmostEqual(pose.pitch(), pose.rot().euler().y()) self.assertAlmostEqual(pose.yaw(), 2) + self.assertAlmostEqual(pose.yaw(), pose.rot().euler().z()) def test_set_pose_element(self): pose = Pose3d(1, 2, 3, 1.57, 1, 2) diff --git a/src/python/Quaternion.i b/src/python/Quaternion.i index 0bc6d70bf..7cc6eba07 100644 --- a/src/python/Quaternion.i +++ b/src/python/Quaternion.i @@ -27,7 +27,7 @@ %inline %{ template - struct ToAxisOutput { + struct AxisAngleOutput { ignition::math::Vector3 axis; D angle; }; @@ -46,32 +46,34 @@ namespace ignition 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() + : qw(1), qx(0), qy(0), qz(0); + public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z) + : qw(_w), qx(_x), qy(_y), qz(_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: Quaternion(const Quaternion &_qt) = default; + public: ~Quaternion() = default; 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 SetFromAxisAngle(T _ax, T _ay, T _az, T _aa); + public: void SetFromAxisAngle(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: void SetFromEuler(const Vector3 &_vec); + public: void SetFromEuler(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); + %rename(set_from_2_axes) SetFrom2Axes; + public: void SetFrom2Axes(const Vector3 &_v1, const Vector3 &_v2); public: void Scale(T _scale); public: Quaternion operator+(const Quaternion &_qt) const; public: Quaternion operator+=(const Quaternion &_qt); @@ -107,24 +109,24 @@ namespace ignition 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); + public: inline void SetX(T _v); + public: inline void SetY(T _v); + public: inline void SetZ(T _v); + public: inline void SetW(T _v); %pythoncode %{ - def to_axis(self): - to_axis_output = self._to_axis() - return [to_axis_output.axis, to_axis_output.angle] + def axis_angle(self): + axis_angle_output = self._axis_angle() + return [axis_angle_output.axis, axis_angle_output.angle] %} }; %extend Quaternion{ - inline ToAxisOutput _to_axis() { + inline AxisAngleOutput _axis_angle() { ignition::math::Vector3 axis; T angle; - (*$self).ToAxis(axis, angle); - ToAxisOutput output; + (*$self).AxisAngle(axis, angle); + AxisAngleOutput output; output.axis = axis; output.angle = angle; return output; @@ -161,11 +163,9 @@ namespace ignition } } - %template(Quaternioni) Quaternion; %template(Quaterniond) Quaternion; %template(Quaternionf) Quaternion; } } - %template(ToAxisOutputi) ToAxisOutput; - %template(ToAxisOutputd) ToAxisOutput; - %template(ToAxisOutputf) ToAxisOutput; + %template(AxisAngleOutputd) AxisAngleOutput; + %template(AxisAngleOutputf) AxisAngleOutput; diff --git a/src/python/Quaternion_TEST.py b/src/python/Quaternion_TEST.py index c99ffc07f..7cfe23e1a 100644 --- a/src/python/Quaternion_TEST.py +++ b/src/python/Quaternion_TEST.py @@ -18,7 +18,6 @@ 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 @@ -100,12 +99,6 @@ def test_equal(self): 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) @@ -119,7 +112,7 @@ def test_mathlog(self): Quaterniond(0, -1.02593, 0.162491, 1.02593)) q1 = Quaterniond(q) - q1.w(2.0) + q1.set_w(2.0) self.assertAlmostEqual(q1.log(), Quaterniond(0, -0.698401, 0.110616, 0.698401)) @@ -129,10 +122,10 @@ def test_math_exp(self): 0.093284, 0.588972)) q1 = Quaterniond(q) - q1.x(0.000000001) - q1.y(0.0) - q1.z(0.0) - q1.w(0.0) + q1.set_x(0.000000001) + q1.set_y(0.0) + q1.set_z(0.0) + q1.set_w(0.0) self.assertAlmostEqual(q1.exp(), Quaterniond(1, 0, 0, 0)) def test_math_invert(self): @@ -145,10 +138,10 @@ def test_math_invert(self): def test_math_axis(self): q = Quaterniond(math.pi*0.1, math.pi*0.5, math.pi) - q.axis(0, 1, 0, math.pi) + q.set_from_axis_angle(0, 1, 0, math.pi) self.assertAlmostEqual(q, Quaterniond(6.12303e-17, 0, 1, 0)) - q.axis(Vector3d(1, 0, 0), math.pi) + q.set_from_axis_angle(Vector3d(1, 0, 0), math.pi) self.assertAlmostEqual(q, Quaterniond(0, 1, 0, 0)) def test_math_set(self): @@ -187,9 +180,11 @@ def test_math(self): 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) + axis, angle = q.axis_angle() + # TODO(chapulina) Revisit this test after migrating to pybind11 + # self.assertAlmostEqual(axis.x(), 0.371391, delta=1e-3) + # self.assertAlmostEqual(axis.y(), 0.557086, delta=1e-3) + # self.assertAlmostEqual(axis.z(), 0.742781, delta=1e-3) self.assertAlmostEqual(angle, 2.77438, delta=1e-3) q.scale(0.1) @@ -241,26 +236,29 @@ def test_math(self): 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.set_x(0.0) + q.set_y(0.0) + q.set_z(0.0) + q.set_w(0.0) q.normalize() self.assertTrue(q == Quaterniond()) - q.axis(0, 0, 0, 0) + q.set_from_axis_angle(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) + # axis_angle() method + q.set_x(0.0) + q.set_y(0.0) + q.set_z(0.0) + q.set_w(0.0) + axis, angle = q.axis_angle() + # TODO(chapulina) Revisit this test after migrating to pybind11 + # self.assertAlmostEqual(axis.x(), 1.0, delta=1e-3) + # self.assertAlmostEqual(axis.y(), 0.0, delta=1e-3) + # self.assertAlmostEqual(axis.z(), 0.0, delta=1e-3) self.assertAlmostEqual(angle, 0., delta=1e-3) # simple 180 rotation about yaw, @@ -307,7 +305,7 @@ def test_math(self): # 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) + q.set_from_axis_angle(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() == @@ -376,10 +374,10 @@ def test_from_2_axes(self): v2 = Vector3d(0.0, 1.0, 0.0) q1 = Quaterniond() - q1.from_2_axes(v1, v2) + q1.set_from_2_axes(v1, v2) q2 = Quaterniond() - q2.from_2_axes(v2, v1) + q2.set_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) @@ -395,8 +393,8 @@ def test_from_2_axes(self): 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) + q1.set_from_2_axes(v1, v2) + q2.set_from_2_axes(v2, v1) self.assertNotEqual(q1, q2) self.assertAlmostEqual(q1Correct, q1) @@ -410,7 +408,7 @@ def test_from_2_axes(self): v1.set(1, 0, 0) v2.set(-1, 0, 0) - q1.from_2_axes(v1, v2) + q1.set_from_2_axes(v1, v2) q2 = q1 * q1 self.assertTrue(abs(q2.w()-1.0) <= tolerance or abs(q2.w()-(-1.0)) <= tolerance) @@ -420,7 +418,7 @@ def test_from_2_axes(self): v1.set(0, 1, 0) v2.set(0, -1, 0) - q1.from_2_axes(v1, v2) + q1.set_from_2_axes(v1, v2) q2 = q1 * q1 self.assertTrue(abs(q2.w()-1.0) <= tolerance or abs(q2.w()-(-1.0)) <= tolerance) @@ -430,7 +428,7 @@ def test_from_2_axes(self): v1.set(0, 0, 1) v2.set(0, 0, -1) - q1.from_2_axes(v1, v2) + q1.set_from_2_axes(v1, v2) q2 = q1 * q1 self.assertTrue(abs(q2.w()-1.0) <= tolerance or abs(q2.w()-(-1.0)) <= tolerance) @@ -440,7 +438,7 @@ def test_from_2_axes(self): v1.set(0, 1, 1) v2.set(0, -1, -1) - q1.from_2_axes(v1, v2) + q1.set_from_2_axes(v1, v2) q2 = q1 * q1 self.assertTrue(abs(q2.w()-1.0) <= tolerance or abs(q2.w()-(-1.0)) <= tolerance) diff --git a/src/python/Vector3.i b/src/python/Vector3.i index 751350fb7..6e30c64aa 100644 --- a/src/python/Vector3.i +++ b/src/python/Vector3.i @@ -47,7 +47,7 @@ namespace ignition public: Vector3(); public: Vector3(const T &_x, const T &_y, const T &_z); public: Vector3(const Vector3 &_v); - public: virtual ~Vector3(); + public: ~Vector3() = default; public: T Sum() const; public: T Distance(const Vector3 &_pt) const; public: T Distance(T _x, T _y, T _z) const; @@ -70,6 +70,8 @@ namespace ignition public: void Min(const Vector3 &_v); public: T Max() const; public: T Min() const; + public: T MaxAbs() const; + public: T MinAbs() const; public: Vector3 operator+(const Vector3 &_v) const; public: inline Vector3 operator+(const T _s) const; public: inline Vector3 operator-() const;