eXperiment robotics framework

Quaternion Class Reference
[Quaternions]

The Quaternion is an appropriate (although not very intuitive) representation for 3D rotations and orientations. More...

#include <erf/erf_quaternion.hpp>>

List of all members.

Defining a Quaternion

 Quaternion ()
 Default constructor, builds an identity rotation.
 Quaternion (const Vec3 &axis, float angle)
 Constructor from rotation axis (non null) and angle (in radians).
 Quaternion (float roll, float pitch, float yaw)
void setRollPitchYaw (float roll, float pitch, float yaw)
Vec3 getEuler () const
void setEuler (float roll, float pitch, float yaw)
Vec3 getRollPitchYaw () const
float roll () const
float pitch () const
float yaw () const
 Quaternion (const Vec3 &from, const Vec3 &to)
 Quaternion (float q0, float q1, float q2, float q3)
 Constructor from the four values of a Quaternion.
 Quaternion (const Quaternion &Q)
 Copy constructor.
Quaternionoperator= (const Quaternion &Q)
 Equal operator.
void setAxisAngle (const Vec3 &axis, float angle)
 Sets the Quaternion as a rotation of axis axis and angle angle (in radians).
void setValue (float q0, float q1, float q2, float q3)
 Sets the Quaternion value.
void setFromRotationMatrix (const float m[3][3])
void setFromRotatedBase (const Vec3 &X, const Vec3 &Y, const Vec3 &Z)

Accessing values

Vec3 axis () const
float angle () const
void getAxisAngle (Vec3 &axis, float &angle) const
float operator[] (int i) const
 Bracket operator, with a constant return value.
float & operator[] (int i)
 Bracket operator returning an l-value.

Rotation computations

Quaternionoperator*= (const Quaternion &q)
 Quaternion rotation is composed with q.
Vec3 rotate (const Vec3 &v) const
Vec3 inverseRotate (const Vec3 &v) const
void rotateRoll (float roll)
void rotatePitch (float pitch)
void rotateYaw (float yaw)
Quaternion operator* (const Quaternion &a, const Quaternion &b)
 Returns the composition of the a and b rotations.
Vec3 operator* (const Quaternion &q, const Vec3 &v)
 Returns the image of v by the rotation q.

Inversion

Quaternion inverse () const
 Returns the inverse Quaternion (inverse rotation).
void invert ()
 Inverses the Quaternion (same rotation angle(), but negated axis()).
void negate ()
 Negates all the coefficients of the Quaternion.
float normalize ()
 Normalizes the Quaternion coefficients.

Associated matrix

const GLfloat * matrix () const
void getMatrix (GLfloat m[4][4]) const
void getMatrix (GLfloat m[16]) const
void getRotationMatrix (float m[3][3]) const
const GLfloat * inverseMatrix () const
void getInverseMatrix (GLfloat m[4][4]) const
void getInverseMatrix (GLfloat m[16]) const
void getInverseRotationMatrix (float m[3][3]) const

Slerp interpolation

Quaternion log ()
Quaternion exp ()
static Quaternion slerp (const Quaternion &a, const Quaternion &b, float t, bool allowFlip=true)
static Quaternion squad (const Quaternion &a, const Quaternion &tgA, const Quaternion &tgB, const Quaternion &b, float t)
static float dot (const Quaternion &a, const Quaternion &b)
 Returns the "dot" product of a and b: a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3].
static Quaternion lnDif (const Quaternion &a, const Quaternion &b)
static Quaternion squadTangent (const Quaternion &before, const Quaternion &center, const Quaternion &after)

Random Quaternion

static Quaternion randomQuaternion ()

Public Attributes

union {
   struct {
      float   q0
      float   q1
      float   q2
      float   q3
   } 
   float   q [4]
}; 


Detailed Description

The Quaternion is an appropriate (although not very intuitive) representation for 3D rotations and orientations.

Many tools are provided to ease the definition of a Quaternion: see constructors, setAxisAngle(), setFromRotationMatrix(), setFromRotatedBase().

You can apply the rotation represented by the Quaternion to 3D points using rotate() and inverseRotate(). See also the Frame class that represents a coordinate system and provides other conversion functions like Frame::coordinatesOf() and Frame::transformOf().

You can apply the Quaternion rotation to the OpenGL matrices with code like:

Quaternion q;
glMultMatrixf(q.matrix());
// or using glRotate instead:
glRotate(q.angle()*180.0/M_PI, q.axis().x, q.axis().y, q.axis().z);

Quaternion is part of the namespace, specify Quaternion or use the namespace:

Internal representation

The internal representation of a Quaternion corresponding to a rotation around axis axis, with an angle alpha is made of four floats q[i]:

{q[0],q[1],q[2]} = sin(alpha/2) * {axis[0],axis[1],axis[2]}
q[3] = cos(alpha/2)

Note that certain implementations place the cosine term in first position (instead of last here).

The Quaternion is always normalized, so that its inverse() is actually its conjugate.

See also the Vec3 and Frame classes' documentations.

Definition at line 54 of file erf_quaternion.hpp.


Constructor & Destructor Documentation

Quaternion::Quaternion (  )  [inline]

Default constructor, builds an identity rotation.

Definition at line 60 of file erf_quaternion.hpp.

References q.

Referenced by inverse().

Quaternion::Quaternion ( const Vec3 axis,
float  angle 
) [inline]

Constructor from rotation axis (non null) and angle (in radians).

See also setAxisAngle().

Definition at line 67 of file erf_quaternion.hpp.

References setAxisAngle().

Quaternion::Quaternion ( float  roll,
float  pitch,
float  yaw 
) [inline]

Definition at line 72 of file erf_quaternion.hpp.

References setEuler().

Quaternion::Quaternion ( const Vec3 from,
const Vec3 to 
)

Quaternion::Quaternion ( float  q0,
float  q1,
float  q2,
float  q3 
) [inline]

Constructor from the four values of a Quaternion.

First three values are axis*sin(angle/2) and last one is cos(angle/2).

Attention:
The identity Quaternion is Quaternion(0,0,0,1) and not Quaternion(0,0,0,0) (which is not unitary). The default Quaternion() creates such identity Quaternion.

Definition at line 97 of file erf_quaternion.hpp.

References q.

Quaternion::Quaternion ( const Quaternion Q  )  [inline]

Copy constructor.

Definition at line 106 of file erf_quaternion.hpp.

References q.


Member Function Documentation

void Quaternion::setRollPitchYaw ( float  roll,
float  pitch,
float  yaw 
) [inline]

Definition at line 76 of file erf_quaternion.hpp.

References setEuler().

Vec3 Quaternion::getEuler (  )  const [inline]

Definition at line 80 of file erf_quaternion.hpp.

References getRollPitchYaw().

void Quaternion::setEuler ( float  roll,
float  pitch,
float  yaw 
)

Referenced by Quaternion(), and setRollPitchYaw().

Vec3 Quaternion::getRollPitchYaw (  )  const

Referenced by getEuler().

float Quaternion::roll (  )  const

float Quaternion::pitch (  )  const

float Quaternion::yaw (  )  const

Quaternion& Quaternion::operator= ( const Quaternion Q  )  [inline]

Equal operator.

Definition at line 115 of file erf_quaternion.hpp.

References q.

void Quaternion::setAxisAngle ( const Vec3 axis,
float  angle 
) [inline]

Sets the Quaternion as a rotation of axis axis and angle angle (in radians).

axis does not need to be normalized. A null axis will result in an identity Quaternion.

Definition at line 127 of file erf_quaternion.hpp.

References Vec3::norm(), and q.

Referenced by Quaternion().

void Quaternion::setValue ( float  q0,
float  q1,
float  q2,
float  q3 
) [inline]

Sets the Quaternion value.

See the Quaternion(float, float, float, float) constructor documentation.

Definition at line 149 of file erf_quaternion.hpp.

References q.

void Quaternion::setFromRotationMatrix ( const float  m[3][3]  ) 

void Quaternion::setFromRotatedBase ( const Vec3 X,
const Vec3 Y,
const Vec3 Z 
)

Vec3 Quaternion::axis (  )  const

float Quaternion::angle (  )  const

void Quaternion::getAxisAngle ( Vec3 axis,
float &  angle 
) const

float Quaternion::operator[] ( int  i  )  const [inline]

Bracket operator, with a constant return value.

i must range in [0..3]. See the Quaternion(float, float, float, float) documentation.

Definition at line 171 of file erf_quaternion.hpp.

References q.

float& Quaternion::operator[] ( int  i  )  [inline]

Bracket operator returning an l-value.

i must range in [0..3]. See the Quaternion(float, float, float, float) documentation.

Definition at line 177 of file erf_quaternion.hpp.

References q.

Quaternion& Quaternion::operator*= ( const Quaternion q  )  [inline]

Quaternion rotation is composed with q.

See operator*(), since this is equivalent to this = this * q.

Note:
For efficiency reasons, the resulting Quaternion is not normalized. You may normalize() it after each application in case of numerical drift.

Definition at line 211 of file erf_quaternion.hpp.

Vec3 Quaternion::rotate ( const Vec3 v  )  const

Vec3 Quaternion::inverseRotate ( const Vec3 v  )  const

void Quaternion::rotateRoll ( float  roll  ) 

void Quaternion::rotatePitch ( float  pitch  ) 

void Quaternion::rotateYaw ( float  yaw  ) 

Quaternion Quaternion::inverse (  )  const [inline]

Returns the inverse Quaternion (inverse rotation).

Result has a negated axis() direction and the same angle(). A composition (see operator*()) of a Quaternion and its inverse() results in an identity function.

Use invert() to actually modify the Quaternion.

Definition at line 242 of file erf_quaternion.hpp.

References q, and Quaternion().

void Quaternion::invert (  )  [inline]

Inverses the Quaternion (same rotation angle(), but negated axis()).

See also inverse().

Definition at line 250 of file erf_quaternion.hpp.

References q.

Referenced by negate().

void Quaternion::negate (  )  [inline]

Negates all the coefficients of the Quaternion.

This results in an other representation of the same rotation (opposite rotation angle, but with a negated axis direction: the two cancel out). However, note that the results of axis() and angle() are unchanged after a call to this method since angle() always returns a value in [0,pi].

This method is mainly useful for Quaternion interpolation, so that the spherical interpolation takes the shortest path on the unit sphere. See slerp() for details.

Definition at line 265 of file erf_quaternion.hpp.

References invert(), and q.

float Quaternion::normalize (  )  [inline]

Normalizes the Quaternion coefficients.

This method should not need to be called since we only deal with unit Quaternions. This is however useful to prevent numerical drifts, especially with small rotational increments.

Definition at line 274 of file erf_quaternion.hpp.

References q.

const GLfloat* Quaternion::matrix (  )  const

void Quaternion::getMatrix ( GLfloat  m[4][4]  )  const

void Quaternion::getMatrix ( GLfloat  m[16]  )  const

void Quaternion::getRotationMatrix ( float  m[3][3]  )  const

const GLfloat* Quaternion::inverseMatrix (  )  const

void Quaternion::getInverseMatrix ( GLfloat  m[4][4]  )  const

void Quaternion::getInverseMatrix ( GLfloat  m[16]  )  const

void Quaternion::getInverseRotationMatrix ( float  m[3][3]  )  const

static Quaternion Quaternion::slerp ( const Quaternion a,
const Quaternion b,
float  t,
bool  allowFlip = true 
) [static]

static Quaternion Quaternion::squad ( const Quaternion a,
const Quaternion tgA,
const Quaternion tgB,
const Quaternion b,
float  t 
) [static]

static float Quaternion::dot ( const Quaternion a,
const Quaternion b 
) [inline, static]

Returns the "dot" product of a and b: a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3].

Definition at line 305 of file erf_quaternion.hpp.

Quaternion Quaternion::log (  ) 

Quaternion Quaternion::exp (  ) 

static Quaternion Quaternion::lnDif ( const Quaternion a,
const Quaternion b 
) [static]

static Quaternion Quaternion::squadTangent ( const Quaternion before,
const Quaternion center,
const Quaternion after 
) [static]

static Quaternion Quaternion::randomQuaternion (  )  [static]


Friends And Related Function Documentation

Quaternion operator* ( const Quaternion a,
const Quaternion b 
) [friend]

Returns the composition of the a and b rotations.

The order is important. When applied to a Vec3 v (see operator*(const Quaternion&, const Vec3&) and rotate()) the resulting Quaternion acts as if b was applied first and then a was applied. This is obvious since the image v' of v by the composited rotation satisfies:

    v'= (a*b) * v = a * (b*v) 

Note that a*b usually differs from b*a.

Attention:
For efficiency reasons, the resulting Quaternion is not normalized. Use normalize() in case of numerical drift with small rotation composition.

Definition at line 197 of file erf_quaternion.hpp.

Vec3 operator* ( const Quaternion q,
const Vec3 v 
) [friend]

Returns the image of v by the rotation q.

Same as q.rotate(v). See rotate() and inverseRotate().

Definition at line 220 of file erf_quaternion.hpp.


Member Data Documentation

Definition at line 339 of file erf_quaternion.hpp.

Definition at line 339 of file erf_quaternion.hpp.

Definition at line 339 of file erf_quaternion.hpp.

Definition at line 339 of file erf_quaternion.hpp.

float Quaternion::q[4]

union { ... }


The documentation for this class was generated from the following file:

The miarn project - written by Joao Xavier