www.pudn.com > zfxcengine-0.1.0.zip > ceQuaternion.cpp

// $Id: ceQuaternion.cpp,v 1.12 2005/09/13 00:47:15 andreaskohn Exp $ 
 
#include  
#include  
#include "Core/ceDebug.h" 
#include "Math/ceMath.h" 
 
namespace ZFXCE { 
	using namespace std; 
	//////////////////////////////////////////////////////////////////////////////// 
	void ceQuaternion::MakeFromEuler(FLOAT fPitch, FLOAT fYaw, FLOAT fRoll) 
	{ 
		fPitch *= 0.5f; 
		fYaw   *= 0.5f; 
		fRoll  *= 0.5f; 
 
		const FLOAT cX = cosf(fPitch * 0.5f); 
		const FLOAT cY = cosf(fYaw   * 0.5f); 
		const FLOAT cZ = cosf(fRoll  * 0.5f); 
 
		const FLOAT sX = sinf(fPitch * 0.5f); 
		const FLOAT sY = sinf(fYaw   * 0.5f); 
		const FLOAT sZ = sinf(fRoll  * 0.5f); 
 
		const FLOAT cYcZ = cY * cZ; 
		const FLOAT sYsZ = sY * sZ; 
		const FLOAT cYsZ = cY * sZ; 
		const FLOAT sYcZ = sY * cZ; 
 
		w = cX * cYcZ + sX * sYsZ; 
		x = sX * cYcZ - cX * sYsZ; 
		y = cX * sYcZ + sX * cYsZ; 
		z = cX * cYsZ - sX * sYcZ; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	void ceQuaternion::Conjugate(ceQuaternion q) 
	{ 
		x = -q.x; 
		y = -q.y; 
		z = -q.z; 
		w = q.w; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	void ceQuaternion::GetEulers(FLOAT* pfPitch, FLOAT* pfYaw, FLOAT* pfRoll) const 
	{ 
		const FLOAT q00 = w * w; 
		const FLOAT q11 = x * x; 
		const FLOAT q22 = y * y; 
		const FLOAT q33 = z * z; 
		const FLOAT r11 = q00 + q11 - q22 - q33; 
		const FLOAT r21 = 2 * (x*y + w*z); 
		const FLOAT r31 = 2 * (x*z - w*y); 
		const FLOAT r32 = 2 * (y*z + w*x); 
		const FLOAT r33 = q00 - q11 - q22 + q33; 
		const FLOAT tmp = _fabs(r31); 
		 
		if (tmp > 0.999999) { 
			const FLOAT r12 = 2 * (x*y - w*z); 
			const FLOAT r13 = 2 * (x*z + w*y); 
 
			if(pfPitch) 
				*pfPitch = 0.0f; 
			 
			if(pfYaw) 
				*pfYaw   = (FLOAT)-((cePI * 0.5f) * r31/tmp); 
			 
			if(pfRoll) 
				*pfRoll  = (FLOAT)atan2(-r12, -r31*r13); 
		} 
		else { 
			if (pfPitch) 
				*pfPitch = (FLOAT) atan2(r32, r33); 
 
			if (pfYaw) 
				*pfYaw   = (FLOAT) asin(-r31); 
 
			if (pfRoll) 
				*pfRoll  = (FLOAT) atan2(r21, r11); 
		} 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	void ceQuaternion::GetMatrix(ceMatrix* pM) const 
	{ 
		ce_assert (NULL != pM); 
	 
		pM->Identity(); 
		const FLOAT x2 = x + x;  
		const FLOAT y2 = y + y;  
		const FLOAT z2 = z + z; 
 
		const FLOAT xx = x * x2;    
		const FLOAT xy = x * y2;   
		const FLOAT xz = x * z2; 
 
		const FLOAT yy = y * y2;    
		const FLOAT yz = y * z2;    
		const FLOAT zz = z * z2; 
 
		const FLOAT wx = w * x2;    
		const FLOAT wy = w * y2;    
		const FLOAT wz = w * z2; 
 
		pM->_ms._11 = 1.0f - (yy + zz); 
		pM->_ms._12 = xy - wz; 
		pM->_ms._13 = xz + wy; 
 
		pM->_ms._21 = xy + wz; 
		pM->_ms._22 = 1.0f - (xx + zz); 
		pM->_ms._23 = yz - wx; 
 
		pM->_ms._31 = xz - wy; 
		pM->_ms._32 = yz + wx; 
		pM->_ms._33 = 1.0f - (xx + yy); 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	FLOAT ceQuaternion::GetMagnitude() const 
	{ 
		return sqrtf(x*x + y*y + z*z + w*w); 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion &ceQuaternion::Normalize() 
	{ 
		const FLOAT n = x*x + y*y + z*z + w*w; 
		if (1.0f != n) { 
			operator/=(sqrtf(n)); 
		} 
		return *this; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion &ceQuaternion::operator /=(const FLOAT f) 
	{ 
		if (f != 0.0f) { 
			operator*=(1.0f / f); 
		} 
		return *this; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion ceQuaternion::operator /(const FLOAT f) const 
	{ 
		// XXX:  avoiding div-by-zero violates general / contract! 
		if (f != 0.0f) { 
			const FLOAT fVal = 1 / f; 
			return ceQuaternion(x*fVal, y*fVal, z*fVal, w*fVal); 
		} else { 
			return *this; 
		} 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion &ceQuaternion::operator *=(const FLOAT f) 
	{ 
		x *= f; 
		y *= f; 
		z *= f; 
		w *= f; 
		 
		return *this; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion ceQuaternion::operator *(const FLOAT f) const 
	{ 
		return ceQuaternion(x*f, y*f, z*f, w*f); 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion &ceQuaternion::operator +=(const ceQuaternion& q) 
	{ 
		x += q.x; 
		y += q.y; 
		z += q.z; 
		w += q.w; 
		 
		return *this; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion ceQuaternion::operator +(const ceQuaternion& q) const 
	{ 
		return ceQuaternion(x+q.x, y+q.y, z+q.z, w+q.w); 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion ceQuaternion::operator *(const ceVec4f& v) const 
	{ 
		return ceQuaternion( 
			w*v.x + y*v.z - z*v.y, 
			w*v.y + z*v.x - x*v.z, 
			w*v.z + x*v.y - y*v.x, 
			-(x*v.x + y*v.y + z*v.z) ); 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion ceQuaternion::operator *(const ceVec3f& v) const 
	{ 
		return ceQuaternion(w*v.x + y*v.z - z*v.y, 
							w*v.y + z*v.x - x*v.z, 
							w*v.z + x*v.y - y*v.x, 
							-(x*v.x + y*v.y + z*v.z) ); 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion ceQuaternion::operator *(const ceQuaternion& q) const 
	{ 
		ceQuaternion Result; 
   	 
		Result.x = w*q.x + x*q.w + y*q.z - z*q.y; 
		Result.y = w*q.y + y*q.w + z*q.x - x*q.z; 
		Result.z = w*q.z + z*q.w + x*q.y - y*q.x; 
		Result.w = w*q.w - x*q.x - y*q.y - z*q.z; 
 
		return Result; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceQuaternion &ceQuaternion::operator *= (const ceQuaternion& q) 
	{ 
		const FLOAT _x = w*q.x + x*q.w + y*q.z - z*q.y; 
		const FLOAT _y = w*q.y + y*q.w + z*q.x - x*q.z; 
		const FLOAT _z = w*q.z + z*q.w + x*q.y - y*q.x; 
		const FLOAT _w = w*q.w - x*q.x - y*q.y - z*q.z; 
 
		x = _x; 
		y = _y; 
		z = _z; 
		w = _w; 
		 
		return *this; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	void ceQuaternion::Rotate(const ceQuaternion& q1, const ceQuaternion& q2) 
	{ 
		const ceQuaternion q = q1 * q2 * (~q1); 
		x = q.x; 
		y = q.y; 
		z = q.z; 
		w = q.w; 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
	ceVec4f ceQuaternion::Rotate(const ceVec4f& v) 
	{ 
		const ceQuaternion q(x,y,z,w); 
		const ceQuaternion r = q * v * (~q); 
		x = r.x; 
		y = r.y; 
		z = r.z; 
		w = r.w; 
		 
		// XXX: check if we should return r.w in w.  
		return ceVec4f(r.x, r.y, r.z); 
	} 
	//////////////////////////////////////////////////////////////////////////////// 
 
} // Namespace ZFXCE