Vendor thirdparty Bullet source for upcoming physics server backend

1 files changed, 1344 insertions, 0 deletions

diff --git a/thirdparty/bullet/src/Bullet3Common/b3Vector3.h b/thirdparty/bullet/src/Bullet3Common/b3Vector3.h
new file mode 100644
index 0000000000..16ec02b0ed
--- /dev/null
+++ b/thirdparty/bullet/src/Bullet3Common/b3Vector3.h
@@ -0,0 +1,1344 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef B3_VECTOR3_H
+#define B3_VECTOR3_H
+
+//#include <stdint.h>
+#include "b3Scalar.h"
+#include "b3MinMax.h"
+#include "b3AlignedAllocator.h"
+
+#ifdef B3_USE_DOUBLE_PRECISION
+#define b3Vector3Data b3Vector3DoubleData
+#define b3Vector3DataName "b3Vector3DoubleData"
+#else
+#define b3Vector3Data b3Vector3FloatData
+#define b3Vector3DataName "b3Vector3FloatData"
+#endif //B3_USE_DOUBLE_PRECISION
+
+#if defined B3_USE_SSE
+
+//typedef  uint32_t __m128i __attribute__ ((vector_size(16)));
+
+#ifdef _MSC_VER
+#pragma warning(disable: 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
+#endif
+
+
+#define B3_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x))
+//#define b3_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
+#define b3_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) )
+#define b3_splat3_ps( _a, _i ) b3_pshufd_ps((_a), B3_SHUFFLE(_i,_i,_i, 3) )
+#define b3_splat_ps( _a, _i )  b3_pshufd_ps((_a), B3_SHUFFLE(_i,_i,_i,_i) )
+
+#define b3v3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
+#define b3vAbsMask (_mm_set_epi32( 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
+#define b3vFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF))
+#define b3v3AbsfMask b3CastiTo128f(b3v3AbsiMask)
+#define b3vFFF0fMask b3CastiTo128f(b3vFFF0Mask)
+#define b3vxyzMaskf b3vFFF0fMask
+#define b3vAbsfMask b3CastiTo128f(b3vAbsMask)
+
+
+
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3vMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f};
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3v1110) = {1.0f, 1.0f, 1.0f, 0.0f};
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3vHalf) = {0.5f, 0.5f, 0.5f, 0.5f};
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3v1_5)  = {1.5f, 1.5f, 1.5f, 1.5f};
+
+#endif
+
+#ifdef B3_USE_NEON
+
+const float32x4_t B3_ATTRIBUTE_ALIGNED16(b3vMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f};
+const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3vFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0};
+const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3vAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
+const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3v3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0};
+
+#endif
+
+class b3Vector3;
+class b3Vector4;
+
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+//#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
+inline b3Vector3 b3MakeVector3( b3SimdFloat4 v);
+inline b3Vector4 b3MakeVector4( b3SimdFloat4 vec);
+#endif
+
+inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z);
+inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z, b3Scalar w);
+inline b3Vector4 b3MakeVector4(b3Scalar x,b3Scalar y,b3Scalar z,b3Scalar w);
+
+
+/**@brief b3Vector3 can be used to represent 3D points and vectors.
+ * It has an un-used w component to suit 16-byte alignment when b3Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
+ * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
+ */
+B3_ATTRIBUTE_ALIGNED16(class) b3Vector3
+{
+public:
+#if defined (B3_USE_SSE) || defined(B3_USE_NEON) // _WIN32 || ARM
+        union {
+            b3SimdFloat4      mVec128;
+            float	m_floats[4];
+			struct {float x,y,z,w;};
+
+        };
+#else
+	union
+	{
+        	float	m_floats[4];
+			struct {float	x,y,z,w;};
+	};
+#endif
+
+
+public:
+
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+#if defined (B3_USE_SSE) || defined(B3_USE_NEON) // _WIN32 || ARM
+
+	/*B3_FORCE_INLINE		b3Vector3()
+	{
+	}
+	*/
+
+    B3_FORCE_INLINE	b3SimdFloat4	get128() const
+    {
+        return mVec128;
+    }
+    B3_FORCE_INLINE	void	set128(b3SimdFloat4 v128)
+    {
+        mVec128 = v128;
+    }
+#endif
+
+	public:
+
+
+
+/**@brief Add a vector to this one
+ * @param The vector to add to this one */
+	B3_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		mVec128 = _mm_add_ps(mVec128, v.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vaddq_f32(mVec128, v.mVec128);
+#else
+		m_floats[0] += v.m_floats[0];
+		m_floats[1] += v.m_floats[1];
+		m_floats[2] += v.m_floats[2];
+#endif
+		return *this;
+	}
+
+
+  /**@brief Subtract a vector from this one
+   * @param The vector to subtract */
+	B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		mVec128 = _mm_sub_ps(mVec128, v.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vsubq_f32(mVec128, v.mVec128);
+#else
+		m_floats[0] -= v.m_floats[0];
+		m_floats[1] -= v.m_floats[1];
+		m_floats[2] -= v.m_floats[2];
+#endif
+		return *this;
+	}
+
+  /**@brief Scale the vector
+   * @param s Scale factor */
+	B3_FORCE_INLINE b3Vector3& operator*=(const b3Scalar& s)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		__m128	vs = _mm_load_ss(&s);	//	(S 0 0 0)
+		vs = b3_pshufd_ps(vs, 0x80);	//	(S S S 0.0)
+		mVec128 = _mm_mul_ps(mVec128, vs);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmulq_n_f32(mVec128, s);
+#else
+		m_floats[0] *= s;
+		m_floats[1] *= s;
+		m_floats[2] *= s;
+#endif
+		return *this;
+	}
+
+  /**@brief Inversely scale the vector
+   * @param s Scale factor to divide by */
+	B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
+	{
+		b3FullAssert(s != b3Scalar(0.0));
+
+#if 0 //defined(B3_USE_SSE_IN_API)
+// this code is not faster !
+		__m128 vs = _mm_load_ss(&s);
+		vs = _mm_div_ss(b3v1110, vs);
+		vs = b3_pshufd_ps(vs, 0x00);	//	(S S S S)
+
+		mVec128 = _mm_mul_ps(mVec128, vs);
+
+		return *this;
+#else
+		return *this *= b3Scalar(1.0) / s;
+#endif
+	}
+
+  /**@brief Return the dot product
+   * @param v The other vector in the dot product */
+	B3_FORCE_INLINE b3Scalar dot(const b3Vector3& v) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		__m128 vd = _mm_mul_ps(mVec128, v.mVec128);
+		__m128 z = _mm_movehl_ps(vd, vd);
+		__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
+		vd = _mm_add_ss(vd, y);
+		vd = _mm_add_ss(vd, z);
+		return _mm_cvtss_f32(vd);
+#elif defined(B3_USE_NEON)
+		float32x4_t vd = vmulq_f32(mVec128, v.mVec128);
+		float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));
+		x = vadd_f32(x, vget_high_f32(vd));
+		return vget_lane_f32(x, 0);
+#else
+		return	m_floats[0] * v.m_floats[0] +
+				m_floats[1] * v.m_floats[1] +
+				m_floats[2] * v.m_floats[2];
+#endif
+	}
+
+  /**@brief Return the length of the vector squared */
+	B3_FORCE_INLINE b3Scalar length2() const
+	{
+		return dot(*this);
+	}
+
+  /**@brief Return the length of the vector */
+	B3_FORCE_INLINE b3Scalar length() const
+	{
+		return b3Sqrt(length2());
+	}
+
+  /**@brief Return the distance squared between the ends of this and another vector
+   * This is symantically treating the vector like a point */
+	B3_FORCE_INLINE b3Scalar distance2(const b3Vector3& v) const;
+
+  /**@brief Return the distance between the ends of this and another vector
+   * This is symantically treating the vector like a point */
+	B3_FORCE_INLINE b3Scalar distance(const b3Vector3& v) const;
+
+	B3_FORCE_INLINE b3Vector3& safeNormalize()
+	{
+		b3Scalar l2 = length2();
+		//triNormal.normalize();
+		if (l2 >= B3_EPSILON*B3_EPSILON)
+		{
+			(*this) /= b3Sqrt(l2);
+		}
+		else
+		{
+			setValue(1, 0, 0);
+		}
+		return *this;
+	}
+
+  /**@brief Normalize this vector
+   * x^2 + y^2 + z^2 = 1 */
+	B3_FORCE_INLINE b3Vector3& normalize()
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+        // dot product first
+		__m128 vd = _mm_mul_ps(mVec128, mVec128);
+		__m128 z = _mm_movehl_ps(vd, vd);
+		__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
+		vd = _mm_add_ss(vd, y);
+		vd = _mm_add_ss(vd, z);
+
+        #if 0
+        vd = _mm_sqrt_ss(vd);
+		vd = _mm_div_ss(b3v1110, vd);
+		vd = b3_splat_ps(vd, 0x80);
+		mVec128 = _mm_mul_ps(mVec128, vd);
+        #else
+
+        // NR step 1/sqrt(x) - vd is x, y is output
+        y = _mm_rsqrt_ss(vd); // estimate
+
+        //  one step NR
+        z = b3v1_5;
+        vd = _mm_mul_ss(vd, b3vHalf); // vd * 0.5
+        //x2 = vd;
+        vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0
+        vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0
+        z = _mm_sub_ss(z, vd);  // 1.5 - vd * 0.5 * y0 * y0
+
+        y = _mm_mul_ss(y, z);   // y0 * (1.5 - vd * 0.5 * y0 * y0)
+
+		y = b3_splat_ps(y, 0x80);
+		mVec128 = _mm_mul_ps(mVec128, y);
+
+        #endif
+
+
+		return *this;
+#else
+		return *this /= length();
+#endif
+	}
+
+  /**@brief Return a normalized version of this vector */
+	B3_FORCE_INLINE b3Vector3 normalized() const;
+
+  /**@brief Return a rotated version of this vector
+   * @param wAxis The axis to rotate about
+   * @param angle The angle to rotate by */
+	B3_FORCE_INLINE b3Vector3 rotate( const b3Vector3& wAxis, const b3Scalar angle ) const;
+
+  /**@brief Return the angle between this and another vector
+   * @param v The other vector */
+	B3_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const
+	{
+		b3Scalar s = b3Sqrt(length2() * v.length2());
+		b3FullAssert(s != b3Scalar(0.0));
+		return b3Acos(dot(v) / s);
+	}
+
+  /**@brief Return a vector will the absolute values of each element */
+	B3_FORCE_INLINE b3Vector3 absolute() const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		return b3MakeVector3(_mm_and_ps(mVec128, b3v3AbsfMask));
+#elif defined(B3_USE_NEON)
+		return b3Vector3(vabsq_f32(mVec128));
+#else
+		return b3MakeVector3(
+			b3Fabs(m_floats[0]),
+			b3Fabs(m_floats[1]),
+			b3Fabs(m_floats[2]));
+#endif
+	}
+
+  /**@brief Return the cross product between this and another vector
+   * @param v The other vector */
+	B3_FORCE_INLINE b3Vector3 cross(const b3Vector3& v) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		__m128	T, V;
+
+		T = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 3));	//	(Y Z X 0)
+		V = b3_pshufd_ps(v.mVec128, B3_SHUFFLE(1, 2, 0, 3));	//	(Y Z X 0)
+
+		V = _mm_mul_ps(V, mVec128);
+		T = _mm_mul_ps(T, v.mVec128);
+		V = _mm_sub_ps(V, T);
+
+		V = b3_pshufd_ps(V, B3_SHUFFLE(1, 2, 0, 3));
+		return b3MakeVector3(V);
+#elif defined(B3_USE_NEON)
+		float32x4_t T, V;
+		// form (Y, Z, X, _) of mVec128 and v.mVec128
+		float32x2_t Tlow = vget_low_f32(mVec128);
+		float32x2_t Vlow = vget_low_f32(v.mVec128);
+		T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow);
+		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow);
+
+		V = vmulq_f32(V, mVec128);
+		T = vmulq_f32(T, v.mVec128);
+		V = vsubq_f32(V, T);
+		Vlow = vget_low_f32(V);
+		// form (Y, Z, X, _);
+		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
+		V = (float32x4_t)vandq_s32((int32x4_t)V, b3vFFF0Mask);
+
+		return b3Vector3(V);
+#else
+		return b3MakeVector3(
+			m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
+			m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+			m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
+#endif
+	}
+
+	B3_FORCE_INLINE b3Scalar triple(const b3Vector3& v1, const b3Vector3& v2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		// cross:
+		__m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, B3_SHUFFLE(1, 2, 0, 3));	//	(Y Z X 0)
+		__m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, B3_SHUFFLE(1, 2, 0, 3));	//	(Y Z X 0)
+
+		V = _mm_mul_ps(V, v1.mVec128);
+		T = _mm_mul_ps(T, v2.mVec128);
+		V = _mm_sub_ps(V, T);
+
+		V = _mm_shuffle_ps(V, V, B3_SHUFFLE(1, 2, 0, 3));
+
+		// dot:
+		V = _mm_mul_ps(V, mVec128);
+		__m128 z = _mm_movehl_ps(V, V);
+		__m128 y = _mm_shuffle_ps(V, V, 0x55);
+		V = _mm_add_ss(V, y);
+		V = _mm_add_ss(V, z);
+		return _mm_cvtss_f32(V);
+
+#elif defined(B3_USE_NEON)
+		// cross:
+		float32x4_t T, V;
+		// form (Y, Z, X, _) of mVec128 and v.mVec128
+		float32x2_t Tlow = vget_low_f32(v1.mVec128);
+		float32x2_t Vlow = vget_low_f32(v2.mVec128);
+		T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow);
+		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow);
+
+		V = vmulq_f32(V, v1.mVec128);
+		T = vmulq_f32(T, v2.mVec128);
+		V = vsubq_f32(V, T);
+		Vlow = vget_low_f32(V);
+		// form (Y, Z, X, _);
+		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
+
+		// dot:
+		V = vmulq_f32(mVec128, V);
+		float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));
+		x = vadd_f32(x, vget_high_f32(V));
+		return vget_lane_f32(x, 0);
+#else
+		return
+			m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
+			m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
+			m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
+#endif
+	}
+
+  /**@brief Return the axis with the smallest value
+   * Note return values are 0,1,2 for x, y, or z */
+	B3_FORCE_INLINE int minAxis() const
+	{
+		return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
+	}
+
+  /**@brief Return the axis with the largest value
+   * Note return values are 0,1,2 for x, y, or z */
+	B3_FORCE_INLINE int maxAxis() const
+	{
+		return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
+	}
+
+	B3_FORCE_INLINE int furthestAxis() const
+	{
+		return absolute().minAxis();
+	}
+
+	B3_FORCE_INLINE int closestAxis() const
+	{
+		return absolute().maxAxis();
+	}
+
+
+	B3_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		__m128	vrt = _mm_load_ss(&rt);	//	(rt 0 0 0)
+		b3Scalar s = b3Scalar(1.0) - rt;
+		__m128	vs = _mm_load_ss(&s);	//	(S 0 0 0)
+		vs = b3_pshufd_ps(vs, 0x80);	//	(S S S 0.0)
+		__m128 r0 = _mm_mul_ps(v0.mVec128, vs);
+		vrt = b3_pshufd_ps(vrt, 0x80);	//	(rt rt rt 0.0)
+		__m128 r1 = _mm_mul_ps(v1.mVec128, vrt);
+		__m128 tmp3 = _mm_add_ps(r0,r1);
+		mVec128 = tmp3;
+#elif defined(B3_USE_NEON)
+		float32x4_t vl = vsubq_f32(v1.mVec128, v0.mVec128);
+		vl = vmulq_n_f32(vl, rt);
+		mVec128 = vaddq_f32(vl, v0.mVec128);
+#else
+		b3Scalar s = b3Scalar(1.0) - rt;
+		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+		m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
+		//don't do the unused w component
+		//		m_co[3] = s * v0[3] + rt * v1[3];
+#endif
+	}
+
+  /**@brief Return the linear interpolation between this and another vector
+   * @param v The other vector
+   * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
+	B3_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		__m128	vt = _mm_load_ss(&t);	//	(t 0 0 0)
+		vt = b3_pshufd_ps(vt, 0x80);	//	(rt rt rt 0.0)
+		__m128 vl = _mm_sub_ps(v.mVec128, mVec128);
+		vl = _mm_mul_ps(vl, vt);
+		vl = _mm_add_ps(vl, mVec128);
+
+		return b3MakeVector3(vl);
+#elif defined(B3_USE_NEON)
+		float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
+		vl = vmulq_n_f32(vl, t);
+		vl = vaddq_f32(vl, mVec128);
+
+		return b3Vector3(vl);
+#else
+		return
+			b3MakeVector3(	m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+						m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+						m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
+#endif
+	}
+
+  /**@brief Elementwise multiply this vector by the other
+   * @param v The other vector */
+	B3_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		mVec128 = _mm_mul_ps(mVec128, v.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmulq_f32(mVec128, v.mVec128);
+#else
+		m_floats[0] *= v.m_floats[0];
+		m_floats[1] *= v.m_floats[1];
+		m_floats[2] *= v.m_floats[2];
+#endif
+		return *this;
+	}
+
+	 /**@brief Return the x value */
+		B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
+/**@brief Return the w value */
+		B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
+
+  /**@brief Set the x value */
+		B3_FORCE_INLINE void	setX(b3Scalar _x) { m_floats[0] = _x;};
+  /**@brief Set the y value */
+		B3_FORCE_INLINE void	setY(b3Scalar _y) { m_floats[1] = _y;};
+  /**@brief Set the z value */
+		B3_FORCE_INLINE void	setZ(b3Scalar _z) { m_floats[2] = _z;};
+  /**@brief Set the w value */
+		B3_FORCE_INLINE void	setW(b3Scalar _w) { m_floats[3] = _w;};
+
+	//B3_FORCE_INLINE b3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}
+	//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	B3_FORCE_INLINE	operator       b3Scalar *()       { return &m_floats[0]; }
+	B3_FORCE_INLINE	operator const b3Scalar *() const { return &m_floats[0]; }
+
+	B3_FORCE_INLINE	bool	operator==(const b3Vector3& other) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+        return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
+#else
+		return ((m_floats[3]==other.m_floats[3]) &&
+                (m_floats[2]==other.m_floats[2]) &&
+                (m_floats[1]==other.m_floats[1]) &&
+                (m_floats[0]==other.m_floats[0]));
+#endif
+	}
+
+	B3_FORCE_INLINE	bool	operator!=(const b3Vector3& other) const
+	{
+		return !(*this == other);
+	}
+
+  /**@brief Set each element to the max of the current values and the values of another b3Vector3
+   * @param other The other b3Vector3 to compare with
+   */
+	B3_FORCE_INLINE void	setMax(const b3Vector3& other)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		mVec128 = _mm_max_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmaxq_f32(mVec128, other.mVec128);
+#else
+		b3SetMax(m_floats[0], other.m_floats[0]);
+		b3SetMax(m_floats[1], other.m_floats[1]);
+		b3SetMax(m_floats[2], other.m_floats[2]);
+		b3SetMax(m_floats[3], other.m_floats[3]);
+#endif
+	}
+
+  /**@brief Set each element to the min of the current values and the values of another b3Vector3
+   * @param other The other b3Vector3 to compare with
+   */
+	B3_FORCE_INLINE void	setMin(const b3Vector3& other)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		mVec128 = _mm_min_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vminq_f32(mVec128, other.mVec128);
+#else
+		b3SetMin(m_floats[0], other.m_floats[0]);
+		b3SetMin(m_floats[1], other.m_floats[1]);
+		b3SetMin(m_floats[2], other.m_floats[2]);
+		b3SetMin(m_floats[3], other.m_floats[3]);
+#endif
+	}
+
+	B3_FORCE_INLINE void 	setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+	{
+		m_floats[0]=_x;
+		m_floats[1]=_y;
+		m_floats[2]=_z;
+		m_floats[3] = b3Scalar(0.f);
+	}
+
+	void	getSkewSymmetricMatrix(b3Vector3* v0,b3Vector3* v1,b3Vector3* v2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+
+		__m128 V  = _mm_and_ps(mVec128, b3vFFF0fMask);
+		__m128 V0 = _mm_xor_ps(b3vMzeroMask, V);
+		__m128 V2 = _mm_movelh_ps(V0, V);
+
+		__m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
+
+        V0 = _mm_shuffle_ps(V0, V, 0xDB);
+		V2 = _mm_shuffle_ps(V2, V, 0xF9);
+
+		v0->mVec128 = V0;
+		v1->mVec128 = V1;
+		v2->mVec128 = V2;
+#else
+		v0->setValue(0.		,-getZ()		,getY());
+		v1->setValue(getZ()	,0.			,-getX());
+		v2->setValue(-getY()	,getX()	,0.);
+#endif
+	}
+
+	void setZero()
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
+#elif defined(B3_USE_NEON)
+		int32x4_t vi = vdupq_n_s32(0);
+		mVec128 = vreinterpretq_f32_s32(vi);
+#else
+		setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+#endif
+	}
+
+	B3_FORCE_INLINE bool isZero() const
+	{
+		return m_floats[0] == b3Scalar(0) && m_floats[1] == b3Scalar(0) && m_floats[2] == b3Scalar(0);
+	}
+
+	B3_FORCE_INLINE bool fuzzyZero() const
+	{
+		return length2() < B3_EPSILON;
+	}
+
+	B3_FORCE_INLINE	void	serialize(struct	b3Vector3Data& dataOut) const;
+
+	B3_FORCE_INLINE	void	deSerialize(const struct	b3Vector3Data& dataIn);
+
+	B3_FORCE_INLINE	void	serializeFloat(struct	b3Vector3FloatData& dataOut) const;
+
+	B3_FORCE_INLINE	void	deSerializeFloat(const struct	b3Vector3FloatData& dataIn);
+
+	B3_FORCE_INLINE	void	serializeDouble(struct	b3Vector3DoubleData& dataOut) const;
+
+	B3_FORCE_INLINE	void	deSerializeDouble(const struct	b3Vector3DoubleData& dataIn);
+
+        /**@brief returns index of maximum dot product between this and vectors in array[]
+         * @param array The other vectors
+         * @param array_count The number of other vectors
+         * @param dotOut The maximum dot product */
+        B3_FORCE_INLINE   long    maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
+
+        /**@brief returns index of minimum dot product between this and vectors in array[]
+         * @param array The other vectors
+         * @param array_count The number of other vectors
+         * @param dotOut The minimum dot product */
+        B3_FORCE_INLINE   long    minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const;
+
+    /* create a vector as  b3Vector3( this->dot( b3Vector3 v0 ), this->dot( b3Vector3 v1), this->dot( b3Vector3 v2 ))  */
+    B3_FORCE_INLINE b3Vector3  dot3( const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2 ) const
+    {
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+
+        __m128 a0 = _mm_mul_ps( v0.mVec128, this->mVec128 );
+        __m128 a1 = _mm_mul_ps( v1.mVec128, this->mVec128 );
+        __m128 a2 = _mm_mul_ps( v2.mVec128, this->mVec128 );
+        __m128 b0 = _mm_unpacklo_ps( a0, a1 );
+        __m128 b1 = _mm_unpackhi_ps( a0, a1 );
+        __m128 b2 = _mm_unpacklo_ps( a2, _mm_setzero_ps() );
+        __m128 r = _mm_movelh_ps( b0, b2 );
+        r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 ));
+        a2 = _mm_and_ps( a2, b3vxyzMaskf);
+        r = _mm_add_ps( r, b3CastdTo128f (_mm_move_sd( b3CastfTo128d(a2), b3CastfTo128d(b1) )));
+        return b3MakeVector3(r);
+
+#elif defined(B3_USE_NEON)
+        static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
+        float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
+        float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128);
+        float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128);
+        float32x2x2_t zLo = vtrn_f32( vget_high_f32(a0), vget_high_f32(a1));
+        a2 = (float32x4_t) vandq_u32((uint32x4_t) a2, xyzMask );
+        float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
+        float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
+        return b3Vector3( vcombine_f32(b0, b1) );
+#else
+		return b3MakeVector3( dot(v0), dot(v1), dot(v2));
+#endif
+    }
+};
+
+/**@brief Return the sum of two vectors (Point symantics)*/
+B3_FORCE_INLINE b3Vector3
+operator+(const b3Vector3& v1, const b3Vector3& v2)
+{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+	return b3MakeVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
+#elif defined(B3_USE_NEON)
+	return b3MakeVector3(vaddq_f32(v1.mVec128, v2.mVec128));
+#else
+	return b3MakeVector3(
+			v1.m_floats[0] + v2.m_floats[0],
+			v1.m_floats[1] + v2.m_floats[1],
+			v1.m_floats[2] + v2.m_floats[2]);
+#endif
+}
+
+/**@brief Return the elementwise product of two vectors */
+B3_FORCE_INLINE b3Vector3
+operator*(const b3Vector3& v1, const b3Vector3& v2)
+{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+	return b3MakeVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
+#elif defined(B3_USE_NEON)
+	return b3MakeVector3(vmulq_f32(v1.mVec128, v2.mVec128));
+#else
+	return b3MakeVector3(
+			v1.m_floats[0] * v2.m_floats[0],
+			v1.m_floats[1] * v2.m_floats[1],
+			v1.m_floats[2] * v2.m_floats[2]);
+#endif
+}
+
+/**@brief Return the difference between two vectors */
+B3_FORCE_INLINE b3Vector3
+operator-(const b3Vector3& v1, const b3Vector3& v2)
+{
+#if (defined(B3_USE_SSE_IN_API)  && defined(B3_USE_SSE))
+
+	//	without _mm_and_ps this code causes slowdown in Concave moving
+	__m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
+	return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask));
+#elif defined(B3_USE_NEON)
+	float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128);
+	return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask));
+#else
+	return b3MakeVector3(
+			v1.m_floats[0] - v2.m_floats[0],
+			v1.m_floats[1] - v2.m_floats[1],
+			v1.m_floats[2] - v2.m_floats[2]);
+#endif
+}
+
+/**@brief Return the negative of the vector */
+B3_FORCE_INLINE b3Vector3
+operator-(const b3Vector3& v)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
+	__m128 r = _mm_xor_ps(v.mVec128, b3vMzeroMask);
+	return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask));
+#elif defined(B3_USE_NEON)
+	return b3MakeVector3((b3SimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)b3vMzeroMask));
+#else
+	return b3MakeVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
+#endif
+}
+
+/**@brief Return the vector scaled by s */
+B3_FORCE_INLINE b3Vector3
+operator*(const b3Vector3& v, const b3Scalar& s)
+{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+	__m128	vs = _mm_load_ss(&s);	//	(S 0 0 0)
+	vs = b3_pshufd_ps(vs, 0x80);	//	(S S S 0.0)
+	return b3MakeVector3(_mm_mul_ps(v.mVec128, vs));
+#elif defined(B3_USE_NEON)
+	float32x4_t r = vmulq_n_f32(v.mVec128, s);
+	return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask));
+#else
+	return b3MakeVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
+#endif
+}
+
+/**@brief Return the vector scaled by s */
+B3_FORCE_INLINE b3Vector3
+operator*(const b3Scalar& s, const b3Vector3& v)
+{
+	return v * s;
+}
+
+/**@brief Return the vector inversely scaled by s */
+B3_FORCE_INLINE b3Vector3
+operator/(const b3Vector3& v, const b3Scalar& s)
+{
+	b3FullAssert(s != b3Scalar(0.0));
+#if 0 //defined(B3_USE_SSE_IN_API)
+// this code is not faster !
+	__m128 vs = _mm_load_ss(&s);
+    vs = _mm_div_ss(b3v1110, vs);
+	vs = b3_pshufd_ps(vs, 0x00);	//	(S S S S)
+
+	return b3Vector3(_mm_mul_ps(v.mVec128, vs));
+#else
+	return v * (b3Scalar(1.0) / s);
+#endif
+}
+
+/**@brief Return the vector inversely scaled by s */
+B3_FORCE_INLINE b3Vector3
+operator/(const b3Vector3& v1, const b3Vector3& v2)
+{
+#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE))
+	__m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
+	vec = _mm_and_ps(vec, b3vFFF0fMask);
+	return b3MakeVector3(vec);
+#elif defined(B3_USE_NEON)
+	float32x4_t x, y, v, m;
+
+	x = v1.mVec128;
+	y = v2.mVec128;
+
+	v = vrecpeq_f32(y);			// v ~ 1/y
+	m = vrecpsq_f32(y, v);		// m = (2-v*y)
+	v = vmulq_f32(v, m);		// vv = v*m ~~ 1/y
+	m = vrecpsq_f32(y, v);		// mm = (2-vv*y)
+	v = vmulq_f32(v, x);		// x*vv
+	v = vmulq_f32(v, m);		// (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
+
+	return b3Vector3(v);
+#else
+	return b3MakeVector3(
+			v1.m_floats[0] / v2.m_floats[0],
+			v1.m_floats[1] / v2.m_floats[1],
+			v1.m_floats[2] / v2.m_floats[2]);
+#endif
+}
+
+/**@brief Return the dot product between two vectors */
+B3_FORCE_INLINE b3Scalar
+b3Dot(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.dot(v2);
+}
+
+
+/**@brief Return the distance squared between two vectors */
+B3_FORCE_INLINE b3Scalar
+b3Distance2(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.distance2(v2);
+}
+
+
+/**@brief Return the distance between two vectors */
+B3_FORCE_INLINE b3Scalar
+b3Distance(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.distance(v2);
+}
+
+/**@brief Return the angle between two vectors */
+B3_FORCE_INLINE b3Scalar
+b3Angle(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.angle(v2);
+}
+
+/**@brief Return the cross product of two vectors */
+B3_FORCE_INLINE b3Vector3
+b3Cross(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.cross(v2);
+}
+
+B3_FORCE_INLINE b3Scalar
+b3Triple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3)
+{
+	return v1.triple(v2, v3);
+}
+
+/**@brief Return the linear interpolation between two vectors
+ * @param v1 One vector
+ * @param v2 The other vector
+ * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
+B3_FORCE_INLINE b3Vector3
+b3Lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t)
+{
+	return v1.lerp(v2, t);
+}
+
+
+
+B3_FORCE_INLINE b3Scalar b3Vector3::distance2(const b3Vector3& v) const
+{
+	return (v - *this).length2();
+}
+
+B3_FORCE_INLINE b3Scalar b3Vector3::distance(const b3Vector3& v) const
+{
+	return (v - *this).length();
+}
+
+B3_FORCE_INLINE b3Vector3 b3Vector3::normalized() const
+{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+	b3Vector3 norm = *this;
+
+	return norm.normalize();
+#else
+	return *this / length();
+#endif
+}
+
+B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Scalar _angle ) const
+{
+	// wAxis must be a unit lenght vector
+
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+
+    __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
+	b3Scalar ssin = b3Sin( _angle );
+    __m128 C = wAxis.cross( b3MakeVector3(mVec128) ).mVec128;
+	O = _mm_and_ps(O, b3vFFF0fMask);
+    b3Scalar scos = b3Cos( _angle );
+
+	__m128 vsin = _mm_load_ss(&ssin);	//	(S 0 0 0)
+    __m128 vcos = _mm_load_ss(&scos);	//	(S 0 0 0)
+
+	__m128 Y = b3_pshufd_ps(O, 0xC9);	//	(Y Z X 0)
+	__m128 Z = b3_pshufd_ps(O, 0xD2);	//	(Z X Y 0)
+	O = _mm_add_ps(O, Y);
+	vsin = b3_pshufd_ps(vsin, 0x80);	//	(S S S 0)
+	O = _mm_add_ps(O, Z);
+    vcos = b3_pshufd_ps(vcos, 0x80);	//	(S S S 0)
+
+    vsin = vsin * C;
+	O = O * wAxis.mVec128;
+	__m128 X = mVec128 - O;
+
+    O = O + vsin;
+	vcos = vcos * X;
+	O = O + vcos;
+
+	return b3MakeVector3(O);
+#else
+	b3Vector3 o = wAxis * wAxis.dot( *this );
+	b3Vector3 _x = *this - o;
+	b3Vector3 _y;
+
+	_y = wAxis.cross( *this );
+
+	return ( o + _x * b3Cos( _angle ) + _y * b3Sin( _angle ) );
+#endif
+}
+
+B3_FORCE_INLINE   long    b3Vector3::maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const
+{
+#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
+    #if defined _WIN32 || defined (B3_USE_SSE)
+        const long scalar_cutoff = 10;
+        long b3_maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
+    #elif defined B3_USE_NEON
+        const long scalar_cutoff = 4;
+        extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
+    #endif
+    if( array_count < scalar_cutoff )
+#else
+
+#endif//B3_USE_SSE || B3_USE_NEON
+    {
+        b3Scalar maxDot = -B3_INFINITY;
+        int i = 0;
+        int ptIndex = -1;
+        for( i = 0; i < array_count; i++ )
+        {
+            b3Scalar dot = array[i].dot(*this);
+
+            if( dot > maxDot )
+            {
+                maxDot = dot;
+                ptIndex = i;
+            }
+        }
+
+		b3Assert(ptIndex>=0);
+        if (ptIndex<0)
+		{
+			ptIndex = 0;
+		}
+        dotOut = maxDot;
+        return ptIndex;
+    }
+#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
+    return b3_maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
+#endif
+}
+
+B3_FORCE_INLINE   long    b3Vector3::minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const
+{
+#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
+    #if defined B3_USE_SSE
+        const long scalar_cutoff = 10;
+        long b3_mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
+    #elif defined B3_USE_NEON
+        const long scalar_cutoff = 4;
+        extern long (*b3_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
+    #else
+        #error unhandled arch!
+    #endif
+
+    if( array_count < scalar_cutoff )
+#endif//B3_USE_SSE || B3_USE_NEON
+    {
+        b3Scalar  minDot = B3_INFINITY;
+        int i = 0;
+        int ptIndex = -1;
+
+        for( i = 0; i < array_count; i++ )
+        {
+            b3Scalar dot = array[i].dot(*this);
+
+            if( dot < minDot )
+            {
+                minDot = dot;
+                ptIndex = i;
+            }
+        }
+
+        dotOut = minDot;
+
+        return ptIndex;
+    }
+#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
+    return b3_mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
+#endif
+}
+
+
+class b3Vector4 : public b3Vector3
+{
+public:
+
+
+
+
+
+
+	B3_FORCE_INLINE b3Vector4 absolute4() const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+		return b3MakeVector4(_mm_and_ps(mVec128, b3vAbsfMask));
+#elif defined(B3_USE_NEON)
+		return b3Vector4(vabsq_f32(mVec128));
+#else
+		return b3MakeVector4(
+			b3Fabs(m_floats[0]),
+			b3Fabs(m_floats[1]),
+			b3Fabs(m_floats[2]),
+			b3Fabs(m_floats[3]));
+#endif
+	}
+
+
+	b3Scalar	getW() const { return m_floats[3];}
+
+
+		B3_FORCE_INLINE int maxAxis4() const
+	{
+		int maxIndex = -1;
+		b3Scalar maxVal = b3Scalar(-B3_LARGE_FLOAT);
+		if (m_floats[0] > maxVal)
+		{
+			maxIndex = 0;
+			maxVal = m_floats[0];
+		}
+		if (m_floats[1] > maxVal)
+		{
+			maxIndex = 1;
+			maxVal = m_floats[1];
+		}
+		if (m_floats[2] > maxVal)
+		{
+			maxIndex = 2;
+			maxVal =m_floats[2];
+		}
+		if (m_floats[3] > maxVal)
+		{
+			maxIndex = 3;
+		}
+
+		return maxIndex;
+	}
+
+
+	B3_FORCE_INLINE int minAxis4() const
+	{
+		int minIndex = -1;
+		b3Scalar minVal = b3Scalar(B3_LARGE_FLOAT);
+		if (m_floats[0] < minVal)
+		{
+			minIndex = 0;
+			minVal = m_floats[0];
+		}
+		if (m_floats[1] < minVal)
+		{
+			minIndex = 1;
+			minVal = m_floats[1];
+		}
+		if (m_floats[2] < minVal)
+		{
+			minIndex = 2;
+			minVal =m_floats[2];
+		}
+		if (m_floats[3] < minVal)
+		{
+			minIndex = 3;
+			minVal = m_floats[3];
+		}
+
+		return minIndex;
+	}
+
+
+	B3_FORCE_INLINE int closestAxis4() const
+	{
+		return absolute4().maxAxis4();
+	}
+
+
+
+
+  /**@brief Set x,y,z and zero w
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+
+
+/*		void getValue(b3Scalar *m) const
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] =m_floats[2];
+		}
+*/
+/**@brief Set the values
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+		B3_FORCE_INLINE void	setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
+		{
+			m_floats[0]=_x;
+			m_floats[1]=_y;
+			m_floats[2]=_z;
+			m_floats[3]=_w;
+		}
+
+
+};
+
+
+///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+B3_FORCE_INLINE void	b3SwapScalarEndian(const b3Scalar& sourceVal, b3Scalar& destVal)
+{
+	#ifdef B3_USE_DOUBLE_PRECISION
+	unsigned char* dest = (unsigned char*) &destVal;
+	unsigned char* src  = (unsigned char*) &sourceVal;
+	dest[0] = src[7];
+    dest[1] = src[6];
+    dest[2] = src[5];
+    dest[3] = src[4];
+    dest[4] = src[3];
+    dest[5] = src[2];
+    dest[6] = src[1];
+    dest[7] = src[0];
+#else
+	unsigned char* dest = (unsigned char*) &destVal;
+	unsigned char* src  = (unsigned char*) &sourceVal;
+	dest[0] = src[3];
+    dest[1] = src[2];
+    dest[2] = src[1];
+    dest[3] = src[0];
+#endif //B3_USE_DOUBLE_PRECISION
+}
+///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+B3_FORCE_INLINE void	b3SwapVector3Endian(const b3Vector3& sourceVec, b3Vector3& destVec)
+{
+	for (int i=0;i<4;i++)
+	{
+		b3SwapScalarEndian(sourceVec[i],destVec[i]);
+	}
+
+}
+
+///b3UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+B3_FORCE_INLINE void	b3UnSwapVector3Endian(b3Vector3& vector)
+{
+
+	b3Vector3	swappedVec;
+	for (int i=0;i<4;i++)
+	{
+		b3SwapScalarEndian(vector[i],swappedVec[i]);
+	}
+	vector = swappedVec;
+}
+
+template <class T>
+B3_FORCE_INLINE void b3PlaneSpace1 (const T& n, T& p, T& q)
+{
+  if (b3Fabs(n[2]) > B3_SQRT12) {
+    // choose p in y-z plane
+    b3Scalar a = n[1]*n[1] + n[2]*n[2];
+    b3Scalar k = b3RecipSqrt (a);
+    p[0] = 0;
+	p[1] = -n[2]*k;
+	p[2] = n[1]*k;
+    // set q = n x p
+    q[0] = a*k;
+	q[1] = -n[0]*p[2];
+	q[2] = n[0]*p[1];
+  }
+  else {
+    // choose p in x-y plane
+    b3Scalar a = n[0]*n[0] + n[1]*n[1];
+    b3Scalar k = b3RecipSqrt (a);
+    p[0] = -n[1]*k;
+	p[1] = n[0]*k;
+	p[2] = 0;
+    // set q = n x p
+    q[0] = -n[2]*p[1];
+	q[1] = n[2]*p[0];
+	q[2] = a*k;
+  }
+}
+
+
+struct	b3Vector3FloatData
+{
+	float	m_floats[4];
+};
+
+struct	b3Vector3DoubleData
+{
+	double	m_floats[4];
+
+};
+
+B3_FORCE_INLINE	void	b3Vector3::serializeFloat(struct	b3Vector3FloatData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = float(m_floats[i]);
+}
+
+B3_FORCE_INLINE void	b3Vector3::deSerializeFloat(const struct	b3Vector3FloatData& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = b3Scalar(dataIn.m_floats[i]);
+}
+
+
+B3_FORCE_INLINE	void	b3Vector3::serializeDouble(struct	b3Vector3DoubleData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = double(m_floats[i]);
+}
+
+B3_FORCE_INLINE void	b3Vector3::deSerializeDouble(const struct	b3Vector3DoubleData& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = b3Scalar(dataIn.m_floats[i]);
+}
+
+
+B3_FORCE_INLINE	void	b3Vector3::serialize(struct	b3Vector3Data& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = m_floats[i];
+}
+
+B3_FORCE_INLINE void	b3Vector3::deSerialize(const struct	b3Vector3Data& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = dataIn.m_floats[i];
+}
+
+
+
+
+inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z)
+{
+	b3Vector3	tmp;
+	tmp.setValue(x,y,z);
+	return tmp;
+}
+
+inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z, b3Scalar w)
+{
+	b3Vector3	tmp;
+	tmp.setValue(x,y,z);
+	tmp.w = w;
+	return tmp;
+}
+
+inline b3Vector4 b3MakeVector4(b3Scalar x,b3Scalar y,b3Scalar z,b3Scalar w)
+{
+	b3Vector4	tmp;
+	tmp.setValue(x,y,z,w);
+	return tmp;
+}
+
+#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
+
+inline b3Vector3 b3MakeVector3( b3SimdFloat4 v)
+{
+        b3Vector3 tmp;
+        tmp.set128(v);
+        return tmp;
+}
+
+inline b3Vector4 b3MakeVector4(b3SimdFloat4 vec)
+{
+	b3Vector4	tmp;
+	tmp.set128(vec);
+	return tmp;
+}
+
+#endif
+
+
+#endif //B3_VECTOR3_H