Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix some warnings, disable gimpact by default in world importer,

Browse Source 
use DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS by default for now,
until we find the issue with some failing test cases in btMultiBody
fix a crashing issue in MyMultiBodyCreator.cpp (uninitialized variable)
disable excessive debug printf in URDF2Bullet
This commit is contained in:
Erwin Coumans
2015-07-27 13:28:47 -07:00
parent ad03dba2fa
commit 26531f3fbc
27 changed files with 248 additions and 290 deletions
Download Patch File Download Diff File Expand all files Collapse all files

346
src/Bullet3Common/b3Vector3.h
View File

@@ -3,8 +3,8 @@ Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans http://bulletphysic
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,
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.
@@ -22,8 +22,6 @@ subject to the following restrictions:
#include "b3MinMax.h"
#include "b3AlignedAllocator.h"
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Vector3Data b3Vector3DoubleData
#define b3Vector3DataName "b3Vector3DoubleData"
@@ -99,7 +97,7 @@ public:
b3SimdFloat4 mVec128;
float m_floats[4];
struct {float x,y,z,w;};
};
#else
union
@@ -133,9 +131,9 @@ public:
public:
/**@brief Add a vector to this one
/**@brief Add a vector to this one
* @param The vector to add to this one */
B3_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v)
{
@@ -144,7 +142,7 @@ public:
#elif defined(B3_USE_NEON)
mVec128 = vaddq_f32(mVec128, v.mVec128);
#else
m_floats[0] += v.m_floats[0];
m_floats[0] += v.m_floats[0];
m_floats[1] += v.m_floats[1];
m_floats[2] += v.m_floats[2];
#endif
@@ -154,20 +152,20 @@ public:
/**@brief Subtract a vector from this one
* @param The vector to subtract */
B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
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[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)
@@ -179,16 +177,16 @@ public:
#elif defined(B3_USE_NEON)
mVec128 = vmulq_n_f32(mVec128, s);
#else
m_floats[0] *= s;
m_floats[0] *= s;
m_floats[1] *= s;
m_floats[2] *= s;
#endif
return *this;
}
/**@brief Inversely scale the vector
/**@brief Inversely scale the vector
* @param s Scale factor to divide by */
B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
{
b3FullAssert(s != b3Scalar(0.0));
@@ -199,7 +197,7 @@ public:
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;
@@ -210,7 +208,7 @@ public:
* @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)
#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);
@@ -219,12 +217,12 @@ public:
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));
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] +
#else
return m_floats[0] * v.m_floats[0] +
m_floats[1] * v.m_floats[1] +
m_floats[2] * v.m_floats[2];
#endif
}
@@ -249,7 +247,7 @@ public:
* This is symantically treating the vector like a point */
B3_FORCE_INLINE b3Scalar distance(const b3Vector3& v) const;
B3_FORCE_INLINE b3Vector3& safeNormalize()
B3_FORCE_INLINE b3Vector3& safeNormalize()
{
b3Vector3 absVec = this->absolute();
int maxIndex = absVec.maxAxis();
@@ -262,35 +260,35 @@ public:
return *this;
}
/**@brief Normalize this vector
/**@brief Normalize this vector
* x^2 + y^2 + z^2 = 1 */
B3_FORCE_INLINE b3Vector3& normalize()
B3_FORCE_INLINE b3Vector3& normalize()
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#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
// 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
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
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)
@@ -299,9 +297,9 @@ public:
#endif
return *this;
#else
#else
return *this /= length();
#endif
}
@@ -310,48 +308,48 @@ public:
B3_FORCE_INLINE b3Vector3 normalized() const;
/**@brief Return a rotated version of this vector
* @param wAxis The axis to rotate about
* @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
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
B3_FORCE_INLINE b3Vector3 absolute() const
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#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
#else
return b3MakeVector3(
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[2]));
#endif
}
/**@brief Return the cross product between this and another vector
/**@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)
@@ -361,7 +359,7 @@ public:
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);
@@ -369,7 +367,7 @@ public:
// 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(
@@ -385,14 +383,14 @@ public:
// 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:
// dot:
V = _mm_mul_ps(V, mVec128);
__m128 z = _mm_movehl_ps(V, V);
__m128 y = _mm_shuffle_ps(V, V, 0x55);
@@ -408,7 +406,7 @@ public:
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);
@@ -416,29 +414,29 @@ public:
// form (Y, Z, X, _);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
// dot:
// dot:
V = vmulq_f32(mVec128, V);
float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(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]) +
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
/**@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
/**@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
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);
}
@@ -448,12 +446,12 @@ public:
return absolute().minAxis();
}
B3_FORCE_INLINE int closestAxis() const
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)
@@ -480,10 +478,10 @@ public:
#endif
}
/**@brief Return the linear interpolation between this and another vector
* @param v The other vector
/**@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
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)
@@ -491,23 +489,23 @@ public:
__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
#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
/**@brief Elementwise multiply this vector by the other
* @param v The other vector */
B3_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v)
{
@@ -515,8 +513,8 @@ public:
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];
#else
m_floats[0] *= v.m_floats[0];
m_floats[1] *= v.m_floats[1];
m_floats[2] *= v.m_floats[2];
#endif
@@ -541,7 +539,7 @@ public:
/**@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 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]; }
@@ -551,10 +549,10 @@ public:
{
#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]) &&
#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
}
@@ -565,7 +563,7 @@ public:
}
/**@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
* @param other The other b3Vector3 to compare with
*/
B3_FORCE_INLINE void setMax(const b3Vector3& other)
{
@@ -582,7 +580,7 @@ public:
}
/**@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
* @param other The other b3Vector3 to compare with
*/
B3_FORCE_INLINE void setMin(const b3Vector3& other)
{
@@ -609,16 +607,16 @@ public:
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;
@@ -634,19 +632,19 @@ public:
#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);
int32x4_t vi = vdupq_n_s32(0);
mVec128 = vreinterpretq_f32_s32(vi);
#else
#else
setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
#endif
}
B3_FORCE_INLINE bool isZero() const
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
B3_FORCE_INLINE bool fuzzyZero() const
{
return length2() < B3_EPSILON;
}
@@ -662,18 +660,18 @@ public:
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 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;
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;
* @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
@@ -691,7 +689,7 @@ public:
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);
@@ -702,15 +700,15 @@ public:
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
#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)
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));
@@ -718,15 +716,15 @@ operator+(const b3Vector3& v1, const b3Vector3& v2)
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[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)
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));
@@ -734,14 +732,14 @@ operator*(const b3Vector3& v1, const b3Vector3& v2)
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[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
B3_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v1, const b3Vector3& v2)
{
#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
@@ -754,28 +752,28 @@ operator-(const b3Vector3& v1, const b3Vector3& v2)
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[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
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));
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
#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
B3_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v, const b3Scalar& s)
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
@@ -791,10 +789,10 @@ operator*(const b3Vector3& v, const b3Scalar& s)
}
/**@brief Return the vector scaled by s */
B3_FORCE_INLINE b3Vector3
B3_FORCE_INLINE b3Vector3
operator*(const b3Scalar& s, const b3Vector3& v)
{
return v * s;
{
return v * s;
}
/**@brief Return the vector inversely scaled by s */
@@ -821,13 +819,13 @@ 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);
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
@@ -838,47 +836,47 @@ operator/(const b3Vector3& v1, const b3Vector3& v2)
return b3Vector3(v);
#else
return b3MakeVector3(
v1.m_floats[0] / v2.m_floats[0],
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);
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);
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);
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);
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 b3Vector3
b3Cross(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.cross(v2);
}
B3_FORCE_INLINE b3Scalar
@@ -888,10 +886,10 @@ b3Triple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3)
}
/**@brief Return the linear interpolation between two vectors
* @param v1 One vector
* @param v2 The other vector
* @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
B3_FORCE_INLINE b3Vector3
b3Lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t)
{
return v1.lerp(v2, t);
@@ -918,7 +916,7 @@ B3_FORCE_INLINE b3Vector3 b3Vector3::normalized() const
#else
return *this / length();
#endif
}
}
B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Scalar _angle ) const
{
@@ -931,25 +929,25 @@ B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Sca
__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;
vsin = vsin * C;
O = O * wAxis.mVec128;
__m128 X = mVec128 - O;
O = O + vsin;
vcos = vcos * X;
O = O + vcos;
O = O + vcos;
return b3MakeVector3(O);
#else
b3Vector3 o = wAxis * wAxis.dot( *this );
@@ -974,7 +972,7 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_
#endif
if( array_count < scalar_cutoff )
#else
#endif//B3_USE_SSE || B3_USE_NEON
{
b3Scalar maxDot = -B3_INFINITY;
@@ -983,7 +981,7 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_
for( i = 0; i < array_count; i++ )
{
b3Scalar dot = array[i].dot(*this);
if( dot > maxDot )
{
maxDot = dot;
@@ -1016,27 +1014,27 @@ B3_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_
#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)
@@ -1049,21 +1047,21 @@ class b3Vector4 : public b3Vector3
{
public:
B3_FORCE_INLINE b3Vector4 absolute4() const
B3_FORCE_INLINE b3Vector4 absolute4() const
{
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#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
#else
return b3MakeVector4(
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[2]),
b3Fabs(m_floats[3]));
#endif
@@ -1126,34 +1124,34 @@ public:
minIndex = 3;
minVal = m_floats[3];
}
return minIndex;
}
B3_FORCE_INLINE int closestAxis4() const
B3_FORCE_INLINE int closestAxis4() const
{
return absolute4().maxAxis4();
}
/**@brief Set x,y,z and zero w
/**@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
/* void getValue(b3Scalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
m[2] =m_floats[2];
}
*/
/**@brief Set the values
/**@brief Set the values
* @param x Value of x
* @param y Value of y
* @param z Value of z
@@ -1299,7 +1297,7 @@ B3_FORCE_INLINE void b3Vector3::deSerialize(const struct b3Vector3Data& dataIn)
}
inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z)
{
@@ -1309,7 +1307,7 @@ inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z)
}
inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z, b3Scalar w)
{
{
b3Vector3 tmp;
tmp.setValue(x,y,z);
tmp.w = w;
@@ -1323,7 +1321,7 @@ inline b3Vector4 b3MakeVector4(b3Scalar x,b3Scalar y,b3Scalar z,b3Scalar w)
return tmp;
}
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
inline b3Vector3 b3MakeVector3( b3SimdFloat4 v)
{