Orbital Aero Model: Matrix Class

This is one of a series of posts for how orientations and rotations are handled in the orbital aero model (original post in series). This Matrix class ties in with the Vector, Euler, and Quaternion classes also posted.

Matrix.h

#pragma once

class Matrix
{
private:

public:
double value[3][3]; // [Row][Column]

// Create an empty matrix.
Matrix();

// Multiplies the matrix by another matrix.
Matrix operator * (const Matrix &matrixToMultiply) const;

// Get an inverted version of the matrix.
Matrix inverted() const;
};

Matrix.cpp

#include "math.h"
#include "Matrix.h"

// Create an empty matrix.
Matrix::Matrix()
{
for (int rowIndex = 0; rowIndex < 3; rowIndex++)
{
for (int columnIndex = 0; columnIndex < 3; columnIndex++)
{
this->value[rowIndex][columnIndex] = 0.0;
}
}
return;
}

// Multiplies this matrix by another matrix.
Matrix Matrix::operator * (const Matrix &matrixToMultiply) const
{
Matrix calculatedMatrix;

for (int i = 0; i <= 2; i++)
{
for (int j = 0; j <= 2; j++)
{
double sum = 0.0;
for (int k = 0; k <= 2; k++)
{
sum = sum + this->value[i][k] * matrixToMultiply.value[k][j];
}
calculatedMatrix.value[i][j] = sum;
}
}

return calculatedMatrix;
}

// Get an inverted version of the matrix.
Matrix Matrix::inverted() const
{
Matrix invertedMatrix;
for (int i = 0; i <= 2; i++)
{
for (int j = 0; j <= 2; j++)
{
invertedMatrix.value[j][i] = this->value[i][j];
}
}

return invertedMatrix;
}

Copyright (c) 2017 Clinton Kam
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Orbital Aero Model: Quaternion Class

This is one of a series of posts for how orientations and rotations are handled in the orbital aero model (original post in series). This Quaternion class ties in with the Vector, Euler, and Matrix classes also posted.

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;

//void Quaternion::FromPYR(float pitch_radians, float yaw_radians, float roll_radians);

// Set the quaternion based on a rotation about the X axis.
void fromXRotation(const double angle_radians);

// Set the quaternion based on a rotation about the Y axis.
void fromYRotation(const double angle_radians);

// Set the quaternion based on a rotation about the Z axis.
void fromZRotation(const double angle_radians);

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

//Quaternion fromYawPitchRoll(Euler ypr);

// Rotate the quaternion about the local axis by a pitch/roll/heading.
void rotateAboutLocalAxis(const Euler &rotationAngle_radians);

// Rotate the quaternion about the world axis.
void Quaternion::rotateAboutWorldAxis(const Euler &rotationAngles_radians);
};

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
{
return Quaternion(this->w + qToAdd.w, this->x + qToAdd.x, this->y + qToAdd.y, this->z + qToAdd.z);
}
  
// 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)
{
this->w += qToAdd.w;
this->x += qToAdd.x;
this->y += qToAdd.y;
this->z += qToAdd.z;

  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.
void Quaternion::fromXRotation(const double angle_radians)
{
this->w = cos(angle_radians / 2);
this->x = sin(angle_radians / 2);
this->y = 0.0;
this->z = 0.0;
return;
}

// Set the quaternion based on a rotation about the Y axis.
void Quaternion::fromYRotation(const double angle_radians)
{
this->w = cos(angle_radians / 2);
this->x = 0.0;
this->y = sin(angle_radians / 2);
this->z = 0.0;
return;
}

// Set the quaternion based on a rotation about the Z axis.
void Quaternion::fromZRotation(const double angle_radians)
{
this->w = cos(angle_radians / 2);
this->x = 0.0;
this->y = 0.0;
this->z = sin(angle_radians / 2);
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[0][0] = 1 - 2 * (yy + zz);
rotationMatrix.value[1][0] = 2 * (xy - zw);
rotationMatrix.value[2][0] = 2 * (xz + yw);

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

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

return rotationMatrix;
}

// Rotate the quaternion about the local axis by a pitch/roll/heading.
void Quaternion::rotateAboutLocalAxis(const Euler &rotationAngles_radians)
{
(*this) *= Quaternion(rotationAngles_radians);

return;
}

// Rotate the quaternion about the world axis.
void Quaternion::rotateAboutWorldAxis(const Euler &rotationAngles_radians)
{
Quaternion xOffset = Quaternion();
Quaternion yOffset = Quaternion();
Quaternion zOffset = Quaternion();

xOffset.fromXRotation(rotationAngles_radians.phi);
yOffset.fromYRotation(rotationAngles_radians.theta);
zOffset.fromZRotation(rotationAngles_radians.psi);
(*this) = xOffset * yOffset * zOffset * (*this);

this->normalize();

return;
}

Copyright (c) 2017 Clinton Kam
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Orbital Aero Model: Euler Class

This is one of a series of posts for how orientations and rotations are handled in the orbital aero model (original post in series). This Euler class ties in with the Vector, Quaternion, and Matrix classes also posted.

Euler.h

#pragma once
#include <math.h>

class Vector;
class Quaternion;
class Matrix;

class Euler
{
private:
Matrix getXRotationMatrix() const;
Matrix getYRotationMatrix() const;
Matrix getZRotationMatrix() const;

public:
// Coordinate system:
// X forward
// Y to the right
// Z down

double psi; // rotate about z (yaw)
double theta; // rotate about y (pitch)
double phi; // rotate about x (roll)

// Create a set of euler angles.
Euler();

// Create euler angles with given values for roll, pitch, and yaw.
Euler(const double newPsi, const double newTheta, const double newPhi);

// Destructor
~Euler();

// Scales the euler angles by a given value.
Euler operator * (const double valueToMultiply) const;

// Clear the euler angles to all 0.
void clear();

// Return if any of the euler angles are non-zero.
bool isNonZero() const;

// Get the transformation matrix for these euler angles.
Matrix transformationMatrix() const;
};

Euler.cpp

#include "Matrix.h"
#include "Vector.h"
#include "Euler.h"

// Create a set of euler angles.
Euler::Euler()
{
this->psi = 0.0;
this->theta = 0.0;
this->phi = 0.0;
return;
}

// Create euler angles with given values for roll, pitch, and yaw.
Euler::Euler(const double newPsi, const double newTheta, const double newPhi)
{
this->psi = newPsi;
this->theta = newTheta;
this->phi = newPhi;
return;
}

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

// Scales the euler angles by a given value.
Euler Euler::operator * (const double valueToMultiply) const
{
return Euler(this->psi * valueToMultiply, this->theta * valueToMultiply, this->phi * valueToMultiply);
}

// Clear the euler angles to all 0.
void Euler::clear()
{
this->psi = 0.0;
this->theta = 0.0;
this->phi = 0.0;
}

// Return if any of the euler angles are non-zero.
bool Euler::isNonZero() const
{
if (this->psi != 0.0 || this->theta != 0.0 || this->phi != 0.0)
{
return true;
}
else
{
return false;
}
}

// Get the transformation matrix for these euler angles.
Matrix Euler::transformationMatrix() const
{
Matrix xRotationMatrix = getXRotationMatrix();
Matrix yRotationMatrix = getYRotationMatrix();
Matrix zRotationMatrix = getZRotationMatrix();
return Matrix(xRotationMatrix * yRotationMatrix * zRotationMatrix);
}

// Create a x-axis rotation matrix.
Matrix Euler::getXRotationMatrix() const
{
double cosRotation = cos(this->phi);
double sinRotation = sin(this->phi);

Matrix xRotationaMatrix;
xRotationaMatrix.value[0][0] = 1.0;
xRotationaMatrix.value[0][1] = 0.0;
xRotationaMatrix.value[0][2] = 0.0;
xRotationaMatrix.value[1][0] = 0.0;
xRotationaMatrix.value[1][1] = cosRotation;
xRotationaMatrix.value[1][2] = sinRotation;
xRotationaMatrix.value[2][0] = 0.0;
xRotationaMatrix.value[2][1] = -sinRotation;
xRotationaMatrix.value[2][2] = cosRotation;

return xRotationaMatrix;
}

// Create a y-axis rotation matrix.
Matrix Euler::getYRotationMatrix() const
{
double cosRotation = cos(this->theta);
double sinRotation = sin(this->theta);

Matrix yRotationaMatrix;
yRotationaMatrix.value[0][0] = cosRotation;
yRotationaMatrix.value[0][1] = 0.0;
yRotationaMatrix.value[0][2] = -sinRotation;
yRotationaMatrix.value[1][0] = 0.0;
yRotationaMatrix.value[1][1] = 1.0;
yRotationaMatrix.value[1][2] = 0.0;
yRotationaMatrix.value[2][0] = sinRotation;
yRotationaMatrix.value[2][1] = 0.0;
yRotationaMatrix.value[2][2] = cosRotation;

return yRotationaMatrix;
}

// Create a z-axis rotation matrix.
Matrix Euler::getZRotationMatrix() const
{
double cosRotation = cos(this->psi);
double sinRotation = sin(this->psi);

Matrix zRotationaMatrix;
zRotationaMatrix.value[0][0] = cosRotation;
zRotationaMatrix.value[0][1] = sinRotation;
zRotationaMatrix.value[0][2] = 0.0;
zRotationaMatrix.value[1][0] = -sinRotation;
zRotationaMatrix.value[1][1] = cosRotation;
zRotationaMatrix.value[1][2] = 0.0;
zRotationaMatrix.value[2][0] = 0.0;
zRotationaMatrix.value[2][1] = 0.0;
zRotationaMatrix.value[2][2] = 1.0;

return zRotationaMatrix;
}

Copyright (c) 2017 Clinton Kam
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Orbital Aero Model: Vector Class

This is one of a series of posts for how orientations and rotations are handled in the orbital aero model (original post in series). This Vector class ties in with the Euler, Quaternion, and Matrix classes also posted.

This is an update of my original Vector class posted in 2015.

Vector.h

#pragma once

template <typename T> int sgn(T val)
{
return (T(0) < val) - (val < T(0));
}

class Euler;
class Quaternion;
class Matrix;

class Vector
{
private:

public:
double x;
double y;
double z;

// Create a vector (0,0,0).
Vector();

// Create an vector form a given X, Y, and Z.
Vector(const double newX, const double newY, const double newZ);

// Sets all value of a vector to a given value.
Vector &operator = (const double newValue);

// Sets the vector to another vector.
Vector &operator = (const Vector &thatVector);

// Gets the reverse of the vector.
Vector operator - ();

// Adds the vector to another vector.
Vector operator + (const Vector &vectorToAdd) const;

// Subtracts the vector by another vector.
Vector operator - (const Vector &vectorToSubtract) const;

// Multiplies the vector by a value.
Vector operator * (const double valueToMultiply) const;

// Multiplies the vector by another vector.
Vector operator * (const Vector &vectorToMultiply) const;

// Multiplies the vector to a matrix.
Vector operator * (const Matrix &matrixToMultiply) const;

// Divides the vector by a value.
Vector operator / (const double valueToDivide) const;

// Adds the vector to another vector.
Vector &operator += (const Vector &vectorToAdd);

// Subtracts the vector by another vector.
Vector &operator -= (const Vector &vectorToSubtract);

// Multiplies the vector by another vector.
Vector &operator *= (double valueToMultiply);

// Divides the vector by another vector.
Vector &operator /= (double valueToDivide);

// Get the magnitude of the vector.
double magnitude() const;

// Get the unit vector.
Vector unit() const;

// Get a vector representing the sign of each component.
Vector sign() const;

// Get the dot product of two vectors.
static double dotProduct(const Vector &vector1, const Vector &vector2);

// Get the cross product of two vectors.
static Vector crossProduct(const Vector &vector1, const Vector &vector2);

// Get the vector rotated by a quaternion.
Vector rotatedBy(const Quaternion &rotationQuat) const;

// Get the vector rotated by euler angles.
Vector rotatedBy(const Euler &rotationAngles_radians) const;
};

Vector.cpp

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

// Create a vector (0,0,0).
Vector::Vector()
{
this->x = 0.0;
this->y = 0.0;
this->z = 0.0;
return;
}

// Create an vector form a given X, Y, and Z.
Vector::Vector(const double newX, const double newY, const double newZ)
{
this->x = newX;
this->y = newY;
this->z = newZ;
return;
}

// Sets all value of a vector to a given value.
Vector &Vector::operator = (const double newValue)
{
this->x = newValue;
this->y = newValue;
this->z = newValue;
return *this;
}

// Sets the vector to another vector.
Vector &Vector::operator = (const Vector &thatVector)
{
// Protect against self-assignment. (Otherwise bad things happen when it's reading from memory it has cleared.)
if (this != &thatVector)
{
this->x = thatVector.x;
this->y = thatVector.y;
this->z = thatVector.z;
}
return *this;
}

// Gets the reverse of the vector.
Vector Vector::operator - ()
{
return Vector(-this->x, -this->y, -this->z);
}

// Adds the vector to another vector.
Vector Vector::operator + (const Vector &vectorToAdd) const
{
return Vector(this->x + vectorToAdd.x, this->y + vectorToAdd.y, this->z + vectorToAdd.z);
}

// Subtracts the vector by another vector.
Vector Vector::operator - (const Vector &vectorToSubtract) const
{
return Vector(this->x - vectorToSubtract.x, this->y - vectorToSubtract.y, this->z - vectorToSubtract.z);
}

// Multiplies the vector by a value.
Vector Vector::operator * (const double valueToMultiply) const
{
return Vector(this->x * valueToMultiply, this->y * valueToMultiply, this->z * valueToMultiply);
}

// Multiplies the vector by another vector.
Vector Vector::operator * (const Vector &vectorToMultiply) const
{
return Vector(this->x * vectorToMultiply.x, this->y * vectorToMultiply.y, this->z * vectorToMultiply.z);
}

// Multiplies the vector to a matrix.
Vector Vector::operator * (const Matrix &matrixToMultiply) const
{
return Vector((matrixToMultiply.value[0][0] * this->x) + (matrixToMultiply.value[0][1] * this->y) + (matrixToMultiply.value[0][2] * this->z),
                  (matrixToMultiply.value[1][0] * this->x) + (matrixToMultiply.value[1][1] * this->y) + (matrixToMultiply.value[1][2] * this->z),
             (matrixToMultiply.value[2][0] * this->x) + (matrixToMultiply.value[2][1] * this->y) + (matrixToMultiply.value[2][2] * this->z));
}

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

// Adds the vector to another vector.
Vector &Vector::operator += (const Vector &vectorToAdd)
{
this->x += vectorToAdd.x;
this->y += vectorToAdd.y;
this->z += vectorToAdd.z;
return *this;
}

// Subtracts the vector by another vector.
Vector &Vector::operator -= (const Vector &vectorToSubtract)
{
this->x -= vectorToSubtract.x;
this->y -= vectorToSubtract.y;
this->z -= vectorToSubtract.z;
return *this;
}

// Multiplies the vector by another vector.
Vector &Vector::operator *= (double valueToMultiply)
{
this->x *= valueToMultiply;
this->y *= valueToMultiply;
this->z *= valueToMultiply;
return *this;
}

// Divides the vector by another vector.
Vector &Vector::operator /= (double valueToDivide)
{
if (valueToDivide != 0.0)
{
this->x /= valueToDivide;
this->y /= valueToDivide;
this->z /= valueToDivide;
}
return *this;
}

// Get the magnitude of the vector.
double Vector::magnitude() const
{
return sqrt( (this->x * this->x) + (this->y * this->y) + (this->z * this->z) );
}

// Get the unit vector.
Vector Vector::unit() const
{
double mag = this->magnitude();

Vector unitVector;
if (mag > 0)
{
unitVector.x = this->x / mag;
unitVector.y = this->y / mag;
unitVector.z = this->z / mag;
}
else
{
unitVector.x = 1.0;
unitVector.y = 0.0;
unitVector.z = 0.0;
}

return unitVector;
}

// Get a vector representing the sign of each component.
Vector Vector::sign() const
{
Vector signVector;
signVector.x = sgn(this->x);
signVector.y = sgn(this->y);
signVector.z = sgn(this->z);
return signVector;
}

// Get the dot product of two vectors.
double Vector::dotProduct(const Vector &vector1, const Vector &vector2)
{
return ((vector1.x * vector2.x) + (vector1.y * vector2.y) + (vector1.z * vector2.z));
}

// Get the cross product of two vectors.
Vector Vector::crossProduct(const Vector &vector1, const Vector &vector2)
{
Vector crossVector;
crossVector.x = (vector1.y * vector2.z) - (vector1.z * vector2.y);
crossVector.y = (vector1.z * vector2.x) - (vector1.x * vector2.z);
crossVector.z = (vector1.x * vector2.y) - (vector1.y * vector2.x);
return crossVector;
}

// Get the vector rotated by a quaternion.
Vector Vector::rotatedBy(const Quaternion &rotationQuat) const
{
Matrix transformationMatrix = rotationQuat.transformationMatrix();
return (*this) * transformationMatrix;
}

// Get the vector rotated by euler angles.
Vector Vector::rotatedBy(const Euler &rotationAngles_radians) const
{
Matrix transformationMatrix = rotationAngles_radians.transformationMatrix();
return (*this) * transformationMatrix;
}


Copyright (c) 2017 Clinton Kam
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Orbital Aero Model: Orientations & Rotations

Orientations in simulations are commonly described via Euler angles or Quaternions.

Euler Angles

Euler angles are a succession of 3 rotations about the axes of a coordinate system. You can read the three Euler angles and visualize in your mind how the object is orientated in space. The order of the rotations matters, but there is no universal order for the rotations. The downside with Euler Angles is they suffer from gimbal lock - at pitch angles of +90 or -90 the rotations fall apart and special conditions have to be made.

https://en.wikipedia.org/wiki/Euler_angles

Quaternions

Quaternions are far less intuitive than Euler angles. A Quaternion representation of an object's orientation consists of 4 numbers: w, x, y, and z (or a, b, c, d, or s, x, y, z or a scalar & a vector or ...). However, even if you know those numbers it's (near?) impossible to visualize in your mind how the object is orientated in space. Rotations of Quaternions are more efficient than Euler angles for a computer to calculate, and they don't have the gimbal lock problem.

https://en.wikipedia.org/wiki/Quaternion

Angles in Orbital Aero Model

For any project doing significant work with angles, you definitely want to use Quaternions internally. However when communicating angles to the user, it's better to use Euler angles.

Using Quaternions is actually fairly simple, but getting a solution off the Internet that "just works" is more difficult than it should be. The options were either gigantic libraries that I only needed a tiny piece of, smaller convoluted source code snippets, or straightforward code snippets that actually failed under certain circumstances!

My solution was to take the best pieces of several sources and simplify them down to just what I needed. A way to represent an orientation, a way to rotate the object in space, and a way to rotate a vector (such as thrust) by an orientation.

Body Axis

A body's axis is defined as follows:
x forward
y right
z down

For my Euler angles, I'm using the convention of rotation of z followed by y followed by x. This matches the IEEE 1278.1 DIS (Distributed Interactive Simulation) standard used by military simulation networks (easy for me to communicate with those simulations).

A body is represented in space by its location vector from the world axis and then the rotation from the world to the body axis.

Using the Rotation Code

The Body class was modified to contain an orientation with the Quaternions class and an angular velocity with the Euler angles class:


class BodyModel
{
   .
   .
   Quaternion orientation_quaternions;
   Euler angularVelocity_radiansPerSecond;
};

In the body's constructor, I clear out the orientation and the angular velocity:


this->orientation_quaternions.clear();

this->angularVelocity_radiansPerSecond.clear();

Quaternion.clear() sets the Quaternions values to: w=1, x=0, y=0, and z=0 which is the equivalent to an Euler angle of 0,0,0 (no rotation).

I'm using SFML (Simple Fast Media Library) to get joystick inputs to control one of the bodies. Each frame I get the inputs and store them in the orbital aero object. SFML returns axis positions +/- 100, and I'm using a 10% dead zone so I don't have excess twitching when things should be static.


sf::Joystick::update();
if (sf::Joystick::isConnected(0))
{
   float joystickDeadZone = 10;
  float joystickX = sf::Joystick::getAxisPosition(0, sf::Joystick::X);
  float joystickY = sf::Joystick::getAxisPosition(0, sf::Joystick::Y);
  float joystickR = sf::Joystick::getAxisPosition(0, sf::Joystick::R);
  float joystickZ = sf::Joystick::getAxisPosition(0, sf::Joystick::Z);
  if (abs(joystickZ) > joystickDeadZone)
  {
  orbitalAero.joystickInputs.roll = (joystickZ - (joystickDeadZone * sgn(joystickZ))) / (100 - joystickDeadZone);
  }
  else
  {
orbitalAero.joystickInputs.roll = 0.0;
  }
  if (abs(joystickR) > joystickDeadZone)
  {
  orbitalAero.joystickInputs.pitch = (joystickR - (joystickDeadZone * sgn(joystickR))) / (100 - joystickDeadZone);
  }
  else
  {
  orbitalAero.joystickInputs.pitch = 0.0;
  }
  if (abs(joystickX) > joystickDeadZone)
  {
      orbitalAero.joystickInputs.yaw = (joystickX - (joystickDeadZone * sgn(joystickX))) / (100 - joystickDeadZone);
  }
  else
  {
  orbitalAero.joystickInputs.yaw = 0.0;
  }

  if (abs(joystickY) > joystickDeadZone)
  {
  orbitalAero.joystickInputs.thrust = -(joystickY - (joystickDeadZone * sgn(joystickY))) / (100 - joystickDeadZone);
  }
  else
  {
  orbitalAero.joystickInputs.thrust = 0.0;
  }
} // Joystick::isConnected(0)

Then in the body's control system called each frame (not described yet), I assign angular velocities and thrust based on the joystick inputs. The following / 10.0 is just a scaling so the body doesn't rotate too quickly for a given input.

hostBody.angularVelocity_radiansPerSecond.phi = hostOrbitalAero.joystickInputs.roll / 10.0;

hostBody.angularVelocity_radiansPerSecond.theta = hostOrbitalAero.joystickInputs.pitch / 10.0;

hostBody.angularVelocity_radiansPerSecond.psi = hostOrbitalAero.joystickInputs.yaw / 10.0;

Currently I'm changing the angular velocities directly based on the joystick, but in the near future I'll model some type of thrusters / other mechanisms to give an off-axis thrust that will cause the body to rotate. I need to get moment of inertia in for that!

Joystick thrust is considered to be along the body x (forward) axis; however for the processForces calculations later I need that thrust along the world axis.

The following code converts the thrust from the body axis to the world axis via the .rotatedBy call. (If the two axis are aligned, then those vectors will be the same. But if the body is turned at some angle relative to the world axis, then the body's thrust vector will also be rotated by the same angle.)

double thrust_newtons = hostOrbitalAero.joystickInputs.thrust * 1000.0;

// All thrust is along the x (forward) axis of the entity.

// Convert thrust from local axis to world axis.

Vector thrustBody_newtons(thrust_newtons, 0, 0);

hostBody.forceThrust_newtons = thrustBody_newtons.rotatedBy(hostBody.orientation_quaternions.inverse());

// Alternate way to rotate the vector using Quaternions converted to Euler angles.

//hostBody.forceThrust_newtons = thrustBody_newtons.rotatedBy(hostBody.orientation_quaternions.inverse().toEulerAngles());

When summing the forces on a body, my summation starts with the thrust force (relative to world axis) and then adds the effects of gravity and atmosphere from all the other bodies (also relative to world axis).

// Function to loop through all bodies and sum the forces acting on them, including forces from
// thrust, gravity, atmospheric drag, and friction.
bool OrbitalAeroModel::processForces()
{
// Loop through all bodies and calculate their forces upon each other.

// The outer loop is for the primary body that we're calculating forces for.
map <uint64, BodyModel>::iterator iteratorPrimaryBody = tableBodies.begin();
while (iteratorPrimaryBody != tableBodies.end())
{
BodyModel &primaryBody = iteratorPrimaryBody->second;

// Only process the primary body if it's active.
if (primaryBody.active)
{
// Create a vector to contain the sum of all forces.
Vector sumForces_newtons;

// Start with the thrust force of any onboard engines.
sumForces_newtons = primaryBody.forceThrust_newtons;
.
.

And finally the angular velocities change the orientation in the processKinematics loop.

//  Translate and rotate bodies based on their mass, moments of inertia, and forces.

bool OrbitalAeroModel::processKinematics(double timeStep_seconds)

{

// Loop through all bodies and reposition them based on the forces.

map <uint64, BodyModel>::iterator iteratorPrimaryBody = tableBodies.begin();

while (iteratorPrimaryBody != tableBodies.end())

{

BodyModel &primaryBody = iteratorPrimaryBody->second;

if (primaryBody.active)

{

.

.

// Rotation

if (primaryBody.angularVelocity_radiansPerSecond.isNonZero())

{

// Rotation around local axis.

primaryBody.orientation_quaternions.rotateAboutLocalAxis(primaryBody.angularVelocity_radiansPerSecond * timeStep_seconds);

}

} // (primaryBody.active)

iteratorPrimaryBody++;

} // (iteratorPrimaryBody != tableBodies.end())

return true;

}

The angular velocities are relative to the local axis of the entity. So when I do my Quaternion rotation, I call the rotateAboutLocalAxis function (as opposed to rotateAboutWorldAxis).

Testing

I've gone away from my .NET app to visualize the bodies in motion. It worked by having a .NET program consume a CLI wrapper library of my native C++ aero library. Too complicated for something that wasn't ideal long term anyways. Soon I'll be putting together a much nicer 3-D viewer. In the mean time, I've been outputting my bodies via network packets to 3-D visualization software - I'll discuss that in another post.

It has been fun modeling a spacecraft over the Earth and flying it around with the joystick. I'll try to get a video up.

Rotation Classes

Rather than overload this post with both the approach and class code, I'll post my Vector, Euler, Quaternion, and Matrix code in the next few posts.

Copyright (c) 2017 Clinton Kam
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.