BulletStats.popProfile();
}
}
protected void prepareConstraints(PersistentManifold manifoldPtr, ContactSolverInfo info, IDebugDraw debugDrawer) {
RigidBody body0 = (RigidBody) manifoldPtr.getBody0();
RigidBody body1 = (RigidBody) manifoldPtr.getBody1();
// only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
{
//#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
//manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
//#endif //FORCE_REFESH_CONTACT_MANIFOLDS
int numpoints = manifoldPtr.getNumContacts();
BulletStats.gTotalContactPoints += numpoints;
Vector3f tmpVec = Stack.alloc(Vector3f.class);
Matrix3f tmpMat3 = Stack.alloc(Matrix3f.class);
Vector3f pos1 = Stack.alloc(Vector3f.class);
Vector3f pos2 = Stack.alloc(Vector3f.class);
Vector3f rel_pos1 = Stack.alloc(Vector3f.class);
Vector3f rel_pos2 = Stack.alloc(Vector3f.class);
Vector3f vel1 = Stack.alloc(Vector3f.class);
Vector3f vel2 = Stack.alloc(Vector3f.class);
Vector3f vel = Stack.alloc(Vector3f.class);
Vector3f totalImpulse = Stack.alloc(Vector3f.class);
Vector3f torqueAxis0 = Stack.alloc(Vector3f.class);
Vector3f torqueAxis1 = Stack.alloc(Vector3f.class);
Vector3f ftorqueAxis0 = Stack.alloc(Vector3f.class);
Vector3f ftorqueAxis1 = Stack.alloc(Vector3f.class);
for (int i = 0; i < numpoints; i++) {
ManifoldPoint cp = manifoldPtr.getContactPoint(i);
if (cp.getDistance() <= 0f) {
cp.getPositionWorldOnA(pos1);
cp.getPositionWorldOnB(pos2);
rel_pos1.sub(pos1, body0.getCenterOfMassPosition(tmpVec));
rel_pos2.sub(pos2, body1.getCenterOfMassPosition(tmpVec));
// this jacobian entry is re-used for all iterations
Matrix3f mat1 = body0.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
mat1.transpose();
Matrix3f mat2 = body1.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
mat2.transpose();
JacobianEntry jac = jacobiansPool.get();
jac.init(mat1, mat2,
rel_pos1, rel_pos2, cp.normalWorldOnB,
body0.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)), body0.getInvMass(),
body1.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)), body1.getInvMass());
float jacDiagAB = jac.getDiagonal();
jacobiansPool.release(jac);
ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
if (cpd != null) {
// might be invalid
cpd.persistentLifeTime++;
if (cpd.persistentLifeTime != cp.getLifeTime()) {
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
//new (cpd) btConstraintPersistentData;
cpd.reset();
cpd.persistentLifeTime = cp.getLifeTime();
}
else {
//printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
}
}
else {
// todo: should this be in a pool?
//void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
//cpd = new (mem)btConstraintPersistentData;
cpd = new ConstraintPersistentData();
//assert(cpd != null);
totalCpd++;
//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
cp.userPersistentData = cpd;
cpd.persistentLifeTime = cp.getLifeTime();
//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
}
assert (cpd != null);
cpd.jacDiagABInv = 1f / jacDiagAB;
// Dependent on Rigidbody A and B types, fetch the contact/friction response func
// perhaps do a similar thing for friction/restutution combiner funcs...
cpd.frictionSolverFunc = frictionDispatch[body0.frictionSolverType][body1.frictionSolverType];
cpd.contactSolverFunc = contactDispatch[body0.contactSolverType][body1.contactSolverType];
body0.getVelocityInLocalPoint(rel_pos1, vel1);
body1.getVelocityInLocalPoint(rel_pos2, vel2);
vel.sub(vel1, vel2);
float rel_vel;
rel_vel = cp.normalWorldOnB.dot(vel);
float combinedRestitution = cp.combinedRestitution;
cpd.penetration = cp.getDistance(); ///btScalar(info.m_numIterations);
cpd.friction = cp.combinedFriction;
cpd.restitution = restitutionCurve(rel_vel, combinedRestitution);
if (cpd.restitution <= 0f) {
cpd.restitution = 0f;
}
// restitution and penetration work in same direction so
// rel_vel
float penVel = -cpd.penetration / info.timeStep;
if (cpd.restitution > penVel) {
cpd.penetration = 0f;
}
float relaxation = info.damping;
if ((info.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
cpd.appliedImpulse *= relaxation;
}
else {
cpd.appliedImpulse = 0f;
}
// for friction
cpd.prevAppliedImpulse = cpd.appliedImpulse;
// re-calculate friction direction every frame, todo: check if this is really needed
TransformUtil.planeSpace1(cp.normalWorldOnB, cpd.frictionWorldTangential0, cpd.frictionWorldTangential1);
//#define NO_FRICTION_WARMSTART 1
//#ifdef NO_FRICTION_WARMSTART
cpd.accumulatedTangentImpulse0 = 0f;
cpd.accumulatedTangentImpulse1 = 0f;
//#endif //NO_FRICTION_WARMSTART
float denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential0);
float denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential0);
float denom = relaxation / (denom0 + denom1);
cpd.jacDiagABInvTangent0 = denom;
denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential1);
denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential1);
denom = relaxation / (denom0 + denom1);
cpd.jacDiagABInvTangent1 = denom;
//btVector3 totalImpulse =
// //#ifndef NO_FRICTION_WARMSTART
// //cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
// //cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
// //#endif //NO_FRICTION_WARMSTART
// cp.normalWorldOnB*cpd.appliedImpulse;
totalImpulse.scale(cpd.appliedImpulse, cp.normalWorldOnB);
///
{
torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
cpd.angularComponentA.set(torqueAxis0);
body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.angularComponentA);
torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
cpd.angularComponentB.set(torqueAxis1);
body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.angularComponentB);
}
{
ftorqueAxis0.cross(rel_pos1, cpd.frictionWorldTangential0);
cpd.frictionAngularComponent0A.set(ftorqueAxis0);
body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent0A);
}
{
ftorqueAxis1.cross(rel_pos1, cpd.frictionWorldTangential1);
cpd.frictionAngularComponent1A.set(ftorqueAxis1);
body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent1A);
}
{
ftorqueAxis0.cross(rel_pos2, cpd.frictionWorldTangential0);
cpd.frictionAngularComponent0B.set(ftorqueAxis0);
body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent0B);
}
{
ftorqueAxis1.cross(rel_pos2, cpd.frictionWorldTangential1);
cpd.frictionAngularComponent1B.set(ftorqueAxis1);
body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent1B);
}
///
// apply previous frames impulse on both bodies
body0.applyImpulse(totalImpulse, rel_pos1);
tmpVec.negate(totalImpulse);
body1.applyImpulse(tmpVec, rel_pos2);
}
}
}
}