From 0ce2c1398c78420c71b2a9faadb854418b991afd Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Thu, 11 Aug 2016 10:42:52 -0400 Subject: [PATCH 1/2] Adding Quat_LookAt() and Quat_FromAxisAngle(). Removing excess copy/paste code. Please check my TAU... Fixed K&R. One-liner IFs. Changed bogus code to accept Quat_FromAxisAngle(vector, angle). Normalized axis vector in Quat_FromAxisAngle(). Missed that little cross. Replaced FVec4_New() with Quat_New(). * Changed bogus code to accept Quat_FromAxisAngle(vector, angle). Normalized axis vector in Quat_FromAxisAngle(). Missed that little cross. Replaced FVec4_New() with Quat_New(). --- include/c3d/maths.h | 26 ++++++++++++++++++++++---- source/maths/quat_fromaxisangle.c | 9 +++++++++ source/maths/quat_lookat.c | 15 +++++++++++++++ 3 files changed, 46 insertions(+), 4 deletions(-) create mode 100644 source/maths/quat_fromaxisangle.c create mode 100644 source/maths/quat_lookat.c diff --git a/include/c3d/maths.h b/include/c3d/maths.h index 1587231..0f9da8f 100644 --- a/include/c3d/maths.h +++ b/include/c3d/maths.h @@ -573,7 +573,7 @@ C3D_FQuat Quat_Pow(C3D_FQuat q, float p); * @brief Cross product of Quaternion and FVec3 * @param[in] lhs Left-side Quaternion * @param[in] rhs Right-side FVec3 - * @return q×v + * @return q × v */ C3D_FVec Quat_CrossFVec3(C3D_FQuat q, C3D_FVec v); @@ -651,7 +651,7 @@ static inline C3D_FQuat Quat_Conjugate(C3D_FQuat q) static inline C3D_FQuat Quat_Inverse(C3D_FQuat q) { // q^-1 = (q.r - q.i - q.j - q.k) / (q.r^2 + q.i^2 + q.j^2 + q.k^2) - // = q* / (q∙q) + // = q* / (q · q) C3D_FQuat c = Quat_Conjugate(q); float d = Quat_Dot(q, q); return Quat_New(c.i/d, c.j/d, c.k/d, c.r/d); @@ -661,11 +661,29 @@ static inline C3D_FQuat Quat_Inverse(C3D_FQuat q) * @brief Cross product of FVec3 and Quaternion * @param[in] lhs Left-side FVec3 * @param[in] rhs Right-side Quaternion - * @return v×q + * @return v × q */ static inline C3D_FVec FVec3_CrossQuat(C3D_FVec v, C3D_FQuat q) { - // v×q = q^-1×v + // v × q = q^-1 × v return Quat_CrossFVec3(Quat_Inverse(q), v); } + +/** + * @brief Quaternion Look At + * @param[in] source C3D_FVec Starting position. Origin of rotation. + * @param[in] target C3D_FVec Target position to orient towards. + * @param[in] forwardVector C3D_FVec The Up vector. + * @param[in] upVector C3D_FVec The Up vector. + * @return Quaternion rotation. + */ +C3D_FQuat Quat_LookAt(C3D_FVec source, C3D_FVec target, C3D_FVec forwardVector, C3D_FVec upVector); + +/** + * @brief Quaternion, created from a given axis and angle in radians. + * @param[in] axis C3D_FVec The axis to rotate around at. + * @param[in] angle float The angle to rotate. Unit: Radians + * @return Quaternion rotation based on the axis and angle. Axis doesn't have to be orthogonal. + */ +C3D_FQuat Quat_FromAxisAngle(C3D_FVec axis, float angle); ///@} diff --git a/source/maths/quat_fromaxisangle.c b/source/maths/quat_fromaxisangle.c new file mode 100644 index 0000000..6edd542 --- /dev/null +++ b/source/maths/quat_fromaxisangle.c @@ -0,0 +1,9 @@ +#include + +C3D_FQuat Quat_FromAxisAngle(C3D_FVec axis, float angle) +{ + float halfAngle = angle / 2.0f; + float scale = sinf(halfAngle); + axis = Quat_Normalize(axis); + return Quat_New(axis.x * scale, axis.y * scale, axis.z * scale, cosf(halfAngle)); +} diff --git a/source/maths/quat_lookat.c b/source/maths/quat_lookat.c new file mode 100644 index 0000000..d04c718 --- /dev/null +++ b/source/maths/quat_lookat.c @@ -0,0 +1,15 @@ +#include +#include + +C3D_FQuat Quat_LookAt(C3D_FVec source, C3D_FVec target, C3D_FVec forwardVector, C3D_FVec upVector) +{ + C3D_FVec forward = FVec3_Normalize(FVec3_Subtract(target, source)); + float dot = FVec3_Dot(forwardVector, forward); + if (dot + 1.0f < FLT_EPSILON) + return Quat_FromAxisAngle(upVector, M_TAU/2.0f); + if (dot - 1.0f < -FLT_EPSILON) + return Quat_Identity(); + float rotationAngle = acosf(dot); + C3D_FVec rotationAxis = FVec3_Normalize(FVec3_Cross(forwardVector, forward)); + return Quat_FromAxisAngle(rotationAxis, rotationAngle); +} From c2e1416a2a3b2a9d9f2cd7149caf0c9d460b48de Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Thu, 11 Aug 2016 12:19:32 -0400 Subject: [PATCH 2/2] Optimizing code, because Quat_FromAxisAngle() will normalize the axis vector. No need to normalize again. --- source/maths/quat_lookat.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/maths/quat_lookat.c b/source/maths/quat_lookat.c index d04c718..034108e 100644 --- a/source/maths/quat_lookat.c +++ b/source/maths/quat_lookat.c @@ -10,6 +10,6 @@ C3D_FQuat Quat_LookAt(C3D_FVec source, C3D_FVec target, C3D_FVec forwardVector, if (dot - 1.0f < -FLT_EPSILON) return Quat_Identity(); float rotationAngle = acosf(dot); - C3D_FVec rotationAxis = FVec3_Normalize(FVec3_Cross(forwardVector, forward)); + C3D_FVec rotationAxis = FVec3_Cross(forwardVector, forward); return Quat_FromAxisAngle(rotationAxis, rotationAngle); }