Added QuaternionInvert()

1 files changed, 18 insertions, 0 deletions

diff --git a/src/raymath.h b/src/raymath.h
index 52e92b50..59d66e56 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -158,6 +158,7 @@ RMDEF void PrintMatrix(Matrix m);                             // Print matrix ut
 //------------------------------------------------------------------------------------
 RMDEF float QuaternionLength(Quaternion quat);                // Compute the length of a quaternion
 RMDEF void QuaternionNormalize(Quaternion *q);                // Normalize provided quaternion
+RMDEF void QuaternionInvert(Quaternion *quat);                // Invert provided quaternion
 RMDEF Quaternion QuaternionMultiply(Quaternion q1, Quaternion q2);    // Calculate two quaternion multiplication
 RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float slerp); // Calculates spherical linear interpolation between two quaternions
 RMDEF Quaternion QuaternionFromMatrix(Matrix matrix);                 // Returns a quaternion for a given rotation matrix
@@ -908,6 +909,23 @@ RMDEF void QuaternionNormalize(Quaternion *q)
     q->w *= ilength;
 }
 
+// Invert provided quaternion
+RMDEF void QuaternionInvert(Quaternion *quat)
+{
+    float length = QuaternionLength(*quat);
+    float lengthSq = length*length;
+    
+    if (lengthSq != 0.0)
+    {
+        float i = 1.0f/lengthSq;
+        
+        quat->x *= -i;
+        quat->y *= -i;
+        quat->z *= -i;
+        quat->w *= i;
+    }
+}
+
 // Calculate two quaternion multiplication
 RMDEF Quaternion QuaternionMultiply(Quaternion q1, Quaternion q2)
 {