Functions | Variables
quaternion.cpp File Reference
#include "quaternion.h"
Include dependency graph for quaternion.cpp:

Go to the source code of this file.

Functions

short Integ_quat (Quaternion &dquat_present, Quaternion &dquat_past, Quaternion &quat, const Real dt)
 Trapezoidal quaternion integration.
Real Integ_Trap_quat_s (const Quaternion &present, Quaternion &past, const Real dt)
 Trapezoidal quaternion scalar part integration.
ReturnMatrix Integ_Trap_quat_v (const Quaternion &present, Quaternion &past, const Real dt)
 Trapezoidal quaternion vector part integration.
ReturnMatrix Omega (const Quaternion &q, const Quaternion &q_dot)
 Return angular velocity from a quaternion and it's time derivative.
Quaternion operator* (const Real c, const Quaternion &q)
 Overload * operator, multiplication by a scalar.
Quaternion operator* (const Quaternion &q, const Real c)
 Overload * operator, multiplication by a scalar.
Quaternion operator/ (const Real c, const Quaternion &q)
 Overload / operator, division by a scalar.
Quaternion operator/ (const Quaternion &q, const Real c)
Quaternion Slerp (const Quaternion &q0, const Quaternion &q1, const Real t)
 Spherical Linear Interpolation.
Quaternion Slerp_prime (const Quaternion &q0, const Quaternion &q1, const Real t)
 Spherical Linear Interpolation derivative.
Quaternion Squad (const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
 Spherical Cubic Interpolation.
Quaternion Squad_prime (const Quaternion &p, const Quaternion &a, const Quaternion &b, const Quaternion &q, const Real t)
 Spherical Cubic Interpolation derivative.

Variables

static const char rcsid [] = "$Id: quaternion.cpp,v 1.18 2005/11/15 19:25:58 gourdeau Exp $"
 RCS/CVS version.

Function Documentation

short Integ_quat ( Quaternion dquat_present,
Quaternion dquat_past,
Quaternion quat,
const Real  dt 
)

Trapezoidal quaternion integration.

Definition at line 585 of file quaternion.cpp.

Real Integ_Trap_quat_s ( const Quaternion present,
Quaternion past,
const Real  dt 
)

Trapezoidal quaternion scalar part integration.

Definition at line 612 of file quaternion.cpp.

ReturnMatrix Integ_Trap_quat_v ( const Quaternion present,
Quaternion past,
const Real  dt 
)

Trapezoidal quaternion vector part integration.

Definition at line 621 of file quaternion.cpp.

ReturnMatrix Omega ( const Quaternion q,
const Quaternion q_dot 
)

Return angular velocity from a quaternion and it's time derivative.

See Quaternion::dot for explanation.

Definition at line 560 of file quaternion.cpp.

Quaternion operator* ( const Real  c,
const Quaternion q 
)

Overload * operator, multiplication by a scalar.

$q = [s, v]$ and let $r \in R$. Then $rq = qr = [r, 0][s, v] = [rs, rv]$

The result is not necessarily a unit quaternion even if $q$ is a unit quaternions.

Definition at line 516 of file quaternion.cpp.

Quaternion operator* ( const Quaternion q,
const Real  c 
)

Overload * operator, multiplication by a scalar.

Definition at line 533 of file quaternion.cpp.

Quaternion operator/ ( const Real  c,
const Quaternion q 
)

Overload / operator, division by a scalar.

Same explanation as multiplication by scaler.

Definition at line 542 of file quaternion.cpp.

Quaternion operator/ ( const Quaternion q,
const Real  c 
)

Definition at line 555 of file quaternion.cpp.

Quaternion Slerp ( const Quaternion q0,
const Quaternion q1,
const Real  t 
)

Spherical Linear Interpolation.

Cite_:Dam

The quaternion $q(t)$ interpolate the quaternions $q_0$ and $q_1$ given the parameter $t$ along the quaternion sphere.

\[ q(t) = c_0(t)q_0 + c_1(t)q_1 \]

where $c_0$ and $c_1$ are real functions with $0\leq t \leq 1$. As $t$ varies between 0 and 1. the values $q(t)$ varies uniformly along the circular arc from $q_0$ and $q_1$. The angle between $q(t)$ and $q_0$ is $\cos(t\theta)$ and the angle between $q(t)$ and $q_1$ is $\cos((1-t)\theta)$. Taking the dot product of $q(t)$ and $q_0$ yields

\[ \cos(t\theta) = c_0(t) + \cos(\theta)c_1(t) \]

and taking the dot product of $q(t)$ and $q_1$ yields

\[ \cos((1-t)\theta) = \cos(\theta)c_0(t) + c_1(t) \]

These are two equations with $c_0$ and $c_1$. The solution is

\[ c_0 = \frac{\sin((1-t)\theta)}{\sin(\theta)} \]

\[ c_1 = \frac{\sin(t\theta)}{sin(\theta)} \]

The interpolation is then

\[ Slerp(q_0, q_1, t) = \frac{q_0\sin((1-t)\theta)+q_1\sin(t\theta)}{\sin(\theta)} \]

If $q_0$ and $q_1$ are unit quaternions the $q(t)$ is also a unit quaternions. For unit quaternions we have

\[ Slerp(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t \]

For t = 0 and t = 1 we have

\[ q_0 = Slerp(q_0, q_1, 0) \]

\[ q_1 = Slerp(q_0, q_1, 1) \]

It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle between q0 ang Gq1 is acute). This choice avoids extra spinning caused by the interpolated rotations.

Definition at line 631 of file quaternion.cpp.

Quaternion Slerp_prime ( const Quaternion q0,
const Quaternion q1,
const Real  t 
)

Spherical Linear Interpolation derivative.

Cite_: Dam

The derivative of the function $q^t$ where $q$ is a constant unit quaternion is

\[ \frac{d}{dt}q^t = q^t log(q) \]

Using the preceding equation the Slerp derivative is then

\[ Slerp'(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t log(q_0^{-1}q_1) \]

It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle between q0 ang Gq1 is acute). This choice avoids extra spinning caused by the interpolated rotations. The result is not necessary a unit quaternion.

Definition at line 692 of file quaternion.cpp.

Quaternion Squad ( const Quaternion p,
const Quaternion a,
const Quaternion b,
const Quaternion q,
const Real  t 
)

Spherical Cubic Interpolation.

Cite_: Dam

Let four quaternions be $q_i$ (p), $s_i$ (a), $s_{i+1}$ (b) and $q_{i+1}$ (q) be the ordered vertices of a quadrilateral. Obtain c from $q_i$ to $q_{i+1}$ interpolation. Obtain d from $s_i$ to $s_{i+1}$ interpolation. Obtain e, the final result, from c to d interpolation.

\[ Squad(q_i, s_i, s_{i+1}, q_{i+1}, t) = Slerp(Slerp(q_i,q_{i+1},t),Slerp(s_i,s_{i+1},t), 2t(1-t)) \]

The intermediate quaternion $s_i$ and $s_{i+1}$ are given by

\[ s_i = q_i exp\Big ( - \frac{log(q_i^{-1}q_{i+1}) + log(q_i^{-1}q_{i-1})}{4}\Big ) \]

Definition at line 725 of file quaternion.cpp.

Quaternion Squad_prime ( const Quaternion p,
const Quaternion a,
const Quaternion b,
const Quaternion q,
const Real  t 
)

Spherical Cubic Interpolation derivative.

Cite_: www.magic-software.com

The derivative of the function $q^t$ where $q$ is a constant unit quaternion is

\[ \frac{d}{dt}q^t = q^t log(q) \]

Recalling that $log(q) = [0, v\theta]$ (see Quaternion::Log()). If the power is a function we have

\[ \frac{d}{dt}q^{f(t)} = f'(t)q^{f(t)}log(q) \]

If $q$ is a function of time and the power is differentiable function of time we have

\[ \frac{d}{dt}(q(t))^{f(t)} = f'(t)(q(t))^{f(t)}log(q) + f(t)(q(t))^{f(t)-1}q'(t) \]

Using these last three equations Squad derivative can be define. Let $U(t)=Slerp(p,q,t)$, $V(t)=Slerp(q,b,t)$, $W(t)=U(t)^{-1}V(t)$. We then have $Squad(p,a,b,q,t)=Slerp(U(t),V(t),2t(1-t))=U(t)W(t)^{2t(1-t)}$

\[ Squad'(p,a,b,q,t) = \frac{d}{dt}\Big [ UW^{2t(1-t)}\Big ] \]

\[ Squad'(p,a,b,q,t) = U\frac{d}{dt}\Big [ W^{2t(1-t)}\Big ] + U'\Big [W^{2t(1-t)}\Big] \]

\[ Squad'(p,a,b,q,t) = U\Big[(2-4t)W^{2t(1-t)}log(W)+2t(1-t)W^{2t(1-t)-1}W'\Big] + U'\Big[W^{2t(1-t)} \Big] \]

where $U'=Ulog(p^{-1}q)$, $V'=Vlog(a^{-1},b)$, $W'=U^{-1}V'-U^{-2}U'V$

The result is not necessarily a unit quaternion even if all the input quaternions are unit.

Definition at line 751 of file quaternion.cpp.


Variable Documentation

const char rcsid[] = "$Id: quaternion.cpp,v 1.18 2005/11/15 19:25:58 gourdeau Exp $" [static]

RCS/CVS version.

Definition at line 67 of file quaternion.cpp.



kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:33