diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath/btVector3.h')
| -rw-r--r-- | extern/bullet2/src/LinearMath/btVector3.h | 341 |
1 files changed, 263 insertions, 78 deletions
diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 96548c6ba60..5d5c39e8587 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -17,127 +17,190 @@ subject to the following restrictions: #ifndef SIMD__VECTOR3_H #define SIMD__VECTOR3_H -#include "btQuadWord.h" -///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 -class btVector3 : public btQuadWord { +#include "btScalar.h" +#include "btScalar.h" +#include "btMinMax.h" +/**@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: - SIMD_FORCE_INLINE btVector3() {} - SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) - : btQuadWord(q) +#if defined (__SPU__) && defined (__CELLOS_LV2__) + union { + vec_float4 mVec128; + btScalar m_floats[4]; + }; +public: + vec_float4 get128() const { + return mVec128; } - - - SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) - :btQuadWord(x,y,z,btScalar(0.)) +public: +#else //__CELLOS_LV2__ __SPU__ +#ifdef BT_USE_SSE // WIN32 + union { + __m128 mVec128; + btScalar m_floats[4]; + }; + SIMD_FORCE_INLINE __m128 get128() const { + return mVec128; } + SIMD_FORCE_INLINE void set128(__m128 v128) + { + mVec128 = v128; + } +#else + btScalar m_floats[4]; +#endif +#endif //__CELLOS_LV2__ __SPU__ + + public: -// SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) -// : btQuadWord(x,y,z,w) -// { -// } + /**@brief No initialization constructor */ + SIMD_FORCE_INLINE btVector3() {} + + /**@brief Constructor from scalars + * @param x X value + * @param y Y value + * @param z Z value + */ + SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_floats[0] = x; + m_floats[1] = y; + m_floats[2] = z; + m_floats[3] = btScalar(0.); + } + +/**@brief Add a vector to this one + * @param The vector to add to this one */ SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) { - m_x += v.x(); m_y += v.y(); m_z += v.z(); + m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; return *this; } - + /**@brief Subtract a vector from this one + * @param The vector to subtract */ SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) { - m_x -= v.x(); m_y -= v.y(); m_z -= v.z(); + m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; return *this; } - + /**@brief Scale the vector + * @param s Scale factor */ SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s) { - m_x *= s; m_y *= s; m_z *= s; + m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; return *this; } + /**@brief Inversely scale the vector + * @param s Scale factor to divide by */ SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) { btFullAssert(s != btScalar(0.0)); return *this *= btScalar(1.0) / s; } + /**@brief Return the dot product + * @param v The other vector in the dot product */ SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const { - return m_x * v.x() + m_y * v.y() + m_z * v.z(); + return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; } + /**@brief Return the length of the vector squared */ SIMD_FORCE_INLINE btScalar length2() const { return dot(*this); } + /**@brief Return the length of the vector */ SIMD_FORCE_INLINE btScalar length() const { return btSqrt(length2()); } + /**@brief Return the distance squared between the ends of this and another vector + * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; + /**@brief Return the distance between the ends of this and another vector + * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; + /**@brief Normalize this vector + * x^2 + y^2 + z^2 = 1 */ SIMD_FORCE_INLINE btVector3& normalize() { return *this /= length(); } + /**@brief Return a normalized version of this vector */ SIMD_FORCE_INLINE btVector3 normalized() const; + /**@brief Rotate 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 ); + /**@brief Return the angle between this and another vector + * @param v The other vector */ SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const { btScalar s = btSqrt(length2() * v.length2()); btFullAssert(s != btScalar(0.0)); return btAcos(dot(v) / s); } - + /**@brief Return a vector will the absolute values of each element */ SIMD_FORCE_INLINE btVector3 absolute() const { return btVector3( - btFabs(m_x), - btFabs(m_y), - btFabs(m_z)); + btFabs(m_floats[0]), + btFabs(m_floats[1]), + btFabs(m_floats[2])); } - + /**@brief Return the cross product between this and another vector + * @param v The other vector */ SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const { return btVector3( - m_y * v.z() - m_z * v.y(), - m_z * v.x() - m_x * v.z(), - m_x * v.y() - m_y * v.x()); + 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]); } SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const { - return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + - m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + - m_z * (v1.x() * v2.y() - v1.y() * v2.x()); + 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]); } + /**@brief Return the axis with the smallest value + * Note return values are 0,1,2 for x, y, or z */ SIMD_FORCE_INLINE int minAxis() const { - return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2); + 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 */ SIMD_FORCE_INLINE int maxAxis() const { - return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0); + return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0); } SIMD_FORCE_INLINE int furthestAxis() const @@ -153,67 +216,150 @@ public: SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt) { btScalar s = btScalar(1.0) - rt; - m_x = s * v0.x() + rt * v1.x(); - m_y = s * v0.y() + rt * v1.y(); - m_z = s * v0.z() + rt * v1.z(); + 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]; } + /**@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) */ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const { - return btVector3(m_x + (v.x() - m_x) * t, - m_y + (v.y() - m_y) * t, - m_z + (v.z() - m_z) * t); + return btVector3(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); } - + /**@brief Elementwise multiply this vector by the other + * @param v The other vector */ SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) { - m_x *= v.x(); m_y *= v.y(); m_z *= v.z(); + m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; return *this; } - + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;}; + /**@brief Set the y value */ + SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;}; + /**@brief Set the z value */ + SIMD_FORCE_INLINE void setZ(btScalar z) {m_floats[2] = z;}; + /**@brief Set the w value */ + SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;}; + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } + + //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } + SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } + + SIMD_FORCE_INLINE bool operator==(const btVector3& other) const + { + 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])); + } + + SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const + { + return !(*this == other); + } + + /**@brief Set each element to the max of the current values and the values of another btVector3 + * @param other The other btVector3 to compare with + */ + SIMD_FORCE_INLINE void setMax(const btVector3& other) + { + btSetMax(m_floats[0], other.m_floats[0]); + btSetMax(m_floats[1], other.m_floats[1]); + btSetMax(m_floats[2], other.m_floats[2]); + btSetMax(m_floats[3], other.w()); + } + /**@brief Set each element to the min of the current values and the values of another btVector3 + * @param other The other btVector3 to compare with + */ + SIMD_FORCE_INLINE void setMin(const btVector3& other) + { + btSetMin(m_floats[0], other.m_floats[0]); + btSetMin(m_floats[1], other.m_floats[1]); + btSetMin(m_floats[2], other.m_floats[2]); + btSetMin(m_floats[3], other.w()); + } + + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3] = 0.f; + } + + void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const + { + v0->setValue(0. ,-z() ,y()); + v1->setValue(z() ,0. ,-x()); + v2->setValue(-y() ,x() ,0.); + } }; +/**@brief Return the sum of two vectors (Point symantics)*/ SIMD_FORCE_INLINE btVector3 operator+(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z()); + return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); } +/**@brief Return the elementwise product of two vectors */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z()); + return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); } +/**@brief Return the difference between two vectors */ SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z()); + return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); } - +/**@brief Return the negative of the vector */ SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v) { - return btVector3(-v.x(), -v.y(), -v.z()); + return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); } +/**@brief Return the vector scaled by s */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btScalar& s) { - return btVector3(v.x() * s, v.y() * s, v.z() * s); + return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); } +/**@brief Return the vector scaled by s */ SIMD_FORCE_INLINE btVector3 operator*(const btScalar& s, const btVector3& v) { return v * s; } +/**@brief Return the vector inversely scaled by s */ SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v, const btScalar& s) { @@ -221,12 +367,14 @@ operator/(const btVector3& v, const btScalar& s) return v * (btScalar(1.0) / s); } +/**@brief Return the vector inversely scaled by s */ SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z()); + return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); } +/**@brief Return the dot product between two vectors */ SIMD_FORCE_INLINE btScalar dot(const btVector3& v1, const btVector3& v2) { @@ -234,7 +382,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) { @@ -242,18 +390,21 @@ 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) { return v1.distance(v2); } +/**@brief Return the angle between two vectors */ SIMD_FORCE_INLINE btScalar angle(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) { @@ -266,6 +417,10 @@ triple(const btVector3& v1, const btVector3& v2, const btVector3& 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) */ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) { @@ -273,10 +428,6 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) } -SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) -{ - return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z(); -} SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const { @@ -316,47 +467,47 @@ public: SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) : btVector3(x,y,z) { - m_unusedW = w; + m_floats[3] = w; } SIMD_FORCE_INLINE btVector4 absolute4() const { return btVector4( - btFabs(m_x), - btFabs(m_y), - btFabs(m_z), - btFabs(m_unusedW)); + btFabs(m_floats[0]), + btFabs(m_floats[1]), + btFabs(m_floats[2]), + btFabs(m_floats[3])); } - btScalar getW() const { return m_unusedW;} + btScalar getW() const { return m_floats[3];} SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; btScalar maxVal = btScalar(-1e30); - if (m_x > maxVal) + if (m_floats[0] > maxVal) { maxIndex = 0; - maxVal = m_x; + maxVal = m_floats[0]; } - if (m_y > maxVal) + if (m_floats[1] > maxVal) { maxIndex = 1; - maxVal = m_y; + maxVal = m_floats[1]; } - if (m_z > maxVal) + if (m_floats[2] > maxVal) { maxIndex = 2; - maxVal = m_z; + maxVal =m_floats[2]; } - if (m_unusedW > maxVal) + if (m_floats[3] > maxVal) { maxIndex = 3; - maxVal = m_unusedW; + maxVal = m_floats[3]; } @@ -371,25 +522,25 @@ public: { int minIndex = -1; btScalar minVal = btScalar(1e30); - if (m_x < minVal) + if (m_floats[0] < minVal) { minIndex = 0; - minVal = m_x; + minVal = m_floats[0]; } - if (m_y < minVal) + if (m_floats[1] < minVal) { minIndex = 1; - minVal = m_y; + minVal = m_floats[1]; } - if (m_z < minVal) + if (m_floats[2] < minVal) { minIndex = 2; - minVal = m_z; + minVal =m_floats[2]; } - if (m_unusedW < minVal) + if (m_floats[3] < minVal) { minIndex = 3; - minVal = m_unusedW; + minVal = m_floats[3]; } return minIndex; @@ -402,6 +553,40 @@ public: 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(btScalar *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 + */ + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3]=w; + } + + + + }; |