protected boolean recoverFromPenetration(CollisionWorld collisionWorld) {
boolean penetration = false;
collisionWorld
.getDispatcher()
.dispatchAllCollisionPairs(
ghostObject.getOverlappingPairCache(),
collisionWorld.getDispatchInfo(),
collisionWorld.getDispatcher());
currentPosition.set(ghostObject.getWorldTransform(new Transform()).origin);
double maxPen = 0.0f;
for (int i = 0; i < ghostObject.getOverlappingPairCache().getNumOverlappingPairs(); i++) {
manifoldArray.clear();
BroadphasePair collisionPair =
ghostObject.getOverlappingPairCache().getOverlappingPairArray().getQuick(i);
if (collisionPair.algorithm != null) {
collisionPair.algorithm.getAllContactManifolds(manifoldArray);
}
for (int j = 0; j < manifoldArray.size(); j++) {
PersistentManifold manifold = manifoldArray.getQuick(j);
double directionSign = manifold.getBody0() == ghostObject ? -1.0f : 1.0f;
for (int p = 0; p < manifold.getNumContacts(); p++) {
ManifoldPoint pt = manifold.getContactPoint(p);
double dist = pt.getDistance();
if (dist < 0.0f) {
if (dist < maxPen) {
maxPen = dist;
touchingNormal.set(pt.normalWorldOnB); // ??
touchingNormal.scale(directionSign);
}
currentPosition.scaleAdd(
directionSign * dist * 0.2f, pt.normalWorldOnB, currentPosition);
penetration = true;
} else {
// printf("touching %f\n", dist);
}
}
// manifold->clearManifold();
}
}
Transform newTrans = ghostObject.getWorldTransform(new Transform());
newTrans.origin.set(currentPosition);
ghostObject.setWorldTransform(newTrans);
// printf("m_touchingNormal =
// %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
// System.out.println("recoverFromPenetration "+penetration+" "+touchingNormal);
return penetration;
}
void updateServerElements() {
for (RigidBody rb : rigidBodies) {
ServerElement element = rigidBodyToServerElement.get(rb);
if (rb != null && rb.getMotionState() != null) {
rb.getMotionState().getWorldTransform(tmpTrans);
element.setTransform(tmpTrans);
}
if (!rb.isActive()) {
rb.activate();
}
// log.debug(" "+element.getPosition());
}
for (Integer id : serverGameArea.getGameState().getDynamicElements().keySet()) {
ServerElement elem = serverGameArea.getGameState().getDynamicElements().get(id);
elem.getCollidees().clear();
}
Integer nbManifold = dynamicsWorld.getDispatcher().getNumManifolds();
for (Integer i = 0; i < nbManifold; i++) {
PersistentManifold pm = dynamicsWorld.getDispatcher().getManifoldByIndexInternal(i);
RigidBody rb0 = (RigidBody) pm.getBody0();
RigidBody rb1 = (RigidBody) pm.getBody1();
ServerElement elem0 = rigidBodyToServerElement.get(rb0);
ServerElement elem1 = rigidBodyToServerElement.get(rb1);
if ((elem0 instanceof ServerBullet) || (elem1 instanceof ServerBullet)) {
log.info("coll " + elem0 + " " + elem1);
}
elem0.addCollidee(elem1);
elem1.addCollidee(elem0);
}
}