From 8097e9ebe2afb4b06821a0cb934ca47a509c9844 Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Wed, 17 Aug 2016 17:20:28 -0400 Subject: [PATCH] Optimizing code. --- include/c3d/maths.h | 5 +---- source/maths/quat_frompitchyawroll.c | 28 ++++++++++++++++++++++++++++ 2 files changed, 29 insertions(+), 4 deletions(-) create mode 100644 source/maths/quat_frompitchyawroll.c diff --git a/include/c3d/maths.h b/include/c3d/maths.h index 4b442b1..e7986fa 100644 --- a/include/c3d/maths.h +++ b/include/c3d/maths.h @@ -676,8 +676,5 @@ static inline C3D_FVec FVec3_CrossQuat(C3D_FVec v, C3D_FQuat q) * @param[in] roll The roll angle in radians. * @return C3D_FQuat The Quaternion equivalent with the pitch, yaw, and roll orientations applied. */ -static inline C3D_FQuat Quat_FromPitchYawRoll(float pitch, float yaw, float roll) -{ - return Quat_RotateZ(Quat_RotateY(Quat_RotateX(Quat_Identity(), pitch, false), yaw, false), roll, false); -} +C3D_FQuat Quat_FromPitchYawRoll(float pitch, float yaw, float roll, bool bRightSide); ///@} diff --git a/source/maths/quat_frompitchyawroll.c b/source/maths/quat_frompitchyawroll.c new file mode 100644 index 0000000..e433542 --- /dev/null +++ b/source/maths/quat_frompitchyawroll.c @@ -0,0 +1,28 @@ +#include + +C3D_FQuat Quat_FromPitchYawRoll(float pitch, float yaw, float roll, bool bRightSide) +{ + float pitch_c = cosf(pitch / 2.0f); + float pitch_s = sinf(pitch / 2.0f); + float yaw_c = cosf(yaw / 2.0f); + float yaw_s = sinf(yaw / 2.0f); + float roll_c = cosf(roll / 2.0f); + float roll_s = sinf(roll / 2.0f); + + if (bRightSide) + { + return Quat_New( + pitch_s * yaw_c * roll_c - pitch_c * yaw_s * roll_s, + pitch_c * yaw_s * roll_c + pitch_s * yaw_c * roll_s, + pitch_c * yaw_c * roll_s - pitch_s * yaw_s * roll_c, + pitch_c * yaw_c * roll_c + pitch_s * yaw_s * roll_s); + } + else + { + return Quat_New( + pitch_s * yaw_c * roll_c + pitch_c * yaw_s * roll_s, + pitch_c * yaw_s * roll_c - pitch_s * yaw_c * roll_s, + pitch_c * yaw_c * roll_s + pitch_s * yaw_s * roll_c, + pitch_c * yaw_c * roll_c - pitch_s * yaw_s * roll_s); + } +}