diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Common/b3Vector3.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Common/b3Vector3.h | 1344 |
1 files changed, 1344 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3Common/b3Vector3.h b/thirdparty/bullet/Bullet3Common/b3Vector3.h new file mode 100644 index 0000000000..16ec02b0ed --- /dev/null +++ b/thirdparty/bullet/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 |