Clint's Projects: orbital aero

Showing posts with label orbital aero. Show all posts

Wednesday, August 30, 2017

Orbital Aero Model: Kinematics (Rotation)

In the first Kinematics post, I went into how bodies are moved in space. This post will go into how bodies are rotated in space.

The amount a body will rotate is not based on force and mass, but instead it is based on the moment applied to the body (the Torque, T) and the body's moment of inertia (I).

Torque

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

Torque (moment) is calculated from the equation:

T is the calculated torque applied.
r is the perpendicular distance from the axis of rotation to the force applied.
F is the force applied.

A simple way of thinking about moment is a wrench tightening a bolt.

If the wrench was 1 foot long with 10 pounds of pressure applied to the end of it, then the calculated moment is:
T = 1 * 10 = 10 foot-pounds of torque

If the wrench was twice as long but the same force applied, then the calculated moment is:
T = 2 * 10 = 20 foot-pounds of torque

Which demonstrates the entire purpose of having long handles on wrenches! They allow people to tighten bolts with less force.

Since the entire simulation (and most of the world) uses metric, I'll be using metric for the rest of this post and the code. So our moment units will be Newton-meters rather than foot-pounds.

For a metric example the wrench is 0.25 meters long with 50 newton of force applied, the calculated moment is:
T = 0.25 * 50 = 12.5 Newton-meters of torque

The torque equation I gave above was just for a single axis plane. The equation to cover a vector force in multiple dimensions uses the cross product:

This equation takes a vector force, F, and a vector position, r, to get the resulting torque, T, that is applied in all 3 axes.

In this diagram, a force vector, F, is applied at the corner of a block with the force location represented by vector r. The torque cross product equation gives the resulting torque in all 3 axes.

For more information on the cross product, see https://en.wikipedia.org/wiki/Cross_product The Vector class has a static function for calculating the cross product which the simulation code below will use.

Moment of Inertia

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

All physical objects have a mass, and all physical objects have a moment of inertia. Where mass is an object's resistance to movement, moment of inertia is an object's resistance to rotation.

And where mass is the same from any orientation, the moment of inertia can vary from orientation. There are simple formulas to calculate moment of inertia for a variety of shapes:

Uniform Solid Sphere (moment of inertia same from any orientation):

Block (moment of inertia varies by axis):

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

What happens if you have a more complex shape? (Two spheres at the end of a long block.) Or you're rotating somewhere other than the axis that the moment of inertia equations were referenced from? (Rotating from the end of the block rather than the center.) In those situations, you use the Parallel-Axis Theorem to recalculate a new moment of inertia.

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

Currently in the Orbital Aero Model, all bodies are single uniform spheres. I have plans to make it so bodies can consist of one of a variety of basic shapes or a combination of many basic shapes to form complex shapes. So I'll hold off on covering the parallel axis theorem until I complete that code.

Angular Acceleration & Velocity

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

In our prior function calls, we calculated the total moment applied to the body. Since we know its moment of inertia, it's easy to calculate acceleration:

T is the torque, in newton-meters.
I is the moment of inertia, in kilogram meters squared.
alpha is the angular acceleration, in radians per second squared.

And with the angular acceleration, we can rotate the object:

theta is the newly calculated orientation, in radians.
theta0 is the initial orientation, in radians.
omega0 is the initial angular velocity, in radians per second.
alpha is the angular acceleration, in radians per second squared.
t is the time step of the simulation, in seconds.

omega is the newly calculated angular velocity, in radians per second.
omega0 is the initial angular velocity, in radians per second.
alpha is the angular acceleration, in radians per second squared.

t is the time step of the simulation, in seconds.

The smaller the time step the more accurate the simulation. However, that also means it's more computational intensive for simulating the same duration of time.

Orbital Aero Code

Now that all the background math has been covered, let's see how it's represented in code.

Body

I added a moment of inertia property to the Body class. Since everything in the orbital aero are considered uniform spheres for now, this returns a moment of inertia based on the body's mass and radius.

class Body
{
public:
Vector momentOfInertiaTotal_kilogramMeters2() const;

}

Vector Body::momentOfInertiaTotal_kilogramMeters2() const

{

// For solid spheres: I = 2/5 * M * R^2

double momentOfInertia_kilogramMeters2 = (double)2.0 / (double)5.0 * this->massTotal_kilograms() * (this->radius_meters * this->radius_meters);

return Vector(momentOfInertia_kilogramMeters2, momentOfInertia_kilogramMeters2, momentOfInertia_kilogramMeters2);

}

Control System

I'll demonstrate 2 ways of calculating and applying a moment to a body.

Reaction Wheel / Directly Calculate Moment from Joystick Deflection

This approach would be more for satellites with reaction wheels. Rather than expelling force via a rocket to rotate the vehicle, an electric motor attached to a flywheel spins causing the vehicle to counter-rotate. https://en.wikipedia.org/wiki/Reaction_wheel

Of course we could go in depth with the physics of the motor and flywheel, but for simplicity we can just scale the joystick input to a torque. And we can use the body's moment of inertia as a general rule for how much to scale the joystick input.

bool ControlSystemModel::update(OrbitalAeroModel &hostOrbitalAero, Body* hostBody, double timeStep_seconds)
{
switch (controlInput)
{
case ControlInputEnum::idle:
// No change to any angular velocities set.
break;
case ControlInputEnum::joystick:
{
// Scale the moment applied by the moment of inertia so our joystick inputs are effective for both
// very small and very large bodies. This will result in 1 radians/second^2 for full joystick deflection.
Vector momentOfIneretiaTotal_kilogramMeters2 = hostBody->momentOfInertiaTotal_kilogramMeters2();
hostBody->momentTotal_newtonMeters.x = hostOrbitalAero.joystickInputs[this->joystickInputsIndex].roll * momentOfIneretiaTotal_kilogramMeters2.x;
hostBody->momentTotal_newtonMeters.y = -hostOrbitalAero.joystickInputs[this->joystickInputsIndex].pitch * momentOfIneretiaTotal_kilogramMeters2.y;
hostBody->momentTotal_newtonMeters.z = hostOrbitalAero.joystickInputs[this->joystickInputsIndex].yaw * momentOfIneretiaTotal_kilogramMeters2.z;

// All thrust is along the x (forward) axis of the entity.
double thrust_newtons = hostOrbitalAero.joystickInputs[this->joystickInputsIndex].thrust * 1000.0;

  // 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());
}
break;
default:
{
this->hostBody->momentTotal_newtonMeters = 0.0;
  this->hostBody->forceThrust_newtons = 0.0;
}
break;
}

return true;
}

Orientation Thrusters / Calculate Moment from Rockets at Edges of Body

This approach is more inline with the Space Shuttle's reaction control system. https://en.wikipedia.org/wiki/Reaction_control_system A combination of small thrusters on various parts of the spacecraft are fired for attitude adjustments. (They could also make translation adjustments if opposing thrusters are not fired to counteract the force applied.)

The code below models 6 thrusters for orientation control. There is a pair for each axis; one on either side of the body. For any thrust created on one side, a thrust in the opposite direction is created on the other side so that the body only rotates (no translation).

These thrusters pretend they can create force in either direction, which in reality you wouldn't do. (Rocket nozzles point in one direction, not two.) Really this is 12 thrusters with math simplified for 6. That's not many though, the Space Shuttle had 44 of these! (But those were also used for minor translation changes and as backups in case of malfunction.)

bool ControlSystemModel::update(OrbitalAeroModel &hostOrbitalAero, Body* hostBody, double timeStep_seconds)
{
switch (controlInput)
{
case ControlInputEnum::idle:
// No change to any angular velocities set.
break;
case ControlInputEnum::joystick:
{
// Orientation Thrusters

// Roll 1 Thruster - Right Wingtip, pointed up/down.
Vector roll1ThrustersForce_newtons(0.0, 0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].roll * 10.0);
Vector roll1ThrustersLocation_meters(0.0, hostBody->radius_meters, 0.0);
Vector roll1ThrustersMoment_newtonMeters = Vector::crossProduct(roll1ThrustersLocation_meters, roll1ThrustersForce_newtons);

// Roll 2 Thruster - Left Wingtip, pointed up/down.
Vector roll2ThrustersForce_newtons(0.0, 0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].roll * -10.0);
Vector roll2ThrustersLocation_meters(0.0, -hostBody->radius_meters, 0.0);
Vector roll2ThrustersMoment_newtonMeters = Vector::crossProduct(roll2ThrustersLocation_meters, roll2ThrustersForce_newtons);

// Pitch 1 Thruster - Nose, pointed up/down.
Vector pitch1ThrustersForce_newtons(0.0, 0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].pitch * 10.0);
Vector pitch1ThrustersLocation_meters(-hostBody->radius_meters, 0.0, 0.0);
Vector pitch1ThrustersMoment_newtonMeters = Vector::crossProduct(pitch1ThrustersLocation_meters, pitch1ThrustersForce_newtons);

// Pitch 2 Thruster - Tail, pointed up/down.
Vector pitch2ThrustersForce_newtons(0.0, 0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].pitch * -10.0);
Vector pitch2ThrustersLocation_meters(hostBody->radius_meters, 0.0, 0.0);
Vector pitch2ThrustersMoment_newtonMeters = Vector::crossProduct(pitch2ThrustersLocation_meters, pitch2ThrustersForce_newtons);

// Yaw 1 Thruster, Nose, pointed left/right.
Vector yaw1ThrustersForce_newtons(0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].yaw * 10.0, 0.0);
Vector yaw1ThrustersLocation_meters(hostBody->radius_meters, 0.0, 0.0);
Vector yaw1ThrustersMoment_newtonMeters = Vector::crossProduct(yaw1ThrustersLocation_meters, yaw1ThrustersForce_newtons);

// Yaw 2 Thruster, Tail, pointed left/right.
Vector yaw2ThrustersForce_newtons(0.0, hostOrbitalAero.joystickInputs[this->joystickInputsIndex].yaw * -10.0, 0.0);
Vector yaw2ThrustersLocation_meters(-hostBody->radius_meters, 0.0, 0.0);
Vector yaw2ThrustersMoment_newtonMeters = Vector::crossProduct(yaw2ThrustersLocation_meters, yaw2ThrustersForce_newtons);

// Main thruster is at the tail along x (forward) axis of the entity.
Vector mainThrustersForce_newtons(hostOrbitalAero.joystickInputs[this->joystickInputsIndex].thrust * 1000.0, 0.0, 0.0);
Vector mainThrustersLocation_meters(-hostBody->radius_meters, 0.0, 0.0);
Vector mainThrustersMoment_newtonMeters = Vector::crossProduct(mainThrustersLocation_meters, mainThrustersForce_newtons);

// Get total thrust.
Vector totalThrustBody_newtons = roll1ThrustersForce_newtons + roll2ThrustersForce_newtons +
pitch1ThrustersForce_newtons + pitch2ThrustersForce_newtons +
yaw1ThrustersForce_newtons + yaw2ThrustersForce_newtons +
mainThrustersForce_newtons;

// Get total moment.
hostBody->momentTotal_newtonMeters = roll1ThrustersMoment_newtonMeters + roll2ThrustersMoment_newtonMeters +
pitch1ThrustersMoment_newtonMeters + pitch2ThrustersMoment_newtonMeters +
yaw1ThrustersMoment_newtonMeters + yaw2ThrustersMoment_newtonMeters +
mainThrustersMoment_newtonMeters;

// Convert thrust from local axis to world axis.
hostBody->forceThrust_newtons = totalThrustBody_newtons.rotatedBy(hostBody->orientation_quaternions.inverse());

break;
default:
{
this->hostBody->momentTotal_newtonMeters = 0.0;
this->hostBody->forceThrust_newtons = 0.0;
}
break;
}

return true;

}

processKinematics()

Kinematics have been updated to calculate the angular acceleration from the applied moment and moment of inertia.

// 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_t, Body*>::iterator iteratorPrimaryBody = tableBodies.begin();
while (iteratorPrimaryBody != tableBodies.end())
{
Body* primaryBody = iteratorPrimaryBody->second;
if (primaryBody->isActive && !primaryBody->isExternal)
{
if (primaryBody->isStationary)
{
// Body Stationary, No Movement
primaryBody->linearAcceleration_metersPerSecond2 = 0.0;
primaryBody->linearVelocity_metersPerSecond = 0.0;
primaryBody->angularAcceleration_radiansPerSecond2.clear();
primaryBody->angularVelocity_radiansPerSecond.clear();
}
else
{
// Translation
primaryBody->linearAcceleration_metersPerSecond2 = primaryBody->forceTotal_newtons() / primaryBody->massTotal_kilograms();
// Distance Travelled = (Initial Linear Velocity * Time) + (0.5 * Linear Acceleration * Time^2)
primaryBody->location_meters += (primaryBody->linearVelocity_metersPerSecond * timeStep_seconds) + (primaryBody->linearAcceleration_metersPerSecond2 * (0.5 * timeStep_seconds * timeStep_seconds));
// Final Linear Velocity = Initial Linear Velocity + (Linear Acceleration * Time)
primaryBody->linearVelocity_metersPerSecond += primaryBody->linearAcceleration_metersPerSecond2 * timeStep_seconds;

// Rotation
primaryBody->angularAcceleration_radiansPerSecond2 = primaryBody->momentTotal_newtonMeters / primaryBody->momentOfInertiaTotal_kilogramMeters2();
// Rotation Amount = (Initial Angular Velocity * Time) + (0.5 * Angular Acceleration * Time^2)
Euler rotationAmount_radians = (primaryBody->angularVelocity_radiansPerSecond * timeStep_seconds) + (primaryBody->angularAcceleration_radiansPerSecond2 * (0.5 * timeStep_seconds * timeStep_seconds));
if (rotationAmount_radians.isNonZero())
{
// Rotate around local axis.
primaryBody->orientation_quaternions.rotateAboutLocalAxis(rotationAmount_radians);
}
// Final Angular Velocity = Initial Angular Velocity + (Angular Acceleration * Time)
primaryBody->angularVelocity_radiansPerSecond += primaryBody->angularAcceleration_radiansPerSecond2 * timeStep_seconds;
}
} // (primaryBody.isActive)
iteratorPrimaryBody++;
} // (iteratorPrimaryBody != tableBodies.end())

return true;
}

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.

Saturday, January 21, 2017

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;
}

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;
}

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;
}

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.