1 files changed, 253 insertions, 0 deletions
diff --git a/Runtime/Math/Simd/quaternion.h b/Runtime/Math/Simd/quaternion.h
new file mode 100644
index 0000000..ccb5077
--- /dev/null
+++ b/Runtime/Math/Simd/quaternion.h
@@ -0,0 +1,253 @@
+#ifndef SIMD_QUATERNION_H
+#define SIMD_QUATERNION_H
+
+#include "Runtime/Math/Simd/math.h"
+
+namespace math
+{
+
+static inline float4 quatIdentity()
+{
+	cvec4f(id, 0,0,0,1);	
+	return float4(id);
+}
+
+static inline float4 quatConj(float4 const& q)
+{
+	cvec4f(conj, -1,-1,-1,1);	
+	return float4(Vmul(q.eval(), conj));
+}
+
+static inline float4 quatMul(float4 const& a, float4 const& b)
+{		
+	return quatConj(a.zxyw()*b.yzxw() - a.ywzx()*b.zywx() - a.wyxz()*b.xwyz() - a.xzwy()*b.wxzy());
+}
+
+static inline float4 quatMulVec(float4 const& q, float4 const& v1)
+{
+	const float4 v = math::vector(v1 + v1);
+	const float4 qv = q*v;
+	return q.w()*(cross(q, v) + q.w()*v) + q*(qv + qv.yzxw() + qv.zxyw()) + quatConj(v1);
+}
+
+static inline float4 quatLerp(float4 const& p, float4 const& q, float1 const& blend)
+{
+   return normalize(p + blend*(q*sgn(dot(p, q)) - p));
+}
+
+static inline float4 quatArcRotate(float4 const& a, float4 const& b)
+{
+	float4 q = cross(a, b);
+	q.w() = dot(a, b) + math::sqrt( float1(dot(a)*dot(b)) );
+	return q;
+}
+
+static inline float4 quatArcRotateX(float4 const& n)
+{
+	return float4(float1::zero(),-n.z(),n.y(),n.x()+float1::one());
+}
+
+static inline float4 quatXcos(float4 const &qn)
+{
+	const float4 qw = qn.w()*qn - float4(0, 0, 0, .5f);
+	const float4 u = qn.x()*qn + float4(1, 1, -1, -1) * qw.wzyx();
+	return u + u;
+}
+
+static inline float4 quatYcos(float4 const &qn)
+{
+	const float4 qw = qn.w()*qn - float4(0, 0, 0, .5f);
+	const float4 v = qn.y()*qn + float4(-1, 1, 1, -1)*qw.zwxy();
+	return v + v;
+}
+
+static inline float4 quatZcos(float4 const &qn)
+{
+	const float4 qw = qn.w()*qn - float4(0, 0, 0, .5f);
+	const float4 w = qn.z()*qn + float4(1, -1, 1, -1)*qw.yxwz();
+
+	return w + w;
+}
+
+static inline float4 quatEulerToQuat(float4 const& euler)
+{
+	float4 s, c; sincos( float1(0.5f)*euler, s, c);
+
+	const float4 t = float4(s.x()*c.z(), s.x()*s.z(), c.x()*s.z(), c.x()*c.z());
+
+	constant_float4( mask, -1.f, 1.f, -1.f, 1.f);
+	return c.y()*t + s.y()*mask*t.zwxy();
+}
+
+static inline float4 quatQuatToEuler(float4 const& q)
+{
+	float4 euler;
+
+	const float4 x = q.x()*q;
+	const float4 y = q.y()*q;
+	const float1 discr = x.z() - y.w();
+
+	if(discr >= float1(.5f - M_EPSF)) 
+	{
+		float1 _y = x.w() - y.z();
+		float1 _x = -x.z() - y.w();
+		
+		euler = float4( atan2( _y.tofloat(), _x.tofloat() ), -M_PI_2f, 0.f, 0.f);
+	} 
+	else 
+	{
+		const float4 w = q.wwwz()*q.wwzz() - float4(.5f, 0.f, 0.f, 0.f);
+		if(discr <= float1(M_EPSF - .5f)) 
+		{
+			float1 _y = x.y() - w.z();
+			float1 _x = y.y() + w.x();
+			euler = float4( atan2( _y.tofloat(), _x.tofloat() ), M_PI_2f, 0.f, 0.f);
+		} 
+		else 
+		{
+			float1 _yX = x.w() + y.z();
+			float1 _xX = w.w() + w.x();
+			float1 discr2 = discr + discr;
+			float1 _yZ = x.y() + w.z();
+			float1 _xZ = x.x() + w.x();
+
+			euler = float4( atan2( _yX.tofloat(), _xX.tofloat() ), -asin( discr2.tofloat() ), atan2( _yZ.tofloat(), _xZ.tofloat() ), 0.f);
+		}
+	}
+	return euler;
+}
+
+// get unit quaternion from rotation matrix
+static inline float4 quatMatrixToQuat(float4 const& u, float4 const& v, float4 const& w)
+{
+	float4 q;
+	if(u.x() >= float1::zero()) 
+	{
+		const float1 t = v.y() + w.z();
+		if(t >= float1::zero()) 
+		{			
+			float1 x(v.z() - w.y());
+			float1 y(w.x() - u.z());
+			float1 z(u.y() - v.x());
+			float1 ww(float1::one() + u.x() + t);
+			q = float4(x, y, z, ww);
+			// Android doesn't like this expression, it does generate the wrong assembly
+			//q = float4(v.z() - w.y(), w.x() - u.z(), u.y() - v.x(), float1::one() + u.x() + t);
+		} 
+		else 
+		{
+			float1 x(float1::one() + u.x() - t);
+			float1 y(u.y() + v.x());
+			float1 z(w.x() + u.z());
+			float1 ww(v.z() - w.y());
+			q = float4(x, y, z, ww);
+			// Android doesn't like this expression, it does generate the wrong assembly
+			//q = float4(float1::one() + u.x() - t, u.y() + v.x(), w.x() + u.z(), v.z() - w.y());
+		}
+	} 
+	else 
+	{
+		const float1 t = v.y() - w.z();
+		if(t >= float1::zero()) 
+		{
+			float1 x(u.y() + v.x());
+			float1 y(float1::one() - u.x() + t);
+			float1 z(v.z() + w.y());
+			float1 ww(w.x() - u.z());
+			q = float4(x, y, z, ww);
+			// Android doesn't like this expression, it does generate the wrong assembly
+			//q = float4(u.y() + v.x(), float1::one() - u.x() + t, v.z() + w.y(), w.x() - u.z());
+		} 
+		else 
+		{
+			float1 x(w.x() + u.z());
+			float1 y(v.z() + w.y());
+			float1 z(float1::one() - u.x() - t);
+			float1 ww(u.y() - v.x());
+			q = float4(x, y, z, ww);
+			// Android doesn't like this expression, it does generate the wrong assembly
+			//q = float4(w.x() + u.z(), v.z() + w.y(), float1::one() - u.x() - t, u.y() - v.x());
+		}
+	}
+	return normalize(q);
+}
+
+static inline float4 quatProjOnYPlane(float4 const& q)
+{
+	constant_float4(yMask, 0,1,0,0);
+	constant_float4(ywMask, 0,1,0,1);
+
+	const float4 lQAlignUp = quatArcRotate(quatYcos(q), yMask );
+	
+	return normalize( quatMul(lQAlignUp,q) * ywMask);
+}
+
+static inline float4 quatClamp(const float4 &q,float maxAngle)
+{
+	float4 ret = q;
+
+	float1 halfCosMaxAnlge = float1(math::cos(0.5f*maxAngle));
+
+	float4 qn = normalize(q);
+	qn = cond(qn.w() < float1::zero(), -qn, qn);
+
+	if(qn.w() < halfCosMaxAnlge)
+	{
+		float1 fact = (halfCosMaxAnlge - qn.w()) / halfCosMaxAnlge;
+
+		ret = qn * (float1::one() - fact); 
+		ret.w() = lerp(halfCosMaxAnlge, float1::one(), fact);
+	}
+
+	return ret;
+}
+
+static inline float4 quatWeight(float4 const& q, float1 const& w)
+{
+   return normalize(float4(q.x()*w,q.y()*w,q.z()*w,q.w()));
+}
+
+static inline float4 quat2Qtan(float4 const& q)
+{
+	float1 w = q.w();	
+	w = cond(w == float1::zero(), float1(M_EPSF), w);
+	return math::vector(q/w);
+}
+
+static inline float4 qtan2Quat(float4 const& q)
+{
+	float4 qn = q;
+	qn.w() = float1::one();
+	return normalize(qn);
+}
+
+static inline float4 ZYRoll2Quat(float4 const& zyroll)
+{
+	return normalize(float4(zyroll.x(),zyroll.y()+zyroll.x()*zyroll.z(),zyroll.z()-zyroll.x()*zyroll.y(), float1::one())); 
+}
+
+static inline float4 quat2ZYRoll(float4 const& q)
+{
+	const float4 qtan = quat2Qtan(q);
+	const float1 qtanx = qtan.x();
+	const float1 x2p1 = float1::one()+qtanx*qtanx;
+	return float4(qtanx,(qtan.y()-qtanx*qtan.z())/x2p1,(qtan.z()+qtanx*qtan.y())/x2p1,float1::zero());	
+}
+
+static inline float4 RollZY2Quat(float4 const& zyroll)
+{
+	return normalize(float4(zyroll.x(),zyroll.y()-zyroll.x()*zyroll.z(),zyroll.z()+zyroll.x()*zyroll.y(),float1::one())); 
+}
+
+static inline float4 quat2RollZY(float4 const& q)
+{
+	const float4 qtan = quat2Qtan(q);
+	const float1 qtanx = qtan.x();
+	const float1 x2p1 = float1::one()+qtanx*qtanx;
+	return float4(qtanx,(qtan.y()+qtanx*qtan.z())/x2p1,(qtan.z()-qtanx*qtan.y())/x2p1,float1::zero());	
+}
+
+}
+
+#endif
+