Loading...
Searching...
No Matches
quaternion.h
Go to the documentation of this file.
// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
#ifndef BALL_MATHS_QUATERNION_H
#define BALL_MATHS_QUATERNION_H
#include <BALL/MATHS/matrix44.h>
#include <BALL/MATHS/vector3.h>
#include <Eigen/Geometry>
#include <iostream>
namespace BALL
{
template <typename T>
class TQuaternion
: public Eigen::Quaternion<T> //{/*...*/};
{
public:
TQuaternion();
TQuaternion(const TQuaternion& q);
TQuaternion(const Eigen::Quaternion<T>& q);
TQuaternion(const T& a, const T& b, const T& c, const T& d);
TQuaternion(const TVector3<T>& axis, const T& angle);
~TQuaternion();
void clear();
void set(const TQuaternion& q);
void set(const Eigen::Quaternion<T>& q);
void set(const T& a, const T& b, const T& c, const T& d);
TQuaternion& operator = (const TQuaternion& q);
TQuaternion& operator = (const Eigen::Quaternion<T>& q);
bool operator == (const TQuaternion<T>& q);
void setIdentity();
TQuaternion<T>& normalize();
void swap(TQuaternion& q);
void fromAxisAngle(const TVector3<T>& axis, const T& angle);
void fromEulerAngles(const T& yaw, const T& pitch, const T& roll);
void toAxisAngle(TVector3<T>& axis, T& angle);
void toEulerAngles(T& yaw, T& pitch, T& roll);
void get(TQuaternion& q) const;
T getAngle() const;
TVector3<T> getAxis();
TMatrix4x4<T>& getRotationMatrix(TMatrix4x4<T>& m) const;
TQuaternion getInverse() const;
TQuaternion getConjugate() const;
T& w();
const T& w() const;
T& i();
const T& i() const;
T& j();
const T& j() const;
T& k();
const T& k() const;
void dump(std::ostream& s = std::cout, Size depth = 0) const;
};
template <typename T>
TQuaternion<T>::TQuaternion()
: Eigen::Quaternion<T>()
{
this->setIdentity();
}
template <typename T>
TQuaternion<T>::TQuaternion(const TQuaternion& q)
: Eigen::Quaternion<T>(q)
{
}
template <typename T>
TQuaternion<T>::TQuaternion(const Eigen::Quaternion<T>& q)
: Eigen::Quaternion<T>(q)
{
}
template <typename T>
TQuaternion<T>::TQuaternion(const T& a, const T& b, const T& c, const T& d)
: Eigen::Quaternion<T>(a, b, c, d)
{
}
template <typename T>
TQuaternion<T>::TQuaternion(const TVector3<T>& axis, const T& angle)
: Eigen::Quaternion<T>()
{
fromAxisAngle(axis, angle);
}
template <typename T>
TQuaternion<T>::~TQuaternion()
{
}
template <typename T>
void TQuaternion<T>::clear()
{
this->setIdentity();
}
template <typename T>
void TQuaternion<T>::set(const TQuaternion<T>& q)
{
Eigen::Quaternion<T>::operator= (q);
}
template <typename T>
void TQuaternion<T>::set(const Eigen::Quaternion<T>& q)
{
Eigen::Quaternion<T>::operator= (q);
}
template <typename T>
BALL_INLINE
void TQuaternion<T>::set(const T& a, const T& b, const T& c, const T& d)
{
w() = a;
i() = b;
j() = c;
k() = d;
}
template <typename T>
BALL_INLINE
TQuaternion<T>& TQuaternion<T>::operator = (const Eigen::Quaternion<T>& q)
{
set(q);
return *this;
}
template <typename T>
BALL_INLINE
TQuaternion<T>& TQuaternion<T>::operator = (const TQuaternion<T>& q)
{
set(q);
return *this;
}
template <typename T>
BALL_INLINE
bool TQuaternion<T>::operator == (const TQuaternion<T>& q)
{
return Eigen::Quaternion<T>::isApprox(q);
}
template <typename T>
BALL_INLINE
void TQuaternion<T>::setIdentity()
{
Eigen::Quaternion<T>::setIdentity();
}
template <typename T>
BALL_INLINE
TQuaternion<T>& TQuaternion<T>::normalize()
{
Eigen::Quaternion<T>::normalize();
return *this;
}
template <typename T>
void TQuaternion<T>::swap(TQuaternion<T>& q)
{
TQuaternion<T> tmp(q);
q = *this;
*this = tmp;
}
template <typename T>
void TQuaternion<T>::fromAxisAngle(const TVector3<T>& axis, const T& angle)
{
TVector3<T> axis_normalized(axis);
axis_normalized.normalize();
Eigen::AngleAxis<T> aa(angle, Eigen::Matrix<T, 3, 1>(axis_normalized.x, axis_normalized.y, axis_normalized.z));
Eigen::Quaternion<T> q(aa);
set(q.w(), q.x(), q.y(), q.z());
}
template <typename T>
void TQuaternion<T>::fromEulerAngles(const T& yaw, const T& pitch, const T& roll)
{
T half_yaw = yaw / 2.0;
T half_pitch = pitch / 2.0;
T half_roll = roll / 2.0;
T cosYaw = cos(half_yaw);
T sinYaw = sin(half_yaw);
T cosPitch = cos(half_pitch);
T sinPitch = sin(half_pitch);
T cosRoll = cos(half_roll);
T sinRoll = sin(half_roll);
w() = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
i() = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
j() = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
k() = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
}
template <typename T>
void TQuaternion<T>::toAxisAngle(TVector3<T>& axis, T& angle)
{
Eigen::AngleAxis<T> aa(*this);
Eigen::Matrix<T, 3, 1> eigen_axis = aa.axis();
axis.set(eigen_axis[0], eigen_axis[1], eigen_axis[2]);
angle = aa.angle();
}
template <typename T>
void TQuaternion<T>::toEulerAngles(T& yaw, T& pitch, T& roll)
{
TMatrix4x4<T> matrix;
getRotationMatrix(matrix);
T sinYaw, cosYaw, sinPitch, cosPitch, sinRoll, cosRoll;
sinPitch = -matrix(2,0);
cosPitch = sqrt(1 - sinPitch*sinPitch);
if ( fabs(cosPitch) > Constants::EPSILON)
{
sinRoll = matrix(2,1) / cosPitch;
cosRoll = matrix(2,2) / cosPitch;
sinYaw = matrix(1,0) / cosPitch;
cosYaw = matrix(0,0) / cosPitch;
}
else
{
sinRoll = -matrix(1,2);
cosRoll = matrix(1,1);
sinYaw = 0;
cosYaw = 1;
}
// yaw
yaw = atan2(sinYaw, cosYaw);
// pitch
pitch = atan2(sinPitch, cosPitch);
// roll
roll = atan2(sinRoll, cosRoll);
}
template <typename T>
BALL_INLINE
void TQuaternion<T>::get(TQuaternion<T>& q) const
{
q.set(*this);
}
template <typename T>
T TQuaternion<T>::getAngle() const
{
Eigen::AngleAxis<T> aa(*this);
return aa.angle();
}
template <typename T>
TVector3<T> TQuaternion<T>::getAxis()
{
Eigen::AngleAxis<T> aa(*this);
Eigen::Matrix<T, 3, 1> eigen_axis = aa.axis();
return TVector3<T>(eigen_axis[0], eigen_axis[1], eigen_axis[2]);
}
template <typename T>
TMatrix4x4<T>& TQuaternion<T>::getRotationMatrix(TMatrix4x4<T>& m) const
{
Eigen::Matrix<T, 3, 3> eigen_rot = this->toRotationMatrix();
m.set(
eigen_rot(0, 0), eigen_rot(0, 1), eigen_rot(0, 2), 0.0,
eigen_rot(1, 0), eigen_rot(1, 1), eigen_rot(1, 2), 0.0,
eigen_rot(2, 0), eigen_rot(2, 1), eigen_rot(2, 2), 0.0,
0.0, 0.0, 0.0, 1.0
);
return m;
}
template <typename T>
BALL_INLINE
TQuaternion<T> TQuaternion<T>::getInverse() const
{
return Eigen::Quaternion<T>::inverse();
}
template <typename T>
BALL_INLINE
TQuaternion<T> TQuaternion<T>::getConjugate() const
{
return Eigen::Quaternion<T>::conjugate();
}
template <typename T>
const T& TQuaternion<T>::w() const
{
return Eigen::Quaternion<T>::coeffs()[3];
}
template <typename T>
T& TQuaternion<T>::w()
{
return Eigen::Quaternion<T>::coeffs()[3];
}
template <typename T>
const T& TQuaternion<T>::i() const
{
return Eigen::Quaternion<T>::coeffs()[0];
}
template <typename T>
T& TQuaternion<T>::i()
{
return Eigen::Quaternion<T>::coeffs()[0];
}
template <typename T>
const T& TQuaternion<T>::j() const
{
return Eigen::Quaternion<T>::coeffs()[1];
}
template <typename T>
T& TQuaternion<T>::j()
{
return Eigen::Quaternion<T>::coeffs()[1];
}
template <typename T>
const T& TQuaternion<T>::k() const
{
return Eigen::Quaternion<T>::coeffs()[2];
}
template <typename T>
T& TQuaternion<T>::k()
{
return Eigen::Quaternion<T>::coeffs()[2];
}
template <typename T>
std::istream& operator >>(std::istream& s, TQuaternion<T>& q)
{
char c;
s >> c >> q.w() >> c >> q.i() >> c >> q.j() >> c >> q.k() >> c;
return s;
}
template <typename T>
std::ostream& operator << (std::ostream& s, const TQuaternion<T>& q)
{
s << '(' << q.w() << ',' << q.i() << ','
<< q.j() << ',' << q.k() << ')';
return s;
}
template <typename T>
void TQuaternion<T>::dump(std::ostream& s, Size depth) const
{
BALL_DUMP_STREAM_PREFIX(s);
BALL_DUMP_HEADER(s, this, this);
BALL_DUMP_DEPTH(s, depth);
s << " w: " << w() << std::endl;
BALL_DUMP_DEPTH(s, depth);
s << " i: " << i() << std::endl;
BALL_DUMP_DEPTH(s, depth);
s << " j: " << j() << std::endl;
BALL_DUMP_DEPTH(s, depth);
s << " k: " << k() << std::endl;
BALL_DUMP_STREAM_SUFFIX(s);
}
typedef TQuaternion<float> Quaternion;
} // namespace BALL
#endif // BALL_MATHS_QUATERNION_H
BALL::TMatrix4x4
Default Type.
Definition matrix44.h:68
BALL::TMatrix4x4::set
void set(const T *ptr)
Definition matrix44.h:825
BALL::TQuaternion
Definition quaternion.h:25
BALL::TQuaternion::TQuaternion
TQuaternion(const Eigen::Quaternion< T > &q)
Definition quaternion.h:307
BALL::TQuaternion::set
void set(const TQuaternion &q)
Definition quaternion.h:337
BALL::TQuaternion::getAxis
TVector3< T > getAxis()
Definition quaternion.h:502
BALL::TQuaternion::w
T & w()
Definition quaternion.h:546
BALL::TQuaternion::TQuaternion
TQuaternion(const T &a, const T &b, const T &c, const T &d)
Definition quaternion.h:313
BALL::TQuaternion::j
const T & j() const
Definition quaternion.h:564
BALL::TQuaternion::TQuaternion
TQuaternion()
Definition quaternion.h:294
BALL::TQuaternion::dump
void dump(std::ostream &s=std::cout, Size depth=0) const
Definition quaternion.h:605
BALL::TQuaternion::getConjugate
TQuaternion getConjugate() const
Definition quaternion.h:534
BALL::TQuaternion::k
const T & k() const
Definition quaternion.h:576
BALL::TQuaternion::fromAxisAngle
void fromAxisAngle(const TVector3< T > &axis, const T &angle)
Definition quaternion.h:407
BALL::TQuaternion::fromEulerAngles
void fromEulerAngles(const T &yaw, const T &pitch, const T &roll)
Definition quaternion.h:419
BALL::TQuaternion::get
void get(TQuaternion &q) const
Definition quaternion.h:488
BALL::TQuaternion::swap
void swap(TQuaternion &q)
Definition quaternion.h:398
BALL::TQuaternion::i
const T & i() const
Definition quaternion.h:552
BALL::TQuaternion::TQuaternion
TQuaternion(const TQuaternion &q)
Definition quaternion.h:301
BALL::TQuaternion::TQuaternion
TQuaternion(const TVector3< T > &axis, const T &angle)
Definition quaternion.h:319
BALL::TQuaternion::getRotationMatrix
TMatrix4x4< T > & getRotationMatrix(TMatrix4x4< T > &m) const
Definition quaternion.h:511
BALL::TQuaternion::w
const T & w() const
Definition quaternion.h:540
BALL::TQuaternion::k
T & k()
Definition quaternion.h:582
BALL::TQuaternion::toAxisAngle
void toAxisAngle(TVector3< T > &axis, T &angle)
Definition quaternion.h:441
BALL::TQuaternion::~TQuaternion
~TQuaternion()
Definition quaternion.h:326
BALL::TQuaternion::j
T & j()
Definition quaternion.h:570
BALL::TQuaternion::operator=
TQuaternion & operator=(const TQuaternion &q)
Definition quaternion.h:368
BALL::TQuaternion::operator==
bool operator==(const TQuaternion< T > &q)
Equality operator Returns true if the given Quaternion q is equal to this.
Definition quaternion.h:376
BALL::TQuaternion::getAngle
T getAngle() const
Definition quaternion.h:494
BALL::TQuaternion::i
T & i()
Definition quaternion.h:558
BALL::TQuaternion::clear
void clear()
Definition quaternion.h:331
BALL::TQuaternion::normalize
TQuaternion< T > & normalize()
Definition quaternion.h:390
BALL::TQuaternion::set
void set(const T &a, const T &b, const T &c, const T &d)
Definition quaternion.h:350
BALL::TQuaternion::setIdentity
void setIdentity()
Definition quaternion.h:383
BALL::TQuaternion::getInverse
TQuaternion getInverse() const
Definition quaternion.h:527
BALL::TQuaternion::set
void set(const Eigen::Quaternion< T > &q)
Definition quaternion.h:343
BALL::TQuaternion::toEulerAngles
void toEulerAngles(T &yaw, T &pitch, T &roll)
Definition quaternion.h:452
BALL::TVector3
Definition vector3.h:71
BALL::TVector3::x
T x
Definition vector3.h:489
BALL::TVector3::y
T y
Definition vector3.h:493
BALL::TVector3::normalize
TVector3 & normalize()
Definition vector3.h:770
BALL::TVector3::set
void set(const T *ptr)
Definition vector3.h:615
BALL::TVector3::z
T z
Definition vector3.h:497
BALL_INLINE
#define BALL_INLINE
Definition debug.h:15
BALL::operator<<
BALL_EXPORT std::ostream & operator<<(std::ostream &os, const Exception::GeneralException &e)
BALL_DUMP_STREAM_PREFIX
#define BALL_DUMP_STREAM_PREFIX(os)
Definition macros.h:391
BALL_DUMP_STREAM_SUFFIX
#define BALL_DUMP_STREAM_SUFFIX(os)
Definition macros.h:395
BALL_DUMP_DEPTH
#define BALL_DUMP_DEPTH(os, depth)
Definition macros.h:390
BALL_DUMP_HEADER
#define BALL_DUMP_HEADER(os, cl, ob)
Definition macros.h:393
matrix44.h
BALL::Constants::EPSILON
BALL_EXTERN_VARIABLE double EPSILON
Definition constants.h:43
BALL
Definition constants.h:13
BALL::Size
BALL_SIZE_TYPE Size
Definition COMMON/global.h:114
BALL::operator>>
std::istream & operator>>(std::istream &is, TRegularData1D< ValueType > &grid)
Input operator.
Definition regularData1D.h:802
BALL::Quaternion
TQuaternion< float > Quaternion
Definition quaternion.h:626
vector3.h