Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2019-2020 INRIA CNRS |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include <pinocchio/math/quaternion.hpp> |
6 |
|
|
#include <pinocchio/spatial/se3.hpp> |
7 |
|
|
|
8 |
|
|
#include <boost/variant.hpp> // to avoid C99 warnings |
9 |
|
|
|
10 |
|
|
#include <boost/test/unit_test.hpp> |
11 |
|
|
#include <boost/utility/binary.hpp> |
12 |
|
|
|
13 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
14 |
|
|
|
15 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_assignQuaternion) |
16 |
|
|
{ |
17 |
|
|
using namespace pinocchio; |
18 |
|
✗ |
const int max_tests = 1e5; |
19 |
|
✗ |
for (int k = 0; k < max_tests; ++k) |
20 |
|
|
{ |
21 |
|
✗ |
const SE3 M(SE3::Random()); |
22 |
|
✗ |
SE3::Quaternion quat_ref(M.rotation()); |
23 |
|
|
|
24 |
|
✗ |
SE3::Quaternion quat; |
25 |
|
✗ |
quaternion::assignQuaternion(quat, M.rotation()); |
26 |
|
|
|
27 |
|
✗ |
BOOST_CHECK(quat.coeffs().isApprox(quat_ref.coeffs())); |
28 |
|
|
} |
29 |
|
|
} |
30 |
|
|
|
31 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_uniformRandom) |
32 |
|
|
{ |
33 |
|
✗ |
srand(0); |
34 |
|
|
|
35 |
|
|
using namespace pinocchio; |
36 |
|
✗ |
Eigen::Quaternion<double> q; |
37 |
|
|
|
38 |
|
✗ |
for (int i = 0; i < (1 << 10); ++i) |
39 |
|
|
{ |
40 |
|
✗ |
quaternion::uniformRandom(q); |
41 |
|
✗ |
BOOST_CHECK_MESSAGE( |
42 |
|
|
(q.coeffs().array().abs() <= 1).all(), |
43 |
|
|
"Quaternion coeffs out of bounds: " << i << ' ' << q.coeffs().transpose()); |
44 |
|
|
} |
45 |
|
|
} |
46 |
|
|
|
47 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_isNormalized) |
48 |
|
|
{ |
49 |
|
✗ |
srand(0); |
50 |
|
|
|
51 |
|
|
using namespace pinocchio; |
52 |
|
|
typedef Eigen::Quaternion<double> Quaternion; |
53 |
|
|
typedef Quaternion::Coefficients Vector4; |
54 |
|
|
|
55 |
|
|
#ifdef NDEBUG |
56 |
|
|
const int max_test = 1e6; |
57 |
|
|
#else |
58 |
|
✗ |
const int max_test = 1e2; |
59 |
|
|
#endif |
60 |
|
✗ |
for (int i = 0; i < max_test; ++i) |
61 |
|
|
{ |
62 |
|
✗ |
Quaternion q; |
63 |
|
✗ |
q.coeffs() = Vector4::Random() + Vector4::Constant(2); |
64 |
|
✗ |
BOOST_CHECK(!quaternion::isNormalized(q)); |
65 |
|
|
|
66 |
|
✗ |
q.normalize(); |
67 |
|
✗ |
BOOST_CHECK(quaternion::isNormalized(q)); |
68 |
|
|
} |
69 |
|
|
|
70 |
|
|
// Specific check for the Zero vector |
71 |
|
✗ |
BOOST_CHECK(!quaternion::isNormalized(Quaternion(Vector4::Zero()))); |
72 |
|
|
} |
73 |
|
|
|
74 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
75 |
|
|
|