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;
}
protected void stepDown(CollisionWorld collisionWorld, double dt) {
Transform start = new Transform();
Transform end = new Transform();
// phase 3: down
double additionalDownStep = (wasOnGround /*&& !onGround()*/) ? stepHeight : 0.0f;
Vector3d step_drop = new Vector3d();
step_drop.scale(currentStepOffset + additionalDownStep, upAxisDirection[upAxis]);
double downVelocity =
(additionalDownStep == 0.0f && verticalVelocity < 0.0f ? -verticalVelocity : 0.0f) * dt;
Vector3d gravity_drop = new Vector3d();
gravity_drop.scale(downVelocity, upAxisDirection[upAxis]);
targetPosition.sub(step_drop);
targetPosition.sub(gravity_drop);
start.setIdentity();
end.setIdentity();
start.origin.set(currentPosition);
end.origin.set(targetPosition);
KinematicClosestNotMeConvexResultCallback callback =
new KinematicClosestNotMeConvexResultCallback(
ghostObject, upAxisDirection[upAxis], maxSlopeCosine);
callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
if (useGhostObjectSweepTest) {
ghostObject.convexSweepTest(
convexShape,
start,
end,
callback,
collisionWorld.getDispatchInfo().allowedCcdPenetration);
} else {
collisionWorld.convexSweepTest(convexShape, start, end, callback);
}
if (callback.hasHit()) {
// we dropped a fraction of the height -> hit floor
currentPosition.interpolate(currentPosition, targetPosition, callback.closestHitFraction);
verticalVelocity = 0.0f;
verticalOffset = 0.0f;
} else {
// we dropped the full height
currentPosition.set(targetPosition);
}
}
protected void stepUp(CollisionWorld world) {
// phase 1: up
Transform start = new Transform();
Transform end = new Transform();
targetPosition.scaleAdd(
stepHeight + (verticalOffset > 0.0 ? verticalOffset : 0.0f),
upAxisDirection[upAxis],
currentPosition);
start.setIdentity();
end.setIdentity();
/* FIXME: Handle penetration properly */
start.origin.scaleAdd(
convexShape.getMargin() + addedMargin, upAxisDirection[upAxis], currentPosition);
end.origin.set(targetPosition);
// Find only sloped/flat surface hits, avoid wall and ceiling hits...
Vector3d up = new Vector3d();
up.scale(-1f, upAxisDirection[upAxis]);
KinematicClosestNotMeConvexResultCallback callback =
new KinematicClosestNotMeConvexResultCallback(ghostObject, up, 0.0f);
callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
if (useGhostObjectSweepTest) {
ghostObject.convexSweepTest(
convexShape, start, end, callback, world.getDispatchInfo().allowedCcdPenetration);
} else {
world.convexSweepTest(convexShape, start, end, callback);
}
if (callback.hasHit()) {
// we moved up only a fraction of the step height
currentStepOffset = stepHeight * callback.closestHitFraction;
currentPosition.interpolate(currentPosition, targetPosition, callback.closestHitFraction);
verticalVelocity = 0.0f;
verticalOffset = 0.0f;
} else {
currentStepOffset = stepHeight;
currentPosition.set(targetPosition);
}
}
public void removeObjectFromCollisionDomain(MTComponent group) {
if (collisionGroups.containsKey(group)) {
collisionGroups.remove(group);
}
ArrayList<CollisionObject> colObjs = this.getAllObjectsForCollisionGroup(group);
Iterator<CollisionObject> iterColObjs = colObjs.iterator();
while (iterColObjs.hasNext()) {
collisionWorld.removeCollisionObject(iterColObjs.next());
}
if (colObjectsForGroup.containsKey(group)) {
this.colObjectsForGroup.remove(group);
}
}
public void addClusterToCollisionDomain(Cluster3DExt cluster) {
ArrayList<CollisionObject> colObjs = new ArrayList<CollisionObject>();
for (int i = 0; i < cluster.getChildren().length; i++) {
colObjs.addAll(
this.getAllObjectsForCollisionGroup(
cluster.getChildren()[i])); // save all collision objects in on object
removeObjectFromCollisionDomain(
cluster.getChildren()[i]); // remove current object from collision world
}
groupId = (short) (groupId << 1); // shift groupId so every group has a unique bit value
for (int i = 0; i < colObjs.size(); i++) {
collisionWorld.addCollisionObject(colObjs.get(i), groupId, (short) ~groupId);
}
colObjectsForGroup.put(cluster, colObjs);
}
protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3d walkMove) {
// printf("m_normalizedDirection=%f,%f,%f\n",
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
Transform start = new Transform();
Transform end = new Transform();
targetPosition.add(currentPosition, walkMove);
start.setIdentity();
end.setIdentity();
double fraction = 1.0f;
Vector3d distance2Vec = new Vector3d();
distance2Vec.sub(currentPosition, targetPosition);
double distance2 = distance2Vec.lengthSquared();
// printf("distance2=%f\n",distance2);
/*if (touchingContact) {
if (normalizedDirection.dot(touchingNormal) > 0.0f) {
updateTargetPositionBasedOnCollision(touchingNormal);
}
}*/
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0) {
start.origin.set(currentPosition);
end.origin.set(targetPosition);
KinematicClosestNotMeConvexResultCallback callback =
new KinematicClosestNotMeConvexResultCallback(
ghostObject, upAxisDirection[upAxis], -1.0f);
callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
double margin = convexShape.getMargin();
convexShape.setMargin(margin + addedMargin);
if (useGhostObjectSweepTest) {
ghostObject.convexSweepTest(
convexShape,
start,
end,
callback,
collisionWorld.getDispatchInfo().allowedCcdPenetration);
} else {
collisionWorld.convexSweepTest(convexShape, start, end, callback);
}
convexShape.setMargin(margin);
fraction -= callback.closestHitFraction;
if (callback.hasHit()) {
// we moved only a fraction
Vector3d hitDistanceVec = new Vector3d();
hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
// double hitDistance = hitDistanceVec.length();
// if the distance is farther than the collision margin, move
// if (hitDistance > addedMargin) {
// //printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
// currentPosition.interpolate(currentPosition, targetPosition,
// callback.closestHitFraction);
// }
updateTargetPositionBasedOnCollision(callback.hitNormalWorld);
Vector3d currentDir = new Vector3d();
currentDir.sub(targetPosition, currentPosition);
distance2 = currentDir.lengthSquared();
if (distance2 > BulletGlobals.SIMD_EPSILON) {
currentDir.normalize();
// see Quake2: "If velocity is against original velocity, stop ead to avoid tiny
// oscilations in sloping corners."
if (currentDir.dot(normalizedDirection) <= 0.0f) {
break;
}
} else {
// printf("currentDir: don't normalize a zero vector\n");
break;
}
} else {
// we moved whole way
currentPosition.set(targetPosition);
}
// if (callback.m_closestHitFraction == 0.f)
// break;
}
}
public void addObjectsToCollisionDomain() {
Iterator<Entry<MTComponent, ArrayList<MTTriangleMesh>>> groupIter =
collisionGroups.entrySet().iterator();
int collidesWith = 0;
while (groupIter.hasNext()) {
collidesWith = ~groupId;
Entry<MTComponent, ArrayList<MTTriangleMesh>> element = groupIter.next();
// CollisionShape shape = createMeshShapeFromMTMeshTriangle(element.getValue());
Iterator<MTTriangleMesh> iter = element.getValue().iterator();
while (iter.hasNext()) {
MTComponent comp = iter.next();
CollisionShape shape = createMeshShapeFromMTMeshTriangle((MTTriangleMesh) comp);
/* Transform startTransform = new Transform();
startTransform.setIdentity();
Vector3f vec = new Vector3f();
Vector3D translate = currentMesh.getCenterPointRelativeToParent();
vec.x = translate.x;
vec.y = translate.y;
vec.z = translate.z;
startTransform.origin.set(vec);*/
Transform startTransform = new Transform();
startTransform.setIdentity();
Matrix mat = element.getKey().getGlobalMatrix();
Vector3f vec = new Vector3f(0.0f, 0.0f, 0.0f);
vec.x = mat.m03;
vec.y = mat.m13;
vec.z = mat.m23;
// startTransform.transform(vec);
// Matrix4f mat = CollisionManager.convertMT4JMatrixToMatrix4f(mesh.get);
// startTransform.set(mat);
// startTransform.origin.set(vec);
// mat4f.m03 = mat4f.m03 + vec.x;
// mat4f.m13 = mat4f.m13 + vec.y;
// mat4f.m23 = mat4f.m23 + vec.z;
// mat4f.setTranslation(vec);
startTransform.origin.set(vec);
Vector3f scale = new Vector3f(); // get scale value of global matrix
Vector3D xVec = new Vector3D(mat.m00, mat.m01, mat.m02);
Vector3D yVec = new Vector3D(mat.m10, mat.m11, mat.m12);
Vector3D zVec = new Vector3D(mat.m20, mat.m21, mat.m22);
scale.x = xVec.length();
scale.y = yVec.length();
scale.z = zVec.length();
float[] scaleVals = new float[3];
scale.get(scaleVals);
for (int i = 0; i < 3; i++) // get rotation value by extracting scalation
{
try {
float[] colvals = mat.getRow(i);
for (int j = 0; j < 3; j++) {
colvals[j] = colvals[j] / scaleVals[i];
}
startTransform.basis.setRow(i, colvals);
} catch (Exception e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
// startTransform.set(mat4f);
float mass = 5f; // fake mass value only needed correctly if used with dynamic engine
shape.setLocalScaling(scale);
GImpactMeshShape sh = (GImpactMeshShape) shape;
sh.updateBound();
RigidBody body = localCreateRigidBody(mass, startTransform, shape);
// get Center Of Mass of Triangle Mesh and write it to MTTriangleMesh
Vector3f vecCOM = new Vector3f();
body.getCenterOfMassPosition(vecCOM);
Vector3D vecComMT4J = new Vector3D(vecCOM.x, vecCOM.y, vecCOM.z);
// mesh.setCenterOfMass(vecComMT4J);
// mesh.setMass(100.0f);
// add Object to Collision World
collisionWorld.addCollisionObject(body, groupId, (short) collidesWith);
colObjectToComponent.put(body, comp);
addCollisionObjectToGroup(
comp.getParent(), body); // save association between collision objects and groups
}
groupId = (short) (groupId << 1); // shift groupId so every group has a unique bit value
}
SimulatePreDrawAction calcDynamics =
new SimulatePreDrawAction(collisionWorld, this, scene.getCanvas());
calcDynamics.setCurrentTimeStep(1.f / 1000000000000.0f);
scene.registerPreDrawAction(calcDynamics);
}
