From 8557a6d8d505b404e1fa1e5cacba758a81266003 Mon Sep 17 00:00:00 2001
From: Constantine Tarasenkov <iamvfx@gmail.com>
Date: Tue, 3 Feb 2015 02:25:25 +0300
Subject: [PATCH] Adding VectorRotate() function

---
 src/raymath.c | 30 ++++++++++++++++++++++++++++++
 src/raymath.h |  1 +
 2 files changed, 31 insertions(+)

diff --git a/src/raymath.c b/src/raymath.c
index df098c6a3..194b23ff9 100644
--- a/src/raymath.c
+++ b/src/raymath.c
@@ -131,6 +131,36 @@ void VectorScale(Vector3 *v, float scale)
     v->z *= scale;
 }
 
+// Rotate vector by axis and angle around the pivot point
+void VectorRotate(Vector3 *v, Vector3 pivot, Vector3 axis, float angle)
+{
+    VectorNormalize(&axis);
+    float tx = v->x - pivot.x;
+    float ty = v->y - pivot.y;
+    float tz = v->z - pivot.z;
+
+    float a = (float)(angle * (PI / 180.0));
+    float sina = sin(a);
+    float cosa = cos(a);
+    float cosb = 1.0f - cosa;
+
+    float xrot = tx * (axis.x * axis.x * cosb + cosa)
+               + ty * (axis.x * axis.y * cosb - axis.z * sina)
+               + tz * (axis.x * axis.z * cosb + axis.y * sina);
+
+    float yrot = tx * (axis.y * axis.x * cosb + axis.z * sina)
+               + ty * (axis.y * axis.y * cosb + cosa)
+               + tz * (axis.y * axis.z * cosb - axis.x * sina);
+
+    float zrot = tx * (axis.z * axis.x * cosb - axis.y * sina)
+               + ty * (axis.z * axis.y * cosb + axis.x * sina)
+               + tz * (axis.z * axis.z * cosb + cosa);
+
+    v->x = xrot + pivot.x;
+    v->y = yrot + pivot.y;
+    v->z = zrot + pivot.z;
+}
+
 // Negate provided vector (invert direction)
 void VectorNegate(Vector3 *v)
 {
diff --git a/src/raymath.h b/src/raymath.h
index c8c1a26c3..d1b1af55d 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -86,6 +86,7 @@ Vector3 VectorPerpendicular(Vector3 v);                 // Calculate one vector
 float VectorDotProduct(Vector3 v1, Vector3 v2);         // Calculate two vectors dot product
 float VectorLength(const Vector3 v);                    // Calculate vector lenght
 void VectorScale(Vector3 *v, float scale);              // Scale provided vector
+void VectorRotate(Vector3 *v, Vector3 pivot, Vector3 axis, float angle); // Rotate vector by axis and angle around the pivot point
 void VectorNegate(Vector3 *v);                          // Negate provided vector (invert direction)
 void VectorNormalize(Vector3 *v);                       // Normalize provided vector
 float VectorDistance(Vector3 v1, Vector3 v2);           // Calculate distance between two points