1 files changed, 103 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3Common/shared/b3Quat.h b/thirdparty/bullet/Bullet3Common/shared/b3Quat.h
new file mode 100644
index 0000000000..f262d5e08f
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Common/shared/b3Quat.h
@@ -0,0 +1,103 @@
+#ifndef B3_QUAT_H
+#define B3_QUAT_H
+
+#include "Bullet3Common/shared/b3PlatformDefinitions.h"
+#include "Bullet3Common/shared/b3Float4.h"
+
+#ifdef __cplusplus
+	#include "Bullet3Common/b3Quaternion.h"
+	#include "Bullet3Common/b3Transform.h"
+
+	#define b3Quat b3Quaternion
+	#define b3QuatConstArg const b3Quaternion&
+	inline b3Quat b3QuatInverse(b3QuatConstArg orn)
+	{
+		return orn.inverse();
+	}
+
+	inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg  orientation)
+	{
+		b3Transform tr;
+		tr.setOrigin(translation);
+		tr.setRotation(orientation);
+		return tr(point);
+	}
+
+#else
+	typedef float4	b3Quat;
+	#define b3QuatConstArg const b3Quat
+	
+	
+inline float4 b3FastNormalize4(float4 v)
+{
+	v = (float4)(v.xyz,0.f);
+	return fast_normalize(v);
+}
+	
+inline b3Quat b3QuatMul(b3Quat a, b3Quat b);
+inline b3Quat b3QuatNormalized(b3QuatConstArg in);
+inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);
+inline b3Quat b3QuatInvert(b3QuatConstArg q);
+inline b3Quat b3QuatInverse(b3QuatConstArg q);
+
+inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)
+{
+	b3Quat ans;
+	ans = b3Cross3( a, b );
+	ans += a.w*b+b.w*a;
+//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
+	ans.w = a.w*b.w - b3Dot3F4(a, b);
+	return ans;
+}
+
+inline b3Quat b3QuatNormalized(b3QuatConstArg in)
+{
+	b3Quat q;
+	q=in;
+	//return b3FastNormalize4(in);
+	float len = native_sqrt(dot(q, q));
+	if(len > 0.f)
+	{
+		q *= 1.f / len;
+	}
+	else
+	{
+		q.x = q.y = q.z = 0.f;
+		q.w = 1.f;
+	}
+	return q;
+}
+inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
+{
+	b3Quat qInv = b3QuatInvert( q );
+	float4 vcpy = vec;
+	vcpy.w = 0.f;
+	float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);
+	return out;
+}
+
+
+
+inline b3Quat b3QuatInverse(b3QuatConstArg q)
+{
+	return (b3Quat)(-q.xyz, q.w);
+}
+
+inline b3Quat b3QuatInvert(b3QuatConstArg q)
+{
+	return (b3Quat)(-q.xyz, q.w);
+}
+
+inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)
+{
+	return b3QuatRotate( b3QuatInvert( q ), vec );
+}
+
+inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg  orientation)
+{
+	return b3QuatRotate( orientation, point ) + (translation);
+}
+	
+#endif 
+
+#endif //B3_QUAT_H