/*
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
*
* Bullet Continuous Collision Detection and Physics Library
* Copyright (c) 2003-2008 Erwin Coumans http://www.bulletphysics.com/
*
* This software is provided 'as-is', without any express or implied warranty.
* In no event will the authors be held liable for any damages arising from
* the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
package com.bulletphysics.collision.dispatch;
import com.bulletphysics.BulletGlobals;
import com.bulletphysics.collision.broadphase.CollisionAlgorithm;
import com.bulletphysics.collision.broadphase.CollisionAlgorithmConstructionInfo;
import com.bulletphysics.collision.broadphase.DispatcherInfo;
import com.bulletphysics.collision.narrowphase.PersistentManifold;
import com.bulletphysics.collision.shapes.SphereShape;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.util.ObjectArrayList;
import com.bulletphysics.util.ObjectPool;
import cz.advel.stack.Stack;
import javax.vecmath.Vector3f;
/**
* Provides collision detection between two spheres.
*
* @author jezek2
*/
public class SphereSphereCollisionAlgorithm extends CollisionAlgorithm {
private boolean ownManifold;
private PersistentManifold manifoldPtr;
public void init(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1) {
super.init(ci);
manifoldPtr = mf;
if (manifoldPtr == null) {
manifoldPtr = dispatcher.getNewManifold(col0, col1);
ownManifold = true;
}
}
@Override
public void init(CollisionAlgorithmConstructionInfo ci) {
super.init(ci);
}
@Override
public void destroy() {
if (ownManifold) {
if (manifoldPtr != null) {
dispatcher.releaseManifold(manifoldPtr);
}
manifoldPtr = null;
}
}
@Override
public void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
if (manifoldPtr == null) {
return;
}
Transform tmpTrans1 = Stack.alloc(Transform.class);
Transform tmpTrans2 = Stack.alloc(Transform.class);
resultOut.setPersistentManifold(manifoldPtr);
SphereShape sphere0 = (SphereShape) col0.getCollisionShape();
SphereShape sphere1 = (SphereShape) col1.getCollisionShape();
Vector3f diff = Stack.alloc(Vector3f.class);
diff.sub(col0.getWorldTransform(tmpTrans1).origin, col1.getWorldTransform(tmpTrans2).origin);
float len = diff.length();
float radius0 = sphere0.getRadius();
float radius1 = sphere1.getRadius();
//#ifdef CLEAR_MANIFOLD
//manifoldPtr.clearManifold(); // don't do this, it disables warmstarting
//#endif
// if distance positive, don't generate a new contact
if (len > (radius0 + radius1)) {
//#ifndef CLEAR_MANIFOLD
resultOut.refreshContactPoints();
//#endif //CLEAR_MANIFOLD
return;
}
// distance (negative means penetration)
float dist = len - (radius0 + radius1);
Vector3f normalOnSurfaceB = Stack.alloc(Vector3f.class);
normalOnSurfaceB.set(1f, 0f, 0f);
if (len > BulletGlobals.FLT_EPSILON) {
normalOnSurfaceB.scale(1f / len, diff);
}
Vector3f tmp = Stack.alloc(Vector3f.class);
// point on A (worldspace)
Vector3f pos0 = Stack.alloc(Vector3f.class);
tmp.scale(radius0, normalOnSurfaceB);
pos0.sub(col0.getWorldTransform(tmpTrans1).origin, tmp);
// point on B (worldspace)
Vector3f pos1 = Stack.alloc(Vector3f.class);
tmp.scale(radius1, normalOnSurfaceB);
pos1.add(col1.getWorldTransform(tmpTrans2).origin, tmp);
// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.addContactPoint(normalOnSurfaceB, pos1, dist);
//#ifndef CLEAR_MANIFOLD
resultOut.refreshContactPoints();
//#endif //CLEAR_MANIFOLD
}
@Override
public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
return 1f;
}
@Override
public void getAllContactManifolds(ObjectArrayList<PersistentManifold> manifoldArray) {
if (manifoldPtr != null && ownManifold) {
manifoldArray.add(manifoldPtr);
}
}
////////////////////////////////////////////////////////////////////////////
public static class CreateFunc extends CollisionAlgorithmCreateFunc {
private final ObjectPool<SphereSphereCollisionAlgorithm> pool = ObjectPool.get(SphereSphereCollisionAlgorithm.class);
@Override
public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
SphereSphereCollisionAlgorithm algo = pool.get();
algo.init(null, ci, body0, body1);
return algo;
}
@Override
public void releaseCollisionAlgorithm(CollisionAlgorithm algo) {
pool.release((SphereSphereCollisionAlgorithm)algo);
}
};
}