| #include <gtest/gtest.h> |
| #include <Base/DualQuaternion.h> |
| #include <Base/Matrix.h> |
| #include <Base/Placement.h> |
| #include <Base/Tools.h> |
|
|
| static const double epsilon = 1e-9; |
|
|
| TEST(Placement, TestDefault) |
| { |
| Base::Placement plm; |
| EXPECT_EQ(plm.getPosition().IsNull(), true); |
| EXPECT_EQ(plm.getRotation().isNull(), false); |
| EXPECT_EQ(plm.getRotation().isIdentity(), true); |
| } |
|
|
| TEST(Placement, TestPosRot) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 1, 2, 2); |
| Base::Placement plm(pos, rot); |
| EXPECT_EQ(plm.getPosition(), pos); |
| EXPECT_EQ(plm.getRotation(), rot); |
| } |
|
|
| TEST(Placement, TestPosRotCnt) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 0, 0, 0); |
| Base::Vector3d cnt(4, 5, 6); |
| Base::Placement plm(pos, rot, cnt); |
|
|
| EXPECT_EQ(plm.getPosition(), Base::Vector3d(1, 12, 15)); |
| EXPECT_EQ(plm.getRotation(), rot); |
| } |
|
|
| TEST(Placement, TestMatrix) |
| { |
| Base::Matrix4D mat; |
| mat.rotX(Base::toRadians(90.0)); |
| mat.rotY(Base::toRadians(90.0)); |
| mat.rotZ(Base::toRadians(90.0)); |
| mat.setCol(3, Base::Vector3d(1, 2, 3)); |
| Base::Placement plm(mat); |
| EXPECT_EQ(plm.getPosition(), Base::Vector3d(1, 2, 3)); |
| Base::Vector3d axis; |
| double angle {}; |
| plm.getRotation().getValue(axis, angle); |
| EXPECT_EQ(angle, Base::toRadians(90.0)); |
| EXPECT_EQ(axis.IsEqual(Base::Vector3d(0, 1, 0), 0.001), true); |
| } |
|
|
| TEST(Placement, TestIdentity) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 0, 0, 0); |
| Base::Vector3d cnt(4, 5, 6); |
| Base::Placement plm(pos, rot, cnt); |
| Base::Placement mult = plm * plm.inverse(); |
| EXPECT_EQ(mult.isIdentity(), true); |
| } |
|
|
| TEST(Placement, TestInvert) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 0, 0, 0); |
| Base::Vector3d cnt(4, 5, 6); |
| Base::Placement plm(pos, rot, cnt); |
| plm.invert(); |
| EXPECT_EQ(plm.getPosition(), Base::Vector3d(-1, 12, 15)); |
| EXPECT_EQ(plm.getRotation().isIdentity(), false); |
| EXPECT_EQ(plm.getRotation(), rot); |
| } |
|
|
| TEST(Placement, TestMove) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 0, 0, 0); |
| Base::Vector3d cnt(4, 5, 6); |
| Base::Placement plm(pos, rot, cnt); |
| plm.move(Base::Vector3d(-1, -12, -15)); |
| EXPECT_EQ(plm.getPosition().IsNull(), true); |
| } |
|
|
| TEST(Placement, TestSame) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 0, 0, 0); |
| Base::Vector3d cnt(4, 5, 6); |
| Base::Placement plm(pos, rot, cnt); |
| EXPECT_EQ(plm.isSame(plm), true); |
| EXPECT_EQ(plm.isSame(plm, 0.001), true); |
| EXPECT_EQ(plm == plm, true); |
|
|
| Base::Placement plm2(plm * plm); |
| EXPECT_EQ(plm2.isSame(plm), false); |
| EXPECT_EQ(plm2.isSame(plm, 0.001), false); |
| EXPECT_EQ(plm2 == plm, false); |
| } |
|
|
| TEST(Placement, TestMultiply) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 0, 0, 0); |
| Base::Vector3d cnt(4, 5, 6); |
| Base::Placement plm(pos, rot, cnt); |
|
|
| Base::Placement plm2(plm * plm); |
| EXPECT_EQ(plm2.getRotation().isIdentity(), true); |
| } |
|
|
| TEST(Placement, TestMultRight) |
| { |
| Base::Vector3d pos1(1, 2, 3); |
| Base::Rotation rot1(1, 0, 0, 0); |
| Base::Placement plm1(pos1, rot1); |
|
|
| Base::Vector3d pos2(3, 2, 1); |
| Base::Rotation rot2(0, 1, 0, 0); |
| Base::Placement plm2(pos2, rot2); |
|
|
| plm1.multRight(plm2); |
| EXPECT_EQ(plm1.getRotation(), Base::Rotation(0, 0, 1, 0)); |
| EXPECT_EQ(plm1.getPosition(), Base::Vector3d(4, 0, 2)); |
| } |
|
|
| TEST(Placement, TestMultLeft) |
| { |
| Base::Vector3d pos1(1, 2, 3); |
| Base::Rotation rot1(1, 0, 0, 0); |
| Base::Placement plm1(pos1, rot1); |
|
|
| Base::Vector3d pos2(3, 2, 1); |
| Base::Rotation rot2(0, 1, 0, 0); |
| Base::Placement plm2(pos2, rot2); |
|
|
| plm1.multLeft(plm2); |
| EXPECT_EQ(plm1.getRotation(), Base::Rotation(0, 0, 1, 0)); |
| EXPECT_EQ(plm1.getPosition(), Base::Vector3d(2, 4, -2)); |
| } |
|
|
| TEST(Placement, TestMultVec) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 0, 0, 0); |
| Base::Placement plm(pos, rot); |
|
|
| Base::Vector3d vec {1, 1, 1}; |
| plm.multVec(vec, vec); |
| EXPECT_EQ(vec, Base::Vector3d(2, 1, 2)); |
| } |
|
|
| TEST(Placement, TestDualQuat) |
| { |
| Base::Vector3d pos(1, 2, 3); |
| Base::Rotation rot(1, 0, 0, 0); |
| Base::Placement plm(pos, rot); |
| Base::DualQuat qq = plm.toDualQuaternion(); |
| Base::Placement plm2 = Base::Placement::fromDualQuaternion(qq); |
| EXPECT_EQ(plm.isSame(plm2), true); |
| } |
|
|
| TEST(Placement, TestPow) |
| { |
| Base::Vector3d axis1, axis2; |
| double angle1 {}, angle2 {}; |
|
|
| Base::Vector3d pos(1, 4, 6); |
| Base::Rotation rot(1, 2, 3, 4); |
| Base::Placement plm(pos, rot); |
| rot.getValue(axis1, angle1); |
|
|
| Base::Placement plm2 = plm.pow(1.5); |
| plm2.getRotation().getValue(axis2, angle2); |
| EXPECT_DOUBLE_EQ(angle2, 1.5 * angle1); |
| EXPECT_EQ(axis1.IsEqual(axis2, 0.0001), true); |
| } |
|
|
| TEST(Placement, TestSlerp) |
| { |
| Base::Vector3d pos(1, 4, 6); |
| Base::Rotation rot1(1, 0, 0, 0); |
| Base::Rotation rot2(0, 1, 0, 0); |
| Base::Placement plm1(pos, rot1); |
| Base::Placement plm2(pos, rot2); |
|
|
| Base::Placement plm3 = Base::Placement::slerp(plm1, plm2, 0.0); |
| EXPECT_EQ(plm3.isSame(plm1), true); |
| Base::Placement plm4 = Base::Placement::slerp(plm1, plm2, 1.0); |
| EXPECT_EQ(plm4.isSame(plm2), true); |
| Base::Placement plm5 = Base::Placement::slerp(plm1, plm1, 0.5); |
| EXPECT_EQ(plm5.isSame(plm1), true); |
| Base::Placement plm6 = Base::Placement::slerp(plm1, plm2, 0.5); |
| EXPECT_EQ(plm6.getRotation().isSame(Base::Rotation(1, 1, 0, 0), epsilon), true); |
| EXPECT_EQ(plm6.getPosition().IsEqual(pos, epsilon), true); |
| } |
|
|
| TEST(Placement, TestSclerp) |
| { |
| Base::Vector3d pos(1, 4, 6); |
| Base::Rotation rot1(1, 0, 0, 0); |
| Base::Rotation rot2(0, 1, 0, 0); |
| Base::Placement plm1(pos, rot1); |
| Base::Placement plm2(pos, rot2); |
|
|
| Base::Placement plm3 = Base::Placement::sclerp(plm1, plm2, 0.0); |
| EXPECT_EQ(plm3.isSame(plm1), true); |
| Base::Placement plm4 = Base::Placement::sclerp(plm1, plm2, 1.0); |
| EXPECT_EQ(plm4.isSame(plm2, epsilon), true); |
| Base::Placement plm5 = Base::Placement::sclerp(plm1, plm1, 0.5); |
| EXPECT_EQ(plm5.isSame(plm1), true); |
| Base::Placement plm6 = Base::Placement::sclerp(plm1, plm2, 0.5); |
| EXPECT_EQ(plm6.getRotation().isSame(Base::Rotation(1, 1, 0, 0), epsilon), true); |
| EXPECT_EQ(plm6.getPosition().IsEqual(pos, epsilon), true); |
| } |
|
|
