diff options
Diffstat (limited to 'thirdparty/vhacd/inc/btVector3.h')
| -rw-r--r-- | thirdparty/vhacd/inc/btVector3.h | 954 |
1 files changed, 508 insertions, 446 deletions
diff --git a/thirdparty/vhacd/inc/btVector3.h b/thirdparty/vhacd/inc/btVector3.h index 664728a419..0f2fefbbd5 100644 --- a/thirdparty/vhacd/inc/btVector3.h +++ b/thirdparty/vhacd/inc/btVector3.h @@ -30,387 +30,431 @@ subject to the following restrictions: * 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 */ -//GODOT ADDITION -namespace VHACD { -// - ATTRIBUTE_ALIGNED16(class) -btVector3 { +btVector3 +{ public: #if defined(__SPU__) && defined(__CELLOS_LV2__) - btScalar m_floats[4]; + btScalar m_floats[4]; public: - SIMD_FORCE_INLINE const vec_float4 &get128() const { - return *((const vec_float4 *)&m_floats[0]); - } + SIMD_FORCE_INLINE const vec_float4& get128() const + { + return *((const vec_float4*)&m_floats[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; - } + 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]; + btScalar m_floats[4]; #endif #endif //__CELLOS_LV2__ __SPU__ public: - /**@brief No initialization constructor */ - SIMD_FORCE_INLINE btVector3() {} + /**@brief No initialization constructor */ + SIMD_FORCE_INLINE btVector3() {} - /**@brief Constructor from scalars + /**@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 + 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) { + SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) + { - m_floats[0] += v.m_floats[0]; - m_floats[1] += v.m_floats[1]; - m_floats[2] += v.m_floats[2]; - return *this; - } + 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 + /**@brief Subtract a vector from this one * @param The vector to subtract */ - SIMD_FORCE_INLINE btVector3 &operator-=(const btVector3 &v) { - 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 + SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) + { + 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_floats[0] *= s; - m_floats[1] *= s; - m_floats[2] *= s; - return *this; - } - - /**@brief Inversely scale the vector + SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& 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; - } + SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) + { + btFullAssert(s != btScalar(0.0)); + return * this *= btScalar(1.0) / s; + } - /**@brief Return the dot product + /**@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_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 + SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const + { + 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; + SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; - /**@brief Return the distance between the ends of this and another vector + /**@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; - - SIMD_FORCE_INLINE btVector3 &safeNormalize() { - btVector3 absVec = this->absolute(); - int32_t maxIndex = absVec.maxAxis(); - if (absVec[maxIndex] > 0) { - *this /= absVec[maxIndex]; - return *this /= length(); - } - setValue(1, 0, 0); - return *this; - } - - /**@brief Normalize this vector + SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; + + SIMD_FORCE_INLINE btVector3& safeNormalize() + { + btVector3 absVec = this->absolute(); + int32_t 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() { - return *this /= length(); - } + SIMD_FORCE_INLINE btVector3& normalize() + { + return * this /= length(); + } - /**@brief Return a normalized version of this vector */ - SIMD_FORCE_INLINE btVector3 normalized() const; + /**@brief Return a normalized version of this vector */ + SIMD_FORCE_INLINE btVector3 normalized() const; - /**@brief Return a rotated version of 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) const; + SIMD_FORCE_INLINE btVector3 rotate(const btVector3& wAxis, const btScalar angle) const; - /**@brief Return the angle between this and another vector + /**@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_floats[0]), - btFabs(m_floats[1]), - btFabs(m_floats[2])); - } - /**@brief Return the cross product between this and another 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_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_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_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 + SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const + { + return btVector3( + 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_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 int32_t 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); - } + SIMD_FORCE_INLINE int32_t 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 */ - SIMD_FORCE_INLINE int32_t 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); - } - - SIMD_FORCE_INLINE int32_t furthestAxis() const { - return absolute().minAxis(); - } - - SIMD_FORCE_INLINE int32_t closestAxis() const { - return absolute().maxAxis(); - } - - SIMD_FORCE_INLINE void setInterpolate3(const btVector3 &v0, const btVector3 &v1, btScalar rt) { - btScalar s = btScalar(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]; - } - - /**@brief Return the linear interpolation between this and another vector + SIMD_FORCE_INLINE int32_t 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); + } + + SIMD_FORCE_INLINE int32_t furthestAxis() const + { + return absolute().minAxis(); + } + + SIMD_FORCE_INLINE int32_t closestAxis() const + { + return absolute().maxAxis(); + } + + SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt) + { + btScalar s = btScalar(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]; + } + + /**@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_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 + SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const + { + 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_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[](int32_t i) { return (&m_floats[0])[i]; } - //SIMD_FORCE_INLINE const btScalar& operator[](int32_t 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 + SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) + { + 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[](int32_t i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE const btScalar& operator[](int32_t 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 + 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] = btScalar(0.); - } - - void getSkewSymmetricMatrix(btVector3 * v0, btVector3 * v1, btVector3 * v2) const { - v0->setValue(0., -z(), y()); - v1->setValue(z(), 0., -x()); - 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); + 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] = btScalar(0.); + } + + void getSkewSymmetricMatrix(btVector3 * v0, btVector3 * v1, btVector3 * v2) const + { + v0->setValue(0., -z(), y()); + v1->setValue(z(), 0., -x()); + 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)*/ SIMD_FORCE_INLINE btVector3 -operator+(const btVector3 &v1, const btVector3 &v2) { - 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]); +operator+(const btVector3& v1, const btVector3& v2) +{ + 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.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); +operator*(const btVector3& v1, const btVector3& v2) +{ + 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.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); +operator-(const btVector3& v1, const btVector3& v2) +{ + 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.m_floats[0], -v.m_floats[1], -v.m_floats[2]); +operator-(const btVector3& v) +{ + 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.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); +operator*(const btVector3& v, const btScalar& 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; +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) { - btFullAssert(s != btScalar(0.0)); - return v * (btScalar(1.0) / s); +operator/(const btVector3& v, const btScalar& s) +{ + btFullAssert(s != btScalar(0.0)); + 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.m_floats[0] / v2.m_floats[0], v1.m_floats[1] / v2.m_floats[1], v1.m_floats[2] / v2.m_floats[2]); +operator/(const btVector3& v1, const btVector3& v2) +{ + 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 -btDot(const btVector3 &v1, const btVector3 &v2) { - return v1.dot(v2); +btDot(const btVector3& v1, const btVector3& v2) +{ + return v1.dot(v2); } /**@brief Return the distance squared between two vectors */ SIMD_FORCE_INLINE btScalar -btDistance2(const btVector3 &v1, const btVector3 &v2) { - return v1.distance2(v2); +btDistance2(const btVector3& v1, const btVector3& v2) +{ + return v1.distance2(v2); } /**@brief Return the distance between two vectors */ SIMD_FORCE_INLINE btScalar -btDistance(const btVector3 &v1, const btVector3 &v2) { - return v1.distance(v2); +btDistance(const btVector3& v1, const btVector3& v2) +{ + return v1.distance(v2); } /**@brief Return the angle between two vectors */ SIMD_FORCE_INLINE btScalar -btAngle(const btVector3 &v1, const btVector3 &v2) { - return v1.angle(v2); +btAngle(const btVector3& v1, const btVector3& v2) +{ + return v1.angle(v2); } /**@brief Return the cross product of two vectors */ SIMD_FORCE_INLINE btVector3 -btCross(const btVector3 &v1, const btVector3 &v2) { - return v1.cross(v2); +btCross(const btVector3& v1, const btVector3& v2) +{ + return v1.cross(v2); } SIMD_FORCE_INLINE btScalar -btTriple(const btVector3 &v1, const btVector3 &v2, const btVector3 &v3) { - return v1.triple(v2, v3); +btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3) +{ + return v1.triple(v2, v3); } /**@brief Return the linear interpolation between two vectors @@ -418,236 +462,254 @@ btTriple(const btVector3 &v1, const btVector3 &v2, const btVector3 &v3) { * @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) { - return v1.lerp(v2, t); +lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) +{ + return v1.lerp(v2, t); } -SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3 &v) const { - return (v - *this).length2(); +SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const +{ + return (v - *this).length2(); } -SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3 &v) const { - return (v - *this).length(); +SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const +{ + return (v - *this).length(); } -SIMD_FORCE_INLINE btVector3 btVector3::normalized() const { - return *this / length(); +SIMD_FORCE_INLINE btVector3 btVector3::normalized() const +{ + return *this / length(); } -SIMD_FORCE_INLINE btVector3 btVector3::rotate(const btVector3 &wAxis, const btScalar angle) const { - // wAxis must be a unit lenght vector +SIMD_FORCE_INLINE btVector3 btVector3::rotate(const btVector3& wAxis, const btScalar angle) const +{ + // wAxis must be a unit lenght vector - btVector3 o = wAxis * wAxis.dot(*this); - btVector3 x = *this - o; - btVector3 y; + btVector3 o = wAxis * wAxis.dot(*this); + btVector3 x = *this - o; + btVector3 y; - y = wAxis.cross(*this); + y = wAxis.cross(*this); - return (o + x * btCos(angle) + y * btSin(angle)); + return (o + x * btCos(angle) + y * btSin(angle)); } class btVector4 : public btVector3 { public: - SIMD_FORCE_INLINE btVector4() {} - - SIMD_FORCE_INLINE btVector4(const btScalar &x, const btScalar &y, const btScalar &z, const btScalar &w) : - btVector3(x, y, z) { - m_floats[3] = w; - } - - SIMD_FORCE_INLINE btVector4 absolute4() const { - return btVector4( - btFabs(m_floats[0]), - btFabs(m_floats[1]), - btFabs(m_floats[2]), - btFabs(m_floats[3])); - } - - btScalar getW() const { return m_floats[3]; } - - SIMD_FORCE_INLINE int32_t maxAxis4() const { - int32_t maxIndex = -1; - btScalar maxVal = btScalar(-BT_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; - } - - SIMD_FORCE_INLINE int32_t minAxis4() const { - int32_t minIndex = -1; - btScalar minVal = btScalar(BT_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; - } - - return minIndex; - } - - SIMD_FORCE_INLINE int32_t closestAxis4() const { - return absolute4().maxAxis4(); - } - - /**@brief Set x,y,z and zero w + SIMD_FORCE_INLINE btVector4() {} + + SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) + : btVector3(x, y, z) + { + m_floats[3] = w; + } + + SIMD_FORCE_INLINE btVector4 absolute4() const + { + return btVector4( + btFabs(m_floats[0]), + btFabs(m_floats[1]), + btFabs(m_floats[2]), + btFabs(m_floats[3])); + } + + btScalar getW() const { return m_floats[3]; } + + SIMD_FORCE_INLINE int32_t maxAxis4() const + { + int32_t maxIndex = -1; + btScalar maxVal = btScalar(-BT_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; + } + + SIMD_FORCE_INLINE int32_t minAxis4() const + { + int32_t minIndex = -1; + btScalar minVal = btScalar(BT_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; + } + + return minIndex; + } + + SIMD_FORCE_INLINE int32_t 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(btScalar *m) const + /* void getValue(btScalar *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 * @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; - } + 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; + } }; ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar &sourceVal, btScalar &destVal) { +SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal) +{ #ifdef BT_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]; + 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]; + 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 //BT_USE_DOUBLE_PRECISION } ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3 &sourceVec, btVector3 &destVec) { - for (int32_t i = 0; i < 4; i++) { - btSwapScalarEndian(sourceVec[i], destVec[i]); - } +SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec) +{ + for (int32_t i = 0; i < 4; i++) { + btSwapScalarEndian(sourceVec[i], destVec[i]); + } } ///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3 &vector) { - - btVector3 swappedVec; - for (int32_t i = 0; i < 4; i++) { - btSwapScalarEndian(vector[i], swappedVec[i]); - } - vector = swappedVec; +SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector) +{ + + btVector3 swappedVec; + for (int32_t i = 0; i < 4; i++) { + btSwapScalarEndian(vector[i], swappedVec[i]); + } + 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; - } +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]; + float m_floats[4]; }; struct btVector3DoubleData { - double m_floats[4]; + 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 (int32_t i = 0; i < 4; i++) - dataOut.m_floats[i] = float(m_floats[i]); +SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int32_t i = 0; i < 4; i++) + dataOut.m_floats[i] = float(m_floats[i]); } -SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData &dataIn) { - for (int32_t i = 0; i < 4; i++) - m_floats[i] = btScalar(dataIn.m_floats[i]); +SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn) +{ + for (int32_t 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 (int32_t i = 0; i < 4; i++) - dataOut.m_floats[i] = double(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 (int32_t i = 0; i < 4; i++) + dataOut.m_floats[i] = double(m_floats[i]); } -SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData &dataIn) { - for (int32_t i = 0; i < 4; i++) - m_floats[i] = btScalar(dataIn.m_floats[i]); +SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn) +{ + for (int32_t 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 (int32_t i = 0; i < 4; i++) - dataOut.m_floats[i] = 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 (int32_t i = 0; i < 4; i++) + dataOut.m_floats[i] = m_floats[i]; } -SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data &dataIn) { - for (int32_t i = 0; i < 4; i++) - m_floats[i] = dataIn.m_floats[i]; +SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn) +{ + for (int32_t i = 0; i < 4; i++) + m_floats[i] = dataIn.m_floats[i]; } -//GODOT ADDITION -}; // namespace VHACD -// - #endif //BT_VECTOR3_H |