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

add BspDemo.bsp data file

Browse Source 
add sphere2.urdf
move btSpatialAlgebra into LinearMath
remove some warnings, introduce BT_ZERO, BT_ONE, BT_HALF as defines for 0.f/0., 1.f/1., 0.5f/0.5 respectively
This commit is contained in:
erwincoumans
2015-04-16 10:17:35 -07:00
parent a1bf9c5556
commit 794c8ec064
19 changed files with 448 additions and 420 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@@ -772,14 +772,14 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
//info->m_lowerLimit[srow] = -SIMD_INFINITY;
//info->m_upperLimit[srow] = SIMD_INFINITY;
btScalar dt = 1.0 / info->fps;
btScalar dt = BT_ONE / info->fps;
btScalar kd = limot->m_springDamping;
btScalar ks = limot->m_springStiffness;
btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
// btScalar erp = 0.1;
btScalar cfm = 0.0;
btScalar mA = 1.0 / m_rbA.getInvMass();
btScalar mB = 1.0 / m_rbB.getInvMass();
btScalar cfm = BT_ZERO;
btScalar mA = BT_ONE / m_rbA.getInvMass();
btScalar mB = BT_ONE / m_rbB.getInvMass();
btScalar m = mA > mB ? mB : mA;
btScalar angularfreq = sqrt(ks / m);
@@ -787,7 +787,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
if( 0.25 < angularfreq * dt)
{
ks = 1.0 / dt / dt / 16.0 / m;
ks = BT_ONE / dt / dt / btScalar(16.0) / m;
}
//avoid overdamping
if(kd * dt > m)

2
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -303,7 +303,7 @@ void btHingeConstraint::buildJacobian()
static inline btScalar btNormalizeAnglePositive(btScalar angle)
{
return btFmod(btFmod(angle, 2.0*SIMD_PI) + 2.0*SIMD_PI, 2.0*SIMD_PI);
return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
}

306
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -36,311 +36,7 @@ enum btMultiBodyLinkFlags
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
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?
}
#include "LinearMath/btSpatialAlgebra.h"
#endif
//}