diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath/btVector3.h')
| -rw-r--r-- | extern/bullet2/src/LinearMath/btVector3.h | 172 |
1 files changed, 150 insertions, 22 deletions
diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 5d5c39e8587..068e87c94ab 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -19,30 +19,37 @@ subject to the following restrictions: #include "btScalar.h" -#include "btScalar.h" #include "btMinMax.h" + +#ifdef BT_USE_DOUBLE_PRECISION +#define btVector3Data btVector3DoubleData +#define btVector3DataName "btVector3DoubleData" +#else +#define btVector3Data btVector3FloatData +#define btVector3DataName "btVector3FloatData" +#endif //BT_USE_DOUBLE_PRECISION + + + + /**@brief btVector3 can be used to represent 3D points and vectors. * It has an un-used w component to suit 16-byte alignment when btVector3 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 */ - ATTRIBUTE_ALIGNED16(class) btVector3 { public: #if defined (__SPU__) && defined (__CELLOS_LV2__) - union { - vec_float4 mVec128; btScalar m_floats[4]; - }; public: - vec_float4 get128() const + SIMD_FORCE_INLINE const vec_float4& get128() const { - return mVec128; + return *((const vec_float4*)&m_floats[0]); } public: #else //__CELLOS_LV2__ __SPU__ -#ifdef BT_USE_SSE // WIN32 +#ifdef BT_USE_SSE // _WIN32 union { __m128 mVec128; btScalar m_floats[4]; @@ -141,6 +148,19 @@ public: * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; + SIMD_FORCE_INLINE btVector3& safeNormalize() + { + btVector3 absVec = this->absolute(); + int maxIndex = absVec.maxAxis(); + if (absVec[maxIndex]>0) + { + *this /= absVec[maxIndex]; + return *this /= length(); + } + setValue(1,0,0); + return *this; + } + /**@brief Normalize this vector * x^2 + y^2 + z^2 = 1 */ SIMD_FORCE_INLINE btVector3& normalize() @@ -151,10 +171,10 @@ public: /**@brief Return a normalized version of this vector */ SIMD_FORCE_INLINE btVector3 normalized() const; - /**@brief Rotate this vector + /**@brief Return a rotated version of this vector * @param wAxis The axis to rotate about * @param angle The angle to rotate by */ - SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ); + SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const; /**@brief Return the angle between this and another vector * @param v The other vector */ @@ -306,7 +326,7 @@ public: m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; - m_floats[3] = 0.f; + m_floats[3] = btScalar(0.); } void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const @@ -316,6 +336,33 @@ public: v2->setValue(-y() ,x() ,0.); } + void setZero() + { + setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + } + + SIMD_FORCE_INLINE bool isZero() const + { + return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0); + } + + SIMD_FORCE_INLINE bool fuzzyZero() const + { + return length2() < SIMD_EPSILON; + } + + SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const; + + SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn); + + SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const; + + SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn); + + SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData& dataOut) const; + + SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn); + }; /**@brief Return the sum of two vectors (Point symantics)*/ @@ -376,7 +423,7 @@ operator/(const btVector3& v1, const btVector3& v2) /**@brief Return the dot product between two vectors */ SIMD_FORCE_INLINE btScalar -dot(const btVector3& v1, const btVector3& v2) +btDot(const btVector3& v1, const btVector3& v2) { return v1.dot(v2); } @@ -384,7 +431,7 @@ dot(const btVector3& v1, const btVector3& v2) /**@brief Return the distance squared between two vectors */ SIMD_FORCE_INLINE btScalar -distance2(const btVector3& v1, const btVector3& v2) +btDistance2(const btVector3& v1, const btVector3& v2) { return v1.distance2(v2); } @@ -392,27 +439,27 @@ distance2(const btVector3& v1, const btVector3& v2) /**@brief Return the distance between two vectors */ SIMD_FORCE_INLINE btScalar -distance(const btVector3& v1, const btVector3& v2) +btDistance(const btVector3& v1, const btVector3& v2) { return v1.distance(v2); } /**@brief Return the angle between two vectors */ SIMD_FORCE_INLINE btScalar -angle(const btVector3& v1, const btVector3& v2) +btAngle(const btVector3& v1, const btVector3& v2) { return v1.angle(v2); } /**@brief Return the cross product of two vectors */ SIMD_FORCE_INLINE btVector3 -cross(const btVector3& v1, const btVector3& v2) +btCross(const btVector3& v1, const btVector3& v2) { return v1.cross(v2); } SIMD_FORCE_INLINE btScalar -triple(const btVector3& v1, const btVector3& v2, const btVector3& v3) +btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3) { return v1.triple(v2, v3); } @@ -444,7 +491,7 @@ SIMD_FORCE_INLINE btVector3 btVector3::normalized() const return *this / length(); } -SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle ) +SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle ) const { // wAxis must be a unit lenght vector @@ -488,7 +535,7 @@ public: SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; - btScalar maxVal = btScalar(-1e30); + btScalar maxVal = btScalar(-BT_LARGE_FLOAT); if (m_floats[0] > maxVal) { maxIndex = 0; @@ -521,7 +568,7 @@ public: SIMD_FORCE_INLINE int minAxis4() const { int minIndex = -1; - btScalar minVal = btScalar(1e30); + btScalar minVal = btScalar(BT_LARGE_FLOAT); if (m_floats[0] < minVal) { minIndex = 0; @@ -585,8 +632,6 @@ public: } - - }; @@ -635,4 +680,87 @@ SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector) vector = swappedVec; } +template <class T> +SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q) +{ + if (btFabs(n[2]) > SIMDSQRT12) { + // choose p in y-z plane + btScalar a = n[1]*n[1] + n[2]*n[2]; + btScalar k = btRecipSqrt (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 + btScalar a = n[0]*n[0] + n[1]*n[1]; + btScalar k = btRecipSqrt (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 btVector3FloatData +{ + float m_floats[4]; +}; + +struct btVector3DoubleData +{ + double m_floats[4]; + +}; + +SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& 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]); +} + +SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = btScalar(dataIn.m_floats[i]); +} + + +SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& 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]); +} + +SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = btScalar(dataIn.m_floats[i]); +} + + +SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& 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]; +} + +SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = dataIn.m_floats[i]; +} + + #endif //SIMD__VECTOR3_H |