public void rayTest(
Vector3f rayFromWorld, Vector3f rayToWorld, CollisionWorld.RayResultCallback resultCallback) {
Transform rayFromTrans = new Transform();
rayFromTrans.setIdentity();
rayFromTrans.origin.set(rayFromWorld);
Transform rayToTrans = new Transform();
rayToTrans.setIdentity();
rayToTrans.origin.set(rayToWorld);
Transform tmpTrans = new Transform();
for (int i = 0; i < overlappingObjects.size(); i++) {
CollisionObject collisionObject = overlappingObjects.getQuick(i);
// only perform raycast if filterMask matches
if (resultCallback.needsCollision(collisionObject.getBroadphaseHandle())) {
CollisionWorld.rayTestSingle(
rayFromTrans,
rayToTrans,
collisionObject,
collisionObject.getCollisionShape(),
collisionObject.getWorldTransform(tmpTrans),
resultCallback);
}
}
}
private static int getConstraintIslandId(TypedConstraint lhs) {
int islandId;
CollisionObject rcolObj0 = lhs.getRigidBodyA();
CollisionObject rcolObj1 = lhs.getRigidBodyB();
islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
return islandId;
}
@Override
public boolean needsCollision(BroadphaseProxy proxy0) {
// don't collide with itself
if (proxy0.clientObject == me) {
return false;
}
// don't do CCD when the collision filters are not matching
if (!super.needsCollision(proxy0)) {
return false;
}
CollisionObject otherObj = (CollisionObject) proxy0.clientObject;
// call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
if (dispatcher.needsResponse(me, otherObj)) {
// don't do CCD when there are already contact points (touching contact/penetration)
ObjectArrayList<PersistentManifold> manifoldArray =
new ObjectArrayList<PersistentManifold>();
BroadphasePair collisionPair = pairCache.findPair(me.getBroadphaseHandle(), proxy0);
if (collisionPair != null) {
if (collisionPair.algorithm != null) {
// manifoldArray.resize(0);
collisionPair.algorithm.getAllContactManifolds(manifoldArray);
for (int j = 0; j < manifoldArray.size(); j++) {
PersistentManifold manifold = manifoldArray.getQuick(j);
if (manifold.getNumContacts() > 0) {
return false;
}
}
}
}
}
return true;
}
public static GhostObject upcast(CollisionObject colObj) {
if (colObj.getInternalType() == CollisionObjectType.GHOST_OBJECT) {
return (GhostObject) colObj;
}
return null;
}
public void render() {
FloatBuffer mBuffer = BufferUtils.createFloatBuffer(16);
float[] mFloat = new float[16];
GL11.glPushMatrix();
Vector3d cameraPosition = _parent.getActiveCamera().getPosition();
GL11.glTranslated(-cameraPosition.x, -cameraPosition.y, -cameraPosition.z);
List<CollisionObject> collisionObjects = _discreteDynamicsWorld.getCollisionObjectArray();
for (CollisionObject co : collisionObjects) {
if (co.getClass().equals(BlockRigidBody.class)) {
BlockRigidBody br = (BlockRigidBody) co;
Block block = BlockManager.getInstance().getBlock(br.getType());
Transform t = new Transform();
br.getMotionState().getWorldTransform(t);
t.getOpenGLMatrix(mFloat);
mBuffer.put(mFloat);
mBuffer.flip();
GL11.glPushMatrix();
GL11.glMultMatrix(mBuffer);
if (br.getCollisionShape() == _blockShapeHalf) GL11.glScalef(0.5f, 0.5f, 0.5f);
else if (br.getCollisionShape() == _blockShapeQuarter) GL11.glScalef(0.25f, 0.25f, 0.25f);
block.renderWithLightValue(_parent.getRenderingLightValueAt(new Vector3d(t.origin)));
GL11.glPopMatrix();
}
}
GL11.glPopMatrix();
}
public void awakenRigidBodiesInArea(Vector3f min, Vector3f max) {
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject collisionObject = collisionObjects.getQuick(i);
if (!collisionObject.isStaticOrKinematicObject() && !collisionObject.isActive()) {
Vector3f otherMin = Stack.alloc(Vector3f.class);
Vector3f otherMax = Stack.alloc(Vector3f.class);
collisionObject
.getCollisionShape()
.getAabb(
collisionObject.getWorldTransform(Stack.alloc(Transform.class)),
otherMin,
otherMax);
if (AabbUtil2.testAabbAgainstAabb2(min, max, otherMin, otherMax)) {
collisionObject.activate();
}
}
}
}
@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);
}
}
}
}
public void convexSweepTest(
ConvexShape castShape,
Transform convexFromWorld,
Transform convexToWorld,
CollisionWorld.ConvexResultCallback resultCallback,
float allowedCcdPenetration) {
Transform convexFromTrans = new Transform();
Transform convexToTrans = new Transform();
convexFromTrans.set(convexFromWorld);
convexToTrans.set(convexToWorld);
Vector3f castShapeAabbMin = new Vector3f();
Vector3f castShapeAabbMax = new Vector3f();
// compute AABB that encompasses angular movement
{
Vector3f linVel = new Vector3f();
Vector3f angVel = new Vector3f();
TransformUtil.calculateVelocity(convexFromTrans, convexToTrans, 1f, linVel, angVel);
Transform R = new Transform();
R.setIdentity();
R.setRotation(convexFromTrans.getRotation(new Quat4f()));
castShape.calculateTemporalAabb(R, linVel, angVel, 1f, castShapeAabbMin, castShapeAabbMax);
}
Transform tmpTrans = new Transform();
// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
for (int i = 0; i < overlappingObjects.size(); i++) {
CollisionObject collisionObject = overlappingObjects.getQuick(i);
// only perform raycast if filterMask matches
if (resultCallback.needsCollision(collisionObject.getBroadphaseHandle())) {
// RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
Vector3f collisionObjectAabbMin = new Vector3f();
Vector3f collisionObjectAabbMax = new Vector3f();
collisionObject
.getCollisionShape()
.getAabb(
collisionObject.getWorldTransform(tmpTrans),
collisionObjectAabbMin,
collisionObjectAabbMax);
AabbUtil2.aabbExpand(
collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
float[] hitLambda =
new float[] {1f}; // could use resultCallback.closestHitFraction, but needs testing
Vector3f hitNormal = new Vector3f();
if (AabbUtil2.rayAabb(
convexFromWorld.origin,
convexToWorld.origin,
collisionObjectAabbMin,
collisionObjectAabbMax,
hitLambda,
hitNormal)) {
CollisionWorld.objectQuerySingle(
castShape,
convexFromTrans,
convexToTrans,
collisionObject,
collisionObject.getCollisionShape(),
collisionObject.getWorldTransform(tmpTrans),
resultCallback,
allowedCcdPenetration);
}
}
}
}
public void setWorldTransform(Transform trans) {
body.setWorldTransform(trans);
}
public Transform getWorldTransform() {
body.getWorldTransform(tempTransform);
return tempTransform;
}
