+Unity Runtime codeHEADmaster

1 files changed, 405 insertions, 0 deletions

diff --git a/Runtime/Math/Quaternion.h b/Runtime/Math/Quaternion.h
new file mode 100644
index 0000000..2cabe9c
--- /dev/null
+++ b/Runtime/Math/Quaternion.h
@@ -0,0 +1,405 @@
+#ifndef QUATERNION_H
+#define QUATERNION_H
+
+#include "Matrix3x3.h"
+#include "Matrix4x4.h"
+#include "Vector3.h"
+#include "FloatConversion.h"
+#include <algorithm>
+#include <vector>
+#include "Runtime/Modules/ExportModules.h"
+
+class Quaternionf
+{
+	public:
+
+	float x, y, z, w;
+
+	DEFINE_GET_TYPESTRING_IS_ANIMATION_CHANNEL (Quaternionf)
+	template<class TransferFunction> void Transfer (TransferFunction& transfer);
+
+	Quaternionf () {}
+	Quaternionf (float inX, float inY, float inZ, float inW);
+	explicit Quaternionf (const float* array)	{ x = array[0]; y = array[1]; z = array[2]; w = array[3]; }
+	
+	// methods
+	
+	const float* GetPtr ()const				{ return &x; }
+	float* GetPtr ()								{ return &x; }
+	
+	const float& operator [] (int i)const	{ return GetPtr ()[i]; }
+	float& operator [] (int i)					{ return GetPtr ()[i]; }
+
+	void Set (float inX, float inY, float inZ, float inW);
+	void Set (const Quaternionf& aQuat);
+	void Set (const float* array)	{ x = array[0]; y = array[1]; z = array[2]; w = array[3]; }
+	
+	friend Quaternionf Normalize(const Quaternionf& q) {	return q / Magnitude (q); }
+	friend Quaternionf NormalizeSafe(const Quaternionf& q);
+
+	friend Quaternionf Conjugate(const Quaternionf& q);
+	friend Quaternionf Inverse (const Quaternionf& q);
+
+	friend float SqrMagnitude (const Quaternionf& q);
+	friend float Magnitude (const Quaternionf& q);
+
+	bool operator == (const Quaternionf& q)const		{ return x == q.x && y == q.y && z == q.z && w == q.w; }
+	bool operator != (const Quaternionf& q)const		{ return x != q.x || y != q.y || z != q.z || w != q.w; }
+	
+	Quaternionf&	operator += (const Quaternionf&  aQuat);
+	Quaternionf&	operator -= (const Quaternionf&  aQuat);
+	Quaternionf&	operator *= (const float     	aScalar);
+	Quaternionf&	operator *= (const Quaternionf& 	aQuat);
+	Quaternionf&	operator /= (const float     	aScalar);
+
+	friend Quaternionf operator + (const Quaternionf& lhs, const Quaternionf& rhs)
+	{
+		Quaternionf q (lhs);
+		return q += rhs;
+	}
+	
+	friend Quaternionf	operator - (const Quaternionf& lhs, const Quaternionf& rhs)
+	{
+		Quaternionf t (lhs);
+		return t -= rhs;
+	}
+
+	Quaternionf operator - () const
+	{
+		return Quaternionf(-x, -y, -z, -w);
+	}
+
+	Quaternionf	operator * (const float s) const
+	{
+		return Quaternionf (x*s, y*s, z*s, w*s);
+	}
+	
+	friend Quaternionf	operator * (const float s, const Quaternionf& q)
+	{
+		Quaternionf t (q);
+		return t *= s;
+	}
+	
+	friend Quaternionf	operator / (const Quaternionf& q, const float s)
+	{
+		Quaternionf t (q);
+		return t /= s;
+	}
+	
+	inline friend Quaternionf operator * (const Quaternionf& lhs, const Quaternionf& rhs)
+	{
+		return Quaternionf (
+				lhs.w*rhs.x + lhs.x*rhs.w + lhs.y*rhs.z - lhs.z*rhs.y,
+				lhs.w*rhs.y + lhs.y*rhs.w + lhs.z*rhs.x - lhs.x*rhs.z,
+				lhs.w*rhs.z + lhs.z*rhs.w + lhs.x*rhs.y - lhs.y*rhs.x,
+				lhs.w*rhs.w - lhs.x*rhs.x - lhs.y*rhs.y - lhs.z*rhs.z);
+	}
+	
+	static Quaternionf identity () { return Quaternionf (0.0F, 0.0F, 0.0F, 1.0F); }
+};
+
+bool CompareApproximately (const Quaternionf& q1, const Quaternionf& q2, float epsilon = Vector3f::epsilon);
+
+Quaternionf Lerp( const Quaternionf& q1, const Quaternionf& q2, float t );
+
+Quaternionf EXPORT_COREMODULE Slerp( const Quaternionf& q1, const Quaternionf& q2, float t );
+
+float Dot( const Quaternionf& q1, const Quaternionf& q2 );
+
+Vector3f EXPORT_COREMODULE QuaternionToEuler (const Quaternionf& quat);
+
+std::vector<Vector3f> GetEquivalentEulerAngles (const Quaternionf& quat);
+
+Quaternionf EulerToQuaternion (const Vector3f& euler);
+
+void EXPORT_COREMODULE QuaternionToMatrix (const Quaternionf& q, Matrix3x3f& m);
+
+void EXPORT_COREMODULE MatrixToQuaternion (const Matrix3x3f& m, Quaternionf& q);
+void EXPORT_COREMODULE MatrixToQuaternion (const Matrix4x4f& m, Quaternionf& q);
+
+void QuaternionToMatrix (const Quaternionf& q, Matrix4x4f& m);
+
+void QuaternionToAxisAngle (const Quaternionf& q, Vector3f* axis, float* targetAngle);
+
+Quaternionf AxisAngleToQuaternion (const Vector3f& axis, float angle);
+
+/// Generates a Right handed Quat from a look rotation. Returns if conversion was successful.
+bool LookRotationToQuaternion (const Vector3f& viewVec, const Vector3f& upVec, Quaternionf* res);
+
+
+inline Vector3f RotateVectorByQuat (const Quaternionf& lhs, const Vector3f& rhs)
+{
+//	Matrix3x3f m;
+//	QuaternionToMatrix (lhs, &m);
+//	Vector3f restest = m.MultiplyVector3 (rhs);
+	float x = lhs.x * 2.0F;
+	float y = lhs.y * 2.0F;
+	float z = lhs.z * 2.0F;
+	float xx = lhs.x * x;
+	float yy = lhs.y * y;
+	float zz = lhs.z * z;
+	float xy = lhs.x * y;
+	float xz = lhs.x * z;
+	float yz = lhs.y * z;
+	float wx = lhs.w * x;
+	float wy = lhs.w * y;
+	float wz = lhs.w * z;
+
+	Vector3f res;
+	res.x = (1.0f - (yy + zz)) * rhs.x + (xy - wz)          * rhs.y + (xz + wy)          * rhs.z;
+	res.y = (xy + wz)          * rhs.x + (1.0f - (xx + zz)) * rhs.y + (yz - wx)          * rhs.z;
+	res.z = (xz - wy)          * rhs.x + (yz + wx)          * rhs.y + (1.0f - (xx + yy)) * rhs.z;
+	
+//	AssertIf (!CompareApproximately (restest, res));
+	return res;
+}
+
+// operator overloads
+//  inlines
+
+inline Quaternionf::Quaternionf(float inX, float inY, float inZ, float inW)
+{
+	x = inX;
+	y = inY;
+	z = inZ;
+	w = inW;
+}
+
+template<class TransferFunction> inline
+void Quaternionf::Transfer (TransferFunction& transfer)
+{
+	transfer.AddMetaFlag (kTransferUsingFlowMappingStyle);
+	TRANSFER (x);
+	TRANSFER (y);
+	TRANSFER (z);
+	TRANSFER (w);
+}
+
+inline void Quaternionf::Set (float inX, float inY, float inZ, float inW)
+{
+	x = inX;
+	y = inY;
+	z = inZ;
+	w = inW;
+}
+
+inline void Quaternionf::Set (const Quaternionf& aQuat )
+{
+	x = aQuat.x;
+	y = aQuat.y;
+	z = aQuat.z;
+	w = aQuat.w;
+}
+
+inline Quaternionf Conjugate (const Quaternionf& q)
+{
+	return Quaternionf (-q.x, -q.y, -q.z, q.w);
+}
+
+inline Quaternionf Inverse (const Quaternionf& q)
+{
+	// Is it necessary to divide by SqrMagnitude???
+	Quaternionf res = Conjugate (q);
+	return res;
+}
+ 
+inline float Magnitude(const Quaternionf& q)
+{
+	return SqrtImpl (SqrMagnitude (q));
+}
+
+inline float SqrMagnitude(const Quaternionf& q)
+{
+	return Dot (q, q); 
+}
+
+inline Quaternionf& Quaternionf::operator+= (const Quaternionf& aQuat)
+{
+   x += aQuat.x;
+   y += aQuat.y;
+   z += aQuat.z;
+   w += aQuat.w;
+   return *this;
+}
+
+inline Quaternionf& Quaternionf::operator-= (const Quaternionf& aQuat)
+{
+   x -= aQuat.x;
+   y -= aQuat.y;
+   z -= aQuat.z;
+   w -= aQuat.w;
+   return *this;
+}
+
+inline Quaternionf& Quaternionf::operator *= (float aScalar)
+{
+	x *= aScalar;
+	y *= aScalar;
+	z *= aScalar;
+	w *= aScalar;
+	return *this;
+}
+
+inline Quaternionf&	Quaternionf::operator /= (const float     	aScalar)
+{
+	AssertIf (CompareApproximately (aScalar, 0.0F));
+	x /= aScalar;
+	y /= aScalar;
+	z /= aScalar;
+	w /= aScalar;
+	return *this;
+}
+
+inline Quaternionf&	Quaternionf::operator *= (const Quaternionf& 	rhs)
+{
+	float tempx = w*rhs.x + x*rhs.w + y*rhs.z - z*rhs.y;
+	float tempy = w*rhs.y + y*rhs.w + z*rhs.x - x*rhs.z;
+	float tempz = w*rhs.z + z*rhs.w + x*rhs.y - y*rhs.x;
+	float tempw = w*rhs.w - x*rhs.x - y*rhs.y - z*rhs.z;
+	x = tempx; y = tempy; z = tempz; w = tempw;
+	return *this;		
+}
+
+inline Quaternionf Lerp( const Quaternionf& q1, const Quaternionf& q2, float t )
+{
+	Quaternionf tmpQuat;
+	// if (dot < 0), q1 and q2 are more than 360 deg apart.
+	// The problem is that quaternions are 720deg of freedom.
+	// so we - all components when lerping
+	if (Dot (q1, q2) < 0.0F)
+	{
+		tmpQuat.Set(q1.x + t * (-q2.x - q1.x),
+		            q1.y + t * (-q2.y - q1.y),
+		            q1.z + t * (-q2.z - q1.z),
+		            q1.w + t * (-q2.w - q1.w));
+	}
+	else
+	{
+		tmpQuat.Set(q1.x + t * (q2.x - q1.x),
+		            q1.y + t * (q2.y - q1.y),
+		            q1.z + t * (q2.z - q1.z),
+		            q1.w + t * (q2.w - q1.w));
+	}
+	return Normalize (tmpQuat);
+} 
+
+inline float Dot( const Quaternionf& q1, const Quaternionf& q2 )
+{
+	return (q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w);
+}
+
+float AngularDistance (const Quaternionf& lhs, const Quaternionf& rhs);
+
+
+inline void QuaternionToAxisAngle (const Quaternionf& q, Vector3f* axis, float* targetAngle)
+{
+	AssertIf (! CompareApproximately(SqrMagnitude (q), 1.0F));
+	*targetAngle = 2.0f* acos(q.w);
+	if (CompareApproximately (*targetAngle, 0.0F))
+	{
+		*axis = Vector3f::xAxis;
+		return;
+	}
+		
+	float div = 1.0f / sqrt(1.0f - Sqr (q.w));
+	axis->Set( q.x*div, q.y*div, q.z*div );
+}
+
+inline Quaternionf AxisAngleToQuaternion (const Vector3f& axis, float angle)
+{
+	Quaternionf q;
+	AssertIf (!CompareApproximately (SqrMagnitude (axis), 1.0F));
+	float halfAngle = angle * 0.5F;
+	float s = sin (halfAngle);
+	
+	q.w = cos (halfAngle);
+	q.x = s * axis.x;
+	q.y = s * axis.y;
+	q.z = s * axis.z;
+	return q;
+}
+
+inline Quaternionf AngularVelocityToQuaternion (const Vector3f& axis, float deltaTime)
+{
+	float w = Magnitude(axis);
+	if (w > Vector3f::epsilon)
+	{
+		float v = deltaTime * w * 0.5f;
+		float q = cos(v);
+		float s = sin(v) / w;
+		
+		Quaternionf integrated;
+		integrated.w = q;
+		integrated.x = s * axis.x;
+		integrated.y = s * axis.y;
+		integrated.z = s * axis.z;
+
+		return NormalizeSafe(integrated);
+	}
+	else
+	{
+		return Quaternionf::identity();
+	}
+}
+
+inline Quaternionf AxisAngleToQuaternionSafe (const Vector3f& axis, float angle)
+{
+	Quaternionf q;
+	float mag = Magnitude (axis);
+	if (mag > 0.000001F)
+	{
+		float halfAngle = angle * 0.5F;
+		
+		q.w = cos (halfAngle);
+
+		float s = sin (halfAngle) / mag;
+		q.x = s * axis.x;
+		q.y = s * axis.y;
+		q.z = s * axis.z;
+		return q;
+	}
+	else
+	{
+		return Quaternionf::identity ();
+	}
+}
+
+// Generates a quaternion that rotates lhs into rhs.
+Quaternionf FromToQuaternionSafe (const Vector3f& lhs, const Vector3f& rhs);
+// from and to are assumed to be normalized
+Quaternionf FromToQuaternion (const Vector3f& from, const Vector3f& to);
+
+
+inline bool CompareApproximately (const Quaternionf& q1, const Quaternionf& q2, float epsilon)
+{
+	//return SqrMagnitude (q1 - q2) < epsilon * epsilon;
+	return (SqrMagnitude (q1 - q2) < epsilon * epsilon) || (SqrMagnitude (q1 + q2) < epsilon * epsilon);
+	//return Abs (Dot (q1, q2)) > (1 - epsilon * epsilon);
+}
+
+inline Quaternionf NormalizeSafe (const Quaternionf& q)
+{
+	float mag = Magnitude (q);
+	if (mag < Vector3f::epsilon)
+		return Quaternionf::identity ();	
+	else
+		return q / mag;
+}
+
+inline Quaternionf NormalizeFastEpsilonZero (const Quaternionf& q)
+{
+	float m = SqrMagnitude (q);
+	if (m < Vector3f::epsilon)
+		return Quaternionf(0.0F, 0.0F, 0.0F, 0.0F);	
+	else
+		return q * FastInvSqrt(m);
+}
+
+
+inline bool IsFinite (const Quaternionf& f)
+{
+	return IsFinite(f.x) & IsFinite(f.y) & IsFinite(f.z) & IsFinite(f.w);
+}
+
+
+#endif