20
20
#include < boost/test/unit_test.hpp>
21
21
#include < boost/test/floating_point_comparison.hpp>
22
22
#include < boost/test/output_test_stream.hpp>
23
+ #include < boost/math/constants/constants.hpp>
23
24
24
25
#include < jrl/mal/boost.hh>
25
26
#include < sot/core/matrix-homogeneous.hh>
26
27
#include " sot/core/matrix-twist.hh"
27
28
28
29
using boost::test_tools::output_test_stream;
30
+ using boost::math::constants::pi ;
29
31
30
32
namespace ml = maal::boost;
31
33
@@ -69,18 +71,18 @@ BOOST_AUTO_TEST_CASE (product)
69
71
tx = (10 .*rand ())/RAND_MAX - 5 .;
70
72
ty = (10 .*rand ())/RAND_MAX - 5 .;
71
73
tz = (10 .*rand ())/RAND_MAX - 5 .;
72
- roll = (M_PI *rand ())/RAND_MAX - .5 *M_PI ;
73
- pitch = (M_PI *rand ())/RAND_MAX - .5 *M_PI ;
74
- yaw = (2 *M_PI *rand ())/RAND_MAX - M_PI ;
74
+ roll = (pi < double >() *rand ())/RAND_MAX - .5 *pi < double >() ;
75
+ pitch = (pi < double >() *rand ())/RAND_MAX - .5 *pi < double >() ;
76
+ yaw = (2 *pi < double >() *rand ())/RAND_MAX - pi < double >() ;
75
77
MATRIX_HOMO_INIT (H1, tx, ty, tz, roll, pitch, yaw);
76
78
ml::Matrix M1 (H1);
77
79
dynamicgraph::sot::MatrixHomogeneous H2;
78
80
tx = (10 .*rand ())/RAND_MAX;
79
81
ty = (10 .*rand ())/RAND_MAX - 5 .;
80
82
tz = (10 .*rand ())/RAND_MAX - 5 .;
81
- roll = (M_PI *rand ())/RAND_MAX - .5 *M_PI ;
82
- pitch = (M_PI *rand ())/RAND_MAX - .5 *M_PI ;
83
- yaw = (2 *M_PI *rand ())/RAND_MAX - M_PI ;
83
+ roll = (pi < double >() *rand ())/RAND_MAX - .5 *pi < double >() ;
84
+ pitch = (pi < double >() *rand ())/RAND_MAX - .5 *pi < double >() ;
85
+ yaw = (2 *pi < double >() *rand ())/RAND_MAX - pi < double >() ;
84
86
MATRIX_HOMO_INIT (H2, tx, ty, tz, roll, pitch, yaw);
85
87
ml::Matrix M2 (H2);
86
88
dynamicgraph::sot::MatrixHomogeneous H3 = H1*H2;
@@ -99,17 +101,17 @@ BOOST_AUTO_TEST_CASE (inverse)
99
101
tx = (10 .*rand ())/RAND_MAX - 5 .;
100
102
ty = (10 .*rand ())/RAND_MAX - 5 .;
101
103
tz = (10 .*rand ())/RAND_MAX - 5 .;
102
- roll = (M_PI *rand ())/RAND_MAX - .5 *M_PI ;
103
- pitch = (M_PI *rand ())/RAND_MAX - .5 *M_PI ;
104
- yaw = (2 *M_PI *rand ())/RAND_MAX - M_PI ;
104
+ roll = (pi < double >() *rand ())/RAND_MAX - .5 *pi < double >() ;
105
+ pitch = (pi < double >() *rand ())/RAND_MAX - .5 *pi < double >() ;
106
+ yaw = (2 *pi < double >() *rand ())/RAND_MAX - pi < double >() ;
105
107
MATRIX_HOMO_INIT (H1, tx, ty, tz, roll, pitch, yaw);
106
108
dynamicgraph::sot::MatrixHomogeneous H2;
107
109
tx = (10 .*rand ())/RAND_MAX;
108
110
ty = (10 .*rand ())/RAND_MAX - 5 .;
109
111
tz = (10 .*rand ())/RAND_MAX - 5 .;
110
- roll = (M_PI *rand ())/RAND_MAX - .5 *M_PI ;
111
- pitch = (M_PI *rand ())/RAND_MAX - .5 *M_PI ;
112
- yaw = (2 *M_PI *rand ())/RAND_MAX - M_PI ;
112
+ roll = (pi < double >() *rand ())/RAND_MAX - .5 *pi < double >() ;
113
+ pitch = (pi < double >() *rand ())/RAND_MAX - .5 *pi < double >() ;
114
+ yaw = (2 *pi < double >() *rand ())/RAND_MAX - pi < double >() ;
113
115
MATRIX_HOMO_INIT (H2, tx, ty, tz, roll, pitch, yaw);
114
116
dynamicgraph::sot::MatrixHomogeneous H3 = H1*H2;
115
117
dynamicgraph::sot::MatrixHomogeneous invH1, invH2, invH3;
0 commit comments