## Saturday, January 21, 2017

### Quaternion.h

#pragma once
// Math cobbled together from a variety of sources:
// Introduction into quaternions for spacecraft attitude representation
// Dipl. -Ing. Karsten Groÿekatthöfer, Dr. -Ing. Zizung Yoon, May 31, 2012
// Implementation for a generalized quaternion class - Angela Bennett, 1.25.00
// http://www.gamedev.net/topic/621943-quaternion-object-rotation/
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
// http://www.blitzmax.com/Community/posts.php?topic=66504
// https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions
// http://www.haroldserrano.com/blog/developing-a-math-engine-in-c-implementing-quaternions
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

#include <iostream>
#include <math.h>

class Euler;
class Vector;
class Matrix;

class Quaternion
{
private:
public:

double w;
double x;
double y;
double z;

// Create a quaternion equivalent to euler (0,0,0).
Quaternion();

// Create a quaternion from 4 inputs.
Quaternion(const double newW, const double newX, const double newY, const double newZ);

// Create a quaternion matching to another quaternion.
Quaternion(const Quaternion &newQuaternion);

// Create a quaternion from euler angles (in radians).
Quaternion(const Euler &newEulerAngle);

// Destructor
~Quaternion();

// Sets the quaternion to another quaternion.
Quaternion operator = (const Quaternion &qToCopy);

// Adds the quaternion to another quaternion.
Quaternion operator + (const Quaternion &qToAdd) const;

// Subtracts the quaternion by another quaternion.
Quaternion operator - (const Quaternion &qToSubtract) const;

// Multiplies the quaternion by another quaternion.
Quaternion operator * (const Quaternion &qToMultiply) const;

// Divides the quaternion by another quaternion.
Quaternion operator / (const Quaternion &qToDivide) const;

// Divides the quaternion by a scalar.
Quaternion operator / (const double valueToDivide) const;

// Adds the quaternion to another quaternion.
Quaternion &operator += (const Quaternion &qToAdd);

// Subtracts the quaternion by another quaternion.
Quaternion &operator -= (const Quaternion &qToSubtract);

// Multiplies the quaternion by another quaternion.
Quaternion &operator *= (const Quaternion &qToMultiply);

// Divides the quaternion by another quaternion.
Quaternion &operator /= (const Quaternion &qToDivide);

// Divides the quaternion by a scalar.
Quaternion &operator /= (const double valueToDivide);

// Returns if two quaternions are not equal.
bool operator != (const Quaternion &qToCompare) const;

// Returns if two quaternions are equal.
bool operator == (const Quaternion &qToCompare) const;

// Set the quaternion to the equivalent of euler angle (0,0,0).
void clear();

// Get the norm of the quaternion.
double norm() const;

// Get a quaternion scaled by a given scalar.
Quaternion scaledBy(const double valueToScale) const;

// Get the inverse of the quaternion.
Quaternion inverse() const;

// Get the conjugate of the quaternion.
Quaternion conjugate() const;

// Get the unit (normalization) of the quaternion.
Quaternion unit() const;

// Normalize the quaternion.
void normalize();

// Set the quaternion from euler angles (in radians).
void fromEulerAngles(const Euler &newEulerAngle);

// Get the euler angles (in radians) of the quaternion.
Euler toEulerAngles() const;

// Set the quaternion based on a rotation about the X axis.

// Set the quaternion based on a rotation about the Y axis.

// Set the quaternion based on a rotation about the Z axis.

// Get the transformation matrix for this quaternion orientation.
Matrix transformationMatrix() const;

//Quaternion fromYawPitchRoll(Euler ypr);

// Rotate the quaternion about the world axis.
};

### Quaternion.cpp

#include "math.h"
#include "Euler.h"
#include "Matrix.h"
#include "Quaternion.h"

// Create a quaternion equivalent to euler (0,0,0).
Quaternion::Quaternion()
{
// Equivalent of euler angle (0,0,0).
this->w = 1.0;
this->x = 0.0;
this->y = 0.0;
this->z = 0.0;
return;
}

// Create a quaternion from 4 inputs.
Quaternion::Quaternion(const double newW, const double newX, const double newY, const double newZ)
{
this->w = newW;
this->x = newX;
this->y = newY;
this->z = newZ;
return;
}

// Create a quaternion matching to another quaternion.
Quaternion::Quaternion(const Quaternion &newQuaternion)
{
this->w = newQuaternion.w;
this->x = newQuaternion.x;
this->y = newQuaternion.y;
this->z = newQuaternion.z;
return;

// Create a quaternion from euler angles (in radians).
Quaternion::Quaternion(const Euler &newEulerAngle)
{
this->fromEulerAngles(newEulerAngle);
}

// Destructor
Quaternion::~Quaternion()
{
}

// Sets the quaternion to another quaternion.
Quaternion Quaternion::operator = (const Quaternion &qToCopy)
{
this->w = qToCopy.w;
this->x = qToCopy.x;
this->y = qToCopy.y;
this->z = qToCopy.z;

return *this;
}

// Adds the quaternion to another quaternion.
Quaternion Quaternion::operator + (const Quaternion &qToAdd) const
{
}

// Subtracts the quaternion by another quaternion.
Quaternion Quaternion::operator - (const Quaternion &qToSubtract) const
{
return Quaternion(this->w - qToSubtract.w, this->x - qToSubtract.x, this->y - qToSubtract.y, this->z - qToSubtract.z);
}

// Multiplies the quaternion by another quaternion.
Quaternion Quaternion::operator * (const Quaternion &qToMultiply) const
{
return Quaternion((this->w * qToMultiply.w) - (this->x * qToMultiply.x) - (this->y * qToMultiply.y) - (this->z * qToMultiply.z),
(this->w * qToMultiply.x) + (this->x * qToMultiply.w) + (this->y * qToMultiply.z) - (this->z * qToMultiply.y),
(this->w * qToMultiply.y) + (this->y * qToMultiply.w) + (this->z * qToMultiply.x) - (this->x * qToMultiply.z),
(this->w * qToMultiply.z) + (this->z * qToMultiply.w) + (this->x * qToMultiply.y) - (this->y * qToMultiply.x));
}

// Divides the quaternion by another quaternion.
Quaternion Quaternion::operator / (const Quaternion &qToDivide) const
{
return ((*this) * qToDivide.inverse());
}

// Divides the quaternion by a scalar.
Quaternion Quaternion::operator / (const double valueToDivide) const
{
if (valueToDivide != 0.0)
{
return Quaternion(this->w / valueToDivide, this->x / valueToDivide, this->y / valueToDivide, this->z / valueToDivide);
}
else
{
return Quaternion(this->w, this->x, this->y, this->z);
}
}

// Adds the quaternion to another quaternion.
Quaternion& Quaternion::operator += (const Quaternion &qToAdd)
{

return (*this);
}

// Subtracts the quaternion by another quaternion.
Quaternion& Quaternion::operator -= (const Quaternion &qToSubtract)
{
this->w -= qToSubtract.w;
this->x -= qToSubtract.x;
this->y -= qToSubtract.y;
this->z -= qToSubtract.z;

return (*this);
}

// Multiplies the quaternion by another quaternion.
Quaternion& Quaternion::operator *= (const Quaternion &qToMultiply)
{
double newW = (this->w * qToMultiply.w) - (this->x * qToMultiply.x) - (this->y * qToMultiply.y) - (this->z * qToMultiply.z);
double newX = (this->w * qToMultiply.x) + (this->x * qToMultiply.w) + (this->y * qToMultiply.z) - (this->z * qToMultiply.y);
double newY = (this->w * qToMultiply.y) + (this->y * qToMultiply.w) + (this->z * qToMultiply.x) - (this->x * qToMultiply.z);
double newZ = (this->w * qToMultiply.z) + (this->z * qToMultiply.w) + (this->x * qToMultiply.y) - (this->y * qToMultiply.x);

this->w = newW;
this->x = newX;
this->y = newY;
this->z = newZ;

return (*this);
}

// Divides the quaternion by another quaternion.
Quaternion& Quaternion::operator /= (const Quaternion &qToDivide)
{
(*this) = (*this) * qToDivide.inverse();
return (*this);
}

// Divides the quaternion by a scalar.
Quaternion& Quaternion::operator /= (const double valueToDivide)
{
this->w /= valueToDivide;
this->x /= valueToDivide;
this->y /= valueToDivide;
this->z /= valueToDivide;
return (*this);
}

// Returns if two quaternions are not equal.
bool Quaternion::operator != (const Quaternion &qToCompare) const
{
if ((this->w != qToCompare.w) || (this->x != qToCompare.x) || (this->y != qToCompare.y) || (this->z != qToCompare.z))
{
return true;
}
else
{
return false;
}
}

// Returns if two quaternions are equal.
bool Quaternion::operator == (const Quaternion &qToCompare) const
{
if ((this->w == qToCompare.w) && (this->x == qToCompare.x) && (this->y == qToCompare.y) && (this->z == qToCompare.z))
{
return true;
}
else
{
return false;
}
}

// Set the quaternion to the equivalent of euler angle (0,0,0).
void Quaternion::clear()
{
// Equivalent of euler angle (0,0,0).
this->w = 1.0;
this->x = 0.0;
this->y = 0.0;
this->z = 0.0;
return;
}

// Get the norm of the quaternion.
double Quaternion::norm() const
{
// |q| = sqrt(w*w + x*x + y*y + z*z)
return sqrt( (this->w * this->w) + (this->x * this->x) + (this->y * this->y) + (this->z * this->z) );
}

// Get a quaternion scaled by a given scalar.
Quaternion Quaternion::scaledBy(const double valueToScale) const
{
return Quaternion(this->w * valueToScale, this->x * valueToScale, this->y * valueToScale, this->z * valueToScale);
}

// Get the inverse of the quaternion.
Quaternion Quaternion::inverse() const
{
// q^-1 = q* / |q|
double quatNorm = this->norm();
return (this->conjugate() / (quatNorm * quatNorm));
}

// Get the conjugate of the quaternion.
Quaternion Quaternion::conjugate() const
{
// q* = [w, -x , -y, -z]
return Quaternion(this->w, -this->x, -this->y, -this->z);
}

// Get the unit (normalization) of the quaternion.
Quaternion Quaternion::unit() const
{
// Unit: ||q|| = q / |q|
return ((*this) / this->norm());
}

// Normalize the quaternion.
void Quaternion::normalize()
{
// Unit: ||q|| = q / |q|
(*this) /= this->norm();
return;
}

// Set the quaternion from euler angles (in radians).
void Quaternion::fromEulerAngles(const Euler &newEulerAngle)
{
double halfPsi = newEulerAngle.psi * 0.5;
double halfTheta = newEulerAngle.theta * -0.5;
double halfPhi = newEulerAngle.phi * 0.5;

double cosPsi = cos(halfPsi);
double cosTheta = cos(halfTheta);
double cosPhi = cos(halfPhi);

double sinPsi = sin(halfPsi);
double sinTheta = sin(halfTheta);
double sinPhi = sin(halfPhi);

double cosPsiCosPhi = cosPsi * cosPhi;
double cosPsiSinPhi = cosPsi * sinPhi;
double sinPsiCosPhi = sinPsi * cosPhi;
double sinPsiSinPhi = sinPsi * sinPhi;

this->w = (cosTheta * cosPsiCosPhi) + (sinTheta * sinPsiSinPhi);
this->x = (cosTheta * cosPsiSinPhi) - (sinTheta * sinPsiCosPhi);
this->y = -(cosTheta * sinPsiSinPhi) - (sinTheta * cosPsiCosPhi);
this->z = (cosTheta * sinPsiCosPhi) - (sinTheta * cosPsiSinPhi);

return;
}

// Get the euler angles (in radians) of the quaternion.
Euler Quaternion::toEulerAngles() const
{
Euler newEulerAngle;
double xw = this->x * this->w;
double yw = this->y * this->w;
double zw = this->z * this->w;

double xz = this->x * this->z;
double yz = this->y * this->z;
double xy = this->x * this->y;

double xx = this->x * this->x;
double yy = this->y * this->y;
double zz = this->z * this->z;

newEulerAngle.phi = atan2((2 * (xw + yz)), (1 - 2 * (xx + yy)));
newEulerAngle.theta = asin(2 * (yw - xz));
newEulerAngle.psi = atan2((2 * (zw + xy)), (1 - 2 * (yy + zz)));

while (newEulerAngle.psi < 0.0)
{
newEulerAngle.psi += 2 * 3.1415926;
}
return newEulerAngle;
}

// Set the quaternion based on a rotation about the X axis.
{
this->y = 0.0;
this->z = 0.0;
return;
}

// Set the quaternion based on a rotation about the Y axis.
{
this->x = 0.0;
this->z = 0.0;
return;
}

// Set the quaternion based on a rotation about the Z axis.
{
this->x = 0.0;
this->y = 0.0;
return;
}

// Get the transformation matrix for this quaternion orientation.
Matrix Quaternion::transformationMatrix() const
{
double xx = this->x * this->x;
double xy = this->x * this->y;
double xz = this->x * this->z;
double xw = this->x * this->w;

double yy = this->y * this->y;
double yz = this->y * this->z;
double yw = this->y * this->w;

double zz = this->z * this->z;
double zw = this->z * this->w;

Matrix rotationMatrix;
rotationMatrix.value = 1 - 2 * (yy + zz);
rotationMatrix.value = 2 * (xy - zw);
rotationMatrix.value = 2 * (xz + yw);

rotationMatrix.value = 2 * (xy + zw);
rotationMatrix.value = 1 - 2 * (xx + zz);
rotationMatrix.value = 2 * (yz - xw);

rotationMatrix.value = 2 * (xz - yw);
rotationMatrix.value = 2 * (yz + xw);
rotationMatrix.value = 1 - 2 * (xx + yy);

return rotationMatrix;
}

{

return;
}

// Rotate the quaternion about the world axis.
{
Quaternion xOffset = Quaternion();
Quaternion yOffset = Quaternion();
Quaternion zOffset = Quaternion();

(*this) = xOffset * yOffset * zOffset * (*this);

this->normalize();

return;
}