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
153 template <
typename SpatialVectorType>
154 SpatialVectorType
cross(
const SpatialVectorType &
b)
const
156 SpatialVectorType
out;
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;
365 template <
typename SpatialVectorType>
376 template <
typename SpatialVectorType>
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
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)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
static const pxr::TfToken out("out", pxr::TfToken::Immortal)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
void addLinear(const btVector3 &linear)
const btVector3 & getAngular() const
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
void setAngular(const btVector3 &angular)
void addVector(const btVector3 &angular, const btVector3 &linear)
btSpatialForceVector & operator+=(const btSpatialForceVector &vec)
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 btSpatialForceVector &vec)
btSpatialForceVector operator*(const btScalar &s) const
btSpatialForceVector operator-() 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)
const btVector3 & getLinear() const
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)
btSpatialMotionVector & operator*=(const btScalar &s)
btSpatialMotionVector operator-() const
const btVector3 & getAngular() 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)
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 btSpatialMotionVector &vec)
const btVector3 & getLinear() const
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(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
btMatrix3x3 m_topRightMat
btMatrix3x3 m_bottomLeftMat
btSymmetricSpatialDyad & operator-=(const btSymmetricSpatialDyad &mat)
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)