TEST_F(SO3Test, RotToEulerZYX) {
const kr::Mat3<double> R = Rz * Ry * Rx;
const kr::Vec3<double> rpy = kr::rotToEulerZYX(R);
EXPECT_CLOSE(rpy[0], M_PI / 3);
EXPECT_CLOSE(rpy[1], M_PI / 3);
EXPECT_CLOSE(rpy[2], M_PI / 3);
}
TEST(RodriguesTest, RodriguesToQuat) {
const kr::Vec3<double> rvec_zero(0, 0, 0);
kr::Quatd q0 = kr::rodriguesToQuat(rvec_zero);
EXPECT_EQ(q0.w(), 1.0);
EXPECT_EQ(q0.x(), 0.0);
EXPECT_EQ(q0.y(), 0.0);
EXPECT_EQ(q0.z(), 0.0);
const double r = M_PI / std::sqrt(3);
const kr::Vec3<double> rvec(r, r, r);
kr::Quatd q = kr::rodriguesToQuat(rvec);
EXPECT_CLOSE(q.w(), 0.0);
EXPECT_CLOSE(q.x(), r / M_PI);
}
// Not implemented
TEST_F(SO3Test, RotationXYZ) {
EXPECT_CLOSE(Rx(0, 0), 1.0);
EXPECT_CLOSE(Rx(0, 1), 0.0);
EXPECT_CLOSE(Rx(0, 2), 0.0);
EXPECT_CLOSE(Rx(1, 0), 0.0);
EXPECT_CLOSE(Rx(1, 1), 0.5);
EXPECT_CLOSE(Rx(1, 2), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rx(2, 0), 0.0);
EXPECT_CLOSE(Rx(2, 1), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rx(2, 2), 0.5);
EXPECT_CLOSE(Ry(0, 0), 0.5);
EXPECT_CLOSE(Ry(0, 1), 0.0);
EXPECT_CLOSE(Ry(0, 2), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Ry(1, 0), 0.0);
EXPECT_CLOSE(Ry(1, 1), 1.0);
EXPECT_CLOSE(Ry(1, 2), 0.0);
EXPECT_CLOSE(Ry(2, 0), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Ry(2, 1), 0.0);
EXPECT_CLOSE(Ry(2, 2), 0.5);
EXPECT_CLOSE(Rz(0, 0), 0.5);
EXPECT_CLOSE(Rz(0, 1), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rz(0, 2), 0.0);
EXPECT_CLOSE(Rz(1, 0), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rz(1, 1), 0.5);
EXPECT_CLOSE(Rz(1, 2), 0.0);
EXPECT_CLOSE(Rz(2, 0), 0.0);
EXPECT_CLOSE(Rz(2, 1), 0.0);
EXPECT_CLOSE(Rz(2, 2), 1.0);
}
int main()
{
const int rhb = 10;
const int lhb = 22;
typedef fixed_Point<rhb, lhb> fixed_Point_def;
const fixed_Point_def fp1( 6.375f );
const fixed_Point_def fp2( -4.0f );
// -------------------------------------------------------------------------
EXPECT_EQ( 6 , int(fp1) );
EXPECT_EQ( -4 , int(fp2) );
EXPECT_EQ( fp1 , abs(fp1) );
EXPECT_EQ( -fp2 , abs(fp2) );
// -------------------------------------------------------------------------
// comparison and ordering
assert( fp1 == fixed_Point_def(6.375f) );
assert( fp2 != fp1 );
assert( fp2 < fp1 );
assert( fp1 > fp2 );
assert( fp2 <= fp1 );
assert( fp1 >= fp2 );
// -------------------------------------------------------------------------
// arithmetics
EXPECT_EQ( fixed_Point_def( 2.375f ) , fp1 + fp2 );
EXPECT_EQ( fixed_Point_def( 10.375f ) , fp1 - fp2 );
EXPECT_EQ( fixed_Point_def(-25.5f ) , fp1 * fp2 );
EXPECT_EQ( fixed_Point_def(-1.59375f) , fp1 / fp2 );
// -------------------------------------------------------------------------
// arithmetics assignment
fixed_Point_def
fp3 = fp1; fp3 += fp2; EXPECT_EQ( fixed_Point_def( 2.375f ) , fp3 );
fp3 = fp1; fp3 -= fp2; EXPECT_EQ( fixed_Point_def( 10.375f ) , fp3 );
fp3 = fp1; fp3 *= fp2; EXPECT_EQ( fixed_Point_def(-25.5f ) , fp3 );
fp3 = fp1; fp3 /= fp2; EXPECT_EQ( fixed_Point_def(-1.59375f) , fp3 );
// -------------------------------------------------------------------------
// pre/post - increment/decrement
fp3 = fp1; EXPECT_EQ( fixed_Point_def(7.375f) , ++fp3 );
fp3 = fp1; EXPECT_EQ( fixed_Point_def(5.375f) , --fp3 );
fp3 = fp1; EXPECT_EQ( fixed_Point_def(6.375f) , fp3++ ); EXPECT_EQ( fixed_Point_def(7.375f) , fp3 );
fp3 = fp1; EXPECT_EQ( fixed_Point_def(6.375f) , fp3-- ); EXPECT_EQ( fixed_Point_def(5.375f) , fp3 );
// -------------------------------------------------------------------------
// trigonometric functions
EXPECT_CLOSE( fixed_Point_def(std::sin(0.5f)) , sin(fixed_Point_def(0.5f)) , fixed_Point_def(0.01f) );
EXPECT_CLOSE( fixed_Point_def(std::cos(0.5f)) , cos(fixed_Point_def(0.5f)) , fixed_Point_def(0.01f) );
fixed_Point<8,2> test(16.f);
fixed_Point<40,2> test2(16.f);
fixed_Point<3,2> test3(1.f);
std::cout << sizeof(test) * 8 << std::endl;
std::cout << sizeof(test2) * 8 << std::endl;
std::cout << sizeof(test3) * 8 << std::endl;
return 0;
}