| #include <gtest/gtest.h> |
| #include <Base/CoordinateSystem.h> |
| #include <Base/Exception.h> |
| #include <Base/Placement.h> |
|
|
| |
| TEST(CoordinateSystem, TestDefault) |
| { |
| Base::CoordinateSystem cs; |
| EXPECT_EQ(cs.getPosition(), Base::Vector3d(0, 0, 0)); |
| EXPECT_EQ(cs.getXDirection(), Base::Vector3d(1, 0, 0)); |
| EXPECT_EQ(cs.getYDirection(), Base::Vector3d(0, 1, 0)); |
| EXPECT_EQ(cs.getZDirection(), Base::Vector3d(0, 0, 1)); |
| } |
|
|
| TEST(CoordinateSystem, TestSetAxisFailure) |
| { |
| Base::CoordinateSystem cs; |
|
|
| EXPECT_THROW(cs.setAxes(Base::Vector3d(0, 0, 1), Base::Vector3d(0, 0, 1)), Base::ValueError); |
| } |
|
|
| TEST(CoordinateSystem, TestSetAxis) |
| { |
| Base::CoordinateSystem cs; |
| Base::Axis axis; |
| axis.setBase(Base::Vector3d(1, 2, 3)); |
| axis.setDirection(Base::Vector3d(1, 1, 1)); |
| cs.setAxes(axis, Base::Vector3d(0, 0, 1)); |
|
|
| EXPECT_EQ(cs.getPosition(), Base::Vector3d(1, 2, 3)); |
| EXPECT_EQ(cs.getXDirection(), Base::Vector3d(-1, -1, 2).Normalize()); |
| EXPECT_EQ(cs.getYDirection(), Base::Vector3d(1, -1, 0).Normalize()); |
| EXPECT_EQ(cs.getZDirection(), Base::Vector3d(1, 1, 1).Normalize()); |
| } |
|
|
| TEST(CoordinateSystem, TestSetXDir) |
| { |
| Base::CoordinateSystem cs; |
| cs.setXDirection(Base::Vector3d(1, 1, 1)); |
|
|
| EXPECT_EQ(cs.getXDirection(), Base::Vector3d(1, 1, 0).Normalize()); |
| EXPECT_EQ(cs.getYDirection(), Base::Vector3d(-1, 1, 0).Normalize()); |
| EXPECT_EQ(cs.getZDirection(), Base::Vector3d(0, 0, 1).Normalize()); |
| } |
|
|
| TEST(CoordinateSystem, TestSetYDir) |
| { |
| Base::CoordinateSystem cs; |
| cs.setYDirection(Base::Vector3d(1, 1, 1)); |
|
|
| EXPECT_EQ(cs.getXDirection(), Base::Vector3d(1, -1, 0).Normalize()); |
| EXPECT_EQ(cs.getYDirection(), Base::Vector3d(1, 1, 0).Normalize()); |
| EXPECT_EQ(cs.getZDirection(), Base::Vector3d(0, 0, 1).Normalize()); |
| } |
|
|
| TEST(CoordinateSystem, TestSetZDir) |
| { |
| Base::CoordinateSystem cs; |
| cs.setZDirection(Base::Vector3d(1, 1, 1)); |
|
|
| EXPECT_EQ(cs.getXDirection(), Base::Vector3d(2, -1, -1).Normalize()); |
| EXPECT_EQ(cs.getYDirection(), Base::Vector3d(0, 1, -1).Normalize()); |
| EXPECT_EQ(cs.getZDirection(), Base::Vector3d(1, 1, 1).Normalize()); |
| } |
|
|
| TEST(CoordinateSystem, TestTransformPlacement) |
| { |
| Base::CoordinateSystem cs; |
| Base::CoordinateSystem csT; |
| Base::Placement plm; |
| plm.setPosition(Base::Vector3d(1, 2, 3)); |
| plm.setRotation(Base::Rotation(1, 1, 2, 2)); |
| csT.transform(plm); |
|
|
| Base::Placement dis = cs.displacement(csT); |
| EXPECT_EQ(plm.isSame(dis, 1e-15), true); |
|
|
| Base::Placement disT = csT.displacement(cs); |
| EXPECT_EQ(plm.inverse().isSame(disT, 1e-15), true); |
| } |
|
|
| TEST(CoordinateSystem, TestMultTransformPlacement) |
| { |
| Base::CoordinateSystem cs; |
| Base::CoordinateSystem csT; |
| Base::Placement plm; |
| plm.setPosition(Base::Vector3d(1, 2, 3)); |
| plm.setRotation(Base::Rotation(1, 1, 2, 2)); |
| csT.transform(plm); |
| csT.transform(plm); |
|
|
| plm = plm * plm; |
|
|
| Base::Placement dis = cs.displacement(csT); |
| EXPECT_EQ(plm.isSame(dis, 0.001), true); |
|
|
| Base::Placement disT = csT.displacement(cs); |
| EXPECT_EQ(plm.inverse().isSame(disT, 0.001), true); |
| } |
|
|
| TEST(CoordinateSystem, TestTransformRotation) |
| { |
| Base::CoordinateSystem cs; |
| Base::CoordinateSystem csT; |
| Base::Rotation rot(1, 1, 2, 2); |
| csT.transform(rot); |
|
|
| Base::Placement dis = cs.displacement(csT); |
| EXPECT_EQ(rot.isSame(dis.getRotation(), 1e-15), true); |
|
|
| Base::Placement disT = csT.displacement(cs); |
| EXPECT_EQ(rot.inverse().isSame(disT.getRotation(), 1e-15), true); |
| } |
|
|
| TEST(CoordinateSystem, TestTransformPoint) |
| { |
| Base::CoordinateSystem cs; |
| Base::CoordinateSystem csT; |
| Base::Placement plm; |
| plm.setPosition(Base::Vector3d(1, 2, 3)); |
| plm.setRotation(Base::Rotation(1, 1, 2, 2)); |
| csT.transform(plm); |
|
|
| Base::Vector3d src(-1, 5, 3), dst; |
| plm.inverse().multVec(src, dst); |
| csT.transformTo(src); |
|
|
| EXPECT_EQ(src, dst); |
| } |
|
|
| TEST(CoordinateSystem, TestSetPlacement) |
| { |
| Base::CoordinateSystem cs; |
| Base::CoordinateSystem csT; |
| Base::Placement plm; |
| plm.setPosition(Base::Vector3d(1, 2, 3)); |
| plm.setRotation(Base::Rotation(1, 1, 2, 2)); |
| csT.transform(plm); |
| csT.transform(plm); |
| csT.transform(plm); |
| csT.setPlacement(plm); |
|
|
| Base::Placement dis = cs.displacement(csT); |
| EXPECT_EQ(plm.isSame(dis, 1e-15), true); |
|
|
| Base::Placement disT = csT.displacement(cs); |
| EXPECT_EQ(plm.inverse().isSame(disT, 1e-15), true); |
| } |
|
|
| |
|
|
