diff --git a/src/rcamera.h b/src/rcamera.h index a198fe1fb..9e219897d 100644 --- a/src/rcamera.h +++ b/src/rcamera.h @@ -202,9 +202,55 @@ Matrix GetCameraProjectionMatrix(Camera3D* camera, float aspect); // Rotates the given vector around the given axis // Note: angle must be provided in radians -Vector3 Vector3RotateAxis(Vector3 v, Vector3 axis, float angle) { // TODO there is a better way without the intermediate matrix - Matrix rot = MatrixRotate(axis, angle); - return Vector3Transform(v, rot); +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; } //---------------------------------------------------------------------------------- @@ -340,7 +386,7 @@ void CameraYaw(Camera3D *camera, float angle) Vector3 target_position = Vector3Subtract(camera->target, camera->position); // Rotate view vector around up axis - target_position = Vector3RotateAxis(target_position, up, angle); + target_position = Vector3RotateByAxisAngle(target_position, up, angle); if (camera->mode == CAMERA_FREE || camera->mode == CAMERA_FIRST_PERSON) @@ -390,7 +436,7 @@ void CameraPitch(Camera3D *camera, float angle) Vector3 right = GetCameraRight(camera); // Rotate view vector around right axis - target_position = Vector3RotateAxis(target_position, right, angle); + target_position = Vector3RotateByAxisAngle(target_position, right, angle); if (camera->mode == CAMERA_FREE) { @@ -398,7 +444,7 @@ void CameraPitch(Camera3D *camera, float angle) camera->target = Vector3Add(camera->position, target_position); // Rotate up direction around right axis - camera->up = Vector3RotateAxis(camera->up, right, angle); + camera->up = Vector3RotateByAxisAngle(camera->up, right, angle); } else if (camera->mode == CAMERA_FIRST_PERSON) { @@ -422,7 +468,7 @@ void CameraRoll(Camera3D *camera, float angle) Vector3 forward = GetCameraForward(camera); // Rotate up direction around forward axis - camera->up = Vector3RotateAxis(camera->up, forward, angle); + camera->up = Vector3RotateByAxisAngle(camera->up, forward, angle); } // Moves camera slightly to simulate a walking motion