diff --git a/include/ignition/math/Pose3.hh b/include/ignition/math/Pose3.hh index 247bb7054..dc17169e8 100644 --- a/include/ignition/math/Pose3.hh +++ b/include/ignition/math/Pose3.hh @@ -20,6 +20,7 @@ #include #include #include +#include namespace ignition { @@ -183,7 +184,7 @@ namespace ignition /// then, B + A is the transform from O to Q specified in frame O /// \param[in] _pose Pose3 to add to this pose. /// \return The resulting pose. - public: Pose3 operator+(const Pose3 &_pose) const + public: Pose3 IGN_DEPRECATED(7) operator+(const Pose3 &_pose) const { Pose3 result; @@ -197,7 +198,8 @@ namespace ignition /// \param[in] _pose Pose3 to add to this pose. /// \sa operator+(const Pose3 &_pose) const. /// \return The resulting pose. - public: const Pose3 &operator+=(const Pose3 &_pose) + public: const Pose3 & IGN_DEPRECATED(7) + operator+=(const Pose3 &_pose) { this->p = this->CoordPositionAdd(_pose); this->q = this->CoordRotationAdd(_pose.q); diff --git a/src/Pose_TEST.cc b/src/Pose_TEST.cc index 978c85dfb..3532fb16e 100644 --- a/src/Pose_TEST.cc +++ b/src/Pose_TEST.cc @@ -61,17 +61,22 @@ TEST(PoseTest, Pose) // 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 + // then, A * B is the transform from O to Q specified in frame O math::Pose3d A(math::Vector3d(1, 0, 0), math::Quaterniond(0, 0, IGN_PI/4.0)); math::Pose3d B(math::Vector3d(1, 0, 0), math::Quaterniond(0, 0, IGN_PI/2.0)); - EXPECT_TRUE(math::equal((B + A).Pos().X(), 1.0 + 1.0/sqrt(2))); - EXPECT_TRUE(math::equal((B + A).Pos().Y(), 1.0/sqrt(2))); - EXPECT_TRUE(math::equal((B + A).Pos().Z(), 0.0)); - EXPECT_TRUE(math::equal((B + A).Rot().Euler().X(), 0.0)); - EXPECT_TRUE(math::equal((B + A).Rot().Euler().Y(), 0.0)); - EXPECT_TRUE(math::equal((B + A).Rot().Euler().Z(), 3.0*IGN_PI/4.0)); + EXPECT_TRUE(math::equal((A * B).Pos().X(), 1.0 + 1.0/sqrt(2))); + EXPECT_TRUE(math::equal((A * B).Pos().Y(), 1.0/sqrt(2))); + EXPECT_TRUE(math::equal((A * B).Pos().Z(), 0.0)); + EXPECT_TRUE(math::equal((A * B).Rot().Euler().X(), 0.0)); + EXPECT_TRUE(math::equal((A * B).Rot().Euler().Y(), 0.0)); + EXPECT_TRUE(math::equal((A * B).Rot().Euler().Z(), 3.0*IGN_PI/4.0)); + + IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + EXPECT_EQ(A * B, B + A); + EXPECT_NE(A * B, A + B); + IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } { // If: @@ -135,11 +140,11 @@ TEST(PoseTest, Pose) EXPECT_TRUE(pose1.Rot() == math::Quaterniond(0.946281, -0.0933066, -0.226566, -0.210984)); - pose = math::Pose3d(1, 2, 3, .1, .2, .3) + math::Pose3d(4, 5, 6, .4, .5, .6); + pose = math::Pose3d(4, 5, 6, .4, .5, .6) * math::Pose3d(1, 2, 3, .1, .2, .3); EXPECT_TRUE(pose == math::Pose3d(5.74534, 7.01053, 8.62899, 0.675732, 0.535753, 1.01174)); - pose += pose; + pose *= pose; EXPECT_TRUE(pose == math::Pose3d(11.314, 16.0487, 15.2559, 1.49463, 0.184295, 2.13932));