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

23
.vscode/launch.json vendored
View File

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

6
.vscode/tasks.json vendored
View File

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

67
src/Quaternion/Quaternion.cpp
View File

@@ -1,6 +1,24 @@
#include "Quaternion.h" #include "Quaternion.h"
#include <cmath> #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 float Quaternion::operator[](uint8_t index) const
{ {
if (index < 4) if (index < 4)
@@ -14,7 +32,7 @@ float Quaternion::operator[](uint8_t index) const
Quaternion Quaternion::operator+(const Quaternion &other) 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 & Quaternion &
@@ -31,48 +49,23 @@ Quaternion::Q_Mult(Quaternion &other, Quaternion &buffer) const
Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const
{ {
Quaternion prime{-this->v1, -this->v2, -this->v3, this->w}; Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
static_cast<Matrix<1, 4>>(buffer) = static_cast<Matrix<1, 4>>(other); buffer.v1 = other.v1;
buffer.v2 = other.v2;
buffer.v3 = other.v3;
buffer.w = 0; buffer.w = 0;
Quaternion temp{}; Quaternion temp{};
this->Q_Mult(buffer, temp); this->Q_Mult(buffer, temp);
temp.Q_Mult(prime, buffer); temp.Q_Mult(prime, buffer);
return buffer; return buffer;
} }
Matrix<1, 3> & void Quaternion::Normalize()
Quaternion::ToEulerAngles(Matrix<1, 3> &angleBuffer) const
{ {
// from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w);
// rotation sequence R = Rx * Ry * Rz this->v1 /= magnitude;
// roll (x-axis rotation) this->v2 /= magnitude;
float sinr_cosp = 2 * (this->v2 * this->v3 - this->w * this->v1); this->v3 /= magnitude;
float cosr_cosp = 1 - 2 * (this->v1 * this->v1 + this->v2 * this->v2); this->w /= magnitude;
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;
} }

34
src/Quaternion/Quaternion.h
View File

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

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

@@ -18,39 +18,39 @@ TEST_CASE("Vector Math", "Vector")
SECTION("Initialization") SECTION("Initialization")
{ {
// explicit initialization // explicit initialization
REQUIRE(q1.v1 == 1); REQUIRE(q1.w == 1);
REQUIRE(q1.v2 == 2); REQUIRE(q1.v1 == 2);
REQUIRE(q1.v3 == 3); REQUIRE(q1.v2 == 3);
REQUIRE(q1.w == 4); REQUIRE(q1.v3 == 4);
// fill initialization // fill initialization
Quaternion q3{0}; Quaternion q3{0};
REQUIRE(q3.w == 0);
REQUIRE(q3.v1 == 0); REQUIRE(q3.v1 == 0);
REQUIRE(q3.v2 == 0); REQUIRE(q3.v2 == 0);
REQUIRE(q3.v3 == 0); REQUIRE(q3.v3 == 0);
REQUIRE(q3.w == 0);
// copy initialization // copy initialization
Quaternion q4{q1}; Quaternion q4{q1};
REQUIRE(q4.v1 == 1); REQUIRE(q4.w == 1);
REQUIRE(q4.v2 == 2); REQUIRE(q4.v1 == 2);
REQUIRE(q4.v3 == 3); REQUIRE(q4.v2 == 3);
REQUIRE(q4.w == 4); REQUIRE(q4.v3 == 4);
// matrix initialization // matrix initialization
Matrix<1, 4> m1{1, 2, 3, 4}; Matrix<1, 4> m1{1, 2, 3, 4};
Quaternion q5{m1}; Quaternion q5{m1};
REQUIRE(q5.v1 == 1); REQUIRE(q5.w == 1);
REQUIRE(q5.v2 == 2); REQUIRE(q5.v1 == 2);
REQUIRE(q5.v3 == 3); REQUIRE(q5.v2 == 3);
REQUIRE(q5.w == 4); REQUIRE(q5.v3 == 4);
// array initialization // array initialization
Quaternion q6{std::array<float, 4>{1, 2, 3, 4}}; Quaternion q6{std::array<float, 4>{1, 2, 3, 4}};
REQUIRE(q6.v1 == 1); REQUIRE(q6.w == 1);
REQUIRE(q6.v2 == 2); REQUIRE(q6.v1 == 2);
REQUIRE(q6.v3 == 3); REQUIRE(q6.v2 == 3);
REQUIRE(q6.w == 4); REQUIRE(q6.v3 == 4);
} }
SECTION("Array access") SECTION("Array access")
@@ -64,43 +64,30 @@ TEST_CASE("Vector Math", "Vector")
SECTION("Addition") SECTION("Addition")
{ {
Quaternion q3 = q1 + q2; Quaternion q3 = q1 + q2;
REQUIRE(q3.v1 == 5); REQUIRE(q3.w == 5);
REQUIRE(q3.v2 == 12); REQUIRE(q3.v1 == 12);
REQUIRE(q3.v3 == 21); REQUIRE(q3.v2 == 21);
REQUIRE(q3.w == 32); 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") SECTION("Rotation")
{ {
Quaternion q3; Quaternion q3{Quaternion::FromAngleAndAxis(M_PI / 2, Matrix<1, 3>{0, 0, 1})};
q1.Q_Mult(q2, q3); Quaternion q4{0, 1, 0, 0};
REQUIRE(q3.v1 == 24); Quaternion q5;
REQUIRE(q3.v2 == 48); q3.Rotate(q4, q5);
REQUIRE(q3.v3 == 48); REQUIRE_THAT(q5.v1, Catch::Matchers::WithinRel(0.0f, 1e-6f));
REQUIRE(q3.w == -6); REQUIRE_THAT(q5.v2, Catch::Matchers::WithinRel(1.0f, 1e-6f));
} REQUIRE_THAT(q5.v3, Catch::Matchers::WithinRel(0.0f, 1e-6f));
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));
} }
} }