Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

248
src/LinearMath/btSpatialAlgebra.h
View File

@@ -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