From 9d97a45a78d1b95bc40917bffd7911e6079da36e Mon Sep 17 00:00:00 2001 From: Stephen Seo Date: Tue, 18 Jun 2024 11:06:02 +0900 Subject: [PATCH] Add Axis/Angle related fns, Mat3, refactorings Impl. conversion from Axis/Angle to Mat3 and back (somewhat untested). Added Mat3 and helper functions. Some internal refactorings. --- Changelog.md | 11 ++++ src/sc_sacd.cpp | 165 ++++++++++++++++++++++++++++++++++++++++++++++-- src/sc_sacd.h | 25 ++++++++ src/test.cpp | 50 +++++++++++++++ 4 files changed, 245 insertions(+), 6 deletions(-) diff --git a/Changelog.md b/Changelog.md index afebec6..0b444fe 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,16 @@ # Changelog +## Upcoming Changes + +Refactor some internal operators for Vec3/Mat4 operations (like mult., sum, +etc.) + +Added Mat3 and helper functions related to Mat3. + +Implemented conversion from axis/angle to rotation matrix (Mat3). + +Implemented conversion from rotation matrix (mat3) to axis/angle (untested). + ## Version 3.2.1 Add workflow to build shared/static libs available diff --git a/src/sc_sacd.cpp b/src/sc_sacd.cpp index 7b96a9d..d882ecb 100644 --- a/src/sc_sacd.cpp +++ b/src/sc_sacd.cpp @@ -12,23 +12,30 @@ constexpr float INV_SQRT2 = 0.70710678118654752440F; -SC_SACD_Vec3 operator+(const SC_SACD_Vec3 &a, const SC_SACD_Vec3 &b) { +SC_SACD_Vec3 operator+(SC_SACD_Vec3 a, SC_SACD_Vec3 b) { return SC_SACD_Vec3{a.x + b.x, a.y + b.y, a.z + b.z}; } -SC_SACD_Vec3 operator-(const SC_SACD_Vec3 &a, const SC_SACD_Vec3 &b) { +SC_SACD_Vec3 operator-(SC_SACD_Vec3 a, SC_SACD_Vec3 b) { return SC_SACD_Vec3{a.x - b.x, a.y - b.y, a.z - b.z}; } -SC_SACD_Vec3 operator*(const SC_SACD_Vec3 &a, float scalar) { +SC_SACD_Vec3 operator*(SC_SACD_Vec3 a, float scalar) { return SC_SACD_Vec3{a.x * scalar, a.y * scalar, a.z * scalar}; } -SC_SACD_Vec3 operator/(const SC_SACD_Vec3 &a, float scalar) { +SC_SACD_Vec3 operator/(SC_SACD_Vec3 a, float scalar) { return SC_SACD_Vec3{a.x / scalar, a.y / scalar, a.z / scalar}; } -SC_SACD_Mat4 operator*(const SC_SACD_Mat4 &a, const SC_SACD_Mat4 &b) { +SC_SACD_Mat4 operator+(SC_SACD_Mat4 a, SC_SACD_Mat4 b) { + return SC_SACD_Mat4{a.x0 + b.x0, a.x1 + b.x1, a.x2 + b.x2, a.x3 + b.x3, + a.y0 + b.y0, a.y1 + b.y1, a.y2 + b.y2, a.y3 + b.y3, + a.z0 + b.z0, a.z1 + b.z1, a.z2 + b.z2, a.z3 + b.z3, + a.w0 + b.w0, a.w1 + b.w1, a.w2 + b.w2, a.w3 + b.w3}; +} + +SC_SACD_Mat4 operator*(SC_SACD_Mat4 a, SC_SACD_Mat4 b) { SC_SACD_Mat4 mat; mat.x0 = b.x0 * a.x0 + b.y0 * a.x1 + b.z0 * a.x2 + b.w0 * a.x3; @@ -54,13 +61,58 @@ SC_SACD_Mat4 operator*(const SC_SACD_Mat4 &a, const SC_SACD_Mat4 &b) { return mat; } -SC_SACD_Vec3 operator*(const SC_SACD_Mat4 &mat, const SC_SACD_Vec3 &vec) { +SC_SACD_Vec3 operator*(SC_SACD_Mat4 mat, SC_SACD_Vec3 vec) { return SC_SACD_Vec3{ vec.x * mat.x0 + vec.y * mat.x1 + vec.z * mat.x2 + mat.x3, vec.x * mat.y0 + vec.y * mat.y1 + vec.z * mat.y2 + mat.y3, vec.x * mat.z0 + vec.y * mat.z1 + vec.z * mat.z2 + mat.z3}; } +SC_SACD_Mat3 operator+(SC_SACD_Mat3 a, SC_SACD_Mat3 b) { + return SC_SACD_Mat3{a.x0 + b.x0, a.x1 + b.x1, a.x2 + b.x2, + a.y0 + b.y0, a.y1 + b.y1, a.y2 + b.y2, + a.z0 + b.z0, a.z1 + b.z1, a.z2 + b.z2}; +} + +SC_SACD_Mat3 operator*(SC_SACD_Mat3 a, SC_SACD_Mat3 b) { + return SC_SACD_Mat3{// x0 + a.x0 * b.x0 + a.x1 * b.y0 + a.x2 * b.z0, + // x1 + a.x0 * b.x1 + a.x1 * b.y1 + a.x2 * b.z1, + // x2 + a.x0 * b.x2 + a.x1 * b.y2 + a.x2 * b.z2, + // y0 + a.y0 * b.x0 + a.y1 * b.y0 + a.y2 * b.z0, + // y1 + a.y0 * b.x1 + a.y1 * b.y1 + a.y2 * b.z1, + // y2 + a.y0 * b.x2 + a.y1 * b.y2 + a.y2 * b.z2, + // z0 + a.z0 * b.x0 + a.z1 * b.y0 + a.z2 * b.z0, + // z1 + a.z0 * b.x1 + a.z1 * b.y1 + a.z2 * b.z1, + // z2 + a.z0 * b.x2 + a.z1 * b.y2 + a.z2 * b.z2}; +} + +SC_SACD_Vec3 operator*(SC_SACD_Mat3 mat3, SC_SACD_Vec3 vec) { + return SC_SACD_Vec3{ + mat3.x0 * vec.x + mat3.x1 * vec.y + mat3.x2 * vec.z, + mat3.y0 * vec.x + mat3.y1 * vec.y + mat3.y2 * vec.z, + mat3.z0 * vec.x + mat3.z1 * vec.y + mat3.z2 * vec.z, + }; +} + +SC_SACD_Mat3 operator*(SC_SACD_Mat3 mat3, float scalar) { + return SC_SACD_Mat3{mat3.x0 * scalar, mat3.x1 * scalar, mat3.x2 * scalar, + mat3.y0 * scalar, mat3.y1 * scalar, mat3.y2 * scalar, + mat3.z0 * scalar, mat3.z1 * scalar, mat3.z2 * scalar}; +} + +SC_SACD_Mat3 UHat_Mat3(float x, float y, float z) { + return SC_SACD_Mat3{0.0F, -z, y, z, 0.0F, -x, -y, x, 0.0F}; +} + std::array SC_SACD_Get_Box_Normals(SC_SACD_Generic_Box box) { SC_SACD_Vec3 a, b, c; @@ -413,6 +465,8 @@ SC_SACD_Mat4 SC_SACD_Mat4_Identity(void) { 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F}; } +SC_SACD_Mat4 SC_SACD_Mat4_Sum(SC_SACD_Mat4 a, SC_SACD_Mat4 b) { return a + b; } + SC_SACD_Mat4 SC_SACD_Mat4_Mult(SC_SACD_Mat4 a, SC_SACD_Mat4 b) { return a * b; } SC_SACD_Vec3 SC_SACD_Mat4_Vec3_Mult(SC_SACD_Mat4 mat, SC_SACD_Vec3 vec) { @@ -703,3 +757,102 @@ SC_SACD_Vec3 SC_SACD_Vec3_Reflect(SC_SACD_Vec3 vec, SC_SACD_Vec3 target) { SC_SACD_Vec3 proj = SC_SACD_Vec3_Project(vec, target); return proj * 2.0F - vec; } + +SC_SACD_Mat4 SC_SACD_Mat3_Promote(SC_SACD_Mat3 mat3) { + return SC_SACD_Mat4{mat3.x0, mat3.x1, mat3.x2, 0.0F, mat3.y0, mat3.y1, + mat3.y2, 0.0F, mat3.z0, mat3.z1, mat3.z2, 0.0F, + 0.0F, 0.0F, 0.0F, 1.0F}; +} + +SC_SACD_Mat3 SC_SACD_Mat4_Demote(SC_SACD_Mat4 mat4) { + return SC_SACD_Mat3{mat4.x0, mat4.x1, mat4.x2, mat4.y0, mat4.y1, + mat4.y2, mat4.z0, mat4.z1, mat4.z2}; +} + +SC_SACD_Mat3 SC_SACD_Mat3_Identity() { + return SC_SACD_Mat3{1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}; +} + +SC_SACD_Mat3 SC_SACD_Mat3_Sum(SC_SACD_Mat3 a, SC_SACD_Mat3 b) { return a + b; } + +SC_SACD_Mat3 SC_SACD_Mat3_Mult(SC_SACD_Mat3 a, SC_SACD_Mat3 b) { return a * b; } + +SC_SACD_Vec3 SC_SACD_Mat3_Vec3_Mult(SC_SACD_Mat3 mat3, SC_SACD_Vec3 vec) { + return mat3 * vec; +} + +SC_SACD_Mat3 SC_SACD_Rotation_Mat3_XAxis(float x_radians) { + SC_SACD_Mat3 mat; + + mat.x0 = 1.0F; + mat.x1 = 0.0F; + mat.x2 = 0.0F; + + mat.y0 = 0.0F; + mat.y1 = std::cos(x_radians); + mat.y2 = -std::sin(x_radians); + + mat.z0 = 0.0F; + mat.z1 = -mat.y2; + mat.z2 = mat.y1; + + return mat; +} + +SC_SACD_Mat3 SC_SACD_Rotation_Mat3_YAxis(float y_radians) { + SC_SACD_Mat3 mat; + + mat.x0 = std::cos(y_radians); + mat.x1 = 0.0F; + mat.x2 = std::sin(y_radians); + + mat.y0 = 0.0F; + mat.y1 = 1.0F; + mat.y2 = 0.0F; + + mat.z0 = -mat.x2; + mat.z1 = 0.0F; + mat.z2 = mat.x0; + + return mat; +} + +SC_SACD_Mat3 SC_SACD_Rotation_Mat3_ZAxis(float z_radians) { + SC_SACD_Mat3 mat; + + mat.x0 = std::cos(z_radians); + mat.x1 = -std::sin(z_radians); + mat.x2 = 0.0F; + + mat.y0 = -mat.x1; + mat.y1 = mat.x0; + mat.y2 = 0.0F; + + mat.z0 = 0.0F; + mat.z1 = 0.0F; + mat.z2 = 1.0F; + + return mat; +} + +SC_SACD_Mat3 SC_SACD_Scale_Mat3(float x, float y, float z) { + return SC_SACD_Mat3{x, 0.0f, 0.0f, 0.0f, y, 0.0f, 0.0f, 0.0f, z}; +} + +SC_SACD_Mat3 SC_SACD_ExpMap(SC_SACD_Vec3 axis, float angle) { + axis = SC_SACD_Vec3_Normalize(axis); + auto uhat = UHat_Mat3(axis.x, axis.y, axis.z); + auto uhat_squared = uhat * uhat; + return SC_SACD_Mat3_Identity() + uhat * std::sin(angle) + + uhat_squared * (1 - std::cos(angle)); +} + +float SC_SACD_LogMap_Angle(SC_SACD_Mat3 mat3) { + float trace = mat3.x0 + mat3.y1 + mat3.z2; + return std::acos((trace - 1.0F) / 2.0F); +} + +SC_SACD_Vec3 SC_SACD_LogMap_Axis(SC_SACD_Mat3 mat3, float angle) { + return SC_SACD_Vec3{mat3.y2 - mat3.z1, mat3.z0 - mat3.x2, mat3.x1 - mat3.y0} * + (1.0F / (2.0F * std::sin(angle))); +} diff --git a/src/sc_sacd.h b/src/sc_sacd.h index ae05156..f1d07ad 100644 --- a/src/sc_sacd.h +++ b/src/sc_sacd.h @@ -24,6 +24,12 @@ typedef struct SC_SACD_EXPORT SC_SACD_Vec3 { float x, y, z; } SC_SACD_Vec3; +typedef struct SC_SACD_EXPORT SC_SACD_Mat3 { + float x0, x1, x2; + float y0, y1, y2; + float z0, z1, z2; +} SC_SACD_Mat3; + typedef struct SC_SACD_EXPORT SC_SACD_Mat4 { float x0, x1, x2, x3; float y0, y1, y2, y3; @@ -100,6 +106,8 @@ SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Cross_Product(SC_SACD_Vec3 a, SC_SACD_EXPORT SC_SACD_Mat4 SC_SACD_Mat4_Identity(void); +SC_SACD_EXPORT SC_SACD_Mat4 SC_SACD_Mat4_Sum(SC_SACD_Mat4 a, SC_SACD_Mat4 b); + SC_SACD_EXPORT SC_SACD_Mat4 SC_SACD_Mat4_Mult(SC_SACD_Mat4 a, SC_SACD_Mat4 b); SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Mat4_Vec3_Mult(SC_SACD_Mat4 mat, @@ -155,6 +163,23 @@ SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Vec3_Project(SC_SACD_Vec3 vec, SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Vec3_Reflect(SC_SACD_Vec3 vec, SC_SACD_Vec3 target); +SC_SACD_EXPORT SC_SACD_Mat4 SC_SACD_Mat3_Promote(SC_SACD_Mat3 mat3); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Mat4_Demote(SC_SACD_Mat4 mat4); + +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Mat3_Identity(void); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Mat3_Sum(SC_SACD_Mat3 a, SC_SACD_Mat3 b); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Mat3_Mult(SC_SACD_Mat3 a, SC_SACD_Mat3 b); +SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Mat3_Vec3_Mult(SC_SACD_Mat3 mat3, + SC_SACD_Vec3 vec); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_XAxis(float x_radians); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_YAxis(float y_radians); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_ZAxis(float z_radians); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Scale_Mat3(float x, float y, float z); + +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_ExpMap(SC_SACD_Vec3 axis, float angle); +SC_SACD_EXPORT float SC_SACD_LogMap_Angle(SC_SACD_Mat3 mat3); +SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_LogMap_Axis(SC_SACD_Mat3 mat3, float angle); + #ifdef __cplusplus } #endif diff --git a/src/test.cpp b/src/test.cpp index 1fe322c..de93344 100644 --- a/src/test.cpp +++ b/src/test.cpp @@ -902,6 +902,56 @@ int main() { CHECK_FLOAT(vec.z, std::sqrt(9.0F / 14.0F)); } + // Test axis-angle to rotation matrix. + { + // About Z-axis. + SC_SACD_Vec3 axis{0.0F, 0.0F, 1.0F}; + float angle = std::numbers::pi_v / 2.0F; + + SC_SACD_Mat3 rot_from_axis_angle = SC_SACD_ExpMap(axis, angle); + SC_SACD_Mat3 rotation_mat = SC_SACD_Rotation_Mat3_ZAxis(angle); + + SC_SACD_Vec3 vec{1.0F, 1.0F, 1.0F}; + + SC_SACD_Vec3 transformed_vec0 = SC_SACD_Mat3_Vec3_Mult(rot_from_axis_angle, vec); + SC_SACD_Vec3 transformed_vec1 = SC_SACD_Mat3_Vec3_Mult(rotation_mat, vec); + + CHECK_FLOAT(transformed_vec0.x, transformed_vec1.x); + CHECK_FLOAT(transformed_vec0.y, transformed_vec1.y); + CHECK_FLOAT(transformed_vec0.z, transformed_vec1.z); + + // About X-axis. + axis = SC_SACD_Vec3{1.0F, 0.0F, 0.0F}; + + rot_from_axis_angle = SC_SACD_ExpMap(axis, angle); + rotation_mat = SC_SACD_Rotation_Mat3_XAxis(angle); + + transformed_vec0 = SC_SACD_Mat3_Vec3_Mult(rot_from_axis_angle, vec); + transformed_vec1 = SC_SACD_Mat3_Vec3_Mult(rotation_mat, vec); + + CHECK_FLOAT(transformed_vec0.x, transformed_vec1.x); + CHECK_FLOAT(transformed_vec0.y, transformed_vec1.y); + CHECK_FLOAT(transformed_vec0.z, transformed_vec1.z); + + // About Y-axis. + axis = SC_SACD_Vec3{0.0F, 1.0F, 0.0F}; + + rot_from_axis_angle = SC_SACD_ExpMap(axis, angle); + rotation_mat = SC_SACD_Rotation_Mat3_YAxis(angle); + + transformed_vec0 = SC_SACD_Mat3_Vec3_Mult(rot_from_axis_angle, vec); + transformed_vec1 = SC_SACD_Mat3_Vec3_Mult(rotation_mat, vec); + + CHECK_FLOAT(transformed_vec0.x, transformed_vec1.x); + CHECK_FLOAT(transformed_vec0.y, transformed_vec1.y); + CHECK_FLOAT(transformed_vec0.z, transformed_vec1.z); + } + + // Test rotation matrix to axis-angle. + { + // TODO: implement rotating Vec3 with axis-angle so that this can be tested. + } + std::cout << "Checks checked: " << checks_checked << '\n' << "Checks passed: " << checks_passed << '\n';