/*
* 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.util.ObjectPool;
import com.bulletphysics.collision.broadphase.CollisionAlgorithm;
import com.bulletphysics.collision.broadphase.CollisionAlgorithmConstructionInfo;
import com.bulletphysics.collision.broadphase.DispatcherInfo;
import com.bulletphysics.collision.narrowphase.ConvexCast;
import com.bulletphysics.collision.narrowphase.ConvexPenetrationDepthSolver;
import com.bulletphysics.collision.narrowphase.DiscreteCollisionDetectorInterface.ClosestPointInput;
import com.bulletphysics.collision.narrowphase.GjkConvexCast;
import com.bulletphysics.collision.narrowphase.GjkPairDetector;
import com.bulletphysics.collision.narrowphase.PersistentManifold;
import com.bulletphysics.collision.narrowphase.SimplexSolverInterface;
import com.bulletphysics.collision.narrowphase.VoronoiSimplexSolver;
import com.bulletphysics.collision.shapes.ConvexShape;
import com.bulletphysics.collision.shapes.SphereShape;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.util.ObjectArrayList;
import cz.advel.stack.Stack;
import javax.vecmath.Vector3f;
/**
* ConvexConvexAlgorithm collision algorithm implements time of impact, convex
* closest points and penetration depth calculations.
*
* @author jezek2
*/
public class ConvexConvexAlgorithm extends CollisionAlgorithm {
protected final ObjectPool<ClosestPointInput> pointInputsPool = ObjectPool.get(ClosestPointInput.class);
private GjkPairDetector gjkPairDetector = new GjkPairDetector();
public boolean ownManifold;
public PersistentManifold manifoldPtr;
public boolean lowLevelOfDetail;
public void init(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver pdSolver) {
super.init(ci);
gjkPairDetector.init(null, null, simplexSolver, pdSolver);
this.manifoldPtr = mf;
this.ownManifold = false;
this.lowLevelOfDetail = false;
}
@Override
public void destroy() {
if (ownManifold) {
if (manifoldPtr != null) {
dispatcher.releaseManifold(manifoldPtr);
}
manifoldPtr = null;
}
}
public void setLowLevelOfDetail(boolean useLowLevel) {
this.lowLevelOfDetail = useLowLevel;
}
/**
* Convex-Convex collision algorithm.
*/
@Override
public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
if (manifoldPtr == null) {
// swapped?
manifoldPtr = dispatcher.getNewManifold(body0, body1);
ownManifold = true;
}
resultOut.setPersistentManifold(manifoldPtr);
// #ifdef USE_BT_GJKEPA
// btConvexShape* shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
// btConvexShape* shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
// const btScalar radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
// btGjkEpaSolver::sResults results;
// if(btGjkEpaSolver::Collide( shape0,body0->getWorldTransform(),
// shape1,body1->getWorldTransform(),
// radialmargin,results))
// {
// dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
// resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
// }
// #else
ConvexShape min0 = (ConvexShape) body0.getCollisionShape();
ConvexShape min1 = (ConvexShape) body1.getCollisionShape();
ClosestPointInput input = pointInputsPool.get();
input.init();
// JAVA NOTE: original: TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.setMinkowskiA(min0);
gjkPairDetector.setMinkowskiB(min1);
input.maximumDistanceSquared = min0.getMargin() + min1.getMargin() + manifoldPtr.getContactBreakingThreshold();
input.maximumDistanceSquared *= input.maximumDistanceSquared;
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
// input.m_maximumDistanceSquared = btScalar(1e30);
body0.getWorldTransform(input.transformA);
body1.getWorldTransform(input.transformB);
gjkPairDetector.getClosestPoints(input, resultOut, dispatchInfo.debugDraw);
pointInputsPool.release(input);
// #endif
if (ownManifold) {
resultOut.refreshContactPoints();
}
}
private static boolean disableCcd = false;
@Override
public float calculateTimeOfImpact(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Transform tmpTrans1 = Stack.alloc(Transform.class);
Transform tmpTrans2 = Stack.alloc(Transform.class);
// Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
// Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
// col0->m_worldTransform,
float resultFraction = 1f;
tmp.sub(col0.getInterpolationWorldTransform(tmpTrans1).origin, col0.getWorldTransform(tmpTrans2).origin);
float squareMot0 = tmp.lengthSquared();
tmp.sub(col1.getInterpolationWorldTransform(tmpTrans1).origin, col1.getWorldTransform(tmpTrans2).origin);
float squareMot1 = tmp.lengthSquared();
if (squareMot0 < col0.getCcdSquareMotionThreshold() &&
squareMot1 < col1.getCcdSquareMotionThreshold()) {
return resultFraction;
}
if (disableCcd) {
return 1f;
}
Transform tmpTrans3 = Stack.alloc(Transform.class);
Transform tmpTrans4 = Stack.alloc(Transform.class);
// An adhoc way of testing the Continuous Collision Detection algorithms
// One object is approximated as a sphere, to simplify things
// Starting in penetration should report no time of impact
// For proper CCD, better accuracy and handling of 'allowed' penetration should be added
// also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
// Convex0 against sphere for Convex1
{
ConvexShape convex0 = (ConvexShape) col0.getCollisionShape();
SphereShape sphere1 = new SphereShape(col1.getCcdSweptSphereRadius()); // todo: allow non-zero sphere sizes, for better approximation
ConvexCast.CastResult result = new ConvexCast.CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2),
col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
// store result.m_fraction in both bodies
if (col0.getHitFraction() > result.fraction) {
col0.setHitFraction(result.fraction);
}
if (col1.getHitFraction() > result.fraction) {
col1.setHitFraction(result.fraction);
}
if (resultFraction > result.fraction) {
resultFraction = result.fraction;
}
}
}
// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = (ConvexShape) col1.getCollisionShape();
SphereShape sphere0 = new SphereShape(col0.getCcdSweptSphereRadius()); // todo: allow non-zero sphere sizes, for better approximation
ConvexCast.CastResult result = new ConvexCast.CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2),
col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
//store result.m_fraction in both bodies
if (col0.getHitFraction() > result.fraction) {
col0.setHitFraction(result.fraction);
}
if (col1.getHitFraction() > result.fraction) {
col1.setHitFraction(result.fraction);
}
if (resultFraction > result.fraction) {
resultFraction = result.fraction;
}
}
}
return resultFraction;
}
@Override
public void getAllContactManifolds(ObjectArrayList<PersistentManifold> manifoldArray) {
// should we use ownManifold to avoid adding duplicates?
if (manifoldPtr != null && ownManifold) {
manifoldArray.add(manifoldPtr);
}
}
public PersistentManifold getManifold() {
return manifoldPtr;
}
////////////////////////////////////////////////////////////////////////////
public static class CreateFunc extends CollisionAlgorithmCreateFunc {
private final ObjectPool<ConvexConvexAlgorithm> pool = ObjectPool.get(ConvexConvexAlgorithm.class);
public ConvexPenetrationDepthSolver pdSolver;
public SimplexSolverInterface simplexSolver;
public CreateFunc(SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver pdSolver) {
this.simplexSolver = simplexSolver;
this.pdSolver = pdSolver;
}
@Override
public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
ConvexConvexAlgorithm algo = pool.get();
algo.init(ci.manifold, ci, body0, body1, simplexSolver, pdSolver);
return algo;
}
@Override
public void releaseCollisionAlgorithm(CollisionAlgorithm algo) {
pool.release((ConvexConvexAlgorithm)algo);
}
}
}