remove a lot of warnings (more todo in demos and serialization code)
This commit is contained in:
Erwin Coumans
@@ -87,7 +87,10 @@ btMultiBody::btMultiBody(int n_links,
|
||||
bool fixedBase,
|
||||
bool canSleep,
|
||||
bool multiDof)
|
||||
: m_baseQuat(0, 0, 0, 1),
|
||||
:
|
||||
m_baseCollider(0),
|
||||
m_basePos(0,0,0),
|
||||
m_baseQuat(0, 0, 0, 1),
|
||||
m_baseMass(mass),
|
||||
m_baseInertia(inertia),
|
||||
|
||||
@@ -95,18 +98,19 @@ btMultiBody::btMultiBody(int n_links,
|
||||
m_awake(true),
|
||||
m_canSleep(canSleep),
|
||||
m_sleepTimer(0),
|
||||
m_baseCollider(0),
|
||||
|
||||
m_linearDamping(0.04f),
|
||||
m_angularDamping(0.04f),
|
||||
m_useGyroTerm(true),
|
||||
m_maxAppliedImpulse(1000.f),
|
||||
m_maxAppliedImpulse(1000.f),
|
||||
m_maxCoordinateVelocity(100.f),
|
||||
m_hasSelfCollision(true),
|
||||
m_dofCount(0),
|
||||
__posUpdated(false),
|
||||
m_hasSelfCollision(true),
|
||||
m_isMultiDof(multiDof),
|
||||
__posUpdated(false),
|
||||
m_dofCount(0),
|
||||
m_posVarCnt(0),
|
||||
m_useRK4(false), m_useGlobalVelocities(false)
|
||||
m_useRK4(false),
|
||||
m_useGlobalVelocities(false)
|
||||
{
|
||||
|
||||
if(!m_isMultiDof)
|
||||
@@ -119,7 +123,7 @@ btMultiBody::btMultiBody(int n_links,
|
||||
m_links.resize(n_links);
|
||||
m_matrixBuf.resize(n_links + 1);
|
||||
|
||||
m_basePos.setValue(0, 0, 0);
|
||||
|
||||
m_baseForce.setValue(0, 0, 0);
|
||||
m_baseTorque.setValue(0, 0, 0);
|
||||
}
|
||||
@@ -1257,7 +1261,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular()));
|
||||
//
|
||||
zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
|
||||
btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
|
||||
//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
|
||||
////clamp parent's omega
|
||||
//btScalar parOmegaMod = temp.length();
|
||||
//btScalar parOmegaModMax = 1000;
|
||||
@@ -2010,7 +2014,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
{
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]);
|
||||
//?? btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]);
|
||||
|
||||
Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
|
||||
- SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
|
||||
@@ -2352,7 +2356,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
|
||||
// 'Downward' loop.
|
||||
for (int i = num_links - 1; i >= 0; --i)
|
||||
{
|
||||
btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
|
||||
// btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
|
||||
Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
|
||||
Y[i] += force[6 + i]; // add joint torque
|
||||
|
||||
@@ -2437,7 +2441,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
|
||||
/////////////////
|
||||
*/
|
||||
|
||||
int dummy = 0;
|
||||
//int dummy = 0;
|
||||
}
|
||||
|
||||
void btMultiBody::stepPositions(btScalar dt)
|
||||
|
||||
@@ -510,8 +510,15 @@ public:
|
||||
void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
|
||||
bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
|
||||
|
||||
bool __posUpdated;
|
||||
|
||||
bool isPosUpdated() const
|
||||
{
|
||||
return __posUpdated;
|
||||
}
|
||||
void setPosUpdated(bool updated)
|
||||
{
|
||||
__posUpdated = updated;
|
||||
}
|
||||
|
||||
private:
|
||||
btMultiBody(const btMultiBody &); // not implemented
|
||||
void operator=(const btMultiBody &); // not implemented
|
||||
@@ -535,6 +542,7 @@ private:
|
||||
|
||||
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
|
||||
|
||||
|
||||
private:
|
||||
|
||||
btMultiBodyLinkCollider* m_baseCollider;//can be NULL
|
||||
@@ -550,9 +558,7 @@ private:
|
||||
|
||||
btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
|
||||
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
|
||||
int m_dofCount, m_posVarCnt;
|
||||
|
||||
bool m_useRK4, m_useGlobalVelocities;
|
||||
|
||||
//
|
||||
// realBuf:
|
||||
@@ -596,6 +602,9 @@ private:
|
||||
btScalar m_maxCoordinateVelocity;
|
||||
bool m_hasSelfCollision;
|
||||
bool m_isMultiDof;
|
||||
bool __posUpdated;
|
||||
int m_dofCount, m_posVarCnt;
|
||||
bool m_useRK4, m_useGlobalVelocities;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,364 +1,366 @@
|
||||
#include "btMultiBodyConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
|
||||
|
||||
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
|
||||
:m_bodyA(bodyA),
|
||||
m_bodyB(bodyB),
|
||||
m_linkA(linkA),
|
||||
m_linkB(linkB),
|
||||
m_numRows(numRows),
|
||||
m_isUnilateral(isUnilateral),
|
||||
m_maxAppliedImpulse(100),
|
||||
m_jacSizeA(0),
|
||||
m_jacSizeBoth(0)
|
||||
{
|
||||
|
||||
if(bodyA)
|
||||
{
|
||||
if(bodyA->isMultiDof())
|
||||
m_jacSizeA = (6 + bodyA->getNumDofs());
|
||||
else
|
||||
m_jacSizeA = (6 + bodyA->getNumLinks());
|
||||
}
|
||||
|
||||
if(bodyB)
|
||||
{
|
||||
if(bodyB->isMultiDof())
|
||||
m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
|
||||
else
|
||||
m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
|
||||
}
|
||||
else
|
||||
m_jacSizeBoth = m_jacSizeA;
|
||||
|
||||
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
|
||||
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
|
||||
}
|
||||
|
||||
btMultiBodyConstraint::~btMultiBodyConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
|
||||
{
|
||||
for (int i = 0; i < ndof; ++i)
|
||||
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
|
||||
}
|
||||
|
||||
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA, btScalar* jacOrgB,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btVector3& posAworld, const btVector3& posBworld,
|
||||
btScalar posError,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar lowerLimit, btScalar upperLimit,
|
||||
btScalar relaxation,
|
||||
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
solverConstraint.m_multiBodyA = m_bodyA;
|
||||
solverConstraint.m_multiBodyB = m_bodyB;
|
||||
solverConstraint.m_linkA = m_linkA;
|
||||
solverConstraint.m_linkB = m_linkB;
|
||||
|
||||
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
|
||||
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
|
||||
|
||||
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
|
||||
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
|
||||
|
||||
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
|
||||
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
|
||||
|
||||
btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
|
||||
if (bodyA)
|
||||
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
|
||||
if (bodyB)
|
||||
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
|
||||
if (solverConstraint.m_deltaVelAindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
|
||||
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
|
||||
} else
|
||||
{
|
||||
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
|
||||
//resize..
|
||||
solverConstraint.m_jacAindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
|
||||
//copy/determine
|
||||
if(jacOrgA)
|
||||
{
|
||||
for (int i=0;i<ndofA;i++)
|
||||
data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
if(multiBodyA->isMultiDof())
|
||||
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
else
|
||||
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
}
|
||||
|
||||
//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
|
||||
//resize..
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
|
||||
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
||||
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
//determine..
|
||||
if(multiBodyA->isMultiDof())
|
||||
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
else
|
||||
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
|
||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_contactNormal1 = contactNormalOnB;
|
||||
}
|
||||
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
if (solverConstraint.m_deltaVelBindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
|
||||
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
|
||||
//resize..
|
||||
solverConstraint.m_jacBindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
|
||||
//copy/determine..
|
||||
if(jacOrgB)
|
||||
{
|
||||
for (int i=0;i<ndofB;i++)
|
||||
data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
if(multiBodyB->isMultiDof())
|
||||
multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
else
|
||||
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
}
|
||||
|
||||
//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
|
||||
//resize..
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
||||
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
//determine..
|
||||
if(multiBodyB->isMultiDof())
|
||||
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
|
||||
else
|
||||
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
|
||||
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
|
||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
solverConstraint.m_contactNormal2 = -contactNormalOnB;
|
||||
}
|
||||
{
|
||||
|
||||
btVector3 vec;
|
||||
btScalar denom0 = 0.f;
|
||||
btScalar denom1 = 0.f;
|
||||
btScalar* jacB = 0;
|
||||
btScalar* jacA = 0;
|
||||
btScalar* deltaVelA = 0;
|
||||
btScalar* deltaVelB = 0;
|
||||
int ndofA = 0;
|
||||
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
btScalar j = jacA[i] ;
|
||||
btScalar l = deltaVelA[i];
|
||||
denom0 += j*l;
|
||||
}
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
|
||||
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
|
||||
}
|
||||
//
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
|
||||
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
btScalar j = jacB[i] ;
|
||||
btScalar l = deltaVelB[i];
|
||||
denom1 += j*l;
|
||||
}
|
||||
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
|
||||
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
|
||||
}
|
||||
//determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i])
|
||||
if (multiBodyA && (multiBodyA==multiBodyB))
|
||||
{
|
||||
// ndof1 == ndof2 in this case
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
denom1 += jacB[i] * deltaVelA[i];
|
||||
denom1 += jacA[i] * deltaVelB[i];
|
||||
}
|
||||
}
|
||||
//
|
||||
btScalar d = denom0+denom1;
|
||||
if (btFabs(d)>SIMD_EPSILON)
|
||||
{
|
||||
|
||||
solverConstraint.m_jacDiagABInv = relaxation/(d);
|
||||
}
|
||||
else
|
||||
{
|
||||
solverConstraint.m_jacDiagABInv = 1.f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//compute rhs and remaining solverConstraint fields
|
||||
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
|
||||
|
||||
btScalar rel_vel = 0.f;
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
{
|
||||
btVector3 vel1,vel2;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA ; ++i)
|
||||
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB ; ++i)
|
||||
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
||||
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
|
||||
}
|
||||
|
||||
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
|
||||
}
|
||||
|
||||
|
||||
///warm starting (or zero if disabled)
|
||||
/*
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
||||
|
||||
if (solverConstraint.m_appliedImpulse)
|
||||
{
|
||||
if (multiBodyA)
|
||||
{
|
||||
btScalar impulse = solverConstraint.m_appliedImpulse;
|
||||
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
|
||||
} else
|
||||
{
|
||||
if (rb0)
|
||||
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
btScalar impulse = solverConstraint.m_appliedImpulse;
|
||||
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
multiBodyB->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
|
||||
} else
|
||||
{
|
||||
if (rb1)
|
||||
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
} else
|
||||
*/
|
||||
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
|
||||
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = penetrationImpulse;
|
||||
}
|
||||
|
||||
solverConstraint.m_cfm = 0.f;
|
||||
solverConstraint.m_lowerLimit = lowerLimit;
|
||||
solverConstraint.m_upperLimit = upperLimit;
|
||||
}
|
||||
|
||||
return rel_vel;
|
||||
|
||||
}
|
||||
#include "btMultiBodyConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
|
||||
|
||||
|
||||
|
||||
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
|
||||
:m_bodyA(bodyA),
|
||||
m_bodyB(bodyB),
|
||||
m_linkA(linkA),
|
||||
m_linkB(linkB),
|
||||
m_numRows(numRows),
|
||||
m_jacSizeA(0),
|
||||
m_jacSizeBoth(0),
|
||||
m_isUnilateral(isUnilateral),
|
||||
m_maxAppliedImpulse(100)
|
||||
{
|
||||
|
||||
if(bodyA)
|
||||
{
|
||||
if(bodyA->isMultiDof())
|
||||
m_jacSizeA = (6 + bodyA->getNumDofs());
|
||||
else
|
||||
m_jacSizeA = (6 + bodyA->getNumLinks());
|
||||
}
|
||||
|
||||
if(bodyB)
|
||||
{
|
||||
if(bodyB->isMultiDof())
|
||||
m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
|
||||
else
|
||||
m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
|
||||
}
|
||||
else
|
||||
m_jacSizeBoth = m_jacSizeA;
|
||||
|
||||
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
|
||||
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
|
||||
}
|
||||
|
||||
btMultiBodyConstraint::~btMultiBodyConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
|
||||
{
|
||||
for (int i = 0; i < ndof; ++i)
|
||||
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
|
||||
}
|
||||
|
||||
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA, btScalar* jacOrgB,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btVector3& posAworld, const btVector3& posBworld,
|
||||
btScalar posError,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar lowerLimit, btScalar upperLimit,
|
||||
btScalar relaxation,
|
||||
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
solverConstraint.m_multiBodyA = m_bodyA;
|
||||
solverConstraint.m_multiBodyB = m_bodyB;
|
||||
solverConstraint.m_linkA = m_linkA;
|
||||
solverConstraint.m_linkB = m_linkB;
|
||||
|
||||
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
|
||||
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
|
||||
|
||||
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
|
||||
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
|
||||
|
||||
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
|
||||
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
|
||||
|
||||
btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
|
||||
if (bodyA)
|
||||
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
|
||||
if (bodyB)
|
||||
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
|
||||
if (solverConstraint.m_deltaVelAindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
|
||||
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
|
||||
} else
|
||||
{
|
||||
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
|
||||
//resize..
|
||||
solverConstraint.m_jacAindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
|
||||
//copy/determine
|
||||
if(jacOrgA)
|
||||
{
|
||||
for (int i=0;i<ndofA;i++)
|
||||
data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
if(multiBodyA->isMultiDof())
|
||||
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
else
|
||||
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
}
|
||||
|
||||
//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
|
||||
//resize..
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
|
||||
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
||||
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
//determine..
|
||||
if(multiBodyA->isMultiDof())
|
||||
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
else
|
||||
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
|
||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_contactNormal1 = contactNormalOnB;
|
||||
}
|
||||
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
if (solverConstraint.m_deltaVelBindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
|
||||
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
|
||||
//resize..
|
||||
solverConstraint.m_jacBindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
|
||||
//copy/determine..
|
||||
if(jacOrgB)
|
||||
{
|
||||
for (int i=0;i<ndofB;i++)
|
||||
data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
if(multiBodyB->isMultiDof())
|
||||
multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
else
|
||||
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
}
|
||||
|
||||
//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
|
||||
//resize..
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
||||
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
//determine..
|
||||
if(multiBodyB->isMultiDof())
|
||||
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
|
||||
else
|
||||
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
|
||||
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
|
||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
solverConstraint.m_contactNormal2 = -contactNormalOnB;
|
||||
}
|
||||
{
|
||||
|
||||
btVector3 vec;
|
||||
btScalar denom0 = 0.f;
|
||||
btScalar denom1 = 0.f;
|
||||
btScalar* jacB = 0;
|
||||
btScalar* jacA = 0;
|
||||
btScalar* deltaVelA = 0;
|
||||
btScalar* deltaVelB = 0;
|
||||
int ndofA = 0;
|
||||
//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
btScalar j = jacA[i] ;
|
||||
btScalar l = deltaVelA[i];
|
||||
denom0 += j*l;
|
||||
}
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
|
||||
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
|
||||
}
|
||||
//
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
|
||||
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
btScalar j = jacB[i] ;
|
||||
btScalar l = deltaVelB[i];
|
||||
denom1 += j*l;
|
||||
}
|
||||
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
|
||||
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
|
||||
}
|
||||
//determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i])
|
||||
if (multiBodyA && (multiBodyA==multiBodyB))
|
||||
{
|
||||
// ndof1 == ndof2 in this case
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
denom1 += jacB[i] * deltaVelA[i];
|
||||
denom1 += jacA[i] * deltaVelB[i];
|
||||
}
|
||||
}
|
||||
//
|
||||
btScalar d = denom0+denom1;
|
||||
if (btFabs(d)>SIMD_EPSILON)
|
||||
{
|
||||
|
||||
solverConstraint.m_jacDiagABInv = relaxation/(d);
|
||||
}
|
||||
else
|
||||
{
|
||||
solverConstraint.m_jacDiagABInv = 1.f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//compute rhs and remaining solverConstraint fields
|
||||
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
|
||||
|
||||
btScalar rel_vel = 0.f;
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
{
|
||||
btVector3 vel1,vel2;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA ; ++i)
|
||||
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
|
||||
}
|
||||
else if(rb0)
|
||||
{
|
||||
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB ; ++i)
|
||||
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
||||
|
||||
}
|
||||
else if(rb1)
|
||||
{
|
||||
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
|
||||
}
|
||||
|
||||
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
|
||||
}
|
||||
|
||||
|
||||
///warm starting (or zero if disabled)
|
||||
/*
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
||||
|
||||
if (solverConstraint.m_appliedImpulse)
|
||||
{
|
||||
if (multiBodyA)
|
||||
{
|
||||
btScalar impulse = solverConstraint.m_appliedImpulse;
|
||||
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
|
||||
} else
|
||||
{
|
||||
if (rb0)
|
||||
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
btScalar impulse = solverConstraint.m_appliedImpulse;
|
||||
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
multiBodyB->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
|
||||
} else
|
||||
{
|
||||
if (rb1)
|
||||
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
} else
|
||||
*/
|
||||
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
|
||||
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = penetrationImpulse;
|
||||
}
|
||||
|
||||
solverConstraint.m_cfm = 0.f;
|
||||
solverConstraint.m_lowerLimit = lowerLimit;
|
||||
solverConstraint.m_upperLimit = upperLimit;
|
||||
}
|
||||
|
||||
return rel_vel;
|
||||
|
||||
}
|
||||
|
||||
@@ -1,160 +1,160 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_CONSTRAINT_H
|
||||
#define BT_MULTIBODY_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "btMultiBody.h"
|
||||
|
||||
class btMultiBody;
|
||||
struct btSolverInfo;
|
||||
|
||||
#include "btMultiBodySolverConstraint.h"
|
||||
|
||||
struct btMultiBodyJacobianData
|
||||
{
|
||||
btAlignedObjectArray<btScalar> m_jacobians;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
btAlignedObjectArray<btMatrix3x3> scratch_m;
|
||||
btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
|
||||
int m_fixedBodyId;
|
||||
|
||||
};
|
||||
|
||||
|
||||
class btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btMultiBody* m_bodyA;
|
||||
btMultiBody* m_bodyB;
|
||||
int m_linkA;
|
||||
int m_linkB;
|
||||
|
||||
int m_numRows;
|
||||
int m_jacSizeA;
|
||||
int m_jacSizeBoth;
|
||||
int m_posOffset;
|
||||
|
||||
bool m_isUnilateral;
|
||||
|
||||
btScalar m_maxAppliedImpulse;
|
||||
|
||||
|
||||
// data block laid out as follows:
|
||||
// cached impulses. (one per row.)
|
||||
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
|
||||
// positions. (one per row.)
|
||||
btAlignedObjectArray<btScalar> m_data;
|
||||
|
||||
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
|
||||
|
||||
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA, btScalar* jacOrgB,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btVector3& posAworld, const btVector3& posBworld,
|
||||
btScalar posError,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar lowerLimit, btScalar upperLimit,
|
||||
btScalar relaxation = 1.f,
|
||||
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
public:
|
||||
|
||||
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
|
||||
virtual ~btMultiBodyConstraint();
|
||||
|
||||
|
||||
|
||||
virtual int getIslandIdA() const =0;
|
||||
virtual int getIslandIdB() const =0;
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)=0;
|
||||
|
||||
int getNumRows() const
|
||||
{
|
||||
return m_numRows;
|
||||
}
|
||||
|
||||
btMultiBody* getMultiBodyA()
|
||||
{
|
||||
return m_bodyA;
|
||||
}
|
||||
btMultiBody* getMultiBodyB()
|
||||
{
|
||||
return m_bodyB;
|
||||
}
|
||||
|
||||
// current constraint position
|
||||
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
|
||||
// NOTE: ignored position for friction rows.
|
||||
btScalar getPosition(int row) const
|
||||
{
|
||||
return m_data[m_posOffset + row];
|
||||
}
|
||||
|
||||
void setPosition(int row, btScalar pos)
|
||||
{
|
||||
m_data[m_posOffset + row] = pos;
|
||||
}
|
||||
|
||||
|
||||
bool isUnilateral() const
|
||||
{
|
||||
return m_isUnilateral;
|
||||
}
|
||||
|
||||
// jacobian blocks.
|
||||
// each of size 6 + num_links. (jacobian2 is null if no body2.)
|
||||
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
|
||||
btScalar* jacobianA(int row)
|
||||
{
|
||||
return &m_data[m_numRows + row * m_jacSizeBoth];
|
||||
}
|
||||
const btScalar* jacobianA(int row) const
|
||||
{
|
||||
return &m_data[m_numRows + (row * m_jacSizeBoth)];
|
||||
}
|
||||
btScalar* jacobianB(int row)
|
||||
{
|
||||
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
|
||||
}
|
||||
const btScalar* jacobianB(int row) const
|
||||
{
|
||||
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
|
||||
}
|
||||
|
||||
btScalar getMaxAppliedImpulse() const
|
||||
{
|
||||
return m_maxAppliedImpulse;
|
||||
}
|
||||
void setMaxAppliedImpulse(btScalar maxImp)
|
||||
{
|
||||
m_maxAppliedImpulse = maxImp;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_CONSTRAINT_H
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_CONSTRAINT_H
|
||||
#define BT_MULTIBODY_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "btMultiBody.h"
|
||||
|
||||
class btMultiBody;
|
||||
struct btSolverInfo;
|
||||
|
||||
#include "btMultiBodySolverConstraint.h"
|
||||
|
||||
struct btMultiBodyJacobianData
|
||||
{
|
||||
btAlignedObjectArray<btScalar> m_jacobians;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
btAlignedObjectArray<btMatrix3x3> scratch_m;
|
||||
btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
|
||||
int m_fixedBodyId;
|
||||
|
||||
};
|
||||
|
||||
|
||||
class btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btMultiBody* m_bodyA;
|
||||
btMultiBody* m_bodyB;
|
||||
int m_linkA;
|
||||
int m_linkB;
|
||||
|
||||
int m_numRows;
|
||||
int m_jacSizeA;
|
||||
int m_jacSizeBoth;
|
||||
int m_posOffset;
|
||||
|
||||
bool m_isUnilateral;
|
||||
|
||||
btScalar m_maxAppliedImpulse;
|
||||
|
||||
|
||||
// data block laid out as follows:
|
||||
// cached impulses. (one per row.)
|
||||
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
|
||||
// positions. (one per row.)
|
||||
btAlignedObjectArray<btScalar> m_data;
|
||||
|
||||
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
|
||||
|
||||
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA, btScalar* jacOrgB,
|
||||
const btVector3& contactNormalOnB,
|
||||
const btVector3& posAworld, const btVector3& posBworld,
|
||||
btScalar posError,
|
||||
const btContactSolverInfo& infoGlobal,
|
||||
btScalar lowerLimit, btScalar upperLimit,
|
||||
btScalar relaxation = 1.f,
|
||||
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
public:
|
||||
|
||||
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
|
||||
virtual ~btMultiBodyConstraint();
|
||||
|
||||
|
||||
|
||||
virtual int getIslandIdA() const =0;
|
||||
virtual int getIslandIdB() const =0;
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)=0;
|
||||
|
||||
int getNumRows() const
|
||||
{
|
||||
return m_numRows;
|
||||
}
|
||||
|
||||
btMultiBody* getMultiBodyA()
|
||||
{
|
||||
return m_bodyA;
|
||||
}
|
||||
btMultiBody* getMultiBodyB()
|
||||
{
|
||||
return m_bodyB;
|
||||
}
|
||||
|
||||
// current constraint position
|
||||
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
|
||||
// NOTE: ignored position for friction rows.
|
||||
btScalar getPosition(int row) const
|
||||
{
|
||||
return m_data[m_posOffset + row];
|
||||
}
|
||||
|
||||
void setPosition(int row, btScalar pos)
|
||||
{
|
||||
m_data[m_posOffset + row] = pos;
|
||||
}
|
||||
|
||||
|
||||
bool isUnilateral() const
|
||||
{
|
||||
return m_isUnilateral;
|
||||
}
|
||||
|
||||
// jacobian blocks.
|
||||
// each of size 6 + num_links. (jacobian2 is null if no body2.)
|
||||
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
|
||||
btScalar* jacobianA(int row)
|
||||
{
|
||||
return &m_data[m_numRows + row * m_jacSizeBoth];
|
||||
}
|
||||
const btScalar* jacobianA(int row) const
|
||||
{
|
||||
return &m_data[m_numRows + (row * m_jacSizeBoth)];
|
||||
}
|
||||
btScalar* jacobianB(int row)
|
||||
{
|
||||
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
|
||||
}
|
||||
const btScalar* jacobianB(int row) const
|
||||
{
|
||||
return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
|
||||
}
|
||||
|
||||
btScalar getMaxAppliedImpulse() const
|
||||
{
|
||||
return m_maxAppliedImpulse;
|
||||
}
|
||||
void setMaxAppliedImpulse(btScalar maxImp)
|
||||
{
|
||||
m_maxAppliedImpulse = maxImp;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_CONSTRAINT_H
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,86 +1,86 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
#include "btMultiBodySolverConstraint.h"
|
||||
|
||||
|
||||
class btMultiBody;
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
|
||||
protected:
|
||||
|
||||
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
|
||||
|
||||
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
|
||||
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
|
||||
|
||||
btMultiBodyJacobianData m_data;
|
||||
|
||||
//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
|
||||
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
|
||||
int m_tmpNumMultiBodyConstraints;
|
||||
|
||||
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
|
||||
void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
|
||||
|
||||
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
|
||||
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
|
||||
btScalar* jacA,btScalar* jacB,
|
||||
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
const btVector3& contactNormal,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
#include "btMultiBodySolverConstraint.h"
|
||||
|
||||
|
||||
class btMultiBody;
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
|
||||
protected:
|
||||
|
||||
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
|
||||
|
||||
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
|
||||
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
|
||||
|
||||
btMultiBodyJacobianData m_data;
|
||||
|
||||
//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
|
||||
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
|
||||
int m_tmpNumMultiBodyConstraints;
|
||||
|
||||
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
|
||||
void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
|
||||
|
||||
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
|
||||
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
|
||||
btScalar* jacA,btScalar* jacB,
|
||||
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
const btVector3& contactNormal,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,56 +1,56 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
#define BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
|
||||
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
|
||||
|
||||
|
||||
class btMultiBody;
|
||||
class btMultiBodyConstraint;
|
||||
class btMultiBodyConstraintSolver;
|
||||
struct MultiBodyInplaceSolverIslandCallback;
|
||||
|
||||
///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
|
||||
///This implementation is still preliminary/experimental.
|
||||
class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
|
||||
{
|
||||
protected:
|
||||
btAlignedObjectArray<btMultiBody*> m_multiBodies;
|
||||
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
|
||||
btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
|
||||
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
|
||||
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
|
||||
|
||||
virtual void calculateSimulationIslands();
|
||||
virtual void updateActivationState(btScalar timeStep);
|
||||
virtual void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
public:
|
||||
|
||||
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
|
||||
|
||||
virtual ~btMultiBodyDynamicsWorld ();
|
||||
|
||||
virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
|
||||
|
||||
virtual void removeMultiBody(btMultiBody* body);
|
||||
|
||||
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
|
||||
|
||||
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
|
||||
};
|
||||
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
#define BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
|
||||
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
|
||||
|
||||
|
||||
class btMultiBody;
|
||||
class btMultiBodyConstraint;
|
||||
class btMultiBodyConstraintSolver;
|
||||
struct MultiBodyInplaceSolverIslandCallback;
|
||||
|
||||
///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
|
||||
///This implementation is still preliminary/experimental.
|
||||
class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
|
||||
{
|
||||
protected:
|
||||
btAlignedObjectArray<btMultiBody*> m_multiBodies;
|
||||
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
|
||||
btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
|
||||
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
|
||||
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
|
||||
|
||||
virtual void calculateSimulationIslands();
|
||||
virtual void updateActivationState(btScalar timeStep);
|
||||
virtual void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
public:
|
||||
|
||||
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
|
||||
|
||||
virtual ~btMultiBodyDynamicsWorld ();
|
||||
|
||||
virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
|
||||
|
||||
virtual void removeMultiBody(btMultiBody* body);
|
||||
|
||||
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
|
||||
|
||||
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
|
||||
};
|
||||
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
|
||||
@@ -1,145 +1,145 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodyJointLimitConstraint.h"
|
||||
#include "btMultiBody.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
|
||||
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
|
||||
//:btMultiBodyConstraint(body,0,link,-1,2,true),
|
||||
:btMultiBodyConstraint(body,body,link,link,2,true),
|
||||
m_lowerBound(lower),
|
||||
m_upperBound(upper)
|
||||
{
|
||||
// the data.m_jacobians never change, so may as well
|
||||
// initialize them here
|
||||
|
||||
// note: we rely on the fact that data.m_jacobians are
|
||||
// always initialized to zero by the Constraint ctor
|
||||
|
||||
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
|
||||
|
||||
// row 0: the lower bound
|
||||
jacobianA(0)[offset] = 1;
|
||||
// row 1: the upper bound
|
||||
//jacobianA(1)[offset] = -1;
|
||||
|
||||
jacobianB(1)[offset] = -1;
|
||||
}
|
||||
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodyJointLimitConstraint::getIslandIdA() const
|
||||
{
|
||||
if(m_bodyA)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
for (int i=0;i<m_bodyA->getNumLinks();i++)
|
||||
{
|
||||
if (m_bodyA->getLink(i).m_collider)
|
||||
return m_bodyA->getLink(i).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyJointLimitConstraint::getIslandIdB() const
|
||||
{
|
||||
if(m_bodyB)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
|
||||
for (int i=0;i<m_bodyB->getNumLinks();i++)
|
||||
{
|
||||
col = m_bodyB->getLink(i).m_collider;
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// only positions need to be updated -- data.m_jacobians and force
|
||||
// directions were set in the ctor and never change.
|
||||
|
||||
// row 0: the lower bound
|
||||
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
|
||||
|
||||
// row 1: the upper bound
|
||||
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
|
||||
|
||||
for (int row=0;row<getNumRows();row++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
constraintRow.m_multiBodyA = m_bodyA;
|
||||
constraintRow.m_multiBodyB = m_bodyB;
|
||||
const btScalar posError = 0; //why assume it's zero?
|
||||
const btVector3 dummy(0, 0, 0);
|
||||
|
||||
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
|
||||
{
|
||||
btScalar penetration = getPosition(row);
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = - rel_vel;// * damping;
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
velocityError = -penetration / infoGlobal.m_timeStep;
|
||||
} else
|
||||
{
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
}
|
||||
|
||||
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
constraintRow.m_rhs = velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = penetrationImpulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodyJointLimitConstraint.h"
|
||||
#include "btMultiBody.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
|
||||
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
|
||||
//:btMultiBodyConstraint(body,0,link,-1,2,true),
|
||||
:btMultiBodyConstraint(body,body,link,link,2,true),
|
||||
m_lowerBound(lower),
|
||||
m_upperBound(upper)
|
||||
{
|
||||
// the data.m_jacobians never change, so may as well
|
||||
// initialize them here
|
||||
|
||||
// note: we rely on the fact that data.m_jacobians are
|
||||
// always initialized to zero by the Constraint ctor
|
||||
|
||||
unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
|
||||
|
||||
// row 0: the lower bound
|
||||
jacobianA(0)[offset] = 1;
|
||||
// row 1: the upper bound
|
||||
//jacobianA(1)[offset] = -1;
|
||||
|
||||
jacobianB(1)[offset] = -1;
|
||||
}
|
||||
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodyJointLimitConstraint::getIslandIdA() const
|
||||
{
|
||||
if(m_bodyA)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
for (int i=0;i<m_bodyA->getNumLinks();i++)
|
||||
{
|
||||
if (m_bodyA->getLink(i).m_collider)
|
||||
return m_bodyA->getLink(i).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyJointLimitConstraint::getIslandIdB() const
|
||||
{
|
||||
if(m_bodyB)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
|
||||
for (int i=0;i<m_bodyB->getNumLinks();i++)
|
||||
{
|
||||
col = m_bodyB->getLink(i).m_collider;
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// only positions need to be updated -- data.m_jacobians and force
|
||||
// directions were set in the ctor and never change.
|
||||
|
||||
// row 0: the lower bound
|
||||
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
|
||||
|
||||
// row 1: the upper bound
|
||||
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
|
||||
|
||||
for (int row=0;row<getNumRows();row++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
constraintRow.m_multiBodyA = m_bodyA;
|
||||
constraintRow.m_multiBodyB = m_bodyB;
|
||||
const btScalar posError = 0; //why assume it's zero?
|
||||
const btVector3 dummy(0, 0, 0);
|
||||
|
||||
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
|
||||
{
|
||||
btScalar penetration = getPosition(row);
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = - rel_vel;// * damping;
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
velocityError = -penetration / infoGlobal.m_timeStep;
|
||||
} else
|
||||
{
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
}
|
||||
|
||||
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
constraintRow.m_rhs = velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = penetrationImpulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,44 +1,44 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
struct btSolverInfo;
|
||||
|
||||
class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btScalar m_lowerBound;
|
||||
btScalar m_upperBound;
|
||||
public:
|
||||
|
||||
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
|
||||
virtual ~btMultiBodyJointLimitConstraint();
|
||||
|
||||
virtual int getIslandIdA() const;
|
||||
virtual int getIslandIdB() const;
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
struct btSolverInfo;
|
||||
|
||||
class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btScalar m_lowerBound;
|
||||
btScalar m_upperBound;
|
||||
public:
|
||||
|
||||
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
|
||||
virtual ~btMultiBodyJointLimitConstraint();
|
||||
|
||||
virtual int getIslandIdA() const;
|
||||
virtual int getIslandIdB() const;
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
|
||||
|
||||
@@ -110,7 +110,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
|
||||
btScalar penetration = 0;
|
||||
|
||||
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
|
||||
}
|
||||
|
||||
|
||||
@@ -381,8 +381,7 @@ struct btMultibodyLink
|
||||
eInvalid
|
||||
};
|
||||
|
||||
eFeatherstoneJointType m_jointType;
|
||||
int m_dofCount, m_posVarCount; //redundant but handy
|
||||
|
||||
|
||||
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
|
||||
// for prismatic: m_axesTop[0] = zero;
|
||||
@@ -429,6 +428,10 @@ struct btMultibodyLink
|
||||
|
||||
class btMultiBodyLinkCollider* m_collider;
|
||||
int m_flags;
|
||||
|
||||
|
||||
int m_dofCount, m_posVarCount; //redundant but handy
|
||||
eFeatherstoneJointType m_jointType;
|
||||
|
||||
// ctor: set some sensible defaults
|
||||
btMultibodyLink()
|
||||
|
||||
@@ -1,92 +1,92 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
#define BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
#include "btMultiBody.h"
|
||||
|
||||
class btMultiBodyLinkCollider : public btCollisionObject
|
||||
{
|
||||
//protected:
|
||||
public:
|
||||
|
||||
btMultiBody* m_multiBody;
|
||||
int m_link;
|
||||
|
||||
|
||||
btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
|
||||
:m_multiBody(multiBody),
|
||||
m_link(link)
|
||||
{
|
||||
m_checkCollideWith = true;
|
||||
//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
|
||||
//this means that some constraints might point to bodies that are not in the islands, causing crashes
|
||||
//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
|
||||
{
|
||||
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
|
||||
}
|
||||
// else
|
||||
//{
|
||||
// m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
|
||||
//}
|
||||
|
||||
m_internalType = CO_FEATHERSTONE_LINK;
|
||||
}
|
||||
static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
|
||||
{
|
||||
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
|
||||
return (btMultiBodyLinkCollider*)colObj;
|
||||
return 0;
|
||||
}
|
||||
static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
|
||||
{
|
||||
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
|
||||
return (btMultiBodyLinkCollider*)colObj;
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
|
||||
{
|
||||
const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
|
||||
if (!other)
|
||||
return true;
|
||||
if (other->m_multiBody != this->m_multiBody)
|
||||
return true;
|
||||
if (!m_multiBody->hasSelfCollision())
|
||||
return false;
|
||||
|
||||
//check if 'link' has collision disabled
|
||||
if (m_link>=0)
|
||||
{
|
||||
const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
|
||||
if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
|
||||
return false;
|
||||
}
|
||||
|
||||
if (other->m_link>=0)
|
||||
{
|
||||
const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
|
||||
if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
#define BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
#include "btMultiBody.h"
|
||||
|
||||
class btMultiBodyLinkCollider : public btCollisionObject
|
||||
{
|
||||
//protected:
|
||||
public:
|
||||
|
||||
btMultiBody* m_multiBody;
|
||||
int m_link;
|
||||
|
||||
|
||||
btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
|
||||
:m_multiBody(multiBody),
|
||||
m_link(link)
|
||||
{
|
||||
m_checkCollideWith = true;
|
||||
//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
|
||||
//this means that some constraints might point to bodies that are not in the islands, causing crashes
|
||||
//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
|
||||
{
|
||||
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
|
||||
}
|
||||
// else
|
||||
//{
|
||||
// m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
|
||||
//}
|
||||
|
||||
m_internalType = CO_FEATHERSTONE_LINK;
|
||||
}
|
||||
static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
|
||||
{
|
||||
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
|
||||
return (btMultiBodyLinkCollider*)colObj;
|
||||
return 0;
|
||||
}
|
||||
static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
|
||||
{
|
||||
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
|
||||
return (btMultiBodyLinkCollider*)colObj;
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual bool checkCollideWithOverride(const btCollisionObject* co) const
|
||||
{
|
||||
const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
|
||||
if (!other)
|
||||
return true;
|
||||
if (other->m_multiBody != this->m_multiBody)
|
||||
return true;
|
||||
if (!m_multiBody->hasSelfCollision())
|
||||
return false;
|
||||
|
||||
//check if 'link' has collision disabled
|
||||
if (m_link>=0)
|
||||
{
|
||||
const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
|
||||
if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
|
||||
return false;
|
||||
}
|
||||
|
||||
if (other->m_link>=0)
|
||||
{
|
||||
const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
|
||||
if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
|
||||
|
||||
@@ -119,7 +119,6 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
|
||||
contactNormalOnB[i%3] = -1;
|
||||
#endif
|
||||
|
||||
btScalar penetration = 0;
|
||||
|
||||
// Convert local points back to world
|
||||
btVector3 pivotAworld = m_pivotInA;
|
||||
|
||||
@@ -1,62 +1,62 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#ifndef BT_MULTIBODY_POINT2POINT_H
|
||||
#define BT_MULTIBODY_POINT2POINT_H
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
|
||||
//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
|
||||
class btMultiBodyPoint2Point : public btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btRigidBody* m_rigidBodyA;
|
||||
btRigidBody* m_rigidBodyB;
|
||||
btVector3 m_pivotInA;
|
||||
btVector3 m_pivotInB;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
|
||||
btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
|
||||
|
||||
virtual ~btMultiBodyPoint2Point();
|
||||
|
||||
virtual int getIslandIdA() const;
|
||||
virtual int getIslandIdB() const;
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
const btVector3& getPivotInB() const
|
||||
{
|
||||
return m_pivotInB;
|
||||
}
|
||||
|
||||
void setPivotInB(const btVector3& pivotInB)
|
||||
{
|
||||
m_pivotInB = pivotInB;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_POINT2POINT_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#ifndef BT_MULTIBODY_POINT2POINT_H
|
||||
#define BT_MULTIBODY_POINT2POINT_H
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
|
||||
//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
|
||||
class btMultiBodyPoint2Point : public btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
btRigidBody* m_rigidBodyA;
|
||||
btRigidBody* m_rigidBodyB;
|
||||
btVector3 m_pivotInA;
|
||||
btVector3 m_pivotInB;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
|
||||
btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
|
||||
|
||||
virtual ~btMultiBodyPoint2Point();
|
||||
|
||||
virtual int getIslandIdA() const;
|
||||
virtual int getIslandIdB() const;
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
const btVector3& getPivotInB() const
|
||||
{
|
||||
return m_pivotInB;
|
||||
}
|
||||
|
||||
void setPivotInB(const btVector3& pivotInB)
|
||||
{
|
||||
m_pivotInB = pivotInB;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_POINT2POINT_H
|
||||
|
||||
@@ -1,85 +1,85 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
class btMultiBody;
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_solverBodyIdB(-1), m_multiBodyA(0), m_multiBodyB(0), m_linkA(-1), m_linkB(-1)
|
||||
{}
|
||||
|
||||
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
|
||||
int m_jacAindex;
|
||||
int m_deltaVelBindex;
|
||||
int m_jacBindex;
|
||||
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal1;
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
|
||||
|
||||
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
mutable btSimdScalar m_appliedPushImpulse;
|
||||
mutable btSimdScalar m_appliedImpulse;
|
||||
|
||||
btScalar m_friction;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_rhs;
|
||||
btScalar m_cfm;
|
||||
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
btScalar m_rhsPenetration;
|
||||
union
|
||||
{
|
||||
void* m_originalContactPoint;
|
||||
btScalar m_unusedPadding4;
|
||||
};
|
||||
|
||||
int m_overrideNumSolverIterations;
|
||||
int m_frictionIndex;
|
||||
|
||||
int m_solverBodyIdA;
|
||||
btMultiBody* m_multiBodyA;
|
||||
int m_linkA;
|
||||
|
||||
int m_solverBodyIdB;
|
||||
btMultiBody* m_multiBodyB;
|
||||
int m_linkB;
|
||||
|
||||
enum btSolverConstraintType
|
||||
{
|
||||
BT_SOLVER_CONTACT_1D = 0,
|
||||
BT_SOLVER_FRICTION_1D
|
||||
};
|
||||
};
|
||||
|
||||
typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
|
||||
|
||||
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
class btMultiBody;
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1)
|
||||
{}
|
||||
|
||||
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
|
||||
int m_jacAindex;
|
||||
int m_deltaVelBindex;
|
||||
int m_jacBindex;
|
||||
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal1;
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
|
||||
|
||||
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
mutable btSimdScalar m_appliedPushImpulse;
|
||||
mutable btSimdScalar m_appliedImpulse;
|
||||
|
||||
btScalar m_friction;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_rhs;
|
||||
btScalar m_cfm;
|
||||
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
btScalar m_rhsPenetration;
|
||||
union
|
||||
{
|
||||
void* m_originalContactPoint;
|
||||
btScalar m_unusedPadding4;
|
||||
};
|
||||
|
||||
int m_overrideNumSolverIterations;
|
||||
int m_frictionIndex;
|
||||
|
||||
int m_solverBodyIdA;
|
||||
btMultiBody* m_multiBodyA;
|
||||
int m_linkA;
|
||||
|
||||
int m_solverBodyIdB;
|
||||
btMultiBody* m_multiBodyB;
|
||||
int m_linkB;
|
||||
|
||||
enum btSolverConstraintType
|
||||
{
|
||||
BT_SOLVER_CONTACT_1D = 0,
|
||||
BT_SOLVER_FRICTION_1D
|
||||
};
|
||||
};
|
||||
|
||||
typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
|
||||
|
||||
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
|
||||
Reference in New Issue
Block a user