Quaternion QuaternionNormal(Quaternion quaternion) { float magnitude = sqrt(quaternion.x * quaternion.x + quaternion.y * quaternion.y + quaternion.z * quaternion.z + quaternion.w * quaternion.w); return QuaternionInit(quaternion.x / magnitude, quaternion.y / magnitude, quaternion.z / magnitude, quaternion.w / magnitude); }
Quaternion QuaternionInvert(Quaternion quaternion) { float length = 1.0f / (quaternion.x * quaternion.x + quaternion.y * quaternion.y + quaternion.z * quaternion.z + quaternion.w * quaternion.w); return QuaternionInit(quaternion.x * -length, quaternion.y * -length, quaternion.z * -length, quaternion.w * length); }
inline Quaternion QuaternionInitFromAngleAxis(const float thetaRadians, const Vector3 &axis) { // TODO: Normalize Axis. const float halfAngle = 0.5f * thetaRadians; const float sinAngle = Sin(halfAngle); const float w = Cos(halfAngle); const float x = sinAngle * axis.x; const float y = sinAngle * axis.y; const float z = sinAngle * axis.z; return QuaternionInit(w, x, y, z); }
Quaternion QuaternionMultiply(Quaternion quaternion1, Quaternion quaternion2) { Vector3D vector1 = QuaternionGetVector3D(quaternion1); Vector3D vector2 = QuaternionGetVector3D(quaternion2); float angle = quaternion1.w * quaternion2.w - Vector3DDotProduct(vector1, vector2); Vector3D cross = Vector3DCrossProduct(vector1, vector2); vector1.x *= quaternion2.w; vector1.y *= quaternion2.w; vector1.z *= quaternion2.w; vector2.x *= quaternion1.w; vector2.y *= quaternion1.w; vector2.z *= quaternion1.w; return QuaternionInit(vector1.x + vector2.x + cross.x, vector1.y + vector2.y + cross.y, vector1.z + vector2.z + cross.z, angle); }
inline Quaternion QuaternionInitFromEulerAngles(float xDeg, float yDeg, float zDeg) { // const float halfRadX = 0.5f * CaffMath::DegToRad(xDeg); // const float halfRadY = 0.5f * CaffMath::DegToRad(yDeg); // const float halfRadZ = 0.5f * CaffMath::DegToRad(zDeg); // // const float cosX = CaffMath::Cos(halfRadX); // const float cosY = CaffMath::Cos(halfRadY); // const float cosZ = CaffMath::Cos(halfRadZ); // // const float sinX = CaffMath::Sin(halfRadX); // const float sinY = CaffMath::Sin(halfRadY); // const float sinZ = CaffMath::Sin(halfRadZ); // // const float w = cosZ * cosY * cosX + sinZ * sinY * sinX; // const float i = cosZ * cosY * sinX - sinZ * sinY * cosX; // const float j = cosZ * sinY * cosX + sinZ * cosY * sinX; // const float k = sinZ * cosY * cosX - cosZ * sinY * sinX; // // Quaternion quat; // quat.w = w; // quat.x = i; // quat.y = j; // quat.z = k; // // return quat; float cz = CaffMath::Cos(CaffMath::DegToRad(zDeg) * 0.5f); float sz = CaffMath::Sin(CaffMath::DegToRad(zDeg) * 0.5f); float cy = CaffMath::Cos(CaffMath::DegToRad(yDeg) * 0.5f); float sy = CaffMath::Sin(CaffMath::DegToRad(yDeg) * 0.5f); float cx = CaffMath::Cos(CaffMath::DegToRad(xDeg) * 0.5f); float sx = CaffMath::Sin(CaffMath::DegToRad(xDeg) * 0.5f); float w = cz*cy*cx + sz*sy*sz; float x = cz*cy*sx - sz*sy*cz; float y = cz*sy*cx + sz*cy*sz; float z = sz*cy*cx - cz*sy*sz; return QuaternionInit(w, x, y, z); }
Quaternion QuaternionSlerp(Quaternion start, Quaternion end, float alpha) { float startWeight, endWeight; float difference = start.x * end.x + start.y * end.y + start.z * end.z + start.w * end.w; if(1.0f - fabs(difference) > QUATERNION_SLERP_TO_LERP_SWITCH_THRESHOLD) { float theta = acosf(fabs(difference)); float oneOverSinTheta = 1.0f / sin(theta); startWeight = sinf(theta * (1.0f - alpha)) * oneOverSinTheta; endWeight = sinf(theta * alpha) * oneOverSinTheta; if(difference < 0.0f) { startWeight = -startWeight; } } else { startWeight = 1.0f - alpha; endWeight = alpha; } Quaternion result = QuaternionInit(start.x * startWeight + end.x * endWeight, start.y * startWeight + end.y * endWeight, start.z * startWeight + end.z * endWeight, start.w * startWeight + end.w * endWeight); return QuaternionNormal(result); }
inline Quaternion QuaternionConjugate(const Quaternion toConjugate) { return QuaternionInit(toConjugate.w, -toConjugate.x, -toConjugate.y, -toConjugate.z); }
inline Quaternion QuaternionAdd(const Quaternion left, const Quaternion right) { return QuaternionInit(left.w + right.w, left.x + right.x, left.y + right.y, left.z + right.z); }
Quaternion QuaternionInitAxisAngle(Vector3D axis, float angle) { angle *= 0.5f; axis = Vector3DNormal(axis); float sinAngle = sinf(angle); return QuaternionInit(axis.x * sinAngle, axis.y * sinAngle, axis.z * sinAngle, cosf(angle)); }
Quaternion QuaternionLoadIdentity(void) { return QuaternionInit(0.0f, 0.0f, 0.0f, 1.0f); }