void testEulerConversion(double yaw, double pitch, double roll){
Pose3D p;
p.setFromEuler(0, 0, 0, yaw, pitch, roll);
Euler e = p.getEuler();
EXPECT_FLOAT_EQ(e.yaw, yaw);
EXPECT_FLOAT_EQ(e.pitch, pitch);
EXPECT_FLOAT_EQ(e.roll, roll);
}
TEST(Pose3D, DefaultConstructor){
Pose3D p;
//EXPECT_EQ(0, p.getMagnitude());
Euler e = p.getEuler();
EXPECT_EQ(0, e.yaw);
EXPECT_EQ(0, e.pitch);
EXPECT_EQ(0, e.roll);
Quaternion q = p.getQuaternion();
EXPECT_EQ(0, q.x);
EXPECT_EQ(0, q.y);
EXPECT_EQ(0, q.z);
EXPECT_EQ(1, q.w);
Position x = p.getPosition();
EXPECT_EQ(0, x.x);
EXPECT_EQ(0, x.y);
EXPECT_EQ(0, x.z);
}
