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;
}