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