Quaternion Class Reference

This class contains the 4 components of a quaternion and the standard operators and functions needed for quaternion arithmetic. More...

#include <Quaternion.h>

Public Member Functions
	Quaternion ()
	Constructor; sets quaternion value to (1,0,0,0) More...
	Quaternion (Mdouble q0, Mdouble q1, Mdouble q2, Mdouble q3)
	Alternative constructor. Sets quaternion value to (q0,q1,q2,q3) More...
	Quaternion (Vec3D normal)
void	setUnity ()
	Sets quaternion value to (1,0,0,0) More...
bool	isUnity () const
	Checks if the quaternion value is (1,0,0,0) More...
Quaternion	operator+ (const Quaternion &a) const
	Adds another quaternion and returns the result. More...
Quaternion	operator- (const Quaternion &a) const
	Subtracts another quaternion and returns the result. More...
Quaternion	operator* (Mdouble a) const
	Multiplies by a scalar. More...
Quaternion	operator/ (Mdouble a) const
	Divides by a scalar. More...
Quaternion &	operator+= (const Quaternion &a)
	Adds another quaternion. More...
Quaternion &	operator-= (const Quaternion &a)
	Subtracts another quaternion. More...
Quaternion &	operator*= (Mdouble a)
	Multiplies *this by a scalar. More...
Quaternion &	operator/= (Mdouble a)
	Divides by a scalar. More...
void	normalise ()
	Makes this Quaternion unit length |q|=1. More...
void	setLength (Mdouble length)
	Make this Quaternion a certain length |q|=length. More...
Mdouble	getLength () const
	Calculates the length of this Quaternion: \( \sqrt{a\cdot a} \). More...
Mdouble	getLengthSquared () const
	Calculates the squared length of this Quaternion: \( a\cdot a \). More...
Mdouble	getComponent (int index) const
	Returns the requested component of this Quaternion. More...
Quaternion	angularVelocityBodyFixedFrameToAngularDisplacement (Vec3D v) const
Quaternion	angularDisplacementTimeDerivative (Vec3D v) const
	Converts an angular momentum v=omega into a quaternion rate of change, q(t+dt)-q(t)/dt. More...
void	updateAngularDisplacement (Vec3D angularVelocityDt)
Vec3D	applyCInverse (Quaternion q) const
	Converts quaternion rate of change into an angular momentum omega. More...
void	setComponent (int index, double val)
	Sets the requested component of this Quaternion to the requested value. More...
bool	isEqualTo (const Quaternion &other, double tol) const
	Checks if the length this Quaternion is equal the length of other with a certain tolerance. More...
Vec3D	getEuler () const
	Convert a quaternion to Euler angles. See Wikipedia for details. More...
void	setEuler (const Vec3D &e)
	Convert Euler angles to a quaternion. See Wikipedia for details. More...
Mdouble	getAngleZ () const
	Converts a quaternion to the rotation angle in the XY plane (for Mercury2D). See Wikipedia for details. More...
void	setAngleZ (Mdouble psi)
	Converts the rotation angle in the XY plane into a quaternion (for Mercury2D). See Wikipedia for details. More...
MatrixSymmetric3D	rotateInverseInertiaTensor (const MatrixSymmetric3D &invI) const
	Converts the inverse inertia tensor from the reference frame to the lab frame; see See QuaternionsWouter.pdf for details, where this operation is denoted by \(A*I^{-1}*A^T\). More...
void	rotateTensor (SmallMatrix< 3, 3 > I) const
Vec3D	getAxis () const
	Converts the quaternions into a normal vector by rotating the vector x=(1,0,0); see See Wiki for details. More...
void	setOrientationViaNormal (Vec3D normal)
void	getRotationMatrix (SmallMatrix< 3, 3 > &A) const
void	rotate (Vec3D &position) const
Vec3D	rotate (const Vec3D &position) const
void	rotate (SmallVector< 3 > &position) const
void	rotateBack (Vec3D &position) const
Vec3D	rotateBack (const Vec3D &position) const
Mdouble	getDistance (Vec3D p, Vec3D p0) const

Static Public Member Functions
static Mdouble	getDistance (const Quaternion &a, const Quaternion &b)
	Calculates the distance between two Quaternion: \( \sqrt{\left(a-b\right) \cdot \left(a-b\right)} \). More...
static Mdouble	getDistanceSquared (const Quaternion &a, const Quaternion &b)
	Calculates the squared distance between two Quaternion: \( \left(a-b\right) \cdot \left(a-b\right) \). More...
static Mdouble	getLength (const Quaternion &a)
	Calculates the length of a Quaternion: \( \sqrt{a\cdot a} \). More...
static Mdouble	getLengthSquared (const Quaternion &a)
	Calculates the squared length of a Quaternion: \( a\cdot a \). More...
static Quaternion	getUnitQuaternion (const Quaternion &a)
	Returns a unit Quaternion based on a. More...
static Quaternion	multiplyQuaternions (const Quaternion &q, const Quaternion &r)
static Quaternion	fromAxisAndAngle (const Vec3D &axis, const Mdouble &angle)
	Creates a quaternion from a given rotation axis and an angle. More...

Public Attributes
Mdouble	q0
	the zeroth component of the quaternion q = (q0,q1,q2,q3) More...
Mdouble	q1
	the first component of the quaternion q = (q0,q1,q2,q3) More...
Mdouble	q2
	the second component of the quaternion q = (q0,q1,q2,q3) More...
Mdouble	q3
	the third component of the quaternion q = (q0,q1,q2,q3) More...

Friends
std::ostream &	operator<< (std::ostream &os, const Quaternion &a)
	Adds elements to an output stream. More...
std::istream &	operator>> (std::istream &is, Quaternion &a)
	Adds elements to an input stream. More...
Quaternion	operator+ (Mdouble a, const Quaternion &b)
	Adds a scalar to a quaternion. More...
Quaternion	operator- (Mdouble a, const Quaternion &b)
	Subtracts the elements of a quaternion from a scalar. More...
Quaternion	operator- (const Quaternion &a)
	Subtracts a quaternion. More...
Quaternion	operator* (Mdouble a, const Quaternion &b)
	Multiplies all elements by a scalar. More...

Detailed Description

This class contains the 4 components of a quaternion and the standard operators and functions needed for quaternion arithmetic.

Implementation of a 3D quaternion (by Vitaliy). Modifications 21:9:2009 - Added the inclusion guards and some include objects

Todo:

Need to generalize this to n-dimensional quaternions of any type

A quaternion is a four-dimensional vector q = (q0,q1,q2,q3) that satisfies |q|^2=q0^2+q1^2+q2^2+q3^2=1. It can be used to describe the orientation of an object in space.

The properties (e.g. inertia tensor) of any interactable (particle or wall) in MercuryDPM are described in its reference frame. The quaternion is used to rotate the object into the lab frame (the system geometry). A few examples to help visualising quaternions can be found here .

The unit quaternion, q=(1,0,0,0) denotes the state where the lab frame and the reference frame is identical.

To see how to convert a quaternion to Euler angles or to compare it to a rotation of an object around a axis, see Wikipedia for details.

Constructor & Destructor Documentation

Quaternion() [1/3]

Quaternion::Quaternion

(

)

Constructor; sets quaternion value to (1,0,0,0)

Default constructor

{
    setUnity();
}

References setUnity().

Referenced by angularDisplacementTimeDerivative(), angularVelocityBodyFixedFrameToAngularDisplacement(), fromAxisAndAngle(), operator*(), operator+(), operator-(), operator/(), and setOrientationViaNormal().

Quaternion() [2/3]

Quaternion::Quaternion	(	Mdouble	q0,
		Mdouble	q1,
		Mdouble	q2,
		Mdouble	q3
	)

Alternative constructor. Sets quaternion value to (q0,q1,q2,q3)

Alternative constructor, lets you define all four elements.

Parameters

[in]	q0	the q0-component
[in]	q1	the q1-component
[in]	q2	the q2-component
[in]	q3	the q3-component

{
    this->q0 = q0;
    this->q1 = q1;
    this->q2 = q2;
    this->q3 = q3;
}

References q0, q1, q2, and q3.

Quaternion() [3/3]

Quaternion::Quaternion ( Vec3D  normal )

inline

Todo:

should be explicit as teh conversion is not unique

Parameters

normal

    {
        setOrientationViaNormal(normal);
    }

References WallFunction::normal(), and setOrientationViaNormal().

Member Function Documentation

angularDisplacementTimeDerivative()

Quaternion Quaternion::angularDisplacementTimeDerivative

(

Vec3D

v

)

const

Converts an angular momentum v=omega into a quaternion rate of change, q(t+dt)-q(t)/dt.

Given v = &omega * dt, with omega the angular velocity, this computes the change in angular displacement to be added in the time integration. This is equivalent to applying the matrix &tilde{C}

{
    return 0.5 * Quaternion(
            -q1 * v.X - q2 * v.Y - q3 * v.Z,
            q0 * v.X + q3 * v.Y - q2 * v.Z,
            -q3 * v.X + q0 * v.Y + q1 * v.Z,
            q2 * v.X - q1 * v.Y + q0 * v.Z);
}

References q0, q1, q2, q3, Quaternion(), and v.

Referenced by updateAngularDisplacement().

angularVelocityBodyFixedFrameToAngularDisplacement()

Quaternion Quaternion::angularVelocityBodyFixedFrameToAngularDisplacement

(

Vec3D

v

)

const

Todo:

should the arguments be passed by reference?

Todo:

rename to angularVelocityBodyFixedFrameToAngularDisplacement?

{
    return Quaternion(
            -q1 * v.X - q2 * v.Y - q3 * v.Z,
            q0 * v.X - q3 * v.Y + q2 * v.Z,
            q3 * v.X + q0 * v.Y - q1 * v.Z,
            -q2 * v.X + q1 * v.Y + q0 * v.Z);
}

References q0, q1, q2, q3, Quaternion(), and v.

applyCInverse()

Vec3D Quaternion::applyCInverse

(

Quaternion

q

)

const

Converts quaternion rate of change into an angular momentum omega.

Todo:

rename to angularDisplacementToAngularVelocity?

{
    return 2.0 * Vec3D(
            -q1 * q.q0 + q0 * q.q1 - q3 * q.q2 + q2 * q.q3,
            -q2 * q.q0 + q3 * q.q1 + q0 * q.q2 - q1 * q.q3,
            -q3 * q.q0 - q2 * q.q1 + q1 * q.q2 + q0 * q.q3);
}

References Eigen::numext::q, q0, q1, q2, and q3.

fromAxisAndAngle()

static Quaternion Quaternion::fromAxisAndAngle ( const Vec3D &  axis, const Mdouble &  angle  )

inlinestatic

Creates a quaternion from a given rotation axis and an angle.

Parameters

axis	The rotation axis
angle	The angle to rotate around the axis

    {
        double axisMagnitude = axis.getLength();
        logger.assert_debug(axisMagnitude > 0, "Quaternion::fromAxisAndangle: Axis Magnitude is %. Cannot compute a quaternion.", axisMagnitude);
 
        Mdouble halfAngle = angle / 2.0;
        Mdouble sinHalfAngle = std::sin(halfAngle);
        Mdouble q0 = std::cos(halfAngle);
        Mdouble q1 = axis.X * sinHalfAngle / axisMagnitude;
        Mdouble q2 = axis.Y * sinHalfAngle / axisMagnitude;
        Mdouble q3 = axis.Z * sinHalfAngle / axisMagnitude;
        return Quaternion(q0, q1, q2, q3);
    };

References Jeffery_Solution::angle(), cos(), Vec3D::getLength(), logger, q0, q1, q2, q3, Quaternion(), sin(), Vec3D::X, Vec3D::Y, and Vec3D::Z.

getAngleZ()

Mdouble Quaternion::getAngleZ

(

)

const

Converts a quaternion to the rotation angle in the XY plane (for Mercury2D). See Wikipedia for details.

{
    return -std::atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
}

References atan2(), q0, q1, q2, and q3.

Referenced by MarbleRun::actionsAfterTimeStep().

getAxis()

Vec3D Quaternion::getAxis

(

)

const

Converts the quaternions into a normal vector by rotating the vector x=(1,0,0); see See Wiki for details.

{
    const Mdouble q00 = q0*q0;
    const Mdouble q11 = q1*q1;
    const Mdouble q22 = q2*q2;
    const Mdouble q33 = q3*q3;
    const Mdouble q12 = q1*q2;
    const Mdouble q30 = q3*q0;
    const Mdouble q13 = q1*q3;
    const Mdouble q20 = q2*q0;
    return Vec3D(q00 - q33 + q11 - q22, 2.0 * (q12 + q30), 2.0 * (q13 - q20));
}

References q0, q1, q2, and q3.

Referenced by InfiniteWall::createVTK(), AxisymmetricIntersectionOfWalls::getDistanceAndNormal(), HorizontalBaseScrew::getDistanceAndNormal(), InfiniteWall::getDistanceAndNormal(), InfiniteWall::getDistanceNormalOverlapSuperquadric(), and InfiniteWall::getNormal().

getComponent()

Mdouble Quaternion::getComponent

(

int

index

)

const

Returns the requested component of this Quaternion.

returns the quaternion element belonging to the given index.

Parameters

[in]

index

the index of interest (should be 0, 1 or 2)

Returns

the value of the quaternion element belonging to the given index

{
    switch (index)
    {
        case 0:
            return q0;
        case 1:
            return q1;
        case 2:
            return q2;
        case 3:
            return q3;
        default:
            logger(ERROR,
                   "[Quaternion::getComponent] Index = %, which is too high for a 3D quaternion (should be 0-2).",
                   index);
            return 0;
    }
}

getDistance() [1/2]

Mdouble Quaternion::getDistance ( const Quaternion &  a, const Quaternion &  b  )

static

Calculates the distance between two Quaternion: \( \sqrt{\left(a-b\right) \cdot \left(a-b\right)} \).

Calculates the distance (i.e. the length of the difference) between two quaternions NB: this is a STATIC function!

Parameters

[in]	a	the first quaternion
[in]	b	the second quaternion

Returns

the distance between the two arguments.

{
    return std::sqrt(getDistanceSquared(a, b));
}

Referenced by InfiniteWall::getDistance(), and Quaternion< float >::rotateInverseInertiaTensor().

getDistance() [2/2]

Mdouble Quaternion::getDistance	(	Vec3D	p,
		Vec3D	p0
	)		const

Calculates the distance from a wall through p0 whose normal is the vector (1,0,0). Used for the calculation of the distance in InfiniteWalls

{
    const Mdouble q00 = q0 * q0;
    const Mdouble q02 = 2 * q0 * q2;
    const Mdouble q03 = 2 * q0 * q3;
    const Mdouble q11 = q1 * q1;
    const Mdouble q12 = 2 * q1 * q2;
    const Mdouble q13 = 2 * q1 * q3;
    const Mdouble q22 = q2 * q2;
    const Mdouble q33 = q3 * q3;
    return (q00 + q11 - q22 - q33) * (p0.X - p.X) +
           ((q12 + q03) * (p0.Y - p.Y) + (q13 - q02) * (p0.Z - p.Z));
}

References p, p0, q0, q1, q2, and q3.

getDistanceSquared()

Mdouble Quaternion::getDistanceSquared ( const Quaternion &  a, const Quaternion &  b  )

static

Calculates the squared distance between two Quaternion: \( \left(a-b\right) \cdot \left(a-b\right) \).

Calculates the square of the distance (i.e. the length of the difference) between two quaternions. NB: this is a STATIC function!

Parameters

[in]	a	the first quaternion
[in]	b	the second quaternion

Returns

the square of the distance between the two arguments.

{
    return ((a.q0 - b.q0) * (a.q0 - b.q0) + (a.q1 - b.q1) * (a.q1 - b.q1) + (a.q2 - b.q2) * (a.q2 - b.q2) +
            (a.q3 - b.q3) * (a.q3 - b.q3));
}

Referenced by Quaternion< float >::getLength().

getEuler()

Vec3D Quaternion::getEuler

(

)

const

Convert a quaternion to Euler angles. See Wikipedia for details.

Get the Euler angles of this quaternion. Example of visualising Euler angles can be found here

{
    Mdouble sinp = 2 * (q0 * q2 - q3 * q1);
    Mdouble pitch;
 
    if ((std::abs(sinp) < 1))
    {
        pitch = std::asin(sinp);
    }
    else
    {
        pitch = copysign(constants::pi, sinp);
    }
 
    return Vec3D(
            std::atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)),
            pitch,
            std::atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)));
}

References abs(), Eigen::bfloat16_impl::asin(), atan2(), constants::pi, q0, q1, q2, and q3.

getLength() [1/2]

Mdouble Quaternion::getLength

(

)

const

Calculates the length of this Quaternion: \( \sqrt{a\cdot a} \).

Calculates the length of this quaternion

Returns

the (scalar) length of this quaternion

{
    return std::sqrt(getLengthSquared());
}

Referenced by setLength().

getLength() [2/2]

Mdouble Quaternion::getLength ( const Quaternion &  a )

static

Calculates the length of a Quaternion: \( \sqrt{a\cdot a} \).

Calculates the length of a given quaternion NB: this is a STATIC function!

Parameters

[in]

a

quaternion to be measured.

Returns

length of the argument.

{
    return a.getLength();
}

getLengthSquared() [1/2]

Mdouble Quaternion::getLengthSquared

(

)

const

Calculates the squared length of this Quaternion: \( a\cdot a \).

Calculates the square of the length of itself

Returns

the square of the length of this quaternion

{
    return (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
}

References q0, q1, q2, and q3.

Referenced by Quaternion< float >::getAxis(), and normalise().

getLengthSquared() [2/2]

Mdouble Quaternion::getLengthSquared ( const Quaternion &  a )

static

Calculates the squared length of a Quaternion: \( a\cdot a \).

Calculates the square of the length of a given quaternion. NB: this is a STATIC function!

Parameters

[in]

a

the quaternion.

Returns

the square of the length of the argument.

{
    return (a.q0 * a.q0 + a.q1 * a.q1 + a.q2 * a.q2 + a.q3 * a.q3);
}

getRotationMatrix()

void Quaternion::getRotationMatrix

(

SmallMatrix< 3, 3 > &

A

)

const

Retrieves the rotation matrix

Parameters

[out]

A

The rotation matrix

{
    
    Mdouble q00 = q0 * q0;
    Mdouble q01 = 2 * q0 * q1;
    Mdouble q02 = 2 * q0 * q2;
    Mdouble q03 = 2 * q0 * q3;
    Mdouble q11 = q1 * q1;
    Mdouble q12 = 2 * q1 * q2;
    Mdouble q13 = 2 * q1 * q3;
    Mdouble q22 = q2 * q2;
    Mdouble q23 = 2 * q2 * q3;
    Mdouble q33 = q3 * q3;
    A(0, 0) = q00 + q11 - q22 - q33;
    A(1, 0) = q12 + q03;
    A(2, 0) = q13 - q02;
    A(0, 1) = q12 - q03;
    A(1, 1) = q00 - q11 + q22 - q33;
    A(2, 1) = q23 + q01;
    A(0, 2) = q13 + q02;
    A(1, 2) = q23 - q01;
    A(2, 2) = q00 - q11 - q22 + q33;
}

References q0, q1, q2, and q3.

Referenced by SuperQuadricParticle::computeHessianLabFixed(), rotate(), and rotateTensor().

getUnitQuaternion()

Quaternion Quaternion::getUnitQuaternion ( const Quaternion &  a )

static

Returns a unit Quaternion based on a.

Calculates the unit quaternion of a given quaternion (unless it is a quaternion with zero length; in that case it returns a 3D quaternion with each element equal to zero). NB: this is a STATIC function!

Parameters

[in]

a

the quaternion of interest

Returns

unit quaternion in the direction of the argument (unless the argument has length zero; in that case a zero-quaternion).

{
    Mdouble Length2 = a.getLengthSquared();
    if (Length2 != 0.0)
        return a / std::sqrt(Length2);
    else
        return Quaternion(1, 0, 0, 0);
}

isEqualTo()

bool Quaternion::isEqualTo	(	const Quaternion &	other,
		double	tol
	)		const

Checks if the length this Quaternion is equal the length of other with a certain tolerance.

Checks if the length of the quaternion is equal to the one given in the first argument (other), with a tolerance given in the second argument (tol).

Parameters

[in]	other	the 3D quaternion to check against
[in]	tol	the tolerance

Returns

returns TRUE if the difference between the lengths of this quaternion and that given in the first argument (other) is smaller than the given tolerance.

{
    return (getDistance(*this, other) <= tol * tol);
}

isUnity()

bool Quaternion::isUnity ( ) const

inline

Checks if the quaternion value is (1,0,0,0)

Checks if ALL elements are zero

Returns

TRUE if q0 equals one and ALL other elements are zero

Bug:

use isEqual instead of ==

    {
        return q0 == 1.0 && q1 == 0.0 && q2 == 0.0 && q3 == 0.0;
    };

References q0, q1, q2, and q3.

multiplyQuaternions()

static Quaternion Quaternion::multiplyQuaternions ( const Quaternion &  q, const Quaternion &  r  )

inlinestatic

                                                                                    {
        double w = q.q0 * r.q0 - q.q1 * r.q1 - q.q2 * r.q2 - q.q3 * r.q3;
        double x = q.q0 * r.q1 + q.q1 * r.q0 + q.q2 * r.q3 - q.q3 * r.q2;
        double y = q.q0 * r.q2 - q.q1 * r.q3 + q.q2 * r.q0 + q.q3 * r.q1;
        double z = q.q0 * r.q3 + q.q1 * r.q2 - q.q2 * r.q1 + q.q3 * r.q0;
        return {w, x, y, z};
    }

References Eigen::numext::q, UniformPSDSelfTest::r, w, plotDoE::x, and y.

normalise()

void Quaternion::normalise

(

)

Makes this Quaternion unit length |q|=1.

Normalises the quaternion, i.e. divides all elements by the quaternions length (resulting in a quaternion in the same direction, but with unit length).

{
    const Mdouble length2 = getLengthSquared();
    if (length2 == 0)
    {
        logger(ERROR, "Normalizing a quaternion of length 0");
    }
    *this /= sqrt(length2);
}

References getLengthSquared().

Referenced by setOrientationViaNormal(), and updateAngularDisplacement().

operator*()

Quaternion Quaternion::operator*

(

Mdouble

a

)

const

Multiplies by a scalar.

Multiplies each element with a scalar

Parameters

[in]

a

the scalar to be multiplied with

Returns

the resulting quaternion

{
    return Quaternion(q0 * a, q1 * a, q2 * a, q3 * a);
}

References a, q0, q1, q2, q3, and Quaternion().

operator*=()

Quaternion & Quaternion::operator*=

(

Mdouble

a

)

Multiplies *this by a scalar.

Multiplies each element by a scalar

Parameters

[in]

a

scalar to be multiplied by

Returns

(reference to) itself, i.e. resulting quaternion

{
    q0 *= a;
    q1 *= a;
    q2 *= a;
    q3 *= a;
    return *this;
}

References a, q0, q1, and q2.

operator+()

Quaternion Quaternion::operator+

(

const Quaternion &

a

)

const

Adds another quaternion and returns the result.

Adds quaternion to itself

Parameters

[in]

a

quaternion to be added

Returns

resulting 3D quaternion

{
    return Quaternion(q0 + a.q0, q1 + a.q1, q2 + a.q2, q3 + a.q3);
}

References a, q0, q1, q2, q3, and Quaternion().

operator+=()

Quaternion & Quaternion::operator+=

(

const Quaternion &

a

)

Adds another quaternion.

Adds a quaternion to itself

Parameters

[in]

a

quaternion to be added

Returns

(reference to) itself, i.e. resulting quaternion

{
    q0 += a.q0;
    q1 += a.q1;
    q2 += a.q2;
    q3 += a.q3;
    return *this;
}

References a, q0, q1, q2, and q3.

operator-()

Quaternion Quaternion::operator-

(

const Quaternion &

a

)

const

Subtracts another quaternion and returns the result.

Subtracts quaternion from itself

Parameters

[in]

a

quaternion to be subtracted

Returns

resulting quaternion

{
    return Quaternion(q0 - a.q0, q1 - a.q1, q2 - a.q2, q3 - a.q3);
}

References a, q0, q1, q2, q3, and Quaternion().

operator-=()

Quaternion & Quaternion::operator-=

(

const Quaternion &

a

)

Subtracts another quaternion.

Subtracts a quaternion from itself

Parameters

[in]

a

quaternion to be subtracted

Returns

(reference to) itself, i.e. resulting quaternion

{
    q0 -= a.q0;
    q1 -= a.q1;
    q2 -= a.q2;
    q3 -= a.q3;
    return *this;
}

References a, q0, and q1.

operator/()

Quaternion Quaternion::operator/

(

Mdouble

a

)

const

Divides by a scalar.

Divides each element by a scalar

Parameters

[in]

a

the scalar to be divided by

Returns

resulting quaternion

{
    return Quaternion(q0 / a, q1 / a, q2 / a, q3 / a);
}

References a, q0, q1, q2, q3, and Quaternion().

operator/=()

Quaternion & Quaternion::operator/=

(

Mdouble

a

)

Divides by a scalar.

Divides each element by a scalar

Parameters

[in]

a

scalar to be divided by

Returns

(reference to) itself, i.e. resulting quaternion

{
    q0 /= a;
    q1 /= a;
    q2 /= a;
    q3 /= a;
    return *this;
}

rotate() [1/3]

Vec3D Quaternion::rotate

(

const Vec3D &

position

)

const

Returns the rotated position

{
    Mdouble q00 = q0 * q0;
    Mdouble q01 = 2 * q0 * q1;
    Mdouble q02 = 2 * q0 * q2;
    Mdouble q03 = 2 * q0 * q3;
    Mdouble q11 = q1 * q1;
    Mdouble q12 = 2 * q1 * q2;
    Mdouble q13 = 2 * q1 * q3;
    Mdouble q22 = q2 * q2;
    Mdouble q23 = 2 * q2 * q3;
    Mdouble q33 = q3 * q3;
    return Matrix3D(
            q00 + q11 - q22 - q33, q12 - q03, q13 + q02,
            q12 + q03, q00 - q11 + q22 - q33, q23 - q01,
            q13 - q02, q23 + q01, q00 - q11 - q22 + q33) * position;
}

References q0, q1, q2, and q3.

rotate() [2/3]

void Quaternion::rotate

(

SmallVector< 3 > &

position

)

const

Applies the rotation to a position

Rotate the given vector from the body-fixed angles to the lab-fixed angles. This is the same as multiplying with the rotation matrix, A.

{
    SmallMatrix<3, 3> A;
    getRotationMatrix(A);
    position = A * position;
}

References getRotationMatrix().

rotate() [3/3]

void Quaternion::rotate

(

Vec3D &

position

)

const

Applies the rotation to a position

{
    Mdouble q00 = q0 * q0;
    Mdouble q01 = 2 * q0 * q1;
    Mdouble q02 = 2 * q0 * q2;
    Mdouble q03 = 2 * q0 * q3;
    Mdouble q11 = q1 * q1;
    Mdouble q12 = 2 * q1 * q2;
    Mdouble q13 = 2 * q1 * q3;
    Mdouble q22 = q2 * q2;
    Mdouble q23 = 2 * q2 * q3;
    Mdouble q33 = q3 * q3;
    position = Matrix3D(
            q00 + q11 - q22 - q33, q12 - q03, q13 + q02,
            q12 + q03, q00 - q11 + q22 - q33, q23 - q01,
            q13 - q02, q23 + q01, q00 - q11 - q22 + q33) * position;
}

References q0, q1, q2, and q3.

Referenced by SuperQuadricParticle::computeShapeGradientLabFixed(), NurbsWall::getDistanceAndNormal(), Screw::getDistanceAndNormal(), BasicIntersectionOfWalls::getDistanceAndNormal(), BasicUnionOfWalls::getDistanceAndNormal(), IntersectionOfWalls::getDistanceAndNormal(), LevelSetWall::getDistanceAndNormal(), BasicIntersectionOfWalls::getVTK(), BasicUnionOfWalls::getVTK(), TriangleMeshWall::updateBoundingBoxGlobal(), TriangleWall::updateVertexAndNormal(), BasicUnionOfWalls::writeVTK(), IntersectionOfWalls::writeVTK(), LevelSetWall::writeVTK(), NurbsWall::writeVTK(), Screw::writeVTK(), ScrewsymmetricIntersectionOfWalls::writeVTK(), AxisymmetricIntersectionOfWalls::writeVTK(), NurbsWall::writeWallDetailsVTK(), and WearableNurbsWall::writeWallDetailsVTK().

rotateBack() [1/2]

Vec3D Quaternion::rotateBack

(

const Vec3D &

position

)

const

Returns the inverse rotated position

{
    Mdouble q00 = q0 * q0;
    Mdouble q01 = 2 * q0 * q1;
    Mdouble q02 = 2 * q0 * q2;
    Mdouble q03 = 2 * q0 * q3;
    Mdouble q11 = q1 * q1;
    Mdouble q12 = 2 * q1 * q2;
    Mdouble q13 = 2 * q1 * q3;
    Mdouble q22 = q2 * q2;
    Mdouble q23 = 2 * q2 * q3;
    Mdouble q33 = q3 * q3;
    return Matrix3D(
            q00 + q11 - q22 - q33, q12 + q03, q13 - q02,
            q12 - q03, q00 - q11 + q22 - q33, q23 + q01,
            q13 + q02, q23 - q01, q00 - q11 - q22 + q33) * position;
}

References q0, q1, q2, and q3.

rotateBack() [2/2]

void Quaternion::rotateBack

(

Vec3D &

position

)

const

Applies the inverse rotation to a position

Rotate the given vector from the lab-fixed angles to the body-fixed angles. This is the same as multiplying with the inverse/transpose of the rotation matrix, A^T = A^{-1}.

{
    Mdouble q00 = q0 * q0;
    Mdouble q01 = 2 * q0 * q1;
    Mdouble q02 = 2 * q0 * q2;
    Mdouble q03 = 2 * q0 * q3;
    Mdouble q11 = q1 * q1;
    Mdouble q12 = 2 * q1 * q2;
    Mdouble q13 = 2 * q1 * q3;
    Mdouble q22 = q2 * q2;
    Mdouble q23 = 2 * q2 * q3;
    Mdouble q33 = q3 * q3;
    position = Matrix3D(
            q00 + q11 - q22 - q33, q12 + q03, q13 - q02,
            q12 - q03, q00 - q11 + q22 - q33, q23 + q01,
            q13 + q02, q23 - q01, q00 - q11 - q22 + q33) * position;
}

References q0, q1, q2, and q3.

Referenced by SuperQuadricParticle::computeHessianLabFixed(), SuperQuadricParticle::computeShape(), SuperQuadricParticle::computeShapeGradientLabFixed(), NurbsWall::getDistanceAndNormal(), Screw::getDistanceAndNormal(), BasicIntersectionOfWalls::getDistanceAndNormal(), BasicUnionOfWalls::getDistanceAndNormal(), IntersectionOfWalls::getDistanceAndNormal(), LevelSetWall::getDistanceAndNormal(), and WearableNurbsWall::storeDebris().

rotateInverseInertiaTensor()

MatrixSymmetric3D Quaternion::rotateInverseInertiaTensor

(

const MatrixSymmetric3D &

invI

)

const

Converts the inverse inertia tensor from the reference frame to the lab frame; see See QuaternionsWouter.pdf for details, where this operation is denoted by \(A*I^{-1}*A^T\).

Todo:

move link to where it belongs http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another This is the same as rotateTensor, but than for MatrixSymmetric3D instead of SmallMatrix<3,3>.

{
    Mdouble a = 1 - 2 * q2 * q2 - 2 * q3 * q3;
    Mdouble b = 2 * q1 * q2 - 2 * q0 * q3;
    Mdouble c = 2 * q1 * q3 + 2 * q0 * q2;
    Mdouble d = 2 * q1 * q2 + 2 * q0 * q3;
    Mdouble e = 1 - 2 * q1 * q1 - 2 * q3 * q3;
    Mdouble f = 2 * q2 * q3 - 2 * q0 * q1;
    Mdouble g = 2 * q1 * q3 - 2 * q0 * q2;
    Mdouble h = 2 * q2 * q3 + 2 * q0 * q1;
    Mdouble i = 1 - 2 * q1 * q1 - 2 * q2 * q2;
    MatrixSymmetric3D ans = MatrixSymmetric3D(
            invI.XX * a * a + 2 * invI.XY * a * b + 2 * invI.XZ * a * c + invI.YY * b * b + 2 * invI.YZ * b * c +
            invI.ZZ * c * c,
            d * (invI.XX * a + invI.XY * b + invI.XZ * c) + e * (invI.XY * a + invI.YY * b + invI.YZ * c) +
            f * (invI.XZ * a + invI.YZ * b + invI.ZZ * c),
            g * (invI.XX * a + invI.XY * b + invI.XZ * c) + h * (invI.XY * a + invI.YY * b + invI.YZ * c) +
            i * (invI.XZ * a + invI.YZ * b + invI.ZZ * c),
            invI.XX * d * d + 2 * invI.XY * d * e + 2 * invI.XZ * d * f + invI.YY * e * e + 2 * invI.YZ * e * f +
            invI.ZZ * f * f,
            g * (invI.XX * d + invI.XY * e + invI.XZ * f) + h * (invI.XY * d + invI.YY * e + invI.YZ * f) +
            i * (invI.XZ * d + invI.YZ * e + invI.ZZ * f),
            invI.XX * g * g + 2 * invI.XY * g * h + 2 * invI.XZ * g * i + invI.YY * h * h + 2 * invI.YZ * h * i +
            invI.ZZ * i * i
    );
    return ans;
}

References a, b, calibrate::c, e(), f(), i, q0, q1, q2, q3, MatrixSymmetric3D::XX, MatrixSymmetric3D::XY, MatrixSymmetric3D::XZ, MatrixSymmetric3D::YY, MatrixSymmetric3D::YZ, and MatrixSymmetric3D::ZZ.

rotateTensor()

void Quaternion::rotateTensor

(

SmallMatrix< 3, 3 >

I

)

const

{
    SmallMatrix<3, 3> A;
    getRotationMatrix(A);
    I = A * I * A.transpose();
}

References getRotationMatrix(), and I.

setAngleZ()

void Quaternion::setAngleZ

(

Mdouble

psi

)

Converts the rotation angle in the XY plane into a quaternion (for Mercury2D). See Wikipedia for details.

{
    //assuming theta=phi=0
    q0 = std::cos(-0.5 * psi);
    q1 = 0;
    q2 = 0;
    q3 = std::sin(-0.5 * psi);
}

References cos(), q0, q1, q2, q3, and sin().

setComponent()

void Quaternion::setComponent	(	int	index,
		double	val
	)

Sets the requested component of this Quaternion to the requested value.

Sets the element of the quaternion belonging to the first argument (index) to the value given in the second argument (val).

Parameters

[in]	index	index of element of interest,
[in]	val	value to be set

{
    switch (index)
    {
        case 0:
            q0 = val;
            break;
        case 1:
            q1 = val;
            break;
        case 2:
            q2 = val;
            break;
        case 3:
            q3 = val;
            break;
        default:
            logger(ERROR,
                   "[Quaternion::setComponent] Index = %, which is too high for a 3D quaternion (should be 0-2).",
                   index);
    }
}

setEuler()

void Quaternion::setEuler

(

const Vec3D &

e

)

Convert Euler angles to a quaternion. See Wikipedia for details.

{
    Mdouble cp = std::cos(0.5 * e.Y);
    Mdouble sp = std::sin(0.5 * e.Y);
    Mdouble cr = std::cos(0.5 * e.X);
    Mdouble sr = std::sin(0.5 * e.X);
    Mdouble cy = std::cos(0.5 * e.Z);
    Mdouble sy = std::sin(0.5 * e.Z);
    q0 = cr * cp * cy + sr * sp * sy;
    q1 = sr * cp * cy - cr * sp * sy;
    q2 = cr * sp * cy + sr * cp * sy;
    q3 = cr * cp * sy - sr * sp * cy;
}

References cos(), e(), q0, q1, q2, q3, and sin().

Referenced by BaseInteractable::setOrientationViaEuler().

setLength()

void Quaternion::setLength

(

Mdouble

length

)

Make this Quaternion a certain length |q|=length.

Sets the length of the quaternion to a given scalar (while maintaining the direction).

Parameters

[in]

length

the length to be set

{
    *this /= this->getLength() * length;
}

References getLength().

setOrientationViaNormal()

void Quaternion::setOrientationViaNormal

(

Vec3D

normal

)

Used to the the normal of an InfiniteWall that has a normal into the x-direction by default. This can be changed by resetting the wall orientation; thus the normal is the vector (1,0,0) rotated by this quaternion.

Defines one possible orientation that rotates the x-axis into the direction given by normal

Parameters

[in]

normal

the vector that the x-axis is rotated into.

{
    //if the normal vector cannot be read properly
//    if (normal.getLengthSquared() < 1e-20)
//    {
//        setUnity();
//        return;
//    }
    
    normal.normalise();
    
    if (normal.X <= -1 + 1e-14)
    {
        *this = Quaternion(0, 0, 1, 0);
        return;
    }
    
    Vec3D half = Vec3D(1, 0, 0) + normal;
    q0 = half.X;
    q1 = 0;
    q2 = -half.Z;
    q3 = half.Y;
    normalise();
    //note, it can technically happen that normalising a normalised vector slightly changes the result.
}

References e(), WallFunction::normal(), normalise(), q0, q1, q2, q3, Quaternion(), Vec3D::X, Vec3D::Y, and Vec3D::Z.

Referenced by Quaternion(), and BaseInteractable::setOrientationViaNormal().

setUnity()

void Quaternion::setUnity

(

)

Sets quaternion value to (1,0,0,0)

Sets q0 to 1, and all other elements to zero.

{
    q0 = 1.0;
    q1 = 0.0;
    q2 = 0.0;
    q3 = 0.0;
}

References q0, q1, q2, and q3.

Referenced by BaseInteractable::BaseInteractable(), and Quaternion().

updateAngularDisplacement()

void Quaternion::updateAngularDisplacement

(

Vec3D

angularVelocityDt

)

{
    *this += angularDisplacementTimeDerivative(angularVelocityDt);
    //const Quaternion q = *this;
    normalise();
    //logger(INFO,"%|%",q,*this);
}

References angularDisplacementTimeDerivative(), and normalise().

Referenced by BaseInteractable::rotate().

Friends And Related Function Documentation

operator*

Quaternion operator* ( Mdouble  a, const Quaternion &  b  )

friend

Multiplies all elements by a scalar.

Multiplies each element of a given quaternion (b) by a given scalar (a). NB: this is a global function and a friend of the Quaternion class. Gets called when a scalar multiplication of the form (Mdouble) * (Quaternion) is performed.

Parameters

[in]	a	the scalar
[in]	b	the quaternion

Returns

the resulting quaternion

{
    return Quaternion(b.q0 * a, b.q1 * a, b.q2 * a, b.q3 * a);
}

operator+

Quaternion operator+ ( Mdouble  a, const Quaternion &  b  )

friend

Adds a scalar to a quaternion.

Adds a scalar to the elements of given quaternion NB this is a global function and a friend of the Quaternion class. Gets called when addition operation of the form (Mdouble) + (Quaternion) is performed.

Parameters

[in]	a	the scalar to be added
[in]	b	the quaternion the scalar gets added to.

Returns

the resulting quaternion.

{
    return Quaternion(b.q0 + a, b.q1 + a, b.q2 + a, b.q3 + a);
}

operator- [1/2]

Quaternion operator- ( const Quaternion &  a )

friend

Subtracts a quaternion.

Returns the negative of a given quaternion. NB: this is a global function and a friend of the Quaternion class. Gets called when a negation operation of the form - (Quaternion) is performed.

Parameters

[in]

a

the quaternion to be negated

Returns

the negated quaternion

{
    return Quaternion(-a.q0, -a.q1, -a.q2, -a.q3);
}

operator- [2/2]

Quaternion operator- ( Mdouble  a, const Quaternion &  b  )

friend

Subtracts the elements of a quaternion from a scalar.

Subtracts each element of a given quaternion from a scalar NB this is a global function and a friend of the Quaternion class. Gets called when subtraction operation of the form (Mdouble) - (Quaternion) is performed.

Parameters

[in]	a	the scalar
[in]	b	the quaternion to be subtracted the scalar gets subtracted from.

Returns

the resulting quaternion.

{
    return Quaternion(a - b.q0, a - b.q1, a - b.q2, a - b.q3);
}

operator<<

std::ostream& operator<< ( std::ostream &  os, const Quaternion &  a  )

friend

Adds elements to an output stream.

Adds all elements of the quaternion to an output stream. NB: this is a global function and a friend of the Quaternion class!

Parameters

[in]	os	the output stream,
[in]	a	The quaternion of interest

Returns

the output stream with quaternion elements added

{
    os << a.q0 << ' ' << a.q1 << ' ' << a.q2 << ' ' << a.q3;
    return os;
}

operator>>

std::istream& operator>> ( std::istream &  is, Quaternion &  a  )

friend

Adds elements to an input stream.

Reads all elements of a given quaternion from an input stream. NB: this is a global function and a friend of the Quaternion class!

Parameters

[in,out]	is	the input stream
[in,out]	a	the quaternion to be read in

Returns

the input stream from which the quaternion elements were read

{
    is >> a.q0 >> a.q1 >> a.q2 >> a.q3;
    return is;
}

Member Data Documentation

q0

Mdouble Quaternion::q0

the zeroth component of the quaternion q = (q0,q1,q2,q3)

Referenced by angularDisplacementTimeDerivative(), angularVelocityBodyFixedFrameToAngularDisplacement(), applyCInverse(), fromAxisAndAngle(), getAngleZ(), getAxis(), getDistance(), getEuler(), getLengthSquared(), getRotationMatrix(), isUnity(), operator*(), operator*=(), Quaternion< float >::operator*=(), operator+(), operator+=(), operator-(), operator-=(), operator/(), Quaternion(), rotate(), rotateBack(), rotateInverseInertiaTensor(), setAngleZ(), setEuler(), setOrientationViaNormal(), and setUnity().

q1

Mdouble Quaternion::q1

the first component of the quaternion q = (q0,q1,q2,q3)

Referenced by angularDisplacementTimeDerivative(), angularVelocityBodyFixedFrameToAngularDisplacement(), applyCInverse(), fromAxisAndAngle(), getAngleZ(), getAxis(), getDistance(), getEuler(), getLengthSquared(), getRotationMatrix(), isUnity(), operator*(), operator*=(), Quaternion< float >::operator*=(), operator+(), operator+=(), operator-(), operator-=(), operator/(), Quaternion(), rotate(), rotateBack(), rotateInverseInertiaTensor(), setAngleZ(), setEuler(), setOrientationViaNormal(), and setUnity().

q2

Mdouble Quaternion::q2

the second component of the quaternion q = (q0,q1,q2,q3)

Referenced by angularDisplacementTimeDerivative(), angularVelocityBodyFixedFrameToAngularDisplacement(), applyCInverse(), fromAxisAndAngle(), getAngleZ(), getAxis(), getDistance(), getEuler(), getLengthSquared(), getRotationMatrix(), isUnity(), operator*(), Quaternion< float >::operator*=(), operator*=(), operator+(), operator+=(), operator-(), operator/(), Quaternion(), rotate(), rotateBack(), rotateInverseInertiaTensor(), setAngleZ(), setEuler(), setOrientationViaNormal(), and setUnity().

q3

Mdouble Quaternion::q3

the third component of the quaternion q = (q0,q1,q2,q3)

Referenced by angularDisplacementTimeDerivative(), angularVelocityBodyFixedFrameToAngularDisplacement(), applyCInverse(), fromAxisAndAngle(), getAngleZ(), getAxis(), getDistance(), getEuler(), getLengthSquared(), getRotationMatrix(), isUnity(), operator*(), Quaternion< float >::operator*=(), operator+(), operator+=(), operator-(), operator/(), Quaternion(), rotate(), rotateBack(), rotateInverseInertiaTensor(), setAngleZ(), setEuler(), setOrientationViaNormal(), and setUnity().

---

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

• Kernel/Math/Quaternion.h
• Quaternion.cc