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;
}
public float reactionForce() {
float force = 0;
int totalContacts = 0;
for (PersistentManifold m : contactManifolds) {
int numContacts = m.getNumContacts();
totalContacts += numContacts;
for (int i = 0; i < numContacts; ++i) {
ManifoldPoint p = m.getContactPoint(i);
force += p.appliedImpulse;
}
}
return totalContacts != 0 ? force / totalContacts : 0;
}
@Override
public void debugDrawWorld() {
if (getDebugDrawer() != null
&& (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_CONTACT_POINTS) != 0) {
int numManifolds = getDispatcher().getNumManifolds();
Vector3f color = Stack.alloc(Vector3f.class);
color.set(0f, 0f, 0f);
for (int i = 0; i < numManifolds; i++) {
PersistentManifold contactManifold = getDispatcher().getManifoldByIndexInternal(i);
// btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
// btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
int numContacts = contactManifold.getNumContacts();
for (int j = 0; j < numContacts; j++) {
ManifoldPoint cp = contactManifold.getContactPoint(j);
getDebugDrawer()
.drawContactPoint(
cp.positionWorldOnB,
cp.normalWorldOnB,
cp.getDistance(),
cp.getLifeTime(),
color);
}
}
}
if (getDebugDrawer() != null
&& (getDebugDrawer().getDebugMode()
& (DebugDrawModes.DRAW_WIREFRAME | DebugDrawModes.DRAW_AABB))
!= 0) {
int i;
Transform tmpTrans = Stack.alloc(Transform.class);
Vector3f minAabb = Stack.alloc(Vector3f.class);
Vector3f maxAabb = Stack.alloc(Vector3f.class);
Vector3f colorvec = Stack.alloc(Vector3f.class);
// todo: iterate over awake simulation islands!
for (i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
if (getDebugDrawer() != null
&& (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
Vector3f color = Stack.alloc(Vector3f.class);
color.set(255f, 255f, 255f);
switch (colObj.getActivationState()) {
case CollisionObject.ACTIVE_TAG:
color.set(255f, 255f, 255f);
break;
case CollisionObject.ISLAND_SLEEPING:
color.set(0f, 255f, 0f);
break;
case CollisionObject.WANTS_DEACTIVATION:
color.set(0f, 255f, 255f);
break;
case CollisionObject.DISABLE_DEACTIVATION:
color.set(255f, 0f, 0f);
break;
case CollisionObject.DISABLE_SIMULATION:
color.set(255f, 255f, 0f);
break;
default:
{
color.set(255f, 0f, 0f);
}
}
debugDrawObject(colObj.getWorldTransform(tmpTrans), colObj.getCollisionShape(), color);
}
if (debugDrawer != null && (debugDrawer.getDebugMode() & DebugDrawModes.DRAW_AABB) != 0) {
colorvec.set(1f, 0f, 0f);
colObj.getCollisionShape().getAabb(colObj.getWorldTransform(tmpTrans), minAabb, maxAabb);
debugDrawer.drawAabb(minAabb, maxAabb, colorvec);
}
}
Vector3f wheelColor = Stack.alloc(Vector3f.class);
Vector3f wheelPosWS = Stack.alloc(Vector3f.class);
Vector3f axle = Stack.alloc(Vector3f.class);
Vector3f tmp = Stack.alloc(Vector3f.class);
for (i = 0; i < vehicles.size(); i++) {
for (int v = 0; v < vehicles.getQuick(i).getNumWheels(); v++) {
wheelColor.set(0, 255, 255);
if (vehicles.getQuick(i).getWheelInfo(v).raycastInfo.isInContact) {
wheelColor.set(0, 0, 255);
} else {
wheelColor.set(255, 0, 255);
}
wheelPosWS.set(vehicles.getQuick(i).getWheelInfo(v).worldTransform.origin);
axle.set(
vehicles
.getQuick(i)
.getWheelInfo(v)
.worldTransform
.basis
.getElement(0, vehicles.getQuick(i).getRightAxis()),
vehicles
.getQuick(i)
.getWheelInfo(v)
.worldTransform
.basis
.getElement(1, vehicles.getQuick(i).getRightAxis()),
vehicles
.getQuick(i)
.getWheelInfo(v)
.worldTransform
.basis
.getElement(2, vehicles.getQuick(i).getRightAxis()));
// m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
// debug wheels (cylinders)
tmp.add(wheelPosWS, axle);
debugDrawer.drawLine(wheelPosWS, tmp, wheelColor);
debugDrawer.drawLine(
wheelPosWS,
vehicles.getQuick(i).getWheelInfo(v).raycastInfo.contactPointWS,
wheelColor);
}
}
if (getDebugDrawer() != null && getDebugDrawer().getDebugMode() != 0) {
for (i = 0; i < actions.size(); i++) {
actions.getQuick(i).debugDraw(debugDrawer);
}
}
}
}