diff --git a/src/raymath.h b/src/raymath.h index d292af1b1..0868c62f7 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) {