Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
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);
}
Exemplo n.º 3
0
// 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);
}
Exemplo n.º 4
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;
}