void test_eigensolver_generic()
{
int s = 0;
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( eigensolver(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( eigensolver(MatrixXd(s,s)) );
TEST_SET_BUT_UNUSED_VARIABLE(s)
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_2( eigensolver(MatrixXd(1,1)) );
CALL_SUBTEST_2( eigensolver(MatrixXd(2,2)) );
CALL_SUBTEST_3( eigensolver(Matrix<double,1,1>()) );
CALL_SUBTEST_4( eigensolver(Matrix2d()) );
}
CALL_SUBTEST_1( eigensolver_verify_assert(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( eigensolver_verify_assert(MatrixXd(s,s)) );
CALL_SUBTEST_3( eigensolver_verify_assert(Matrix<double,1,1>()) );
CALL_SUBTEST_4( eigensolver_verify_assert(Matrix2d()) );
// Test problem size constructors
CALL_SUBTEST_5(EigenSolver<MatrixXf> tmp(s));
// regression test for bug 410
CALL_SUBTEST_2(
{
MatrixXd A(1,1);
A(0,0) = std::sqrt(-1.); // is Not-a-Number
Eigen::EigenSolver<MatrixXd> solver(A);
VERIFY_IS_EQUAL(solver.info(), NumericalIssue);
}
);
void test_eigensolver_generic()
{
int s = 0;
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( eigensolver(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( eigensolver(MatrixXd(s,s)) );
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_2( eigensolver(MatrixXd(1,1)) );
CALL_SUBTEST_2( eigensolver(MatrixXd(2,2)) );
CALL_SUBTEST_3( eigensolver(Matrix<double,1,1>()) );
CALL_SUBTEST_4( eigensolver(Matrix2d()) );
}
CALL_SUBTEST_1( eigensolver_verify_assert(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( eigensolver_verify_assert(MatrixXd(s,s)) );
CALL_SUBTEST_3( eigensolver_verify_assert(Matrix<double,1,1>()) );
CALL_SUBTEST_4( eigensolver_verify_assert(Matrix2d()) );
// Test problem size constructors
CALL_SUBTEST_5(EigenSolver<MatrixXf> tmp(s));
// regression test for bug 410
CALL_SUBTEST_2(
{
MatrixXd A(1,1);
A(0,0) = std::sqrt(-1.);
Eigen::EigenSolver<MatrixXd> solver(A);
MatrixXd V(1, 1);
V(0,0) = solver.eigenvectors()(0,0).real();
}
);
Matrix2d Matrix2d::inverse( bool* pbIsSingular, double epsilon )
{
double determinant = m_elements[ 0 ] * m_elements[ 3 ] - m_elements[ 2 ] * m_elements[ 1 ];
bool isSingular = ( abs( determinant ) < epsilon );
if( isSingular )
{
if( pbIsSingular != NULL )
{
*pbIsSingular = true;
}
return Matrix2d();
}
else
{
if( pbIsSingular != NULL )
{
*pbIsSingular = false;
}
double reciprocalDeterminant = 1.0 / determinant;
return Matrix2d
(
m_elements[ 3 ] * reciprocalDeterminant, -m_elements[ 2 ] * reciprocalDeterminant,
-m_elements[ 1 ] * reciprocalDeterminant, m_elements[ 0 ] * reciprocalDeterminant
);
}
}
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_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_5( testVectorType(Vector4d()) ); // regression test for bug 232
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_8( testVectorType(Vector3f()) );
CALL_SUBTEST_8( testVectorType(Vector4f()) );
CALL_SUBTEST_8( testVectorType(Matrix<float,8,1>()) );
CALL_SUBTEST_8( testVectorType(Matrix<float,1,1>()) );
CALL_SUBTEST_9( testVectorType(VectorXi(internal::random<int>(1,300))) );
CALL_SUBTEST_9( testVectorType(Matrix<int,1,1>()) );
}
#ifdef EIGEN_TEST_PART_6
// Assignment of a RowVectorXd to a MatrixXd (regression test for bug #79).
VERIFY( (MatrixXd(RowVectorXd::LinSpaced(3, 0, 1)) - RowVector3d(0, 0.5, 1)).norm() < std::numeric_limits<double>::epsilon() );
#endif
}
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)
}
Matrix2d Matrix2d::coordSystem(const Point2d& origin, float scaleX,
float scaleY, float angle)
{
if (mgIsZero(scaleY)) scaleY = scaleX;
float s = sinf(angle);
float c = cosf(angle);
return Matrix2d(c*scaleX, s*scaleX, -s*scaleY, c*scaleY, origin.x, origin.y);
}
void test_matrix_function()
{
CALL_SUBTEST_1(testMatrixType(Matrix<float,1,1>()));
CALL_SUBTEST_2(testMatrixType(Matrix3cf()));
CALL_SUBTEST_3(testMatrixType(MatrixXf(8,8)));
CALL_SUBTEST_4(testMatrixType(Matrix2d()));
CALL_SUBTEST_5(testMatrixType(Matrix<double,5,5,RowMajor>()));
CALL_SUBTEST_6(testMatrixType(Matrix4cd()));
CALL_SUBTEST_7(testMatrixType(MatrixXd(13,13)));
}
void test_eigen2_inverse()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( inverse(Matrix<double,1,1>()) );
CALL_SUBTEST_2( inverse(Matrix2d()) );
CALL_SUBTEST_3( inverse(Matrix3f()) );
CALL_SUBTEST_4( inverse(Matrix4f()) );
CALL_SUBTEST_5( inverse(MatrixXf(8,8)) );
CALL_SUBTEST_6( inverse(MatrixXcd(7,7)) );
}
}
// static
Matrix2d Matrix2d::rotation( double degrees )
{
double c = cos( degrees );
double s = sin( degrees );
return Matrix2d
(
c, -s,
s, c
);
}
void test_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(50,50)) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(5,7)) );
CALL_SUBTEST_4( testVectorType(VectorXd(51)) );
CALL_SUBTEST_5( testVectorType(VectorXd(41)) );
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(51)) );
CALL_SUBTEST_8( testVectorType(VectorXf(41)) );
CALL_SUBTEST_9( testVectorType(Vector3f()) );
}
void test_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_5( testVectorType(Vector4d()) ); // regression test for bug 232
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_8( testVectorType(Vector3f()) );
CALL_SUBTEST_8( testVectorType(Matrix<float,1,1>()) );
}
}
void test_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_5( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_8( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_9( testVectorType(Vector3f()) );
}
}
void test_eigensolver_generalized_real()
{
for(int i = 0; i < g_repeat; i++) {
int s = 0;
CALL_SUBTEST_1( generalized_eigensolver_real(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(s,s)) );
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(1,1)) );
CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(2,2)) );
CALL_SUBTEST_3( generalized_eigensolver_real(Matrix<double,1,1>()) );
CALL_SUBTEST_4( generalized_eigensolver_real(Matrix2d()) );
TEST_SET_BUT_UNUSED_VARIABLE(s)
}
}
void test_inverse()
{
int s;
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( inverse(Matrix<double,1,1>()) );
CALL_SUBTEST_2( inverse(Matrix2d()) );
CALL_SUBTEST_3( inverse(Matrix3f()) );
CALL_SUBTEST_4( inverse(Matrix4f()) );
CALL_SUBTEST_4( inverse(Matrix<float,4,4,DontAlign>()) );
s = internal::random<int>(50,320);
CALL_SUBTEST_5( inverse(MatrixXf(s,s)) );
s = internal::random<int>(25,100);
CALL_SUBTEST_6( inverse(MatrixXcd(s,s)) );
CALL_SUBTEST_7( inverse(Matrix4d()) );
CALL_SUBTEST_7( inverse(Matrix<double,4,4,DontAlign>()) );
}
}
void test_matrix_exponential()
{
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_2(test2dHyperbolicRotation<double>(1e-14));
CALL_SUBTEST_1(test2dHyperbolicRotation<float>(1e-5));
CALL_SUBTEST_6(testPascal<float>(1e-6));
CALL_SUBTEST_5(testPascal<double>(1e-15));
CALL_SUBTEST_2(randomTest(Matrix2d(), 1e-13));
CALL_SUBTEST_7(randomTest(Matrix<double,3,3,RowMajor>(), 1e-13));
CALL_SUBTEST_3(randomTest(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(randomTest(MatrixXd(8,8), 1e-13));
CALL_SUBTEST_1(randomTest(Matrix2f(), 1e-4));
CALL_SUBTEST_5(randomTest(Matrix3cf(), 1e-4));
CALL_SUBTEST_1(randomTest(Matrix4f(), 1e-4));
CALL_SUBTEST_6(randomTest(MatrixXf(8,8), 1e-4));
}
void test_matrix_power()
{
typedef Matrix<double,3,3,RowMajor> Matrix3dRowMajor;
typedef Matrix<long double,Dynamic,Dynamic> MatrixXe;
typedef Matrix<long double,Dynamic,1> VectorXe;
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_2(testMatrixVector(Matrix2d(), Vector2d(), 1e-13));
CALL_SUBTEST_7(testMatrixVector(Matrix3dRowMajor(), MatrixXd(3,5), 1e-13));
CALL_SUBTEST_3(testMatrixVector(Matrix4cd(), Vector4cd(), 1e-13));
CALL_SUBTEST_4(testMatrixVector(MatrixXd(8,8), VectorXd(8), 2e-12));
CALL_SUBTEST_1(testMatrixVector(Matrix2f(), Vector2f(), 1e-4));
CALL_SUBTEST_5(testMatrixVector(Matrix3cf(), Vector3cf(), 1e-4));
CALL_SUBTEST_8(testMatrixVector(Matrix4f(), Vector4f(), 1e-4));
CALL_SUBTEST_6(testMatrixVector(MatrixXf(8,8), VectorXf(8), 1e-3));
CALL_SUBTEST_9(testMatrixVector(MatrixXe(7,7), VectorXe(7), 1e-13));
}
inline
Matrix2d
Quaternion2d::rotationMatrix() const {
return Matrix2d(cosAngle(), sinAngle(), -sinAngle(), cosAngle());
}
Matrix2d Matrix2d::coordSystem(const Vector2d& e0, const Vector2d& e1,
const Point2d& origin)
{
return Matrix2d(e0, e1, origin);
}
double full_minimization(vector<double>& coeffs, int num_movements)
{
#pragma omp parallel for num_threads(NUM_PARALLEL)
for (int i=0; i < all_threads.size(); i++)
{
cout << "Start index " << i << endl;
vector<Thread> thread_seq;
thread_seq.resize(0);
thread_seq = construct_thread_sequence_forward(i, num_movements, thread_seq);
vector<Vector3d> points, goal_points;
vector<double> twist_angles, goal_twist_angles;
vector<Matrix3d> material_frames, goal_material_frames;
Thread runningThread = thread_seq[0];
Matrix2d bend_coeffs = Matrix2d(2,2);
bend_coeffs << coeffs[0], coeffs[2],
coeffs[2], coeffs[1];
runningThread.set_coeffs_normalized(bend_coeffs, coeffs[3], coeffs[4]);
// scores_two_points_thread = scores_each_movement in iso case
// scores_each_thread is aggregate movement error over all motions per thread
for (int seq = 0; seq < num_movements+1; seq++) {
scores_two_points_thread[(num_movements+1)*i +seq] = 0.0;
scores_each_thread[i] = 0.0;
scores_two_points_thread_norm[(num_movements+1)*i +seq] = 0.0;
scores_each_thread_norm[i] = 0.0;
if (seq < thread_seq.size()) {
Thread intermediateConfig = thread_seq[seq];
runningThread.match_start_and_end_constraints(intermediateConfig, THREAD_MOVEMENT_STEPS);
runningThread.get_thread_data(points, twist_angles, material_frames);
intermediateConfig.get_thread_data(goal_points, goal_twist_angles, goal_material_frames);
for (int j = 0; j < points.size(); j++) {
scores_each_thread[i] += pow((points[j] - goal_points[j]).norm(),2);
scores_each_thread_norm[i] += (points[j] - goal_points[j]).norm();
if (j > 0 && j < points.size() - 1) {
Vector3d goal_top = goal_points[j] + RIBBON_HALF_WIDTH * goal_material_frames[j].col(1).normalized();
Vector3d goal_bot = goal_points[j] - RIBBON_HALF_WIDTH * goal_material_frames[j].col(1).normalized();
Vector3d current_top = points[j] + RIBBON_HALF_WIDTH * material_frames[j].col(1).normalized();
Vector3d current_bot = points[j] - RIBBON_HALF_WIDTH * material_frames[j].col(1).normalized();
scores_two_points_thread[(num_movements+1)*i + seq] += (goal_top - current_top).squaredNorm();
scores_two_points_thread[(num_movements+1)*i + seq] += (goal_bot - current_bot).squaredNorm();
scores_two_points_thread_norm[(num_movements+1)*i + seq] += (goal_top - current_top).norm();
scores_two_points_thread_norm[(num_movements+1)*i + seq] += (goal_bot - current_bot).norm();
}
}
scores_two_points_thread[(num_movements+1)*i + seq] = sqrt(scores_two_points_thread[(num_movements+1)*i + seq]);
scores_each_thread[i] = sqrt(scores_each_thread[i]);
}
}
}
double fx = 0.0;
for (int i=0; i < all_threads.size(); i++)
{
fx += scores_each_thread[i];
fx += scores_two_points_thread[i];
}
fx /= (2.0*(double)scores_each_thread.size());
return fx;
}
