Cynopolis/Vector3D
1
0
Fork 0
Code Issues 3 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Finished implimenting quaternion basics

Browse Source
This commit is contained in:
Quinn
2025-02-07 23:40:56 -05:00
parent aa8056240a
commit 55ff4aa693
5 changed files with 107 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
.vscode/launch.json vendored
View File

@@ -8,14 +8,14 @@
"name": "Debug Matrix Unit Tests",
"type": "cppdbg",
"request": "launch",
"program": "${workspaceFolder}/build/unit-tests/matrix-tests",
"args": [],
"program": "${workspaceFolder}/build/src/Matrix/matrix-tests",
"args": [],
"stopAtEntry": false,
"cwd": "${workspaceFolder}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"miDebuggerPath": "/usr/bin/gdb", // Adjust to your debugger path
"MIMode": "gdb",
"miDebuggerPath": "/usr/bin/gdb", // Adjust to your debugger path
"setupCommands": [
{
"description": "Enable pretty-printing for gdb",
@@ -23,20 +23,29 @@
"ignoreFailures": true
}
],
"preLaunchTask": "build_tests", // Task to compile unit tests
"preLaunchTask": "build_tests", // Task to compile unit tests
"internalConsoleOptions": "openOnSessionStart"
},
{
"name": "Run Matrix Unit Tests",
"type": "cpp",
"name": "Debug Quaternion Unit Tests",
"type": "cppdbg",
"request": "launch",
"program": "${workspaceFolder}/build/unit-tests/matrix-tests",
"args": [],
"program": "${workspaceFolder}/build/src/Quaternion/quaternion-tests",
"args": [],
"stopAtEntry": false,
"cwd": "${workspaceFolder}",
"environment": [],
"externalConsole": false,
"preLaunchTask": "build_tests", // Compile unit tests before running
"MIMode": "gdb",
"miDebuggerPath": "/usr/bin/gdb", // Adjust to your debugger path
"setupCommands": [
{
"description": "Enable pretty-printing for gdb",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
],
"preLaunchTask": "build_tests", // Task to compile unit tests
"internalConsoleOptions": "openOnSessionStart"
}
]

6
.vscode/tasks.json vendored
View File

@@ -4,12 +4,14 @@
{
"label": "build_tests",
"type": "shell",
"command": "cd build && ninja matrix-tests",
"command": "cd build && ninja",
"group": {
"kind": "build",
"isDefault": true
},
"problemMatcher": ["$gcc"],
"problemMatcher": [
"$gcc"
],
"detail": "Generated task to build unit test executable"
}
]

67
src/Quaternion/Quaternion.cpp
View File

@@ -1,6 +1,24 @@
#include "Quaternion.h"
#include <cmath>
/**
* @brief Create a quaternion from an angle and axis
* @param angle The angle to rotate by
* @param axis The axis to rotate around
*/
Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis)
{
const float halfAngle = angle / 2;
const float sinHalfAngle = sin(halfAngle);
Matrix<1, 3> normalizedAxis{};
axis.Normalize(normalizedAxis);
return Quaternion{
static_cast<float>(cos(halfAngle)),
normalizedAxis.Get(0, 0) * sinHalfAngle,
normalizedAxis.Get(0, 1) * sinHalfAngle,
normalizedAxis.Get(0, 2) * sinHalfAngle};
}
float Quaternion::operator[](uint8_t index) const
{
if (index < 4)
@@ -14,7 +32,7 @@ float Quaternion::operator[](uint8_t index) const
Quaternion Quaternion::operator+(const Quaternion &other) const
{
return Quaternion{this->v1 * other.v1, this->v2 * other.v2, this->v3 * other.v3, this->w * other.w};
return Quaternion{this->w * other.w, this->v1 * other.v1, this->v2 * other.v2, this->v3 * other.v3};
}
Quaternion &
@@ -31,48 +49,23 @@ Quaternion::Q_Mult(Quaternion &other, Quaternion &buffer) const
Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const
{
Quaternion prime{-this->v1, -this->v2, -this->v3, this->w};
static_cast<Matrix<1, 4>>(buffer) = static_cast<Matrix<1, 4>>(other);
Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
buffer.v1 = other.v1;
buffer.v2 = other.v2;
buffer.v3 = other.v3;
buffer.w = 0;
Quaternion temp{};
this->Q_Mult(buffer, temp);
temp.Q_Mult(prime, buffer);
return buffer;
}
Matrix<1, 3> &
Quaternion::ToEulerAngles(Matrix<1, 3> &angleBuffer) const
void Quaternion::Normalize()
{
// from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
// rotation sequence R = Rx * Ry * Rz
// roll (x-axis rotation)
float sinr_cosp = 2 * (this->v2 * this->v3 - this->w * this->v1);
float cosr_cosp = 1 - 2 * (this->v1 * this->v1 + this->v2 * this->v2);
angleBuffer[0][0] = atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
float sinp = -2 * (this->w * this->v2 + this->v3 * this->v1);
if (abs(sinp) >= 1)
angleBuffer[0][1] = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
angleBuffer[0][1] = asin(sinp);
// yaw (z-axis rotation)
float siny_cosp = 2 * (this->v1 * this->v2 - this->w * this->v3);
float cosy_cosp = 1 - 2 * (this->v2 * this->v2 + this->v3 * this->v3);
angleBuffer[0][2] = atan2(siny_cosp, cosy_cosp);
return angleBuffer;
}
Matrix<3, 3> &Quaternion::ToRotationMatrix(Matrix<3, 3> &rotationMatrixBuffer) const
{
// eq. 4
// from https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
Matrix<3, 3> temp{1.0 - 2.0 * this->v2 * this->v2 - 2.0 * this->v3 * this->v3, 2.0 * this->v1 * this->v2 - 2.0 * this->v3 * this->w, 2.0 * this->v1 * this->v3 + 2.0 * this->v2 * this->w,
2.0 * this->v1 * this->v2 + 2.0 * this->v3 * this->w, 1.0 - 2.0 * this->v1 * this->v1 - 2.0 * this->v3 * this->v3, 2.0 * this->v2 * this->v3 - 2.0 * this->v1 * this->w,
2.0 * this->v1 * this->v3 - 2.0 * this->v2 * this->w, 2.0 * this->v2 * this->v3 + 2.0 * this->v1 * this->w, 1.0 - 2.0 * this->v1 * this->v1 - 2.0 * this->v2 * this->v2};
temp.Transpose(rotationMatrixBuffer);
return rotationMatrixBuffer;
float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w);
this->v1 /= magnitude;
this->v2 /= magnitude;
this->v3 /= magnitude;
this->w /= magnitude;
}

34
src/Quaternion/Quaternion.h
View File

@@ -8,11 +8,18 @@ class Quaternion : public Matrix<1, 4>
public:
Quaternion() : Matrix<1, 4>() {}
Quaternion(float fillValue) : Matrix<1, 4>(fillValue) {}
Quaternion(float v1, float v2, float v3, float w) : Matrix<1, 4>(v1, v2, v3, w) {}
Quaternion(const Quaternion &q) : Matrix<1, 4>(q.v1, q.v2, q.v3, q.w) {}
Quaternion(float w, float v1, float v2, float v3) : Matrix<1, 4>(w, v1, v2, v3) {}
Quaternion(const Quaternion &q) : Matrix<1, 4>(q.w, q.v1, q.v2, q.v3) {}
Quaternion(const Matrix<1, 4> &matrix) : Matrix<1, 4>(matrix) {}
Quaternion(const std::array<float, 4> &array) : Matrix<1, 4>(array) {}
/**
* @brief Create a quaternion from an angle and axis
* @param angle The angle to rotate by
* @param axis The axis to rotate around
*/
static Quaternion FromAngleAndAxis(float angle, const Matrix<1, 3> &axis);
/**
* @brief Access the elements of the quaternion
* @param index The index of the element to access
@@ -36,7 +43,7 @@ public:
Quaternion &Q_Mult(Quaternion &other, Quaternion &buffer) const;
/**
* @brief Rotate a quaternion by another quaternion
* @brief Rotate a quaternion by this quaternion
* @param other The quaternion to rotate
* @param buffer The buffer to store the result in
*
@@ -44,24 +51,15 @@ public:
Quaternion &Rotate(Quaternion &other, Quaternion &buffer) const;
/**
* @brief Calculate the Euler angles from the quaternion
* @param angleBuffer The buffer to store the angles in
* @return A reference to the buffer
* @brief Normalize the quaternion to a magnitude of 1
*/
Matrix<1, 3> &ToEulerAngles(Matrix<1, 3> &angleBuffer) const;
/**
* @brief Convert the quaternion to a rotation matrix
* @param rotationMatrixBuffer The buffer to store the rotation matrix in
* @return A reference to the buffer
*/
Matrix<3, 3> &ToRotationMatrix(Matrix<3, 3> &rotationMatrixBuffer) const;
void Normalize();
// Give people an easy way to access the elements
float &v1{matrix[0]};
float &v2{matrix[1]};
float &v3{matrix[2]};
float &w{matrix[3]};
float &w{matrix[0]};
float &v1{matrix[1]};
float &v2{matrix[2]};
float &v3{matrix[3]};
};
#endif // QUATERNION_H_

89
src/Quaternion/unit-tests/quaternion-tests.cpp
View File

@@ -18,39 +18,39 @@ TEST_CASE("Vector Math", "Vector")
SECTION("Initialization")
{
// explicit initialization
REQUIRE(q1.v1 == 1);
REQUIRE(q1.v2 == 2);
REQUIRE(q1.v3 == 3);
REQUIRE(q1.w == 4);
REQUIRE(q1.w == 1);
REQUIRE(q1.v1 == 2);
REQUIRE(q1.v2 == 3);
REQUIRE(q1.v3 == 4);
// fill initialization
Quaternion q3{0};
REQUIRE(q3.w == 0);
REQUIRE(q3.v1 == 0);
REQUIRE(q3.v2 == 0);
REQUIRE(q3.v3 == 0);
REQUIRE(q3.w == 0);
// copy initialization
Quaternion q4{q1};
REQUIRE(q4.v1 == 1);
REQUIRE(q4.v2 == 2);
REQUIRE(q4.v3 == 3);
REQUIRE(q4.w == 4);
REQUIRE(q4.w == 1);
REQUIRE(q4.v1 == 2);
REQUIRE(q4.v2 == 3);
REQUIRE(q4.v3 == 4);
// matrix initialization
Matrix<1, 4> m1{1, 2, 3, 4};
Quaternion q5{m1};
REQUIRE(q5.v1 == 1);
REQUIRE(q5.v2 == 2);
REQUIRE(q5.v3 == 3);
REQUIRE(q5.w == 4);
REQUIRE(q5.w == 1);
REQUIRE(q5.v1 == 2);
REQUIRE(q5.v2 == 3);
REQUIRE(q5.v3 == 4);
// array initialization
Quaternion q6{std::array<float, 4>{1, 2, 3, 4}};
REQUIRE(q6.v1 == 1);
REQUIRE(q6.v2 == 2);
REQUIRE(q6.v3 == 3);
REQUIRE(q6.w == 4);
REQUIRE(q6.w == 1);
REQUIRE(q6.v1 == 2);
REQUIRE(q6.v2 == 3);
REQUIRE(q6.v3 == 4);
}
SECTION("Array access")
@@ -64,43 +64,30 @@ TEST_CASE("Vector Math", "Vector")
SECTION("Addition")
{
Quaternion q3 = q1 + q2;
REQUIRE(q3.v1 == 5);
REQUIRE(q3.v2 == 12);
REQUIRE(q3.v3 == 21);
REQUIRE(q3.w == 32);
REQUIRE(q3.w == 5);
REQUIRE(q3.v1 == 12);
REQUIRE(q3.v2 == 21);
REQUIRE(q3.v3 == 32);
}
SECTION("Multiplication")
{
Quaternion q3;
q1.Q_Mult(q2, q3);
REQUIRE(q3.w == -60);
REQUIRE(q3.v1 == 12);
REQUIRE(q3.v2 == 30);
REQUIRE(q3.v3 == 24);
}
SECTION("Rotation")
{
Quaternion q3;
q1.Q_Mult(q2, q3);
REQUIRE(q3.v1 == 24);
REQUIRE(q3.v2 == 48);
REQUIRE(q3.v3 == 48);
REQUIRE(q3.w == -6);
}
SECTION("Euler Angles")
{
Matrix<1, 3> angles;
q1.ToEulerAngles(angles);
REQUIRE_THAT(angles.Get(0, 0), Catch::Matchers::WithinRel(-0.1973956f, 1e-6f));
REQUIRE_THAT(angles.Get(0, 1), Catch::Matchers::WithinRel(0.823212f, 1e-6f));
REQUIRE_THAT(angles.Get(0, 2), Catch::Matchers::WithinRel(1.3734008f, 1e-6f));
}
SECTION("Rotation Matrix")
{
Matrix<3, 3> rotationMatrix;
q1.ToRotationMatrix(rotationMatrix);
REQUIRE_THAT(rotationMatrix.Get(0, 0), Catch::Matchers::WithinRel(0.1333333f, 1e-6f));
REQUIRE_THAT(rotationMatrix.Get(0, 1), Catch::Matchers::WithinRel(-0.6666667f, 1e-6f));
REQUIRE_THAT(rotationMatrix.Get(0, 2), Catch::Matchers::WithinRel(0.7333333f, 1e-6f));
REQUIRE_THAT(rotationMatrix.Get(1, 0), Catch::Matchers::WithinRel(0.9333333f, 1e-6f));
REQUIRE_THAT(rotationMatrix.Get(1, 1), Catch::Matchers::WithinRel(0.3333333f, 1e-6f));
REQUIRE_THAT(rotationMatrix.Get(1, 2), Catch::Matchers::WithinRel(0.1333333f, 1e-6f));
REQUIRE_THAT(rotationMatrix.Get(2, 0), Catch::Matchers::WithinRel(-0.3333333f, 1e-6f));
REQUIRE_THAT(rotationMatrix.Get(2, 1), Catch::Matchers::WithinRel(0.6666667f, 1e-6f));
REQUIRE_THAT(rotationMatrix.Get(2, 2), Catch::Matchers::WithinRel(0.6666667f, 1e-6f));
Quaternion q3{Quaternion::FromAngleAndAxis(M_PI / 2, Matrix<1, 3>{0, 0, 1})};
Quaternion q4{0, 1, 0, 0};
Quaternion q5;
q3.Rotate(q4, q5);
REQUIRE_THAT(q5.v1, Catch::Matchers::WithinRel(0.0f, 1e-6f));
REQUIRE_THAT(q5.v2, Catch::Matchers::WithinRel(1.0f, 1e-6f));
REQUIRE_THAT(q5.v3, Catch::Matchers::WithinRel(0.0f, 1e-6f));
}
}