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

@@ -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

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_Vec3, 3> 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)));
+}

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

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<float> / 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';