if ((numConstraints + numManifolds) == 0) {
// printf("empty\n");
return 0f;
}
PersistentManifold manifold = null;
CollisionObject colObj0 = null, colObj1 = null;
//btRigidBody* rb0=0,*rb1=0;
// //#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
//
// BEGIN_PROFILE("refreshManifolds");
//
// int i;
//
//
//
// for (i=0;i<numManifolds;i++)
// {
// manifold = manifoldPtr[i];
// rb1 = (btRigidBody*)manifold->getBody1();
// rb0 = (btRigidBody*)manifold->getBody0();
//
// manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
//
// }
//
// END_PROFILE("refreshManifolds");
// //#endif //FORCE_REFESH_CONTACT_MANIFOLDS
Transform tmpTrans = Stack.alloc(Transform.class);
//int sizeofSB = sizeof(btSolverBody);
//int sizeofSC = sizeof(btSolverConstraint);
//if (1)
{
//if m_stackAlloc, try to pack bodies/constraints to speed up solving
// btBlock* sablock;
// sablock = stackAlloc->beginBlock();
// int memsize = 16;
// unsigned char* stackMemory = stackAlloc->allocate(memsize);
// todo: use stack allocator for this temp memory
//int minReservation = numManifolds * 2;
//m_tmpSolverBodyPool.reserve(minReservation);
//don't convert all bodies, only the one we need so solver the constraints
/*
{
for (int i=0;i<numBodies;i++)
{
btRigidBody* rb = btRigidBody::upcast(bodies[i]);
if (rb && (rb->getIslandTag() >= 0))
{
btAssert(rb->getCompanionId() < 0);
int solverBodyId = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb);
rb->setCompanionId(solverBodyId);
}
}
}
*/
//m_tmpSolverConstraintPool.reserve(minReservation);
//m_tmpSolverFrictionConstraintPool.reserve(minReservation);
{
int i;
Vector3f rel_pos1 = Stack.alloc(Vector3f.class);
Vector3f rel_pos2 = Stack.alloc(Vector3f.class);
Vector3f pos1 = Stack.alloc(Vector3f.class);
Vector3f pos2 = Stack.alloc(Vector3f.class);
Vector3f vel = Stack.alloc(Vector3f.class);
Vector3f torqueAxis0 = Stack.alloc(Vector3f.class);
Vector3f torqueAxis1 = Stack.alloc(Vector3f.class);
Vector3f vel1 = Stack.alloc(Vector3f.class);
Vector3f vel2 = Stack.alloc(Vector3f.class);
Vector3f frictionDir1 = Stack.alloc(Vector3f.class);
Vector3f frictionDir2 = Stack.alloc(Vector3f.class);
Vector3f vec = Stack.alloc(Vector3f.class);
Matrix3f tmpMat = Stack.alloc(Matrix3f.class);
for (i = 0; i < numManifolds; i++) {
manifold = manifoldPtr.getQuick(manifold_offset+i);
colObj0 = (CollisionObject) manifold.getBody0();
colObj1 = (CollisionObject) manifold.getBody1();
int solverBodyIdA = -1;
int solverBodyIdB = -1;
if (manifold.getNumContacts() != 0) {
if (colObj0.getIslandTag() >= 0) {
if (colObj0.getCompanionId() >= 0) {
// body has already been converted
solverBodyIdA = colObj0.getCompanionId();
}
else {
solverBodyIdA = tmpSolverBodyPool.size();
SolverBody solverBody = bodiesPool.get();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj0);
colObj0.setCompanionId(solverBodyIdA);
}
}
else {
// create a static body
solverBodyIdA = tmpSolverBodyPool.size();
SolverBody solverBody = bodiesPool.get();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj0);
}
if (colObj1.getIslandTag() >= 0) {
if (colObj1.getCompanionId() >= 0) {
solverBodyIdB = colObj1.getCompanionId();
}
else {
solverBodyIdB = tmpSolverBodyPool.size();
SolverBody solverBody = bodiesPool.get();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj1);
colObj1.setCompanionId(solverBodyIdB);
}
}
else {
// create a static body
solverBodyIdB = tmpSolverBodyPool.size();
SolverBody solverBody = bodiesPool.get();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj1);
}
}
float relaxation;
for (int j = 0; j < manifold.getNumContacts(); j++) {
ManifoldPoint cp = manifold.getContactPoint(j);
if (cp.getDistance() <= 0f) {
cp.getPositionWorldOnA(pos1);
cp.getPositionWorldOnB(pos2);