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); + } +}