From 8a3982d62522b6b4b76d321c41999956c2068541 Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Mon, 22 Aug 2016 16:14:26 -0400 Subject: [PATCH] Added Quat_FromPitchYawRoll() helper function. (#25) Quickly converts pitch, yaw, and roll rotations to the Quaternion equivalent orientation, with optimized code. --- include/c3d/maths.h | 9 +++++++++ source/maths/quat_frompitchyawroll.c | 28 ++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+) create mode 100644 source/maths/quat_frompitchyawroll.c diff --git a/include/c3d/maths.h b/include/c3d/maths.h index 1587231..e7986fa 100644 --- a/include/c3d/maths.h +++ b/include/c3d/maths.h @@ -668,4 +668,13 @@ static inline C3D_FVec FVec3_CrossQuat(C3D_FVec v, C3D_FQuat q) // v×q = q^-1×v return Quat_CrossFVec3(Quat_Inverse(q), v); } + +/** + * @brief Converting Pitch, Yaw, and Roll to Quaternion equivalent + * @param[in] pitch The pitch angle in radians. + * @param[in] yaw The yaw angle in radians. + * @param[in] roll The roll angle in radians. + * @return C3D_FQuat The Quaternion equivalent with the pitch, yaw, and roll orientations applied. + */ +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); + } +}