Classes | Functions
geo_quaternion.cpp File Reference
#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include "AnnoyingScalar.h"

Classes

struct  MovableClass
 

Functions

template<typename T >
T bounded_acos (T v)
 
template<typename QuatType >
void check_slerp (const QuatType &q0, const QuatType &q1)
 
template<typename Scalar , int Options>
void quaternion (void)
 
template<typename Scalar >
void mapQuaternion (void)
 
template<typename Scalar >
void quaternionAlignment (void)
 
template<typename PlainObjectType >
void check_const_correctness (const PlainObjectType &)
 
 EIGEN_DECLARE_TEST (geo_quaternion)
 

Function Documentation

◆ bounded_acos()

template<typename T >
T bounded_acos ( T  v)
18  {
19  using std::acos;
20  using std::max;
21  using std::min;
22  return acos((max)(T(-1), (min)(v, T(1))));
23 }
acos
AnnoyingScalar acos(const AnnoyingScalar &x)
Definition: AnnoyingScalar.h:138
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
T
Eigen::Triplet< double > T
Definition: EigenUnitTest.cpp:11
min
#define min(a, b)
Definition: datatypes.h:22
max
#define max(a, b)
Definition: datatypes.h:23

References acos(), max, min, and v.

◆ check_const_correctness()

template<typename PlainObjectType >
void check_const_correctness ( const PlainObjectType &  )
276  {
277  // there's a lot that we can't test here while still having this test compile!
278  // the only possible approach would be to run a script trying to compile stuff and checking that it fails.
279  // CMake can help with that.
280 
281  // verify that map-to-const don't have LvalueBit
282  typedef std::add_const_t<PlainObjectType> ConstPlainObjectType;
283  VERIFY(!(internal::traits<Map<ConstPlainObjectType> >::Flags & LvalueBit));
284  VERIFY(!(internal::traits<Map<ConstPlainObjectType, Aligned> >::Flags & LvalueBit));
285  VERIFY(!(Map<ConstPlainObjectType>::Flags & LvalueBit));
286  VERIFY(!(Map<ConstPlainObjectType, Aligned>::Flags & LvalueBit));
287 }
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:96
Eigen::LvalueBit
const unsigned int LvalueBit
Definition: Constants.h:148
VERIFY
#define VERIFY(a)
Definition: main.h:362
Flags
Extend namespace for flags.
Definition: fsi_chan_precond_driver.cc:56

References Eigen::LvalueBit, and VERIFY.

Referenced by EIGEN_DECLARE_TEST().

◆ check_slerp()

template<typename QuatType >
void check_slerp ( const QuatType &  q0,
const QuatType &  q1 
)
26  {
27  using std::abs;
28  typedef typename QuatType::Scalar Scalar;
29  typedef AngleAxis<Scalar> AA;
30 
31  Scalar largeEps = test_precision<Scalar>();
32 
33  Scalar theta_tot = AA(q1 * q0.inverse()).angle();
34  if (theta_tot > Scalar(EIGEN_PI)) theta_tot = Scalar(2.) * Scalar(EIGEN_PI) - theta_tot;
35  for (Scalar t = 0; t <= Scalar(1.001); t += Scalar(0.1)) {
36  QuatType q = q0.slerp(t, q1);
37  Scalar theta = AA(q * q0.inverse()).angle();
38  VERIFY(abs(q.norm() - 1) < largeEps);
39  if (theta_tot == 0)
40  VERIFY(theta_tot == 0);
41  else
42  VERIFY(abs(theta - t * theta_tot) < largeEps);
43  }
44 }
abs
AnnoyingScalar abs(const AnnoyingScalar &x)
Definition: AnnoyingScalar.h:135
EIGEN_PI
#define EIGEN_PI
Definition: MathFunctions.h:16
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:45
Eigen::AngleAxis
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: AngleAxis.h:52
BiharmonicTestFunctions2::theta
double theta
Definition: two_d_biharmonic.cc:236
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:2019
plotPSD.t
t
Definition: plotPSD.py:36

References abs(), EIGEN_PI, Eigen::numext::q, plotPSD::t, BiharmonicTestFunctions2::theta, and VERIFY.

Referenced by quaternion().

◆ EIGEN_DECLARE_TEST()

EIGEN_DECLARE_TEST ( geo_quaternion  )
301  {
302  for (int i = 0; i < g_repeat; i++) {
303  CALL_SUBTEST_1((quaternion<float, AutoAlign>()));
304  CALL_SUBTEST_1(check_const_correctness(Quaternionf()));
305  CALL_SUBTEST_1((quaternion<float, DontAlign>()));
306  CALL_SUBTEST_1((quaternionAlignment<float>()));
307  CALL_SUBTEST_1(mapQuaternion<float>());
308 
309  CALL_SUBTEST_2((quaternion<double, AutoAlign>()));
310  CALL_SUBTEST_2(check_const_correctness(Quaterniond()));
311  CALL_SUBTEST_2((quaternion<double, DontAlign>()));
312  CALL_SUBTEST_2((quaternionAlignment<double>()));
313  CALL_SUBTEST_2(mapQuaternion<double>());
314 
315 #ifndef EIGEN_TEST_ANNOYING_SCALAR_DONT_THROW
316  AnnoyingScalar::dont_throw = true;
317 #endif
318  CALL_SUBTEST_3((quaternion<AnnoyingScalar, AutoAlign>()));
319  }
320 }
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
AnnoyingScalar::dont_throw
static bool dont_throw
Definition: AnnoyingScalar.h:127
check_const_correctness
void check_const_correctness(const PlainObjectType &)
Definition: geo_quaternion.cpp:276
Eigen::Quaterniond
Quaternion< double > Quaterniond
Definition: Eigen/Eigen/src/Geometry/Quaternion.h:387
Eigen::Quaternionf
Quaternion< float > Quaternionf
Definition: Eigen/Eigen/src/Geometry/Quaternion.h:384
Eigen::g_repeat
static int g_repeat
Definition: main.h:191
CALL_SUBTEST_3
#define CALL_SUBTEST_3(FUNC)
Definition: split_test_helper.h:16
CALL_SUBTEST_1
#define CALL_SUBTEST_1(FUNC)
Definition: split_test_helper.h:4
CALL_SUBTEST_2
#define CALL_SUBTEST_2(FUNC)
Definition: split_test_helper.h:10

References CALL_SUBTEST_1, CALL_SUBTEST_2, CALL_SUBTEST_3, check_const_correctness(), AnnoyingScalar::dont_throw, Eigen::g_repeat, and i.

◆ mapQuaternion()

template<typename Scalar >
void mapQuaternion ( void  )
177  {
178  typedef Map<Quaternion<Scalar>, Aligned> MQuaternionA;
179  typedef Map<const Quaternion<Scalar>, Aligned> MCQuaternionA;
180  typedef Map<Quaternion<Scalar> > MQuaternionUA;
181  typedef Map<const Quaternion<Scalar> > MCQuaternionUA;
182  typedef Quaternion<Scalar> Quaternionx;
183  typedef Matrix<Scalar, 3, 1> Vector3;
184  typedef AngleAxis<Scalar> AngleAxisx;
185 
186  Vector3 v0 = Vector3::Random(), v1 = Vector3::Random();
187  Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
188 
189  EIGEN_ALIGN_MAX Scalar array1[4];
190  EIGEN_ALIGN_MAX Scalar array2[4];
191  EIGEN_ALIGN_MAX Scalar array3[4 + 1];
192  Scalar* array3unaligned = array3 + 1;
193 
194  MQuaternionA mq1(array1);
195  MCQuaternionA mcq1(array1);
196  MQuaternionA mq2(array2);
197  MQuaternionUA mq3(array3unaligned);
198  MCQuaternionUA mcq3(array3unaligned);
199 
200  // std::cerr << array1 << " " << array2 << " " << array3 << "\n";
201  mq1 = AngleAxisx(a, v0.normalized());
202  mq2 = mq1;
203  mq3 = mq1;
204 
205  Quaternionx q1 = mq1;
206  Quaternionx q2 = mq2;
207  Quaternionx q3 = mq3;
208  Quaternionx q4 = MCQuaternionUA(array3unaligned);
209 
210  VERIFY_IS_APPROX(q1.coeffs(), q2.coeffs());
211  VERIFY_IS_APPROX(q1.coeffs(), q3.coeffs());
212  VERIFY_IS_APPROX(q4.coeffs(), q3.coeffs());
213 
214  VERIFY_IS_APPROX(mq1 * (mq1.inverse() * v1), v1);
215  VERIFY_IS_APPROX(mq1 * (mq1.conjugate() * v1), v1);
216 
217  VERIFY_IS_APPROX(mcq1 * (mcq1.inverse() * v1), v1);
218  VERIFY_IS_APPROX(mcq1 * (mcq1.conjugate() * v1), v1);
219 
220  VERIFY_IS_APPROX(mq3 * (mq3.inverse() * v1), v1);
221  VERIFY_IS_APPROX(mq3 * (mq3.conjugate() * v1), v1);
222 
223  VERIFY_IS_APPROX(mcq3 * (mcq3.inverse() * v1), v1);
224  VERIFY_IS_APPROX(mcq3 * (mcq3.conjugate() * v1), v1);
225 
226  VERIFY_IS_APPROX(mq1 * mq2, q1 * q2);
227  VERIFY_IS_APPROX(mq3 * mq2, q3 * q2);
228  VERIFY_IS_APPROX(mcq1 * mq2, q1 * q2);
229  VERIFY_IS_APPROX(mcq3 * mq2, q3 * q2);
230 
231  // Bug 1461, compilation issue with Map<const Quat>::w(), and other reference/constness checks:
232  VERIFY_IS_APPROX(mcq3.coeffs().x() + mcq3.coeffs().y() + mcq3.coeffs().z() + mcq3.coeffs().w(), mcq3.coeffs().sum());
233  VERIFY_IS_APPROX(mcq3.x() + mcq3.y() + mcq3.z() + mcq3.w(), mcq3.coeffs().sum());
234  mq3.w() = 1;
235  const Quaternionx& cq3(q3);
236  VERIFY(&cq3.x() == &q3.x());
237  const MQuaternionUA& cmq3(mq3);
238  VERIFY(&cmq3.x() == &mq3.x());
239  // FIXME the following should be ok. The problem is that currently the LValueBit flag
240  // is used to determine whether we can return a coeff by reference or not, which is not enough for Map<const ...>.
241  // const MCQuaternionUA& cmcq3(mcq3);
242  // VERIFY( &cmcq3.x() == &mcq3.x() );
243 
244  // test cast
245  {
246  Quaternion<float> q1f = mq1.template cast<float>();
247  VERIFY_IS_APPROX(q1f.template cast<Scalar>(), mq1);
248  Quaternion<double> q1d = mq1.template cast<double>();
249  VERIFY_IS_APPROX(q1d.template cast<Scalar>(), mq1);
250  }
251 }
EIGEN_ALIGN_MAX
#define EIGEN_ALIGN_MAX
Definition: ConfigureVectorization.h:146
v1
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9;Map< RowVectorXf > v1(M1.data(), M1.size())
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: Eigen/Eigen/src/Geometry/Quaternion.h:285
Eigen::Aligned
@ Aligned
Definition: Constants.h:242
VERIFY_IS_APPROX
#define VERIFY_IS_APPROX(a, b)
Definition: integer_types.cpp:13
a
const Scalar * a
Definition: level2_cplx_impl.h:32

References a, Eigen::Aligned, EIGEN_ALIGN_MAX, EIGEN_PI, v1(), VERIFY, and VERIFY_IS_APPROX.

◆ quaternion()

template<typename Scalar , int Options>
void quaternion ( void  )
47  {
48  /* this test covers the following files:
49  Quaternion.h
50  */
51  using std::abs;
52  typedef Matrix<Scalar, 3, 1> Vector3;
53  typedef Matrix<Scalar, 3, 3> Matrix3;
54  typedef Quaternion<Scalar, Options> Quaternionx;
55  typedef AngleAxis<Scalar> AngleAxisx;
56 
57  Scalar largeEps = test_precision<Scalar>();
58  if (internal::is_same<Scalar, float>::value) largeEps = Scalar(1e-3);
59 
60  Scalar eps = internal::random<Scalar>() * Scalar(1e-2);
61 
62  Vector3 v0 = Vector3::Random(), v1 = Vector3::Random(), v2 = Vector3::Random(), v3 = Vector3::Random();
63 
64  Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)),
65  b = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
66 
67  // Quaternion: Identity(), setIdentity();
68  Quaternionx q1, q2;
69  q2.setIdentity();
70  VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs());
71  q1.coeffs().setRandom();
72  VERIFY_IS_APPROX(q1.coeffs(), (q1 * q2).coeffs());
73 
74 #ifndef EIGEN_NO_IO
75  // Printing
76  std::ostringstream ss;
77  ss << q2;
78  VERIFY(ss.str() == "0i + 0j + 0k + 1");
79 #endif
80 
81  // concatenation
82  q1 *= q2;
83 
84  q1 = AngleAxisx(a, v0.normalized());
85  q2 = AngleAxisx(a, v1.normalized());
86 
87  // angular distance
88  Scalar refangle = abs(AngleAxisx(q1.inverse() * q2).angle());
89  if (refangle > Scalar(EIGEN_PI)) refangle = Scalar(2) * Scalar(EIGEN_PI) - refangle;
90 
91  if ((q1.coeffs() - q2.coeffs()).norm() > Scalar(10) * largeEps) {
92  VERIFY_IS_MUCH_SMALLER_THAN(abs(q1.angularDistance(q2) - refangle), Scalar(1));
93  }
94 
95  // Action on vector by the q v q* formula
96  VERIFY_IS_APPROX(q1 * v2, (q1 * Quaternionx(Scalar(0), v2) * q1.inverse()).vec());
97  VERIFY_IS_APPROX(q1.inverse() * v2, (q1.inverse() * Quaternionx(Scalar(0), v2) * q1).vec());
98 
99  // rotation matrix conversion
100  VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
101  VERIFY_IS_APPROX(q1 * q2 * v2, q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
102 
103  VERIFY((q2 * q1).isApprox(q1 * q2, largeEps) ||
104  !(q2 * q1 * v2).isApprox(q1.toRotationMatrix() * q2.toRotationMatrix() * v2));
105 
106  q2 = q1.toRotationMatrix();
107  VERIFY_IS_APPROX(q1 * v1, q2 * v1);
108 
109  Matrix3 rot1(q1);
110  VERIFY_IS_APPROX(q1 * v1, rot1 * v1);
111  Quaternionx q3(rot1.transpose() * rot1);
112  VERIFY_IS_APPROX(q3 * v1, v1);
113 
114  // angle-axis conversion
115  AngleAxisx aa = AngleAxisx(q1);
116  VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
117 
118  // Do not execute the test if the rotation angle is almost zero, or
119  // the rotation axis and v1 are almost parallel.
120  if (abs(aa.angle()) > Scalar(5) * test_precision<Scalar>() && (aa.axis() - v1.normalized()).norm() < Scalar(1.99) &&
121  (aa.axis() + v1.normalized()).norm() < Scalar(1.99)) {
122  VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle() * 2, aa.axis())) * v1);
123  }
124 
125  // from two vector creation
126  VERIFY_IS_APPROX(v2.normalized(), (q2.setFromTwoVectors(v1, v2) * v1).normalized());
127  VERIFY_IS_APPROX(v1.normalized(), (q2.setFromTwoVectors(v1, v1) * v1).normalized());
128  VERIFY_IS_APPROX(-v1.normalized(), (q2.setFromTwoVectors(v1, -v1) * v1).normalized());
129  if (internal::is_same<Scalar, double>::value) {
130  v3 = (v1.array() + eps).matrix();
131  VERIFY_IS_APPROX(v3.normalized(), (q2.setFromTwoVectors(v1, v3) * v1).normalized());
132  VERIFY_IS_APPROX(-v3.normalized(), (q2.setFromTwoVectors(v1, -v3) * v1).normalized());
133  }
134 
135  // from two vector creation static function
136  VERIFY_IS_APPROX(v2.normalized(), (Quaternionx::FromTwoVectors(v1, v2) * v1).normalized());
137  VERIFY_IS_APPROX(v1.normalized(), (Quaternionx::FromTwoVectors(v1, v1) * v1).normalized());
138  VERIFY_IS_APPROX(-v1.normalized(), (Quaternionx::FromTwoVectors(v1, -v1) * v1).normalized());
139  if (internal::is_same<Scalar, double>::value) {
140  v3 = (v1.array() + eps).matrix();
141  VERIFY_IS_APPROX(v3.normalized(), (Quaternionx::FromTwoVectors(v1, v3) * v1).normalized());
142  VERIFY_IS_APPROX(-v3.normalized(), (Quaternionx::FromTwoVectors(v1, -v3) * v1).normalized());
143  }
144 
145  // inverse and conjugate
146  VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
147  VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);
148 
149  // test casting
150  Quaternion<float> q1f = q1.template cast<float>();
151  VERIFY_IS_APPROX(q1f.template cast<Scalar>(), q1);
152  Quaternion<double> q1d = q1.template cast<double>();
153  VERIFY_IS_APPROX(q1d.template cast<Scalar>(), q1);
154 
155  // test bug 369 - improper alignment.
156  Quaternionx* q = new Quaternionx;
157  delete q;
158 
159  q1 = Quaternionx::UnitRandom();
160  q2 = Quaternionx::UnitRandom();
161  check_slerp(q1, q2);
162 
163  q1 = AngleAxisx(b, v1.normalized());
164  q2 = AngleAxisx(b + Scalar(EIGEN_PI), v1.normalized());
165  check_slerp(q1, q2);
166 
167  q1 = AngleAxisx(b, v1.normalized());
168  q2 = AngleAxisx(-b, -v1.normalized());
169  check_slerp(q1, q2);
170 
171  q1 = Quaternionx::UnitRandom();
172  q2.coeffs() = -q1.coeffs();
173  check_slerp(q1, q2);
174 }
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
v2
Map< RowVectorXf > v2(M2.data(), M2.size())
b
Scalar * b
Definition: benchVecAdd.cpp:17
matrix
Eigen::Map< Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor >, 0, Eigen::OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: common.h:85
check_slerp
void check_slerp(const QuatType &q0, const QuatType &q1)
Definition: geo_quaternion.cpp:26
VERIFY_IS_NOT_APPROX
#define VERIFY_IS_NOT_APPROX(a, b)
Definition: integer_types.cpp:15
VERIFY_IS_MUCH_SMALLER_THAN
#define VERIFY_IS_MUCH_SMALLER_THAN(a, b)
Definition: main.h:371
CRBond_Bessel::eps
double eps
Definition: crbond_bessel.cc:24
Eigen::internal::isApprox
EIGEN_DEVICE_FUNC bool isApprox(const Scalar &x, const Scalar &y, const typename NumTraits< Scalar >::Real &precision=NumTraits< Scalar >::dummy_precision())
Definition: MathFunctions.h:1923
Eigen::value
squared absolute value
Definition: GlobalFunctions.h:87

References a, abs(), b, check_slerp(), e(), EIGEN_PI, CRBond_Bessel::eps, Eigen::internal::isApprox(), matrix(), Eigen::numext::q, v1(), v2(), Eigen::value, VERIFY, VERIFY_IS_APPROX, VERIFY_IS_MUCH_SMALLER_THAN, and VERIFY_IS_NOT_APPROX.

◆ quaternionAlignment()

template<typename Scalar >
void quaternionAlignment ( void  )
254  {
255  typedef Quaternion<Scalar, AutoAlign> QuaternionA;
256  typedef Quaternion<Scalar, DontAlign> QuaternionUA;
257 
258  EIGEN_ALIGN_MAX Scalar array1[4];
259  EIGEN_ALIGN_MAX Scalar array2[4];
260  EIGEN_ALIGN_MAX Scalar array3[4 + 1];
261  Scalar* arrayunaligned = array3 + 1;
262 
263  QuaternionA* q1 = ::new (reinterpret_cast<void*>(array1)) QuaternionA;
264  QuaternionUA* q2 = ::new (reinterpret_cast<void*>(array2)) QuaternionUA;
265  QuaternionUA* q3 = ::new (reinterpret_cast<void*>(arrayunaligned)) QuaternionUA;
266 
267  q1->coeffs().setRandom();
268  *q2 = *q1;
269  *q3 = *q1;
270 
271  VERIFY_IS_APPROX(q1->coeffs(), q2->coeffs());
272  VERIFY_IS_APPROX(q1->coeffs(), q3->coeffs());
273 }

References EIGEN_ALIGN_MAX, and VERIFY_IS_APPROX.