Changeset View
Changeset View
Standalone View
Standalone View
extern/bullet2/src/LinearMath/btSpatialAlgebra.h
| /* | /* | ||||
| Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien | Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien | ||||
| This software is provided 'as-is', without any express or implied warranty. | This software is provided 'as-is', without any express or implied warranty. | ||||
| In no event will the authors be held liable for any damages arising from the use of this software. | In no event will the authors be held liable for any damages arising from the use of this software. | ||||
| Permission is granted to anyone to use this software for any purpose, | Permission is granted to anyone to use this software for any purpose, | ||||
| including commercial applications, and to alter it and redistribute it freely, | including commercial applications, and to alter it and redistribute it freely, | ||||
| subject to the following restrictions: | subject to the following restrictions: | ||||
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | ||||
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | ||||
| 3. This notice may not be removed or altered from any source distribution. | 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 | ///see BulletDynamics/Featherstone | ||||
| #ifndef BT_SPATIAL_ALGEBRA_H | #ifndef BT_SPATIAL_ALGEBRA_H | ||||
| #define BT_SPATIAL_ALGEBRA_H | #define BT_SPATIAL_ALGEBRA_H | ||||
| #include "btMatrix3x3.h" | #include "btMatrix3x3.h" | ||||
| struct btSpatialForceVector | struct btSpatialForceVector | ||||
| { | { | ||||
| btVector3 m_topVec, m_bottomVec; | btVector3 m_topVec, m_bottomVec; | ||||
| // | // | ||||
| btSpatialForceVector() { setZero(); } | btSpatialForceVector() { setZero(); } | ||||
| btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} | btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} | ||||
| btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) | btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) | ||||
| { | { | ||||
| setValue(ax, ay, az, lx, ly, lz); | 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) | 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) | 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_bottomVec[0] += ax; | ||||
| m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz; | 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 &getLinear() const { return m_topVec; } | ||||
| const btVector3 & getAngular() const { return m_bottomVec; } | const btVector3 &getAngular() const { return m_bottomVec; } | ||||
| // | // | ||||
| void setLinear(const btVector3 &linear) { m_topVec = linear; } | void setLinear(const btVector3 &linear) { m_topVec = linear; } | ||||
| void setAngular(const btVector3 &angular) { m_bottomVec = angular; } | void setAngular(const btVector3 &angular) { m_bottomVec = angular; } | ||||
| // | // | ||||
| void addAngular(const btVector3 &angular) { m_bottomVec += angular; } | void addAngular(const btVector3 &angular) { m_bottomVec += angular; } | ||||
| void addLinear(const btVector3 &linear) { m_topVec += linear; } | 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) | ||||
| btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } | { | ||||
| 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 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 { return btSpatialForceVector(-m_bottomVec, -m_topVec); } | ||||
| btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * 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; } | ||||
| }; | }; | ||||
| struct btSpatialMotionVector | struct btSpatialMotionVector | ||||
| { | { | ||||
| btVector3 m_topVec, m_bottomVec; | btVector3 m_topVec, m_bottomVec; | ||||
| // | // | ||||
| btSpatialMotionVector() { setZero(); } | 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) | 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) | 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_topVec[0] += ax; | ||||
| m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz; | 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 &getAngular() const { return m_topVec; } | ||||
| const btVector3 & getLinear() const { return m_bottomVec; } | const btVector3 &getLinear() const { return m_bottomVec; } | ||||
| // | // | ||||
| void setAngular(const btVector3 &angular) { m_topVec = angular; } | void setAngular(const btVector3 &angular) { m_topVec = angular; } | ||||
| void setLinear(const btVector3 &linear) { m_bottomVec = linear; } | void setLinear(const btVector3 &linear) { m_bottomVec = linear; } | ||||
| // | // | ||||
| void addAngular(const btVector3 &angular) { m_topVec += angular; } | void addAngular(const btVector3 &angular) { m_topVec += angular; } | ||||
| void addLinear(const btVector3 &linear) { m_bottomVec += linear; } | 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 | btScalar dot(const btSpatialForceVector &b) const | ||||
| { | { | ||||
| return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec); | 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 | void cross(const SpatialVectorType &b, SpatialVectorType &out) const | ||||
| { | { | ||||
| out.m_topVec = m_topVec.cross(b.m_topVec); | out.m_topVec = m_topVec.cross(b.m_topVec); | ||||
| out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); | 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 cross(const SpatialVectorType &b) const | ||||
| { | { | ||||
| SpatialVectorType out; | SpatialVectorType out; | ||||
| out.m_topVec = m_topVec.cross(b.m_topVec); | out.m_topVec = m_topVec.cross(b.m_topVec); | ||||
| out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); | out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); | ||||
| return out; | return out; | ||||
| } | } | ||||
| // | // | ||||
| btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } | btSpatialMotionVector &operator+=(const btSpatialMotionVector &vec) | ||||
| 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; } | 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 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 { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } | ||||
| btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } | btSpatialMotionVector operator*(const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } | ||||
| }; | }; | ||||
| struct btSymmetricSpatialDyad | struct btSymmetricSpatialDyad | ||||
| { | { | ||||
| btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; | btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; | ||||
| // | // | ||||
| btSymmetricSpatialDyad() { setIdentity(); } | 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) | void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) | ||||
| { | { | ||||
| m_topLeftMat = topLeftMat; | m_topLeftMat = topLeftMat; | ||||
| m_topRightMat = topRightMat; | m_topRightMat = topRightMat; | ||||
| m_bottomLeftMat = bottomLeftMat; | m_bottomLeftMat = bottomLeftMat; | ||||
| } | } | ||||
| // | // | ||||
| void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) | void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) | ||||
| { | { | ||||
| m_topLeftMat += topLeftMat; | m_topLeftMat += topLeftMat; | ||||
| m_topRightMat += topRightMat; | m_topRightMat += topRightMat; | ||||
| m_bottomLeftMat += bottomLeftMat; | 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_topLeftMat -= mat.m_topLeftMat; | ||||
| m_topRightMat -= mat.m_topRightMat; | m_topRightMat -= mat.m_topRightMat; | ||||
| m_bottomLeftMat -= mat.m_bottomLeftMat; | 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); | return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec); | ||||
| } | } | ||||
| }; | }; | ||||
| struct btSpatialTransformationMatrix | struct btSpatialTransformationMatrix | ||||
| { | { | ||||
| btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; | btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; | ||||
| btVector3 m_trnVec; | btVector3 m_trnVec; | ||||
| // | // | ||||
| enum eOutputOperation | enum eOutputOperation | ||||
| { | { | ||||
| None = 0, | None = 0, | ||||
| Add = 1, | Add = 1, | ||||
| Subtract = 2 | Subtract = 2 | ||||
| }; | }; | ||||
| // | // | ||||
| template<typename SpatialVectorType> | template <typename SpatialVectorType> | ||||
| void transform( const SpatialVectorType &inVec, | void transform(const SpatialVectorType &inVec, | ||||
| SpatialVectorType &outVec, | SpatialVectorType &outVec, | ||||
| eOutputOperation outOp = None) | eOutputOperation outOp = None) | ||||
| { | { | ||||
| if(outOp == None) | if (outOp == None) | ||||
| { | { | ||||
| outVec.m_topVec = m_rotMat * inVec.m_topVec; | outVec.m_topVec = m_rotMat * inVec.m_topVec; | ||||
| outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; | 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_topVec += m_rotMat * inVec.m_topVec; | ||||
| outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; | 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_topVec -= m_rotMat * inVec.m_topVec; | ||||
| outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; | outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; | ||||
| } | } | ||||
| } | } | ||||
| template<typename SpatialVectorType> | template <typename SpatialVectorType> | ||||
| void transformRotationOnly( const SpatialVectorType &inVec, | void transformRotationOnly(const SpatialVectorType &inVec, | ||||
| SpatialVectorType &outVec, | SpatialVectorType &outVec, | ||||
| eOutputOperation outOp = None) | eOutputOperation outOp = None) | ||||
| { | { | ||||
| if(outOp == None) | if (outOp == None) | ||||
| { | { | ||||
| outVec.m_topVec = m_rotMat * inVec.m_topVec; | outVec.m_topVec = m_rotMat * inVec.m_topVec; | ||||
| outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; | 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_topVec += m_rotMat * inVec.m_topVec; | ||||
| outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; | 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_topVec -= m_rotMat * inVec.m_topVec; | ||||
| outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; | outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; | ||||
| } | } | ||||
| } | } | ||||
| template<typename SpatialVectorType> | template <typename SpatialVectorType> | ||||
| void transformInverse( const SpatialVectorType &inVec, | void transformInverse(const SpatialVectorType &inVec, | ||||
| SpatialVectorType &outVec, | SpatialVectorType &outVec, | ||||
| eOutputOperation outOp = None) | eOutputOperation outOp = None) | ||||
| { | { | ||||
| if(outOp == None) | if (outOp == None) | ||||
| { | { | ||||
| outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; | outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; | ||||
| outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(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_topVec += m_rotMat.transpose() * inVec.m_topVec; | ||||
| outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(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_topVec -= m_rotMat.transpose() * inVec.m_topVec; | ||||
| outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); | outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); | ||||
| } | } | ||||
| } | } | ||||
| template<typename SpatialVectorType> | template <typename SpatialVectorType> | ||||
| void transformInverseRotationOnly( const SpatialVectorType &inVec, | void transformInverseRotationOnly(const SpatialVectorType &inVec, | ||||
| SpatialVectorType &outVec, | SpatialVectorType &outVec, | ||||
| eOutputOperation outOp = None) | eOutputOperation outOp = None) | ||||
| { | { | ||||
| if(outOp == None) | if (outOp == None) | ||||
| { | { | ||||
| outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; | outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; | ||||
| outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec; | 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_topVec += m_rotMat.transpose() * inVec.m_topVec; | ||||
| outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; | 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_topVec -= m_rotMat.transpose() * inVec.m_topVec; | ||||
| outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; | outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; | ||||
| } | } | ||||
| } | } | ||||
| void transformInverse( const btSymmetricSpatialDyad &inMat, | void transformInverse(const btSymmetricSpatialDyad &inMat, | ||||
| btSymmetricSpatialDyad &outMat, | btSymmetricSpatialDyad &outMat, | ||||
| eOutputOperation outOp = None) | eOutputOperation outOp = None) | ||||
| { | { | ||||
| const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1], | const btMatrix3x3 r_cross(0, -m_trnVec[2], m_trnVec[1], | ||||
| m_trnVec[2], 0, -m_trnVec[0], | m_trnVec[2], 0, -m_trnVec[0], | ||||
| -m_trnVec[1], m_trnVec[0], 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_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; | 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_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; | 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_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; | 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> | template <typename SpatialVectorType> | ||||
| SpatialVectorType operator * (const SpatialVectorType &vec) | SpatialVectorType operator*(const SpatialVectorType &vec) | ||||
| { | { | ||||
| SpatialVectorType out; | SpatialVectorType out; | ||||
| transform(vec, out); | transform(vec, out); | ||||
| return out; | return out; | ||||
| } | } | ||||
| }; | }; | ||||
| template<typename SpatialVectorType> | template <typename SpatialVectorType> | ||||
| void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) | void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) | ||||
| { | { | ||||
| //output op maybe? | //output op maybe? | ||||
| out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); | out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); | ||||
| out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); | out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); | ||||
| out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); | out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); | ||||
| //maybe simple a*spatTranspose(a) would be nicer? | //maybe simple a*spatTranspose(a) would be nicer? | ||||
| } | } | ||||
| template<typename SpatialVectorType> | template <typename SpatialVectorType> | ||||
| btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) | btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) | ||||
| { | { | ||||
| btSymmetricSpatialDyad out; | btSymmetricSpatialDyad out; | ||||
| out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); | out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); | ||||
| out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); | out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); | ||||
| out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); | out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); | ||||
| return out; | return out; | ||||
| //maybe simple a*spatTranspose(a) would be nicer? | //maybe simple a*spatTranspose(a) would be nicer? | ||||
| } | } | ||||
| #endif //BT_SPATIAL_ALGEBRA_H | #endif //BT_SPATIAL_ALGEBRA_H | ||||