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

unified btMultiBodyConstrained::fillMultiBodyConstraint..(...) mtds + cleaned some of the earlier dirty changes (6DoF grabbing constraint stuff mainly)

Browse Source
This commit is contained in:
kubas
2014-01-09 01:09:44 +01:00
parent 0ba7d69f86
commit ef6abf6490
7 changed files with 254 additions and 925 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -2587,126 +2587,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
}
}
void btMultiBody::fillContactJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
{
// temporary space
int num_links = getNumLinks();
int m_dofCount = getNumDofs();
scratch_v.resize(2*num_links + 2);
scratch_m.resize(num_links + 1);
btVector3 * v_ptr = &scratch_v[0];
btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
scratch_r.resize(m_dofCount);
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
btMatrix3x3 * rot_from_world = &scratch_m[0];
const btVector3 p_minus_com_world = contact_point - m_basePos;
const btVector3 &normal_world = normal; //convenience
rot_from_world[0] = btMatrix3x3(m_baseQuat);
// omega coeffients first.
btVector3 omega_coeffs_world;
omega_coeffs_world = p_minus_com_world.cross(normal_world);
jac[0] = omega_coeffs_world[0];
jac[1] = omega_coeffs_world[1];
jac[2] = omega_coeffs_world[2];
// then v coefficients
jac[3] = normal_world[0];
jac[4] = normal_world[1];
jac[5] = normal_world[2];
//create link-local versions of p_minus_com and normal
p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
n_local[0] = rot_from_world[0] * normal_world;
// Set remaining jac values to zero for now.
for (int i = 6; i < 6 + m_dofCount; ++i)
{
jac[i] = 0;
}
// Qdot coefficients, if necessary.
if (num_links > 0 && link > -1) {
// TODO: speed this up -- don't calculate for m_links we don't need.
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
// which is resulting in repeated work being done...)
// calculate required normals & positions in the local frames.
for (int i = 0; i < num_links; ++i) {
// transform to local frame
const int parent = m_links[i].m_parent;
const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
n_local[i+1] = mtx * n_local[parent+1];
p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
// calculate the jacobian entry
switch(m_links[i].m_jointType)
{
case btMultibodyLink::eRevolute:
{
results[m_links[i].m_dofOffset] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
break;
}
case btMultibodyLink::ePrismatic:
{
results[m_links[i].m_dofOffset] = n_local[i+1].dot(m_links[i].getAxisBottom(0));
break;
}
case btMultibodyLink::eSpherical:
{
results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
break;
}
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
case btMultibodyLink::ePlanar:
{
results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisBottom(1));
results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisBottom(2));
break;
}
#endif
}
}
// Now copy through to output.
//printf("jac[%d] = ", link);
while (link != -1)
{
for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
{
jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
}
link = m_links[link].m_parent;
}
//printf("]\n");
}
}
void btMultiBody::fillContactJacobianMultiDof_test(int link,
void btMultiBody::filConstraintJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal_ang,
const btVector3 &normal_lin,

7
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -389,15 +389,18 @@ public:
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
//multidof version of fillContactJacobian
void fillContactJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
void fillContactJacobianMultiDof_test(int link,
//a more general version of fillContactJacobianMultiDof which does not assume..
//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
void filConstraintJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal_ang,
const btVector3 &normal_lin,

676
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -40,579 +40,42 @@ btMultiBodyConstraint::~btMultiBodyConstraint()
{
}
btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit)
{
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
if (multiBodyA)
{
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; //total dof count of tree A
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
if (constraintRow.m_deltaVelAindex <0) //if this multibody does not have a place allocated in m_deltaVelocities...
{
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); //=> each constrained tree's dofs are represented in m_deltaVelocities
} else
{
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
}
constraintRow.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
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());
for (int i=0;i<ndofA;i++)
data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
if(multiBodyA->isMultiDof())
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
else
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
}
if (multiBodyB)
{
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
if (constraintRow.m_deltaVelBindex <0)
{
constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
constraintRow.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
for (int i=0;i<ndofB;i++)
data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
if(multiBodyB->isMultiDof())
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
else
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[i];
denom0 += j*l;
}
}
if (multiBodyB)
{
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
denom1 += j*l;
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
constraintRow.m_jacDiagABInv = 1.f/(d);
} else
{
constraintRow.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining constraintRow fields
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[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
if (multiBodyB)
{
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
for (int i = 6; i < ndofA ; ++i)
printf("%.4f ", multiBodyA->getVelocityVector()[i]);
printf("\nrel_vel = %.4f\n------------\n", rel_vel);
constraintRow.m_friction = 0.f;
constraintRow.m_appliedImpulse = 0.f;
constraintRow.m_appliedPushImpulse = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse)
{
//combine position and velocity into rhs
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
}
constraintRow.m_cfm = 0.f;
constraintRow.m_lowerLimit = lowerLimit;
constraintRow.m_upperLimit = upperLimit;
}
return rel_vel;
}
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;
}
void btMultiBodyConstraint::fillMultiBodyConstraintMixed_old(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
btVector3 rel_pos1 = posAworld;
btVector3 rel_pos2 = posBworld;
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;
const btVector3& pos1 = posAworld;
const btVector3& pos2 = posBworld;
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;
if (bodyA)
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f;
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);
}
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
#endif
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
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);
#endif
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
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
{
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);
}
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
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);
if(multiBodyB->isMultiDof())
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
else
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
} else
{
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* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[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];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
denom1 += j*l;
}
} else
{
if (rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[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 restitution = 0.f;
btScalar penetration = isFriction? 0 : position+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;
restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
///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 = restitution - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
//commented out on purpose, see below
//if (penetration>0)
//{
// positionalError = 0;
// velocityError = -penetration / infoGlobal.m_timeStep;
//} else
//{
// positionalError = -penetration * erp/infoGlobal.m_timeStep;
//}
//we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints)
//both are valid in general and definitely so in the case of a point2Point constraint
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 = -m_maxAppliedImpulse;
solverConstraint.m_upperLimit = m_maxAppliedImpulse;
}
}
void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
btScalar lowerLimit, btScalar upperLimit,
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
btVector3 rel_pos1 = posAworld;
btVector3 rel_pos2 = posBworld;
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
const btVector3& pos1 = posAworld;
const btVector3& pos2 = posBworld;
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 = pos1 - bodyA->getWorldTransform().getOrigin();
rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f;
rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
if (multiBodyA)
{
@@ -630,7 +93,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
//determine jacobian of this 1D constraint
//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);
@@ -649,11 +112,12 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint)
//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
@@ -679,7 +143,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
//determine jacobian of this 1D constraint
//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);
@@ -697,7 +161,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
}
//determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint)
//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());
@@ -716,7 +180,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
{
btVector3 vec;
@@ -724,78 +187,72 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =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];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[i];
btScalar l = deltaVelA[i];
denom0 += j*l;
}
} else
{
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
}
}
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];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
btScalar l = deltaVelB[i];
denom1 += j*l;
}
} else
}
else if(rb1)
{
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)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
denom1 += jacB[i] * deltaVelA[i];
denom1 += jacA[i] * deltaVelB[i];
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
//
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
} else
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
}
else
{
solverConstraint.m_jacDiagABInv = 1.f;
}
}
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f;
int ndofA = 0;
@@ -827,13 +284,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
@@ -870,17 +320,14 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
}
} else
*/
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
@@ -889,19 +336,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
erp = infoGlobal.m_erp;
}
//commented out on purpose, see below
//if (penetration>0)
//{
// positionalError = 0;
// velocityError = -penetration / infoGlobal.m_timeStep;
//} else
//{
// positionalError = -penetration * erp/infoGlobal.m_timeStep;
//}
//we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints)
//both are valid in general and definitely so in the case of a point2Point constraint
positionalError = -penetration * erp/infoGlobal.m_timeStep;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
@@ -921,8 +355,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
solverConstraint.m_upperLimit = m_maxAppliedImpulse;
solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = upperLimit;
}
return rel_vel;
}

26
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -66,32 +66,16 @@ protected:
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
void fillMultiBodyConstraintMixed_old(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit);
void fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btScalar lowerLimit, btScalar upperLimit,
btScalar relaxation = 1.f,
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
public:

4
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
View File

@@ -99,8 +99,10 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
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 = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,0,m_maxAppliedImpulse);
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;

4
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -82,13 +82,15 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
// directions were set in the ctor and never change.
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
btScalar penetration = 0;
fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse);
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
}
}

341
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
View File

@@ -20,140 +20,13 @@ subject to the following restrictions:
#include "BulletDynamics/Dynamics/btRigidBody.h"
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(body,0,link,-1,3,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB)
{
}
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{
}
int btMultiBodyPoint2Point::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
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 btMultiBodyPoint2Point::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
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 btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// int i=1;
for (int i=0;i<3;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
btVector3 contactNormalOnB(0,0,0);
contactNormalOnB[i] = -1;
btScalar penetration = 0;
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
} else
{
if (m_bodyA)
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
}
btVector3 pivotBworld = m_pivotInB;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
} else
{
if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
}
btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
btScalar relaxation = 1.f;
fillMultiBodyConstraintMixed_old(constraintRow, data,
contactNormalOnB,
pivotAworld, pivotBworld,
position,
infoGlobal,
relaxation,
false);
constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
constraintRow.m_upperLimit = m_maxAppliedImpulse;
}
}
#define BTMBP2PCONSTRAINT_DIM 3
#else
#include "btMultiBodyPoint2Point.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#define BTMBP2PCONSTRAINT_DIM 6
#endif
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(body,0,link,-1,6,false),
:btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
@@ -162,7 +35,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,6,false),
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
@@ -223,7 +96,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
{
// int i=1;
for (int i=0;i<6;i++)
for (int i=0;i<BTMBP2PCONSTRAINT_DIM;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
@@ -231,14 +104,12 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
btVector3 contactNormalOnB(0,0,0);
btVector3 normalAng(0, 0, 0);
if(i >= 3)
contactNormalOnB[i-3] = -1;
else
normalAng[i] = -1;
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
contactNormalOnB[i] = -1;
#else
contactNormalOnB[i%3] = -1;
#endif
btScalar penetration = 0;
@@ -265,30 +136,180 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
}
btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
btScalar relaxation = 1.f;
if(i < 3)
position = 0;
constraintRow.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+m_bodyA->getNumDofs()+6);
btScalar* jac1=&data.m_jacobians[constraintRow.m_jacAindex];
btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
m_bodyA->fillContactJacobianMultiDof_test(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
fillMultiBodyConstraintMixed_old(constraintRow, data,
contactNormalOnB,
pivotAworld, pivotBworld,
position,
infoGlobal,
relaxation,
false);
constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
constraintRow.m_upperLimit = m_maxAppliedImpulse;
fillMultiBodyConstraint(constraintRow, data, 0, 0,
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
#else
const btVector3 dummy(0, 0, 0);
btAssert(m_bodyA->isMultiDof());
btScalar* jac1 = jacobianA(i);
const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
#endif
}
}
#endif
//#include "btMultiBodyPoint2Point.h"
//#include "btMultiBodyLinkCollider.h"
//#include "BulletDynamics/Dynamics/btRigidBody.h"
//
//btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
// :btMultiBodyConstraint(body,0,link,-1,6,false),
// m_rigidBodyA(0),
// m_rigidBodyB(bodyB),
// m_pivotInA(pivotInA),
// m_pivotInB(pivotInB)
//{
//}
//
//btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
// :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,6,false),
// m_rigidBodyA(0),
// m_rigidBodyB(0),
// m_pivotInA(pivotInA),
// m_pivotInB(pivotInB)
//{
//}
//
//
//btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
//{
//}
//
//
//int btMultiBodyPoint2Point::getIslandIdA() const
//{
// if (m_rigidBodyA)
// return m_rigidBodyA->getIslandTag();
//
// 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 btMultiBodyPoint2Point::getIslandIdB() const
//{
// if (m_rigidBodyB)
// return m_rigidBodyB->getIslandTag();
// 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 btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
// btMultiBodyJacobianData& data,
// const btContactSolverInfo& infoGlobal)
//{
//
//// int i=1;
// for (int i=0;i<6;i++)
// {
//
// btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
//
// constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
// constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
//
//
// btVector3 contactNormalOnB(0,0,0);
// btVector3 normalAng(0, 0, 0);
// if(i >= 3)
// contactNormalOnB[i-3] = -1;
// else
// normalAng[i] = -1;
//
//
// btScalar penetration = 0;
//
// // Convert local points back to world
// btVector3 pivotAworld = m_pivotInA;
// if (m_rigidBodyA)
// {
//
// constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
// pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
// } else
// {
// if (m_bodyA)
// pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
// }
// btVector3 pivotBworld = m_pivotInB;
// if (m_rigidBodyB)
// {
// constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
// pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
// } else
// {
// if (m_bodyB)
// pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
//
// }
// btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
// btScalar relaxation = 1.f;
//
// if(i < 3)
// position = 0;
//
// constraintRow.m_jacAindex = data.m_jacobians.size();
// data.m_jacobians.resize(data.m_jacobians.size()+m_bodyA->getNumDofs()+6);
// btScalar* jac1=&data.m_jacobians[constraintRow.m_jacAindex];
//
// m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
//
//
// fillMultiBodyConstraintMixed_old(constraintRow, data,
// contactNormalOnB,
// pivotAworld, pivotBworld,
// position,
// infoGlobal,
// relaxation,
// false);
// constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
// constraintRow.m_upperLimit = m_maxAppliedImpulse;
//
// }
//}