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