diff options
Diffstat (limited to 'thirdparty/bullet/LinearMath/btSpatialAlgebra.h')
| -rw-r--r-- | thirdparty/bullet/LinearMath/btSpatialAlgebra.h | 248 |
1 files changed, 153 insertions, 95 deletions
diff --git a/thirdparty/bullet/LinearMath/btSpatialAlgebra.h b/thirdparty/bullet/LinearMath/btSpatialAlgebra.h index 8e59658bca..6ad67a1081 100644 --- a/thirdparty/bullet/LinearMath/btSpatialAlgebra.h +++ b/thirdparty/bullet/LinearMath/btSpatialAlgebra.h @@ -12,18 +12,17 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -///These spatial algebra classes are used for btMultiBody, +///These spatial algebra classes are used for btMultiBody, ///see BulletDynamics/Featherstone #ifndef BT_SPATIAL_ALGEBRA_H #define BT_SPATIAL_ALGEBRA_H - #include "btMatrix3x3.h" struct btSpatialForceVector -{ - btVector3 m_topVec, m_bottomVec; +{ + btVector3 m_topVec, m_bottomVec; // btSpatialForceVector() { setZero(); } btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} @@ -32,21 +31,34 @@ struct btSpatialForceVector setValue(ax, ay, az, lx, ly, lz); } // - void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; } + void setVector(const btVector3 &angular, const btVector3 &linear) + { + m_topVec = linear; + m_bottomVec = angular; + } void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) { - m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz); + m_bottomVec.setValue(ax, ay, az); + m_topVec.setValue(lx, ly, lz); } // - void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addVector(const btVector3 &angular, const btVector3 &linear) + { + m_topVec += linear; + m_bottomVec += angular; + } void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) { - m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az; - m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz; + m_bottomVec[0] += ax; + m_bottomVec[1] += ay; + m_bottomVec[2] += az; + m_topVec[0] += lx; + m_topVec[1] += ly; + m_topVec[2] += lz; } // - const btVector3 & getLinear() const { return m_topVec; } - const btVector3 & getAngular() const { return m_bottomVec; } + const btVector3 &getLinear() const { return m_topVec; } + const btVector3 &getAngular() const { return m_bottomVec; } // void setLinear(const btVector3 &linear) { m_topVec = linear; } void setAngular(const btVector3 &angular) { m_bottomVec = angular; } @@ -54,14 +66,28 @@ struct btSpatialForceVector void addAngular(const btVector3 &angular) { m_bottomVec += angular; } void addLinear(const btVector3 &linear) { m_topVec += linear; } // - void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + void setZero() + { + m_topVec.setZero(); + m_bottomVec.setZero(); + } // - btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } - btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } - btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } - btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } - btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } - btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } + btSpatialForceVector &operator+=(const btSpatialForceVector &vec) + { + m_topVec += vec.m_topVec; + m_bottomVec += vec.m_bottomVec; + return *this; + } + btSpatialForceVector &operator-=(const btSpatialForceVector &vec) + { + m_topVec -= vec.m_topVec; + m_bottomVec -= vec.m_bottomVec; + return *this; + } + btSpatialForceVector operator-(const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } + btSpatialForceVector operator+(const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } + btSpatialForceVector operator-() const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } + btSpatialForceVector operator*(const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; } }; @@ -70,23 +96,36 @@ struct btSpatialMotionVector btVector3 m_topVec, m_bottomVec; // btSpatialMotionVector() { setZero(); } - btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} + btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} // - void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; } + void setVector(const btVector3 &angular, const btVector3 &linear) + { + m_topVec = angular; + m_bottomVec = linear; + } void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) { - m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz); + m_topVec.setValue(ax, ay, az); + m_bottomVec.setValue(lx, ly, lz); } // - void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addVector(const btVector3 &angular, const btVector3 &linear) + { + m_topVec += linear; + m_bottomVec += angular; + } void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) { - m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az; - m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz; + m_topVec[0] += ax; + m_topVec[1] += ay; + m_topVec[2] += az; + m_bottomVec[0] += lx; + m_bottomVec[1] += ly; + m_bottomVec[2] += lz; } - // - const btVector3 & getAngular() const { return m_topVec; } - const btVector3 & getLinear() const { return m_bottomVec; } + // + const btVector3 &getAngular() const { return m_topVec; } + const btVector3 &getLinear() const { return m_bottomVec; } // void setAngular(const btVector3 &angular) { m_topVec = angular; } void setLinear(const btVector3 &linear) { m_bottomVec = linear; } @@ -94,20 +133,24 @@ struct btSpatialMotionVector void addAngular(const btVector3 &angular) { m_topVec += angular; } void addLinear(const btVector3 &linear) { m_bottomVec += linear; } // - void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + void setZero() + { + m_topVec.setZero(); + m_bottomVec.setZero(); + } // btScalar dot(const btSpatialForceVector &b) const { return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec); } // - template<typename SpatialVectorType> + template <typename SpatialVectorType> void cross(const SpatialVectorType &b, SpatialVectorType &out) const { out.m_topVec = m_topVec.cross(b.m_topVec); out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); } - template<typename SpatialVectorType> + template <typename SpatialVectorType> SpatialVectorType cross(const SpatialVectorType &b) const { SpatialVectorType out; @@ -116,21 +159,36 @@ struct btSpatialMotionVector return out; } // - btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } - btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } - btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; } - btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } - btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } - btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } - btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } + btSpatialMotionVector &operator+=(const btSpatialMotionVector &vec) + { + m_topVec += vec.m_topVec; + m_bottomVec += vec.m_bottomVec; + return *this; + } + btSpatialMotionVector &operator-=(const btSpatialMotionVector &vec) + { + m_topVec -= vec.m_topVec; + m_bottomVec -= vec.m_bottomVec; + return *this; + } + btSpatialMotionVector &operator*=(const btScalar &s) + { + m_topVec *= s; + m_bottomVec *= s; + return *this; + } + btSpatialMotionVector operator-(const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } + btSpatialMotionVector operator+(const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } + btSpatialMotionVector operator-() const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } + btSpatialMotionVector operator*(const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } }; struct btSymmetricSpatialDyad { btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; - // + // btSymmetricSpatialDyad() { setIdentity(); } - btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } + btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } // void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { @@ -146,17 +204,22 @@ struct btSymmetricSpatialDyad m_bottomLeftMat += bottomLeftMat; } // - void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); } + void setIdentity() + { + m_topLeftMat.setIdentity(); + m_topRightMat.setIdentity(); + m_bottomLeftMat.setIdentity(); + } // - btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat) + btSymmetricSpatialDyad &operator-=(const btSymmetricSpatialDyad &mat) { m_topLeftMat -= mat.m_topLeftMat; m_topRightMat -= mat.m_topRightMat; m_bottomLeftMat -= mat.m_bottomLeftMat; - return *this; + return *this; } // - btSpatialForceVector operator * (const btSpatialMotionVector &vec) + btSpatialForceVector operator*(const btSpatialMotionVector &vec) { return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec); } @@ -164,7 +227,7 @@ struct btSymmetricSpatialDyad struct btSpatialTransformationMatrix { - btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; + btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; btVector3 m_trnVec; // enum eOutputOperation @@ -174,128 +237,124 @@ struct btSpatialTransformationMatrix Subtract = 2 }; // - template<typename SpatialVectorType> - void transform( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) + template <typename SpatialVectorType> + void transform(const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) { - if(outOp == None) + if (outOp == None) { outVec.m_topVec = m_rotMat * inVec.m_topVec; outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; } - else if(outOp == Add) + else if (outOp == Add) { outVec.m_topVec += m_rotMat * inVec.m_topVec; outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; } - else if(outOp == Subtract) + else if (outOp == Subtract) { outVec.m_topVec -= m_rotMat * inVec.m_topVec; outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; } - } - template<typename SpatialVectorType> - void transformRotationOnly( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) + template <typename SpatialVectorType> + void transformRotationOnly(const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) { - if(outOp == None) + if (outOp == None) { outVec.m_topVec = m_rotMat * inVec.m_topVec; outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; } - else if(outOp == Add) + else if (outOp == Add) { outVec.m_topVec += m_rotMat * inVec.m_topVec; outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; } - else if(outOp == Subtract) + else if (outOp == Subtract) { outVec.m_topVec -= m_rotMat * inVec.m_topVec; outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; } - } - template<typename SpatialVectorType> - void transformInverse( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) + template <typename SpatialVectorType> + void transformInverse(const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) { - if(outOp == None) + if (outOp == None) { outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); } - else if(outOp == Add) + else if (outOp == Add) { outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); } - else if(outOp == Subtract) + else if (outOp == Subtract) { outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); - } + } } - template<typename SpatialVectorType> - void transformInverseRotationOnly( const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation outOp = None) + template <typename SpatialVectorType> + void transformInverseRotationOnly(const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) { - if(outOp == None) + if (outOp == None) { outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec; } - else if(outOp == Add) + else if (outOp == Add) { outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; } - else if(outOp == Subtract) + else if (outOp == Subtract) { outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; } - } - void transformInverse( const btSymmetricSpatialDyad &inMat, - btSymmetricSpatialDyad &outMat, - eOutputOperation outOp = None) + void transformInverse(const btSymmetricSpatialDyad &inMat, + btSymmetricSpatialDyad &outMat, + eOutputOperation outOp = None) { - const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1], - m_trnVec[2], 0, -m_trnVec[0], - -m_trnVec[1], m_trnVec[0], 0); + const btMatrix3x3 r_cross(0, -m_trnVec[2], m_trnVec[1], + m_trnVec[2], 0, -m_trnVec[0], + -m_trnVec[1], m_trnVec[0], 0); - - if(outOp == None) + if (outOp == None) { - outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topLeftMat = m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat; outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; } - else if(outOp == Add) + else if (outOp == Add) { - outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topLeftMat += m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat; outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; } - else if(outOp == Subtract) + else if (outOp == Subtract) { - outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topLeftMat -= m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat; outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; } } - template<typename SpatialVectorType> - SpatialVectorType operator * (const SpatialVectorType &vec) + template <typename SpatialVectorType> + SpatialVectorType operator*(const SpatialVectorType &vec) { SpatialVectorType out; transform(vec, out); @@ -303,7 +362,7 @@ struct btSpatialTransformationMatrix } }; -template<typename SpatialVectorType> +template <typename SpatialVectorType> void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) { //output op maybe? @@ -314,7 +373,7 @@ void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVecto //maybe simple a*spatTranspose(a) would be nicer? } -template<typename SpatialVectorType> +template <typename SpatialVectorType> btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) { btSymmetricSpatialDyad out; @@ -327,5 +386,4 @@ btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, //maybe simple a*spatTranspose(a) would be nicer? } -#endif //BT_SPATIAL_ALGEBRA_H - +#endif //BT_SPATIAL_ALGEBRA_H |