Merge pull request #132 from Mars2032/master

CalcCommon additions, minor fixes
open-ead · Mar 27, 2024 · 7e78702 · 7e78702
2 parents 42cbaae + 338edc3
commit 7e78702
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 3 deletions.
diff --git a/include/math/seadMathCalcCommon.hpp b/include/math/seadMathCalcCommon.hpp
@@ -369,7 +369,7 @@ inline f64 MathCalcCommon<f64>::abs(f64 x)
 template <typename T>
 inline T MathCalcCommon<T>::max(T a, T b)
 {
-    return a < b ? b : a;
+    return a > b ? a : b;
 }
 
 template <typename T>

diff --git a/include/math/seadQuatCalcCommon.h b/include/math/seadQuatCalcCommon.h
@@ -20,6 +20,7 @@ class QuatCalcCommon
     static bool makeVectorRotation(Base& q, const Vec3& from, const Vec3& to);
     static void set(Base& q, T w, T x, T y, T z);
     static void setRPY(Base& q, T roll, T pitch, T yaw);
+    static void setAxisAngle(Base& q, const Vec3& axis, T angle);
     static void calcRPY(Vec3& rpy, const Base& q);
 };
 

diff --git a/include/math/seadQuatCalcCommon.hpp b/include/math/seadQuatCalcCommon.hpp
@@ -6,7 +6,6 @@
 
 #include <limits>
 #include <math/seadMathCalcCommon.h>
-#include <math/seadQuat.h>
 
 namespace sead
 {
@@ -92,7 +91,7 @@ inline void QuatCalcCommon<T>::slerpTo(Base& out, const Base& q1, const Base& q2
 template <typename T>
 inline void QuatCalcCommon<T>::makeUnit(Base& q)
 {
-    q = Quat<T>::unit;
+    q = Base::unit;
 }
 
 template <typename T>
@@ -149,4 +148,16 @@ inline void QuatCalcCommon<T>::setRPY(Base& q, T roll, T pitch, T yaw)
     set(q, w, x, y, z);
 }
 
+template <typename T>
+inline void QuatCalcCommon<T>::setAxisAngle(Base& q, const Vec3& axis, T angle)
+{
+    T angleRad = MathCalcCommon<T>::deg2rad(angle);
+    angleRad *= 0.5f;
+    q.w = MathCalcCommon<T>::cos(angleRad);
+    T sa = MathCalcCommon<T>::sin(angleRad);
+    q.x = sa * axis.x;
+    q.y = sa * axis.y;
+    q.z = sa * axis.z;
+}
+
 }  // namespace sead
diff --git a/include/math/seadVectorCalcCommon.h b/include/math/seadVectorCalcCommon.h
@@ -28,6 +28,7 @@ class Vector3CalcCommon
     using Base = typename Policies<T>::Vec3Base;
     using Mtx33 = typename Policies<T>::Mtx33Base;
     using Mtx34 = typename Policies<T>::Mtx34Base;
+    using Quat = typename Policies<T>::QuatBase;
 
     static void add(Base& o, const Base& a, const Base& b);
     static void sub(Base& o, const Base& a, const Base& b);
@@ -40,6 +41,8 @@ class Vector3CalcCommon
     static void rotate(Base& o, const Mtx33& m, const Base& a);
     /// Apply a rotation `m` to the vector `a`.
     static void rotate(Base& o, const Mtx34& m, const Base& a);
+    /// Apply a rotation 'q' to the vector 'a'
+    static void rotate(Base& o, const Quat& q, const Base& a);
 
     static void cross(Base& o, const Base& a, const Base& b);
     static T dot(const Base& a, const Base& b);

diff --git a/include/math/seadVectorCalcCommon.hpp b/include/math/seadVectorCalcCommon.hpp
@@ -5,6 +5,7 @@
 #endif  // cafe
 
 #include <math/seadMathCalcCommon.h>
+#include <math/seadQuatCalcCommon.h>
 #ifndef SEAD_MATH_VECTOR_CALC_COMMON_H_
 #include <math/seadVectorCalcCommon.h>
 #endif
@@ -122,6 +123,23 @@ inline void Vector3CalcCommon<T>::rotate(Base& o, const Mtx34& m, const Base& a)
     o.z = m.m[2][0] * tmp.x + m.m[2][1] * tmp.y + m.m[2][2] * tmp.z;
 }
 
+template <typename T>
+inline void Vector3CalcCommon<T>::rotate(Base& o, const Quat& q, const Base& v)
+{
+    Quat r;  // quat-multiplication with 0 on w for v
+    r.x = (q.y * v.z) - (q.z * v.y) + (q.w * v.x);
+    r.y = -(q.x * v.z) + (q.z * v.x) + (q.w * v.y);
+    r.z = (q.x * v.y) - (q.y * v.x) + (q.w * v.z);
+    r.w = -(q.x * v.x) - (q.y * v.y) - (q.z * v.z);
+
+    r.w *= -1;
+
+    // quat-multiplication
+    o.x = (q.w * r.x) - (q.z * r.y) + (q.y * r.z) + (q.x * r.w);
+    o.y = (q.z * r.x) + (q.w * r.y) - (q.x * r.z) + (q.y * r.w);
+    o.z = (q.w * r.z) + (-(q.y * r.x) + (q.x * r.y)) + (q.z * r.w);
+}
+
 template <typename T>
 inline void Vector3CalcCommon<T>::cross(Base& o, const Base& a, const Base& b)
 {