diff options
Diffstat (limited to 'thirdparty/bullet/LinearMath/btSpatialAlgebra.h')
-rw-r--r-- | thirdparty/bullet/LinearMath/btSpatialAlgebra.h | 389 |
1 files changed, 0 insertions, 389 deletions
diff --git a/thirdparty/bullet/LinearMath/btSpatialAlgebra.h b/thirdparty/bullet/LinearMath/btSpatialAlgebra.h deleted file mode 100644 index 6ad67a1081..0000000000 --- a/thirdparty/bullet/LinearMath/btSpatialAlgebra.h +++ /dev/null @@ -1,389 +0,0 @@ -/* -Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien - -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. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -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. -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. -*/ - -///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; - // - btSpatialForceVector() { setZero(); } - 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) - { - setValue(ax, ay, az, lx, ly, lz); - } - // - 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); - } - // - 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; - } - // - 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; } - // - void addAngular(const btVector3 &angular) { m_bottomVec += angular; } - void addLinear(const btVector3 &linear) { m_topVec += linear; } - // - 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; } -}; - -struct btSpatialMotionVector -{ - btVector3 m_topVec, m_bottomVec; - // - btSpatialMotionVector() { setZero(); } - 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 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); - } - // - 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; - } - // - 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; } - // - void addAngular(const btVector3 &angular) { m_topVec += angular; } - void addLinear(const btVector3 &linear) { m_bottomVec += linear; } - // - 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> - 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> - SpatialVectorType cross(const SpatialVectorType &b) const - { - SpatialVectorType out; - out.m_topVec = m_topVec.cross(b.m_topVec); - out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); - 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); } -}; - -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); } - // - void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) - { - m_topLeftMat = topLeftMat; - m_topRightMat = topRightMat; - m_bottomLeftMat = bottomLeftMat; - } - // - void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) - { - m_topLeftMat += topLeftMat; - m_topRightMat += topRightMat; - m_bottomLeftMat += bottomLeftMat; - } - // - void setIdentity() - { - m_topLeftMat.setIdentity(); - m_topRightMat.setIdentity(); - m_bottomLeftMat.setIdentity(); - } - // - btSymmetricSpatialDyad &operator-=(const btSymmetricSpatialDyad &mat) - { - m_topLeftMat -= mat.m_topLeftMat; - m_topRightMat -= mat.m_topRightMat; - m_bottomLeftMat -= mat.m_bottomLeftMat; - return *this; - } - // - 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); - } -}; - -struct btSpatialTransformationMatrix -{ - btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; - btVector3 m_trnVec; - // - enum eOutputOperation - { - None = 0, - Add = 1, - Subtract = 2 - }; - // - template <typename SpatialVectorType> - void transform(const SpatialVectorType &inVec, - SpatialVectorType &outVec, - eOutputOperation 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) - { - 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) - { - 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) - { - if (outOp == None) - { - outVec.m_topVec = m_rotMat * inVec.m_topVec; - outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; - } - else if (outOp == Add) - { - outVec.m_topVec += m_rotMat * inVec.m_topVec; - outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; - } - 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) - { - 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) - { - 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) - { - 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) - { - 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) - { - outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; - outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; - } - 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) - { - 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) - { - 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) - { - 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) - { - 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) - { - SpatialVectorType out; - transform(vec, out); - return out; - } -}; - -template <typename SpatialVectorType> -void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) -{ - //output op maybe? - - out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); - out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); - out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); - //maybe simple a*spatTranspose(a) would be nicer? -} - -template <typename SpatialVectorType> -btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) -{ - btSymmetricSpatialDyad out; - - out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); - out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); - out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); - - return out; - //maybe simple a*spatTranspose(a) would be nicer? -} - -#endif //BT_SPATIAL_ALGEBRA_H |