diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Common/b3Transform.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Common/b3Transform.h | 206 |
1 files changed, 94 insertions, 112 deletions
diff --git a/thirdparty/bullet/Bullet3Common/b3Transform.h b/thirdparty/bullet/Bullet3Common/b3Transform.h index fa480759be..149da9d148 100644 --- a/thirdparty/bullet/Bullet3Common/b3Transform.h +++ b/thirdparty/bullet/Bullet3Common/b3Transform.h @@ -12,11 +12,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef B3_TRANSFORM_H #define B3_TRANSFORM_H - #include "b3Matrix3x3.h" #ifdef B3_USE_DOUBLE_PRECISION @@ -25,46 +23,45 @@ subject to the following restrictions: #define b3TransformData b3TransformFloatData #endif - - - /**@brief The b3Transform class supports rigid transforms with only translation and rotation and no scaling/shear. *It can be used in combination with b3Vector3, b3Quaternion and b3Matrix3x3 linear algebra classes. */ -B3_ATTRIBUTE_ALIGNED16(class) b3Transform { - - ///Storage for the rotation +B3_ATTRIBUTE_ALIGNED16(class) +b3Transform +{ + ///Storage for the rotation b3Matrix3x3 m_basis; - ///Storage for the translation - b3Vector3 m_origin; + ///Storage for the translation + b3Vector3 m_origin; public: - - /**@brief No initialization constructor */ + /**@brief No initialization constructor */ b3Transform() {} - /**@brief Constructor from b3Quaternion (optional b3Vector3 ) + /**@brief Constructor from b3Quaternion (optional b3Vector3 ) * @param q Rotation from quaternion * @param c Translation from Vector (default 0,0,0) */ - explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q, - const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0))) + explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q, + const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0))) : m_basis(q), - m_origin(c) - {} + m_origin(c) + { + } - /**@brief Constructor from b3Matrix3x3 (optional b3Vector3) + /**@brief Constructor from b3Matrix3x3 (optional b3Vector3) * @param b Rotation from Matrix * @param c Translation from Vector default (0,0,0)*/ - explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b, - const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0))) + explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b, + const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0))) : m_basis(b), - m_origin(c) - {} - /**@brief Copy constructor */ - B3_FORCE_INLINE b3Transform (const b3Transform& other) + m_origin(c) + { + } + /**@brief Copy constructor */ + B3_FORCE_INLINE b3Transform(const b3Transform& other) : m_basis(other.m_basis), - m_origin(other.m_origin) + m_origin(other.m_origin) { } - /**@brief Assignment Operator */ + /**@brief Assignment Operator */ B3_FORCE_INLINE b3Transform& operator=(const b3Transform& other) { m_basis = other.m_basis; @@ -72,70 +69,70 @@ public: return *this; } - - /**@brief Set the current transform as the value of the product of two transforms + /**@brief Set the current transform as the value of the product of two transforms * @param t1 Transform 1 * @param t2 Transform 2 * This = Transform1 * Transform2 */ - B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2) { - m_basis = t1.m_basis * t2.m_basis; - m_origin = t1(t2.m_origin); - } + B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2) + { + m_basis = t1.m_basis * t2.m_basis; + m_origin = t1(t2.m_origin); + } -/* void multInverseLeft(const b3Transform& t1, const b3Transform& t2) { + /* void multInverseLeft(const b3Transform& t1, const b3Transform& t2) { b3Vector3 v = t2.m_origin - t1.m_origin; m_basis = b3MultTransposeLeft(t1.m_basis, t2.m_basis); m_origin = v * t1.m_basis; } */ -/**@brief Return the transform of the vector */ + /**@brief Return the transform of the vector */ B3_FORCE_INLINE b3Vector3 operator()(const b3Vector3& x) const { - return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin; + return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin; } - /**@brief Return the transform of the vector */ + /**@brief Return the transform of the vector */ B3_FORCE_INLINE b3Vector3 operator*(const b3Vector3& x) const { return (*this)(x); } - /**@brief Return the transform of the b3Quaternion */ + /**@brief Return the transform of the b3Quaternion */ B3_FORCE_INLINE b3Quaternion operator*(const b3Quaternion& q) const { return getRotation() * q; } - /**@brief Return the basis matrix for the rotation */ - B3_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; } - /**@brief Return the basis matrix for the rotation */ - B3_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; } + /**@brief Return the basis matrix for the rotation */ + B3_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; } + /**@brief Return the basis matrix for the rotation */ + B3_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; } - /**@brief Return the origin vector translation */ - B3_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; } - /**@brief Return the origin vector translation */ - B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; } + /**@brief Return the origin vector translation */ + B3_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; } + /**@brief Return the origin vector translation */ + B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; } - /**@brief Return a quaternion representing the rotation */ - b3Quaternion getRotation() const { + /**@brief Return a quaternion representing the rotation */ + b3Quaternion getRotation() const + { b3Quaternion q; m_basis.getRotation(q); return q; } - - - /**@brief Set from an array + + /**@brief Set from an array * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - void setFromOpenGLMatrix(const b3Scalar *m) + void setFromOpenGLMatrix(const b3Scalar* m) { m_basis.setFromOpenGLSubMatrix(m); - m_origin.setValue(m[12],m[13],m[14]); + m_origin.setValue(m[12], m[13], m[14]); } - /**@brief Fill an array representation + /**@brief Fill an array representation * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - void getOpenGLMatrix(b3Scalar *m) const + void getOpenGLMatrix(b3Scalar * m) const { m_basis.getOpenGLSubMatrix(m); m[12] = m_origin.getX(); @@ -144,80 +141,76 @@ public: m[15] = b3Scalar(1.0); } - /**@brief Set the translational element + /**@brief Set the translational element * @param origin The vector to set the translation to */ - B3_FORCE_INLINE void setOrigin(const b3Vector3& origin) - { + B3_FORCE_INLINE void setOrigin(const b3Vector3& origin) + { m_origin = origin; } B3_FORCE_INLINE b3Vector3 invXform(const b3Vector3& inVec) const; - - /**@brief Set the rotational element by b3Matrix3x3 */ + /**@brief Set the rotational element by b3Matrix3x3 */ B3_FORCE_INLINE void setBasis(const b3Matrix3x3& basis) - { + { m_basis = basis; } - /**@brief Set the rotational element by b3Quaternion */ + /**@brief Set the rotational element by b3Quaternion */ B3_FORCE_INLINE void setRotation(const b3Quaternion& q) { m_basis.setRotation(q); } - - /**@brief Set this transformation to the identity */ + /**@brief Set this transformation to the identity */ void setIdentity() { m_basis.setIdentity(); m_origin.setValue(b3Scalar(0.0), b3Scalar(0.0), b3Scalar(0.0)); } - /**@brief Multiply this Transform by another(this = this * another) + /**@brief Multiply this Transform by another(this = this * another) * @param t The other transform */ - b3Transform& operator*=(const b3Transform& t) + b3Transform& operator*=(const b3Transform& t) { m_origin += m_basis * t.m_origin; m_basis *= t.m_basis; return *this; } - /**@brief Return the inverse of this transform */ + /**@brief Return the inverse of this transform */ b3Transform inverse() const - { + { b3Matrix3x3 inv = m_basis.transpose(); return b3Transform(inv, inv * -m_origin); } - /**@brief Return the inverse of this transform times the other transform + /**@brief Return the inverse of this transform times the other transform * @param t The other transform * return this.inverse() * the other */ - b3Transform inverseTimes(const b3Transform& t) const; + b3Transform inverseTimes(const b3Transform& t) const; - /**@brief Return the product of this transform and the other */ + /**@brief Return the product of this transform and the other */ b3Transform operator*(const b3Transform& t) const; - /**@brief Return an identity transform */ - static const b3Transform& getIdentity() + /**@brief Return an identity transform */ + static const b3Transform& getIdentity() { static const b3Transform identityTransform(b3Matrix3x3::getIdentity()); return identityTransform; } - void serialize(struct b3TransformData& dataOut) const; - - void serializeFloat(struct b3TransformFloatData& dataOut) const; + void serialize(struct b3TransformData & dataOut) const; - void deSerialize(const struct b3TransformData& dataIn); + void serializeFloat(struct b3TransformFloatData & dataOut) const; - void deSerializeDouble(const struct b3TransformDoubleData& dataIn); + void deSerialize(const struct b3TransformData& dataIn); - void deSerializeFloat(const struct b3TransformFloatData& dataIn); + void deSerializeDouble(const struct b3TransformDoubleData& dataIn); + void deSerializeFloat(const struct b3TransformFloatData& dataIn); }; - B3_FORCE_INLINE b3Vector3 b3Transform::invXform(const b3Vector3& inVec) const { @@ -225,80 +218,69 @@ b3Transform::invXform(const b3Vector3& inVec) const return (m_basis.transpose() * v); } -B3_FORCE_INLINE b3Transform -b3Transform::inverseTimes(const b3Transform& t) const +B3_FORCE_INLINE b3Transform +b3Transform::inverseTimes(const b3Transform& t) const { b3Vector3 v = t.getOrigin() - m_origin; - return b3Transform(m_basis.transposeTimes(t.m_basis), - v * m_basis); + return b3Transform(m_basis.transposeTimes(t.m_basis), + v * m_basis); } -B3_FORCE_INLINE b3Transform -b3Transform::operator*(const b3Transform& t) const +B3_FORCE_INLINE b3Transform + b3Transform::operator*(const b3Transform& t) const { - return b3Transform(m_basis * t.m_basis, - (*this)(t.m_origin)); + return b3Transform(m_basis * t.m_basis, + (*this)(t.m_origin)); } /**@brief Test if two transforms have all elements equal */ B3_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2) { - return ( t1.getBasis() == t2.getBasis() && - t1.getOrigin() == t2.getOrigin() ); + return (t1.getBasis() == t2.getBasis() && + t1.getOrigin() == t2.getOrigin()); } - ///for serialization -struct b3TransformFloatData +struct b3TransformFloatData { - b3Matrix3x3FloatData m_basis; - b3Vector3FloatData m_origin; + b3Matrix3x3FloatData m_basis; + b3Vector3FloatData m_origin; }; -struct b3TransformDoubleData +struct b3TransformDoubleData { - b3Matrix3x3DoubleData m_basis; - b3Vector3DoubleData m_origin; + b3Matrix3x3DoubleData m_basis; + b3Vector3DoubleData m_origin; }; - - -B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const +B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const { m_basis.serialize(dataOut.m_basis); m_origin.serialize(dataOut.m_origin); } -B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const +B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const { m_basis.serializeFloat(dataOut.m_basis); m_origin.serializeFloat(dataOut.m_origin); } - -B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn) +B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn) { m_basis.deSerialize(dataIn.m_basis); m_origin.deSerialize(dataIn.m_origin); } -B3_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn) +B3_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn) { m_basis.deSerializeFloat(dataIn.m_basis); m_origin.deSerializeFloat(dataIn.m_origin); } -B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn) +B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn) { m_basis.deSerializeDouble(dataIn.m_basis); m_origin.deSerializeDouble(dataIn.m_origin); } - -#endif //B3_TRANSFORM_H - - - - - - +#endif //B3_TRANSFORM_H |