BALL 1.5.0
Loading...
Searching...
No Matches
quaternion.h
Go to the documentation of this file.
1// -*- Mode: C++; tab-width: 2; -*-
2// vi: set ts=2:
3
4#ifndef BALL_MATHS_QUATERNION_H
5#define BALL_MATHS_QUATERNION_H
6
7
10
11#include <Eigen/Geometry>
12#include <iostream>
13
14
15namespace BALL
16{
22 template <typename T>
24 : public Eigen::Quaternion<T> //{/*...*/};
25 {
26
27 public:
32
39
46
52 TQuaternion(const Eigen::Quaternion<T>& q);
53
62 TQuaternion(const T& a, const T& b, const T& c, const T& d);
63
70 TQuaternion(const TVector3<T>& axis, const T& angle);
71
78
83 void clear();
84
86
90
95 void set(const TQuaternion& q);
96
101 void set(const Eigen::Quaternion<T>& q);
102
110 void set(const T& a, const T& b, const T& c, const T& d);
111
117
122 TQuaternion& operator = (const Eigen::Quaternion<T>& q);
123
129
136
143
149
155 void fromAxisAngle(const TVector3<T>& axis, const T& angle);
156
164 void fromEulerAngles(const T& yaw, const T& pitch, const T& roll);
165
172 void toAxisAngle(TVector3<T>& axis, T& angle);
173
181 void toEulerAngles(T& yaw, T& pitch, T& roll);
182
188 void get(TQuaternion& q) const;
189
194 T getAngle() const;
195
201
208
214
222
226
231 T& w();
232
237 const T& w() const;
238
243 T& i();
244
249 const T& i() const;
250
255 T& j();
256
261 const T& j() const;
262
267 T& k();
268
273 const T& k() const;
274
275
277
280
288 void dump(std::ostream& s = std::cout, Size depth = 0) const;
290 };
292
293 template <typename T>
295 : Eigen::Quaternion<T>()
296 {
297 this->setIdentity();
298 }
299
300 template <typename T>
302 : Eigen::Quaternion<T>(q)
303 {
304 }
305
306 template <typename T>
307 TQuaternion<T>::TQuaternion(const Eigen::Quaternion<T>& q)
308 : Eigen::Quaternion<T>(q)
309 {
310 }
311
312 template <typename T>
313 TQuaternion<T>::TQuaternion(const T& a, const T& b, const T& c, const T& d)
314 : Eigen::Quaternion<T>(a, b, c, d)
315 {
316 }
317
318 template <typename T>
319 TQuaternion<T>::TQuaternion(const TVector3<T>& axis, const T& angle)
320 : Eigen::Quaternion<T>()
321 {
322 fromAxisAngle(axis, angle);
323 }
324
325 template <typename T>
329
330 template <typename T>
332 {
333 this->setIdentity();
334 }
335
336 template <typename T>
338 {
339 Eigen::Quaternion<T>::operator= (q);
340 }
341
342 template <typename T>
343 void TQuaternion<T>::set(const Eigen::Quaternion<T>& q)
344 {
345 Eigen::Quaternion<T>::operator= (q);
346 }
347
348 template <typename T>
350 void TQuaternion<T>::set(const T& a, const T& b, const T& c, const T& d)
351 {
352 w() = a;
353 i() = b;
354 j() = c;
355 k() = d;
356 }
357
358 template <typename T>
360 TQuaternion<T>& TQuaternion<T>::operator = (const Eigen::Quaternion<T>& q)
361 {
362 set(q);
363 return *this;
364 }
365
366 template <typename T>
369 {
370 set(q);
371 return *this;
372 }
373
374 template <typename T>
377 {
378 return Eigen::Quaternion<T>::isApprox(q);
379 }
380
381 template <typename T>
384 {
385 Eigen::Quaternion<T>::setIdentity();
386 }
387
388 template <typename T>
391 {
392 Eigen::Quaternion<T>::normalize();
393
394 return *this;
395 }
396
397 template <typename T>
399 {
400 TQuaternion<T> tmp(q);
401
402 q = *this;
403 *this = tmp;
404 }
405
406 template <typename T>
407 void TQuaternion<T>::fromAxisAngle(const TVector3<T>& axis, const T& angle)
408 {
409 TVector3<T> axis_normalized(axis);
410 axis_normalized.normalize();
411
412 Eigen::AngleAxis<T> aa(angle, Eigen::Matrix<T, 3, 1>(axis_normalized.x, axis_normalized.y, axis_normalized.z));
413 Eigen::Quaternion<T> q(aa);
414
415 set(q.w(), q.x(), q.y(), q.z());
416 }
417
418 template <typename T>
419 void TQuaternion<T>::fromEulerAngles(const T& yaw, const T& pitch, const T& roll)
420 {
421 T half_yaw = yaw / 2.0;
422 T half_pitch = pitch / 2.0;
423 T half_roll = roll / 2.0;
424
425 T cosYaw = cos(half_yaw);
426 T sinYaw = sin(half_yaw);
427
428 T cosPitch = cos(half_pitch);
429 T sinPitch = sin(half_pitch);
430
431 T cosRoll = cos(half_roll);
432 T sinRoll = sin(half_roll);
433
434 w() = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
435 i() = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
436 j() = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
437 k() = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
438 }
439
440 template <typename T>
442 {
443 Eigen::AngleAxis<T> aa(*this);
444
445 Eigen::Matrix<T, 3, 1> eigen_axis = aa.axis();
446 axis.set(eigen_axis[0], eigen_axis[1], eigen_axis[2]);
447 angle = aa.angle();
448 }
449
450
451 template <typename T>
452 void TQuaternion<T>::toEulerAngles(T& yaw, T& pitch, T& roll)
453 {
454 TMatrix4x4<T> matrix;
455 getRotationMatrix(matrix);
456 T sinYaw, cosYaw, sinPitch, cosPitch, sinRoll, cosRoll;
457
458 sinPitch = -matrix(2,0);
459 cosPitch = sqrt(1 - sinPitch*sinPitch);
460
461 if ( fabs(cosPitch) > Constants::EPSILON)
462 {
463 sinRoll = matrix(2,1) / cosPitch;
464 cosRoll = matrix(2,2) / cosPitch;
465 sinYaw = matrix(1,0) / cosPitch;
466 cosYaw = matrix(0,0) / cosPitch;
467 }
468 else
469 {
470 sinRoll = -matrix(1,2);
471 cosRoll = matrix(1,1);
472 sinYaw = 0;
473 cosYaw = 1;
474 }
475
476 // yaw
477 yaw = atan2(sinYaw, cosYaw);
478
479 // pitch
480 pitch = atan2(sinPitch, cosPitch);
481
482 // roll
483 roll = atan2(sinRoll, cosRoll);
484 }
485
486 template <typename T>
489 {
490 q.set(*this);
491 }
492
493 template <typename T>
495 {
496 Eigen::AngleAxis<T> aa(*this);
497
498 return aa.angle();
499 }
500
501 template <typename T>
503 {
504 Eigen::AngleAxis<T> aa(*this);
505 Eigen::Matrix<T, 3, 1> eigen_axis = aa.axis();
506
507 return TVector3<T>(eigen_axis[0], eigen_axis[1], eigen_axis[2]);
508 }
509
510 template <typename T>
512 {
513 Eigen::Matrix<T, 3, 3> eigen_rot = this->toRotationMatrix();
514
515 m.set(
516 eigen_rot(0, 0), eigen_rot(0, 1), eigen_rot(0, 2), 0.0,
517 eigen_rot(1, 0), eigen_rot(1, 1), eigen_rot(1, 2), 0.0,
518 eigen_rot(2, 0), eigen_rot(2, 1), eigen_rot(2, 2), 0.0,
519 0.0, 0.0, 0.0, 1.0
520 );
521
522 return m;
523 }
524
525 template <typename T>
528 {
529 return Eigen::Quaternion<T>::inverse();
530 }
531
532 template <typename T>
535 {
536 return Eigen::Quaternion<T>::conjugate();
537 }
538
539 template <typename T>
540 const T& TQuaternion<T>::w() const
541 {
542 return Eigen::Quaternion<T>::coeffs()[3];
543 }
544
545 template <typename T>
547 {
548 return Eigen::Quaternion<T>::coeffs()[3];
549 }
550
551 template <typename T>
552 const T& TQuaternion<T>::i() const
553 {
554 return Eigen::Quaternion<T>::coeffs()[0];
555 }
556
557 template <typename T>
559 {
560 return Eigen::Quaternion<T>::coeffs()[0];
561 }
562
563 template <typename T>
564 const T& TQuaternion<T>::j() const
565 {
566 return Eigen::Quaternion<T>::coeffs()[1];
567 }
568
569 template <typename T>
571 {
572 return Eigen::Quaternion<T>::coeffs()[1];
573 }
574
575 template <typename T>
576 const T& TQuaternion<T>::k() const
577 {
578 return Eigen::Quaternion<T>::coeffs()[2];
579 }
580
581 template <typename T>
583 {
584 return Eigen::Quaternion<T>::coeffs()[2];
585 }
586
587 template <typename T>
588 std::istream& operator >>(std::istream& s, TQuaternion<T>& q)
589 {
590 char c;
591 s >> c >> q.w() >> c >> q.i() >> c >> q.j() >> c >> q.k() >> c;
592 return s;
593 }
594
595 template <typename T>
596 std::ostream& operator << (std::ostream& s, const TQuaternion<T>& q)
597 {
598 s << '(' << q.w() << ',' << q.i() << ','
599 << q.j() << ',' << q.k() << ')';
600
601 return s;
602 }
603
604 template <typename T>
605 void TQuaternion<T>::dump(std::ostream& s, Size depth) const
606 {
608
609 BALL_DUMP_HEADER(s, this, this);
610
611 BALL_DUMP_DEPTH(s, depth);
612 s << " w: " << w() << std::endl;
613
614 BALL_DUMP_DEPTH(s, depth);
615 s << " i: " << i() << std::endl;
616
617 BALL_DUMP_DEPTH(s, depth);
618 s << " j: " << j() << std::endl;
619
620 BALL_DUMP_DEPTH(s, depth);
621 s << " k: " << k() << std::endl;
622
624 }
625
627
628} // namespace BALL
629
630#endif // BALL_MATHS_QUATERNION_H
#define BALL_DUMP_STREAM_PREFIX(os)
Definition macros.h:391
#define BALL_DUMP_STREAM_SUFFIX(os)
Definition macros.h:395
#define BALL_DUMP_DEPTH(os, depth)
Definition macros.h:390
#define BALL_DUMP_HEADER(os, cl, ob)
Definition macros.h:393
#define BALL_INLINE
Definition debug.h:15
BALL_EXPORT std::ostream & operator<<(std::ostream &os, const Exception::GeneralException &e)
std::istream & operator>>(std::istream &is, TRegularData1D< ValueType > &grid)
Input operator.
TQuaternion< float > Quaternion
Definition quaternion.h:626
BALL_EXTERN_VARIABLE double EPSILON
Definition constants.h:43
Default Type.
Definition matrix44.h:68
void set(const T *ptr)
Definition matrix44.h:825
TQuaternion(const Eigen::Quaternion< T > &q)
Definition quaternion.h:307
void set(const TQuaternion &q)
Definition quaternion.h:337
TVector3< T > getAxis()
Definition quaternion.h:502
TQuaternion(const T &a, const T &b, const T &c, const T &d)
Definition quaternion.h:313
const T & j() const
Definition quaternion.h:564
void dump(std::ostream &s=std::cout, Size depth=0) const
Definition quaternion.h:605
TQuaternion getConjugate() const
Definition quaternion.h:534
const T & k() const
Definition quaternion.h:576
void fromAxisAngle(const TVector3< T > &axis, const T &angle)
Definition quaternion.h:407
void fromEulerAngles(const T &yaw, const T &pitch, const T &roll)
Definition quaternion.h:419
void get(TQuaternion &q) const
Definition quaternion.h:488
void swap(TQuaternion &q)
Definition quaternion.h:398
const T & i() const
Definition quaternion.h:552
TQuaternion(const TQuaternion &q)
Definition quaternion.h:301
TQuaternion(const TVector3< T > &axis, const T &angle)
Definition quaternion.h:319
TMatrix4x4< T > & getRotationMatrix(TMatrix4x4< T > &m) const
Definition quaternion.h:511
const T & w() const
Definition quaternion.h:540
void toAxisAngle(TVector3< T > &axis, T &angle)
Definition quaternion.h:441
TQuaternion & operator=(const TQuaternion &q)
Definition quaternion.h:368
bool operator==(const TQuaternion< T > &q)
Equality operator Returns true if the given Quaternion q is equal to this.
Definition quaternion.h:376
T getAngle() const
Definition quaternion.h:494
TQuaternion< T > & normalize()
Definition quaternion.h:390
void set(const T &a, const T &b, const T &c, const T &d)
Definition quaternion.h:350
TQuaternion getInverse() const
Definition quaternion.h:527
void set(const Eigen::Quaternion< T > &q)
Definition quaternion.h:343
void toEulerAngles(T &yaw, T &pitch, T &roll)
Definition quaternion.h:452
TVector3 & normalize()
Definition vector3.h:770
void set(const T *ptr)
Definition vector3.h:615