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 = 1
	float 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 ;
}