Function TEST

tests/unit/math/test_Math.cpp:737–796

}

TEST(MATH, ARTICULATED_INERTIA_TRANSFORM)
{
  const int iterations = 100000;

  Affine3d A1
      = Translation3d(0.1, 0.2, 0.3)
        * AngleAxisd(0.5, Vector3d(1.0 / sqrt(2.0), 1.0 / sqrt(2.0), 0.0));
  Matrix<double, 6, 6> a1 = Matrix<double, 6, 6>::Identity();
  Matrix<double, 6, 6> a2;

  Isometry3d I1;
  I1.matrix() = A1.matrix();
  Matrix<double, 6, 6> i1 = Matrix<double, 6, 6>::Identity();
  Matrix<double, 6, 6> i2;

  Matrix4d M1;
  M1 = A1.matrix();
  Matrix<double, 6, 6> m1 = Matrix<double, 6, 6>::Identity();
  Matrix<double, 6, 6> m2;

  clock_t start = clock();
  for (int i = 0; i < iterations; i++) {
    a2 = Transform_Affine3d(A1, a1);
  }
  std::cout << "Transform_Affine3d: "
            << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
  // cout << "result: " << a2 << endl;

  start = clock();
  for (int i = 0; i < iterations; i++) {
    i2 = Transform_Isometry3d(I1, i1);
  }
  std::cout << "Transform_Isometry3d: "
            << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
  // cout << "result: " << i2 << endl;

  start = clock();
  for (int i = 0; i < iterations; i++) {
    i2 = Transform_Isometry3d_2(I1, i1);
  }
  std::cout << "Transform_Isometry3d_2: "
            << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
  // cout << "result: " << i2 << endl;

  start = clock();
  for (int i = 0; i < iterations; i++) {
    i2 = Transform_Isometry3d_3(I1, i1);
  }
  std::cout << "Transform_Isometry3d_3: "
            << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
  // cout << "result: " << i2 << endl;

  start = clock();
  for (int i = 0; i < iterations; i++) {
    m2 = Transform_Matrix4d(M1, m1);
  }
  std::cout << "Transform_Matrix4d: "
            << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";