GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/rotation.cpp Lines: 24 24 100.0 %
Date: 2024-01-23 21:41:47 Branches: 117 228 51.3 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2019-2020 INRIA
3
//
4
5
#include <iostream>
6
7
#include <pinocchio/math/rotation.hpp>
8
#include <pinocchio/math/sincos.hpp>
9
#include <Eigen/Geometry>
10
11
#include <boost/variant.hpp> // to avoid C99 warnings
12
13
#include <boost/test/unit_test.hpp>
14
#include <boost/utility/binary.hpp>
15
16
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
17
18
















4
BOOST_AUTO_TEST_CASE(test_toRotationMatrix)
19
{
20
  using namespace pinocchio;
21
2
  const int max_tests = 1e5;
22
200002
  for(int k = 0; k < max_tests; ++k)
23
  {
24
200000
    const double theta = std::rand();
25
    double cos_value, sin_value;
26
27
200000
    pinocchio::SINCOS(theta,&sin_value,&cos_value);
28

200000
    Eigen::Vector3d axis(Eigen::Vector3d::Random().normalized());
29
30

200000
    Eigen::Matrix3d rot; toRotationMatrix(axis,cos_value,sin_value,rot);
31

200000
    Eigen::Matrix3d rot_ref = Eigen::AngleAxisd(theta,axis).toRotationMatrix();
32
33



200000
    BOOST_CHECK(rot.isApprox(rot_ref));
34
  }
35
2
}
36
37
















4
BOOST_AUTO_TEST_CASE(test_orthogonal_projection)
38
{
39
  using namespace pinocchio;
40
2
  const int max_tests = 1e5;
41
42
  typedef Eigen::Vector4d Vector4;
43
  typedef Eigen::Quaterniond Quaternion;
44
  typedef Eigen::Matrix3d Matrix3;
45
46
200002
  for(int k = 0; k < max_tests; ++k)
47
  {
48

200000
    const Vector4 vec4 = Vector4::Random();
49

200000
    const Quaternion quat(vec4.normalized());
50
51
200000
    const Matrix3 rot = quat.toRotationMatrix();
52
53
200000
    Matrix3 rot_proj = orthogonalProjection(rot);
54



200000
    BOOST_CHECK(rot.isApprox(rot_proj));
55
  }
56
57
200002
  for(int k = 0; k < max_tests; ++k)
58
  {
59

200000
    const Matrix3 mat = Matrix3::Random();
60
61
200000
    Matrix3 rot_proj = orthogonalProjection(mat);
62




200000
    BOOST_CHECK((rot_proj.transpose()*rot_proj).isIdentity());
63



200000
    BOOST_CHECK(fabs(rot_proj.determinant() - 1.) <= Eigen::NumTraits<double>::dummy_precision());
64
  }
65
2
}
66
67
BOOST_AUTO_TEST_SUITE_END()
68
69