add Vector3RotateByAxisAngle (#2590)

1 files changed, 52 insertions, 0 deletions

diff --git a/src/raymath.h b/src/raymath.h
index d292af1b..0868c62f 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -753,6 +753,58 @@ RMAPI Vector3 Vector3RotateByQuaternion(Vector3 v, Quaternion q)
     return result;
 }
 
+// Rotates a vector around an axis
+RMAPI Vector3 Vector3RotateByAxisAngle(Vector3 v, Vector3 axis, float angle)
+{
+    // Using Euler-Rodrigues Formula
+    // Ref.: https://en.wikipedia.org/w/index.php?title=Euler%E2%80%93Rodrigues_formula
+
+    Vector3 result = v;
+    
+    // Vector3Normalize(axis);
+    float length = sqrtf(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z);
+    if (length == 0.0f) length = 1.0f;
+    float ilength = 1.0f / length;
+    axis.x *= ilength;
+    axis.y *= ilength;
+    axis.z *= ilength;
+
+    angle /= 2.0f;
+    float a = sinf(angle);
+    float b = axis.x * a;
+    float c = axis.y * a;
+    float d = axis.z * a;
+    a = cosf(angle);
+    Vector3 w = { b, c, d };
+
+    // Vector3CrossProduct(w, v)
+    Vector3 wv = { w.y * v.z - w.z * v.y, w.z * v.x - w.x * v.z, w.x * v.y - w.y * v.x };
+
+    // Vector3CrossProduct(w, wv)
+    Vector3 wwv = { w.y * wv.z - w.z * wv.y, w.z * wv.x - w.x * wv.z, w.x * wv.y - w.y * wv.x };
+
+    // Vector3Scale(wv, 2 * a)
+    a *= 2;
+    wv.x *= a;
+    wv.y *= a;
+    wv.z *= a;
+
+    // Vector3Scale(wwv, 2)
+    wwv.x *= 2;
+    wwv.y *= 2;
+    wwv.z *= 2;
+
+    result.x += wv.x;
+    result.y += wv.y;
+    result.z += wv.z;
+
+    result.x += wwv.x;
+    result.y += wwv.y;
+    result.z += wwv.z;
+
+    return result;
+}
+
 // Calculate linear interpolation between two vectors
 RMAPI Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount)
 {