Quaternion-未測試-實現檔案

#include <assert.h>#include "Quaternion.h"#include "Matrix4x4.h"void Quaternion ::Indentity(){w = 1 ;x = y = z = 0 ;}void Quaternion ::SetToRotateAboutX(float theta){float thetaOver2 = theta / 2 ;w = cos(thetaOver2) ;x = sin(thetaOver2) ;y = 0 ;z = 0 ;} void Quaternion ::SetToRotateAboutY(float theta){float thetaOver2 = theta / 2 ;w = cos(thetaOver2) ;x = 0 ;y = sin(thetaOver2) ;z = 0 ;}void Quaternion ::SetToRotateAboutZ(float theta){float thetaOver2 = theta / 2 ;w = cos(thetaOver2) ;x = 0 ;y = 0 ;z = sin(thetaOver2) ;}void Quaternion ::SetToRotateAboutAxis(const Vector3& axis, float theta){assert(fabs(Vector3.GetMagnitude(axis) - 1) < 0.01) ;float thetaOver2 = theta / 2 ;w = cos(thetaOver2) ;float temp = sin(thetaOver2) ;x = axis.x * temp ;y = axis.y * temp ;z = axis.z * temp ;}void Quaternion ::EulerAnglesToQuaternion(const EulerAngles& e){float bankOver2 = e.bank / 2 ;Quaternion bank ;bank.w = cos(bankOver2) ;bank.x = 0 ;bank.y = 0 ;bank.z = sin(bankOver2) ;float pitchOver2 = e.pitch / 2 ;Quaternion pitch ;pitch.w = cos(pitchOver2) ;pitch.x = sin(pitchOver2) ;pitch.y = 0 ;pitch.z = 0 ;float headingOver2 = e.heading / 2 ;Quaternion heading ;heading.w = cos(headingOver2) ;heading.x = 0 ;heading.y = sin(headingOver2) ;heading.z = 0 ;//bank * pitch * heading*this = bank * pitch ;*this = *this * heading ;}void Quaternion ::MatrixToQuaternion(const Matrix4x4& m){//這幾個變數名,作者起得真不錯!float fourWSquaredMinusOne = m.m11 + m.m22 + m.m33 ;float fourXSquaredMinusOne = m.m11 - m.m22 - m.m33 ;float fourYSquaredMinusOne = m.m22 - m.m11 - m.m33 ;float fourZSquaredMinusOne = m.m33 - m.m11 - m.m22 ;int enumIndex = FourWSquaredMinusOne ;float max = fourWSquaredMinusOne ;if (fourXSquaredMinusOne > max){max = fourXSquaredMinusOne ;enumIndex = FourXSquaredMinusOne ;}if (fourYSquaredMinusOne > max){max = fourYSquaredMinusOne ;enumIndex = FourYSquaredMinusOne ;}if (fourZSquaredMinusOne > max){max = fourZSquaredMinusOne ;enumIndex = FourZSquaredMinusOne ;}float biggest = sqrt(max + 1) / 2 ;switch (enumIndex){case FourWSquaredMinusOne:w = biggest ;x = (m.m23 - m.m32) / (4 * w) ;y = (m.m31 - m.m13) / (4 * w) ;z = (m.m12 - m.m21) / (4 * w) ;break ;case FourXSquaredMinusOne:x = biggest ;w = (m.m23 - m.m32) / (4 * x) ;y = (m.m12 + m.m21) / (4 * x) ;z = (m.m31 + m.m13) / (4 * x) ;break ;case FourYSquaredMinusOne:y = biggest ;w = (m.m31 - m.m13) / (4 * y) ;x = (m.m12 + m.m21) / (4 * y) ;z = (m.m23 + m.m32) / (4 * y) ;break ;case FourZSquaredMinusOne:z = biggest ;w = (m.m12 - m.m21) / (4 * z) ;x = (m.m31 + m.m13) / (4 * z) ;y = (m.m23 + m.m32) / (4 * z) ;break ;}}void Quaternion ::Normalize(void){float magnitude = GetMagnitude(*this) ;w /= magnitude ;x /= magnitude ;y /= magnitude ;z /= magnitude ;}float Quaternion ::GetRotationAngle() const{return acos(w) * 2 ;}Vector3 Quaternion ::GetRotationAxis() const{assert(fabs(GetMagnitude(*this) - 1) < 0.01) ;//sinTheta^1 + cosTheta^2 = 1float sinTheta = sqrt(1 - w * w) ;return Vector3(x / sinTheta, y / sinTheta, z / sinTheta) ;}void Quaternion ::operator= (const Quaternion& q){w = q.w ;x = q.x ;y = q.y ;z = q.z ;}Quaternion Quaternion ::operator * (const Quaternion& q) const{Quaternion result ;result.w = w * q.w - x * q.x - y * q.y - z * q.z ;result.x = w * q.x + x * q.w + z * q.y - y * q.z ;result.y = w * q.y + y * q.w + x * q.z - z * q.x ;result.z = w * q.z + z * q.w + y * q.x - x * q.y ;return result ;}Quaternion& Quaternion ::operator *= (const Quaternion& q){*this = *this * q ;return *this ;}float Quaternion ::GetMagnitude(const Quaternion& q){return sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z) ;}float Quaternion ::DotProduct(const Quaternion& lhs, const Quaternion& rhs){return lhs.w * rhs.w + lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z ;}Quaternion Quaternion ::Slerp(const Quaternion& lhs, const Quaternion& rhs, float t){assert(fabs(GetMagnitude(lhs) - 1) < 0.01) ;assert(fabs(GetMagnitude(rhs) - 1) < 0.01) ;if (t < 0)return lhs ;else if (t > 1)return rhs ;float cosOmega = DotProduct(lhs, rhs) ;Quaternion workedRhs = lhs ;if (cosOmega < 0){workedRhs.w = -lhs.w ;workedRhs.x = -lhs.x ; workedRhs.y = -lhs.y ;workedRhs.z = -lhs.z ;cosOmega = -cosOmega ;}float k0, k1 ;if (cosOmega > 0.9999){k0 = 1 - t ;k1 = t ;}else{float sinOmega = sqrt(1 - cosOmega * cosOmega) ;float omega = atan2(sinOmega, cosOmega) ;k0 = sin((1 - t) * omega) / sinOmega ;k1 = sin(t * omega) / sinOmega ;}Quaternion result ;result.w = k0 * lhs.w + k1 * workedRhs.w ;result.x = k0 * lhs.x + k1 * workedRhs.x ;result.y = k0 * lhs.y + k1 * workedRhs.y ;result.z = k0 * lhs.z + k1 * workedRhs.z ;return result ;}Quaternion Quaternion ::Conjugate(const Quaternion& q){Quaternion result ;result.w = q.w ;result.x = -q.x ;result.y = -q.y ;result.z = -q.z ;return result ;}Quaternion Quaternion ::Pow(const Quaternion& q, float exponent){if (fabs(q.w) > One)return q ;float theta = acos(q.w) * 2 ;float newTheta = theta * exponent ;q.w = cos(newTheta / 2) ;float multiple = sin(newTheta) / sin(theta) ;q.x *= multiple ;q.y *= multiple ;q.z *= multiple ;return q ;}