18#ifndef BT_SPATIAL_ALGEBRA_H
19#define BT_SPATIAL_ALGEBRA_H
147 template <
typename SpatialVectorType>
148 void cross(
const SpatialVectorType &b, SpatialVectorType &out)
const
150 out.m_topVec =
m_topVec.cross(b.m_topVec);
153 template <
typename SpatialVectorType>
154 SpatialVectorType
cross(
const SpatialVectorType &b)
const
156 SpatialVectorType out;
157 out.m_topVec =
m_topVec.cross(b.m_topVec);
240 template <
typename SpatialVectorType>
242 SpatialVectorType &outVec,
247 outVec.m_topVec =
m_rotMat * inVec.m_topVec;
248 outVec.m_bottomVec = -
m_trnVec.cross(outVec.m_topVec) +
m_rotMat * inVec.m_bottomVec;
250 else if (outOp ==
Add)
252 outVec.m_topVec +=
m_rotMat * inVec.m_topVec;
253 outVec.m_bottomVec += -
m_trnVec.cross(outVec.m_topVec) +
m_rotMat * inVec.m_bottomVec;
257 outVec.m_topVec -=
m_rotMat * inVec.m_topVec;
258 outVec.m_bottomVec -= -
m_trnVec.cross(outVec.m_topVec) +
m_rotMat * inVec.m_bottomVec;
262 template <
typename SpatialVectorType>
264 SpatialVectorType &outVec,
269 outVec.m_topVec =
m_rotMat * inVec.m_topVec;
270 outVec.m_bottomVec =
m_rotMat * inVec.m_bottomVec;
272 else if (outOp ==
Add)
274 outVec.m_topVec +=
m_rotMat * inVec.m_topVec;
275 outVec.m_bottomVec +=
m_rotMat * inVec.m_bottomVec;
279 outVec.m_topVec -=
m_rotMat * inVec.m_topVec;
280 outVec.m_bottomVec -=
m_rotMat * inVec.m_bottomVec;
284 template <
typename SpatialVectorType>
286 SpatialVectorType &outVec,
291 outVec.m_topVec =
m_rotMat.transpose() * inVec.m_topVec;
292 outVec.m_bottomVec =
m_rotMat.transpose() * (inVec.m_bottomVec +
m_trnVec.cross(inVec.m_topVec));
294 else if (outOp ==
Add)
296 outVec.m_topVec +=
m_rotMat.transpose() * inVec.m_topVec;
297 outVec.m_bottomVec +=
m_rotMat.transpose() * (inVec.m_bottomVec +
m_trnVec.cross(inVec.m_topVec));
301 outVec.m_topVec -=
m_rotMat.transpose() * inVec.m_topVec;
302 outVec.m_bottomVec -=
m_rotMat.transpose() * (inVec.m_bottomVec +
m_trnVec.cross(inVec.m_topVec));
306 template <
typename SpatialVectorType>
308 SpatialVectorType &outVec,
313 outVec.m_topVec =
m_rotMat.transpose() * inVec.m_topVec;
314 outVec.m_bottomVec =
m_rotMat.transpose() * inVec.m_bottomVec;
316 else if (outOp ==
Add)
318 outVec.m_topVec +=
m_rotMat.transpose() * inVec.m_topVec;
319 outVec.m_bottomVec +=
m_rotMat.transpose() * inVec.m_bottomVec;
323 outVec.m_topVec -=
m_rotMat.transpose() * inVec.m_topVec;
324 outVec.m_bottomVec -=
m_rotMat.transpose() * inVec.m_bottomVec;
342 else if (outOp ==
Add)
356 template <
typename SpatialVectorType>
357 SpatialVectorType
operator*(
const SpatialVectorType &vec)
359 SpatialVectorType out;
365template <
typename SpatialVectorType>
376template <
typename SpatialVectorType>
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btVector3 can be used to represent 3D points and vectors.
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
btSpatialForceVector & operator+=(const btSpatialForceVector &vec)
void addLinear(const btVector3 &linear)
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
btSpatialForceVector & operator-=(const btSpatialForceVector &vec)
void setAngular(const btVector3 &angular)
void addVector(const btVector3 &angular, const btVector3 &linear)
btSpatialForceVector(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)
btSpatialForceVector operator*(const btScalar &s) const
const btVector3 & getAngular() const
btSpatialForceVector operator-() const
const btVector3 & getLinear() const
btSpatialForceVector operator-(const btSpatialForceVector &vec) const
void addAngular(const btVector3 &angular)
btSpatialForceVector operator+(const btSpatialForceVector &vec) const
btSpatialForceVector(const btVector3 &angular, const btVector3 &linear)
void setLinear(const btVector3 &linear)
void setVector(const btVector3 &angular, const btVector3 &linear)
SpatialVectorType cross(const SpatialVectorType &b) const
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
void setLinear(const btVector3 &linear)
const btVector3 & getLinear() const
btSpatialMotionVector & operator-=(const btSpatialMotionVector &vec)
btSpatialMotionVector operator-() const
void addVector(const btVector3 &angular, const btVector3 &linear)
btSpatialMotionVector & operator+=(const btSpatialMotionVector &vec)
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
const btVector3 & getAngular() const
btScalar dot(const btSpatialForceVector &b) const
btSpatialMotionVector operator+(const btSpatialMotionVector &vec) const
void addAngular(const btVector3 &angular)
btSpatialMotionVector operator*(const btScalar &s) const
void setAngular(const btVector3 &angular)
btSpatialMotionVector & operator*=(const btScalar &s)
void addLinear(const btVector3 &linear)
btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear)
btSpatialMotionVector operator-(const btSpatialMotionVector &vec) const
void setVector(const btVector3 &angular, const btVector3 &linear)
btSpatialForceVector operator*(const btSpatialMotionVector &vec)
btSymmetricSpatialDyad & operator-=(const btSymmetricSpatialDyad &mat)
btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
btMatrix3x3 m_topRightMat
btMatrix3x3 m_bottomLeftMat
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)