void Transformable::moveBy(const Vector3d& vector, bool global) {
Vector3d offset;
if (global) {
offset = Matrix3d(globalTransform().inverted() * localTransform()) * vector;
} else {
offset = Matrix3d(localTransform()) * vector;
}
setPosition(position() + offset);
}
void test_matrix_power()
{
CALL_SUBTEST_2(test2dRotation<double>(1e-13));
CALL_SUBTEST_1(test2dRotation<float>(2e-5)); // was 1e-5, relaxed for clang 2.8 / linux / x86-64
CALL_SUBTEST_9(test2dRotation<long double>(1e-13));
CALL_SUBTEST_2(test2dHyperbolicRotation<double>(1e-14));
CALL_SUBTEST_1(test2dHyperbolicRotation<float>(1e-5));
CALL_SUBTEST_9(test2dHyperbolicRotation<long double>(1e-14));
CALL_SUBTEST_10(test3dRotation<double>(1e-13));
CALL_SUBTEST_11(test3dRotation<float>(1e-5));
CALL_SUBTEST_12(test3dRotation<long double>(1e-13));
CALL_SUBTEST_2(testGeneral(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testGeneral(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testGeneral(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testGeneral(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testGeneral(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testGeneral(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testGeneral(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testGeneral(MatrixXf(2,2), 1e-3)); // see bug 614
CALL_SUBTEST_9(testGeneral(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testGeneral(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testGeneral(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testGeneral(Matrix3e(), 1e-13));
CALL_SUBTEST_2(testSingular(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testSingular(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testSingular(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testSingular(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testSingular(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testSingular(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testSingular(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testSingular(MatrixXf(2,2), 1e-3));
CALL_SUBTEST_9(testSingular(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testSingular(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testSingular(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testSingular(Matrix3e(), 1e-13));
CALL_SUBTEST_2(testLogThenExp(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testLogThenExp(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testLogThenExp(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testLogThenExp(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testLogThenExp(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testLogThenExp(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testLogThenExp(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testLogThenExp(MatrixXf(2,2), 1e-3));
CALL_SUBTEST_9(testLogThenExp(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testLogThenExp(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testLogThenExp(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testLogThenExp(Matrix3e(), 1e-13));
}
void test_stdvector_overload()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Affine2f())); // does not need the specialization (2+1)^2 = 9
CALL_SUBTEST_4(check_stdvector_transform(Affine3f()));
CALL_SUBTEST_4(check_stdvector_transform(Affine3d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
void test_qtvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Vector2f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Matrix2f()));
CALL_SUBTEST(check_qtvector_matrix(Vector4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST(check_qtvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST(check_qtvector_matrix(VectorXd(20)));
CALL_SUBTEST(check_qtvector_matrix(RowVectorXf(20)));
CALL_SUBTEST(check_qtvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST(check_qtvector_transform(Transform2f()));
CALL_SUBTEST(check_qtvector_transform(Transform3f()));
CALL_SUBTEST(check_qtvector_transform(Transform3d()));
//CALL_SUBTEST(check_qtvector_transform(Transform4d()));
// some Quaternion
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
}
void test_stdvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Projective2f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3d()));
//CALL_SUBTEST(heck_stdvector_transform(Projective4d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
void test_eigensolver_selfadjoint()
{
int s = 0;
for(int i = 0; i < g_repeat; i++) {
// very important to test 3x3 and 2x2 matrices since we provide special paths for them
CALL_SUBTEST_1( selfadjointeigensolver(Matrix2f()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix2d()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix3d()) );
CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_5( selfadjointeigensolver(MatrixXcd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_9( selfadjointeigensolver(Matrix<std::complex<double>,Dynamic,Dynamic,RowMajor>(s,s)) );
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(1,1)) );
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(2,2)) );
CALL_SUBTEST_6( selfadjointeigensolver(Matrix<double,1,1>()) );
CALL_SUBTEST_7( selfadjointeigensolver(Matrix<double,2,2>()) );
}
// Test problem size constructors
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_8(SelfAdjointEigenSolver<MatrixXf> tmp1(s));
CALL_SUBTEST_8(Tridiagonalization<MatrixXf> tmp2(s));
TEST_SET_BUT_UNUSED_VARIABLE(s)
}
void test_adjoint()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( adjoint(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( adjoint(Matrix3d()) );
CALL_SUBTEST_3( adjoint(Matrix4f()) );
CALL_SUBTEST_4( adjoint(MatrixXcf(internal::random<int>(1,50), internal::random<int>(1,50))) );
CALL_SUBTEST_5( adjoint(MatrixXi(internal::random<int>(1,50), internal::random<int>(1,50))) );
CALL_SUBTEST_6( adjoint(MatrixXf(internal::random<int>(1,50), internal::random<int>(1,50))) );
}
// test a large matrix only once
CALL_SUBTEST_7( adjoint(Matrix<float, 100, 100>()) );
#ifdef EIGEN_TEST_PART_4
{
MatrixXcf a(10,10), b(10,10);
VERIFY_RAISES_ASSERT(a = a.transpose());
VERIFY_RAISES_ASSERT(a = a.transpose() + b);
VERIFY_RAISES_ASSERT(a = b + a.transpose());
VERIFY_RAISES_ASSERT(a = a.conjugate().transpose());
VERIFY_RAISES_ASSERT(a = a.adjoint());
VERIFY_RAISES_ASSERT(a = a.adjoint() + b);
VERIFY_RAISES_ASSERT(a = b + a.adjoint());
// no assertion should be triggered for these cases:
a.transpose() = a.transpose();
a.transpose() += a.transpose();
a.transpose() += a.transpose() + b;
a.transpose() = a.adjoint();
a.transpose() += a.adjoint();
a.transpose() += a.adjoint() + b;
}
#endif
}
void Manifold::Portal::setExit(Manifold::Portal* exit, Matrix3d rotation) {
this->exit = exit;
if(rotation == Matrix3d() || rotation == Matrix3d::Identity()) {
rotate = false;
} else {
this->rotation = rotation;
rotate = true;
}
}
void test_eigen2_product_small()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( product(Matrix<float, 3, 2>()) );
CALL_SUBTEST_2( product(Matrix<int, 3, 5>()) );
CALL_SUBTEST_3( product(Matrix3d()) );
CALL_SUBTEST_4( product(Matrix4d()) );
CALL_SUBTEST_5( product(Matrix4f()) );
}
}
void test_eigen2_triangular()
{
CALL_SUBTEST_8( selfadjoint() );
for(int i = 0; i < g_repeat ; i++) {
CALL_SUBTEST_1( triangular(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( triangular(Matrix<float, 2, 2>()) );
CALL_SUBTEST_3( triangular(Matrix3d()) );
CALL_SUBTEST_4( triangular(MatrixXcf(4, 4)) );
CALL_SUBTEST_5( triangular(Matrix<std::complex<float>,8, 8>()) );
CALL_SUBTEST_6( triangular(MatrixXd(17,17)) );
CALL_SUBTEST_7( triangular(Matrix<float,Dynamic,Dynamic,RowMajor>(5, 5)) );
}
}
void test_adjoint()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST( adjoint(Matrix<float, 1, 1>()) );
CALL_SUBTEST( adjoint(Matrix3d()) );
CALL_SUBTEST( adjoint(Matrix4f()) );
CALL_SUBTEST( adjoint(MatrixXcf(4, 4)) );
CALL_SUBTEST( adjoint(MatrixXi(8, 12)) );
CALL_SUBTEST( adjoint(MatrixXf(21, 21)) );
}
// test a large matrix only once
CALL_SUBTEST( adjoint(Matrix<float, 100, 100>()) );
}
void test_product_trmv()
{
int s;
for(int i = 0; i < g_repeat ; i++) {
CALL_SUBTEST_1( trmv(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( trmv(Matrix<float, 2, 2>()) );
CALL_SUBTEST_3( trmv(Matrix3d()) );
s = ei_random<int>(1,200);
CALL_SUBTEST_4( trmv(MatrixXcf(s,s)) );
s = ei_random<int>(1,200);
CALL_SUBTEST_5( trmv(MatrixXcd(s,s)) );
s = ei_random<int>(1,320);
CALL_SUBTEST_6( trmv(Matrix<float,Dynamic,Dynamic,RowMajor>(s, s)) );
}
}
void test_dontalign()
{
#if defined EIGEN_TEST_PART_1 || defined EIGEN_TEST_PART_5
dontalign(Matrix3d());
dontalign(Matrix4f());
#elif defined EIGEN_TEST_PART_2 || defined EIGEN_TEST_PART_6
dontalign(Matrix3cd());
dontalign(Matrix4cf());
#elif defined EIGEN_TEST_PART_3 || defined EIGEN_TEST_PART_7
dontalign(Matrix<float, 32, 32>());
dontalign(Matrix<std::complex<float>, 32, 32>());
#elif defined EIGEN_TEST_PART_4 || defined EIGEN_TEST_PART_8
dontalign(MatrixXd(32, 32));
dontalign(MatrixXcf(32, 32));
#endif
}
void test_product_trmv()
{
int s;
for(int i = 0; i < g_repeat ; i++) {
CALL_SUBTEST_1( trmv(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( trmv(Matrix<float, 2, 2>()) );
CALL_SUBTEST_3( trmv(Matrix3d()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2);
CALL_SUBTEST_4( trmv(MatrixXcf(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2);
CALL_SUBTEST_5( trmv(MatrixXcd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE);
CALL_SUBTEST_6( trmv(Matrix<float,Dynamic,Dynamic,RowMajor>(s, s)) );
}
EIGEN_UNUSED_VARIABLE(s);
}
void test_product_selfadjoint()
{
int s = 0;
for(int i = 0; i < g_repeat ; i++) {
CALL_SUBTEST_1( product_selfadjoint(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( product_selfadjoint(Matrix<float, 2, 2>()) );
CALL_SUBTEST_3( product_selfadjoint(Matrix3d()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2);
CALL_SUBTEST_4( product_selfadjoint(MatrixXcf(s, s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2);
CALL_SUBTEST_5( product_selfadjoint(MatrixXcd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE);
CALL_SUBTEST_6( product_selfadjoint(MatrixXd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE);
CALL_SUBTEST_7( product_selfadjoint(Matrix<float,Dynamic,Dynamic,RowMajor>(s,s)) );
}
TEST_SET_BUT_UNUSED_VARIABLE(s)
}
void test_product_small()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( product(Matrix<float, 3, 2>()) );
CALL_SUBTEST_2( product(Matrix<int, 3, 5>()) );
CALL_SUBTEST_3( product(Matrix3d()) );
CALL_SUBTEST_4( product(Matrix4d()) );
CALL_SUBTEST_5( product(Matrix4f()) );
CALL_SUBTEST_6( product1x1() );
}
#ifdef EIGEN_TEST_PART_6
{
// test compilation of (outer_product) * vector
Vector3f v = Vector3f::Random();
VERIFY_IS_APPROX( (v * v.transpose()) * v, (v * v.transpose()).eval() * v);
}
#endif
}
void test_triangular()
{
for(int i = 0; i < g_repeat ; i++)
{
int r = ei_random<int>(2,20); EIGEN_UNUSED_VARIABLE(r);
int c = ei_random<int>(2,20); EIGEN_UNUSED_VARIABLE(c);
CALL_SUBTEST_1( triangular_square(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( triangular_square(Matrix<float, 2, 2>()) );
CALL_SUBTEST_3( triangular_square(Matrix3d()) );
CALL_SUBTEST_4( triangular_square(Matrix<std::complex<float>,8, 8>()) );
CALL_SUBTEST_5( triangular_square(MatrixXcd(r,r)) );
CALL_SUBTEST_6( triangular_square(Matrix<float,Dynamic,Dynamic,RowMajor>(r, r)) );
CALL_SUBTEST_7( triangular_rect(Matrix<float, 4, 5>()) );
CALL_SUBTEST_8( triangular_rect(Matrix<double, 6, 2>()) );
CALL_SUBTEST_9( triangular_rect(MatrixXcf(r, c)) );
CALL_SUBTEST_5( triangular_rect(MatrixXcd(r, c)) );
CALL_SUBTEST_6( triangular_rect(Matrix<float,Dynamic,Dynamic,RowMajor>(r, c)) );
}
}
bool QuaternionTest::test()
{
// ctor tests
Quaterniond tquaternion_test( 8., 1., 6., 3. );
for ( size_t i = 0; i < 4; ++i )
{
if ( _quaternion.wxyz[i] != tquaternion_test.wxyz[i] )
{
cout << "test: Quaternion::Quaternion( ... ) failed!" << endl;
failed();
assert( 0 );
}
}
Quaterniond squaternion_test( 8., Vector3d( 1., 6., 3. ) );
for ( size_t i = 0; i < 4; ++i )
{
if ( _quaternion.wxyz[i] != squaternion_test.wxyz[i] )
{
cout << "test: Quaternion::Quaternion( ... ) failed!" << endl;
failed();
assert( 0 );
}
}
Quaterniond uquaternion_test( Matrix3d( 1., 0., 0., 0., 0., 1., 0., -1., 0. ) );
for ( size_t i = 0; i < 4; ++i )
{
if ( i == 0 )
if ( uquaternion_test.wxyz[0] != ( sqrt( 2. ) / 2. ) )
{
cout << "test: Quaternion::Quaternion( ... ) failed!" << endl;
failed();
assert( 0 );
}
if ( i == 1 )
if ( uquaternion_test.wxyz[1] != - ( 1 / sqrt( 2. ) ) )
{
cout << "test: Quaternion::Quaternion( ... ) failed!" << endl;
failed();
assert( 0 );
}
if ( i != 0 && i != 1 )
if ( uquaternion_test.wxyz[i] != 0. )
{
cout << "test: Quaternion::Quaternion( ... ) failed!" << endl;
failed();
assert( 0 );
}
}
// set test
uquaternion_test.set ( 8., 1., 6., 3. );
for ( size_t i = 0; i < 4; ++i )
{
if ( _quaternion.wxyz[i] != uquaternion_test.wxyz[i] )
{
cout << "test: Quaternion::set( ... ) failed!" << endl;
failed();
assert( 0 );
}
}
// operator = tests
tquaternion_test = 1.;
Quaterniond IDENTITY( 1., 0., 0., 0. );
for ( size_t i = 0; i < 4; ++i )
{
if ( IDENTITY.wxyz[i] != tquaternion_test.wxyz[i] )
{
cout << "test: Quaternion::operator=( ... ) failed!" << endl;
failed();
assert( 0 );
}
}
uquaternion_test = _quaternion;
for ( size_t i = 0; i < 4; ++i )
{
if ( _quaternion.wxyz[i] != uquaternion_test.wxyz[i] )
{
cout << "test: Quaternion::operator=( ... ) failed!" << endl;
failed();
assert( 0 );
}
}
//quaternion == / != tests
if ( tquaternion_test != 1. )
{
cout << "test: Quaternion::operator ==() / !=() failed!" << endl;
failed();
assert( 0 );
}
if ( _quaternion != uquaternion_test )
{
cout << "test: Quaternion::operator ==() / !=() failed!" << endl;
failed();
assert( 0 );
}
if ( _quaternion == tquaternion_test )
{
cout << "test: Quaternion::operator ==() / !=() failed!" << endl;
failed();
assert( 0 );
}
//abs() test
if ( _quaternion.abs() != sqrt( 110 ) )
{
cout << "test: Quaternion::abs() failed!" << endl;
failed();
assert( 0 );
}
//conjug() test
Quaterniond cquaternion_test( 8., -1., -6., -3. );
if ( _quaternion.conjug() != cquaternion_test )
{
cout << "test: Quaternion::conjug() failed!" << endl;
failed();
assert( 0 );
}
//quaternion/scalar tests
tquaternion_test.set( 1., 2., 3., 4. );
Quaterniond vquaternion_test( 3., 6., 9., 12. );
tquaternion_test = tquaternion_test * 3.0;
if ( tquaternion_test - vquaternion_test != 0.0 )
{
cout << "test: Quaternion::operator*/( scalar ) failed!" << endl;
cout << tquaternion_test << endl;
failed();
assert( 0 );
}
// quaternion/quaternion tests
tquaternion_test.set( 1., 2., 3., 4. );
if ( tquaternion_test + tquaternion_test + tquaternion_test -vquaternion_test != 0.0f )
{
cout << "test: Quaternion::operator+-( quaternion ) failed!" << endl;
failed();
assert( 0 );
}
squaternion_test.set( -24., 2., 28., 44 );
if ( squaternion_test != tquaternion_test * uquaternion_test )
{
cout << "test: Quaternion::operator*( quaternion ) failed!" << endl;
failed();
assert( 0 );
}
Quaterniond rquaternion_test( 8. / 110., -1. / 110., -6. / 110., -3. / 110. );
if ( _quaternion.invert() != rquaternion_test )
{
cout << "test: Quaternion::invert() failed!" << endl;
failed();
assert( 0 );
}
if ( tquaternion_test.dot( _quaternion ) != 40. )
{
cout << "test: Quaternion::dot( quaternion ) failed!" << endl;
failed();
assert( 0 );
}
Vector3d vector3_test( -30., -4., 18. );
if ( tquaternion_test.cross( _quaternion ) != vector3_test )
{
cout << "test: Quaternion::cross( quaternion ) failed!" << endl;
failed();
assert( 0 );
}
Vector3d yaxis( 1., 0., 0. );
Quaterniond svector3_test( 0., 18., -30., -4. );
Quaterniond result_test( _quaternion.rotate( M_PI / 2.f, yaxis, vector3_test ) );
if ( abs( result_test.abs() - svector3_test.abs() ) > 1e-13 )
{
cout << "test: Quaternion::rotate( T, Vector3, Vector3 ) failed!" << endl;
failed();
assert( 0 );
}
if ( ok )
cout << "Quaternion: all tests passed!" << endl;
return ok;
}
void Transformable::setMatrix(const Matrix4d& matrix) {
setPosition(matrix.translationVector());
setRotation(Matrix3d(matrix).rotationVector());
setScale(Matrix3d(matrix).scaleVector());
}