33 #include <gtest/gtest.h> 43 timeval temp_time_struct;
44 gettimeofday(&temp_time_struct,NULL);
45 srand(temp_time_struct.tv_usec);
52 double roll, pitch, yaw;
54 for (
int solution = 1 ; solution < 2 ; ++solution)
56 m.
getRPY(roll, pitch, yaw, solution);
58 q_from_rpy.
setRPY(roll, pitch, yaw);
62 ASSERT_NEAR(0.0, angle1,
epsilon);
66 m2.
setRPY(roll, pitch, yaw);
72 ASSERT_NEAR(0.0, angle2,
epsilon);
79 unsigned int runs = 400;
82 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
83 for (
unsigned int i = 0; i < runs ; i++ )
85 xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
86 yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
87 zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
91 for (
unsigned int i = 0; i < runs ; i++ )
95 q_baseline.
setRPY(zvalues[i],yvalues[i],xvalues[i]);
100 ASSERT_NEAR(0.0, angle,
epsilon);
113 int main(
int argc,
char **argv){
114 testing::InitGoogleTest(&argc, argv);
115 return RUN_ALL_TESTS();
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Return the ***half*** angle between two quaternions.
int main(int argc, char **argv)
tfScalar angle(const Quaternion &q) const
Return the ***half*** angle between this quaternion and the other.
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
Get the matrix represented as roll pitch and yaw about fixed axes XYZ.
void setRPY(tfScalar roll, tfScalar pitch, tfScalar yaw)
Set the matrix using RPY about XYZ fixed axes.
void testQuatRPY(tf::Quaternion q_baseline)
tfScalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.