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

Replaced normalize with EuclideanNorm
Some checks failed
Merge-Checker / build_and_test (pull_request) Failing after 21s
Details

Browse Source
This commit is contained in:
Quinn
2025-06-02 14:26:41 -04:00
parent 37556c7c81
commit 60a2b12b5f
5 changed files with 177 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/Matrix.cpp
View File

@@ -277,6 +277,11 @@ void Matrix<rows, columns>::ToString(std::string &stringBuffer) const {
}
}
template <uint8_t rows, uint8_t columns>
const float *Matrix<rows, columns>::ToArray() const {
return this->matrix.data();
}
template <uint8_t rows, uint8_t columns>
std::array<float, columns> &
Matrix<rows, columns>::operator[](uint8_t row_index) {
@@ -418,8 +423,8 @@ Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const {
}
template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> &
Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const {
Matrix<rows, columns> Matrix<rows, columns>::EuclideanNorm() const {
float sum{0};
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
@@ -428,14 +433,14 @@ Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const {
}
}
Matrix<rows, columns> result{};
if (sum == 0) {
// this wouldn't do anything anyways
result.Fill(1e+6);
result.Fill(0);
return result;
}
sum = sqrt(sum);
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
result[row_idx][column_idx] = this->Get(row_idx, column_idx) / sum;
@@ -491,11 +496,9 @@ void Matrix<rows, columns>::SetSubMatrix(
template <uint8_t rows, uint8_t columns>
void Matrix<rows, columns>::QRDecomposition(Matrix<rows, columns> &Q,
Matrix<columns, columns> &R) const {
static_assert(columns <= rows, "QR decomposition requires columns <= rows");
// Gram-Schmidt orthogonalization
Matrix<rows, 1> a_col, u, e, proj;
Matrix<rows, 1> q_col;
Matrix<rows, 1> a_col, u, q_col, proj;
Q.Fill(0);
R.Fill(0);
@@ -505,18 +508,17 @@ void Matrix<rows, columns>::QRDecomposition(Matrix<rows, columns> &Q,
for (uint8_t j = 0; j < k; ++j) {
Q.GetColumn(j, q_col);
float r_jk = Matrix<rows, 1>::DotProduct(q_col, a_col);
float r_jk = Matrix<rows, 1>::DotProduct(
q_col, u); // FIXED: use u instead of a_col
R[j][k] = r_jk;
// proj = r_jk * q_j
proj = q_col * r_jk;
u = u - proj;
}
float norm = sqrt(Matrix<rows, 1>::DotProduct(u, u));
if (norm == 0) {
norm = 1e-12f; // avoid div by zero
}
if (norm < 1e-12f)
norm = 1e-12f; // for stability
for (uint8_t i = 0; i < rows; ++i) {
Q[i][k] = u[i][0] / norm;

10
src/Matrix.hpp
View File

@@ -132,7 +132,7 @@ public:
* @brief reduce the matrix so the sum of its elements equal 1
* @param result a buffer to store the result into
*/
Matrix<rows, columns> &Normalize(Matrix<rows, columns> &result) const;
Matrix<rows, columns> EuclideanNorm() const;
/**
* @brief Get a row from the matrix
@@ -159,8 +159,16 @@ public:
*/
constexpr uint8_t GetColumnSize() { return columns; }
/**
* @brief Write a string representation of the matrix into the buffer
*/
void ToString(std::string &stringBuffer) const;
/**
* @brief Returns the internal representation of the matrix as an array
*/
const float *ToArray() const;
/**
* @brief Get an element from the matrix
* @param row the row index of the element

175
src/Quaternion.cpp
View File

@@ -6,115 +6,116 @@
* @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};
Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis) {
const float halfAngle = angle / 2;
const float sinHalfAngle = sin(halfAngle);
Matrix<1, 3> normalizedAxis{};
normalizedAxis = axis.EuclideanNorm();
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)
{
return this->matrix[index];
}
float Quaternion::operator[](uint8_t index) const {
if (index < 4) {
return this->matrix[index];
}
// index out of bounds
return 1e+6;
// index out of bounds
return 1e+6;
}
void Quaternion::operator=(const Quaternion &other)
{
memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
void Quaternion::operator=(const Quaternion &other) {
memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
}
Quaternion Quaternion::operator*(const Quaternion &other) const
{
Quaternion result{};
this->Q_Mult(other, result);
return result;
Quaternion Quaternion::operator*(const Quaternion &other) const {
Quaternion result{};
this->Q_Mult(other, result);
return result;
}
Quaternion Quaternion::operator*(float scalar) const
{
return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar, this->v3 * scalar};
Quaternion Quaternion::operator*(float scalar) const {
return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar,
this->v3 * scalar};
}
Quaternion Quaternion::operator+(const Quaternion &other) const
{
return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2, this->v3 + other.v3};
Quaternion Quaternion::operator+(const Quaternion &other) const {
return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2,
this->v3 + other.v3};
}
Quaternion &
Quaternion::Q_Mult(const Quaternion &other, Quaternion &buffer) const
{
Quaternion &Quaternion::Q_Mult(const Quaternion &other,
Quaternion &buffer) const {
// eq. 6
buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 - other.v3 * this->v3);
buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 + other.v3 * this->v2);
buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w - other.v3 * this->v1);
buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 + other.v3 * this->w);
return buffer;
// eq. 6
buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 -
other.v3 * this->v3);
buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 +
other.v3 * this->v2);
buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w -
other.v3 * this->v1);
buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 +
other.v3 * this->w);
return buffer;
}
Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const
{
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 &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const {
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;
Quaternion temp{};
this->Q_Mult(buffer, temp);
temp.Q_Mult(prime, buffer);
return buffer;
}
void Quaternion::Normalize()
{
float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w);
if (magnitude == 0)
{
return;
}
this->v1 /= magnitude;
this->v2 /= magnitude;
this->v3 /= magnitude;
this->w /= magnitude;
void Quaternion::Normalize() {
float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 +
this->v3 * this->v3 + this->w * this->w);
if (magnitude == 0) {
return;
}
this->v1 /= magnitude;
this->v2 /= magnitude;
this->v3 /= magnitude;
this->w /= magnitude;
}
Matrix<3, 3> Quaternion::ToRotationMatrix() const
{
float xx = this->v1 * this->v1;
float yy = this->v2 * this->v2;
float zz = this->v3 * this->v3;
Matrix<3, 3> rotationMatrix{
1 - 2 * (yy - zz), 2 * (this->v1 * this->v2 - this->v3 * this->w), 2 * (this->v1 * this->v3 + this->v2 * this->w),
2 * (this->v1 * this->v2 + this->v3 * this->w), 1 - 2 * (xx - zz), 2 * (this->v2 * this->v3 - this->v1 * this->w),
2 * (this->v1 * this->v3 - this->v2 * this->w), 2 * (this->v2 * this->v3 + this->v1 * this->w), 1 - 2 * (xx - yy)};
return rotationMatrix;
Matrix<3, 3> Quaternion::ToRotationMatrix() const {
float xx = this->v1 * this->v1;
float yy = this->v2 * this->v2;
float zz = this->v3 * this->v3;
Matrix<3, 3> rotationMatrix{1 - 2 * (yy - zz),
2 * (this->v1 * this->v2 - this->v3 * this->w),
2 * (this->v1 * this->v3 + this->v2 * this->w),
2 * (this->v1 * this->v2 + this->v3 * this->w),
1 - 2 * (xx - zz),
2 * (this->v2 * this->v3 - this->v1 * this->w),
2 * (this->v1 * this->v3 - this->v2 * this->w),
2 * (this->v2 * this->v3 + this->v1 * this->w),
1 - 2 * (xx - yy)};
return rotationMatrix;
};
Matrix<3, 1> Quaternion::ToEulerAngle() const
{
float sqv1 = this->v1 * this->v1;
float sqv2 = this->v2 * this->v2;
float sqv3 = this->v3 * this->v3;
float sqw = this->w * this->w;
Matrix<3, 1> Quaternion::ToEulerAngle() const {
float sqv1 = this->v1 * this->v1;
float sqv2 = this->v2 * this->v2;
float sqv3 = this->v3 * this->v3;
float sqw = this->w * this->w;
Matrix<3, 1> eulerAngle;
{
atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w), (sqv1 - sqv2 - sqv3 + sqw));
asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) / (sqv1 + sqv2 + sqv3 + sqw));
atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w), (-sqv1 - sqv2 + sqv3 + sqw));
};
return eulerAngle;
Matrix<3, 1> eulerAngle;
{
atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w),
(sqv1 - sqv2 - sqv3 + sqw));
asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) /
(sqv1 + sqv2 + sqv3 + sqw));
atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w),
(-sqv1 - sqv2 + sqv3 + sqw));
};
return eulerAngle;
}