/*
|
* 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.collision.broadphase.CollisionAlgorithm;
|
import com.bulletphysics.collision.broadphase.CollisionAlgorithmConstructionInfo;
|
import com.bulletphysics.collision.broadphase.DispatcherInfo;
|
import com.bulletphysics.collision.narrowphase.ConvexCast.CastResult;
|
import com.bulletphysics.collision.narrowphase.PersistentManifold;
|
import com.bulletphysics.collision.narrowphase.SubsimplexConvexCast;
|
import com.bulletphysics.collision.narrowphase.VoronoiSimplexSolver;
|
import com.bulletphysics.collision.shapes.ConcaveShape;
|
import com.bulletphysics.collision.shapes.SphereShape;
|
import com.bulletphysics.collision.shapes.TriangleCallback;
|
import com.bulletphysics.collision.shapes.TriangleShape;
|
import com.bulletphysics.linearmath.Transform;
|
import com.bulletphysics.linearmath.VectorUtil;
|
import com.bulletphysics.util.ObjectArrayList;
|
import com.bulletphysics.util.ObjectPool;
|
import cz.advel.stack.Stack;
|
import javax.vecmath.Vector3f;
|
|
/**
|
* ConvexConcaveCollisionAlgorithm supports collision between convex shapes
|
* and (concave) trianges meshes.
|
*
|
* @author jezek2
|
*/
|
public class ConvexConcaveCollisionAlgorithm extends CollisionAlgorithm {
|
|
private boolean isSwapped;
|
private ConvexTriangleCallback btConvexTriangleCallback;
|
|
public void init(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, boolean isSwapped) {
|
super.init(ci);
|
this.isSwapped = isSwapped;
|
this.btConvexTriangleCallback = new ConvexTriangleCallback(dispatcher, body0, body1, isSwapped);
|
}
|
|
@Override
|
public void destroy() {
|
btConvexTriangleCallback.destroy();
|
}
|
|
@Override
|
public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
|
CollisionObject convexBody = isSwapped ? body1 : body0;
|
CollisionObject triBody = isSwapped ? body0 : body1;
|
|
if (triBody.getCollisionShape().isConcave()) {
|
CollisionObject triOb = triBody;
|
ConcaveShape concaveShape = (ConcaveShape)triOb.getCollisionShape();
|
|
if (convexBody.getCollisionShape().isConvex()) {
|
float collisionMarginTriangle = concaveShape.getMargin();
|
|
resultOut.setPersistentManifold(btConvexTriangleCallback.manifoldPtr);
|
btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
|
|
// Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
|
//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
|
|
btConvexTriangleCallback.manifoldPtr.setBodies(convexBody, triBody);
|
|
concaveShape.processAllTriangles(
|
btConvexTriangleCallback,
|
btConvexTriangleCallback.getAabbMin(Stack.alloc(Vector3f.class)),
|
btConvexTriangleCallback.getAabbMax(Stack.alloc(Vector3f.class)));
|
|
resultOut.refreshContactPoints();
|
}
|
}
|
}
|
|
@Override
|
public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
|
Vector3f tmp = Stack.alloc(Vector3f.class);
|
|
CollisionObject convexbody = isSwapped ? body1 : body0;
|
CollisionObject triBody = isSwapped ? body0 : body1;
|
|
// quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
|
|
// only perform CCD above a certain threshold, this prevents blocking on the long run
|
// because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
|
tmp.sub(convexbody.getInterpolationWorldTransform(Stack.alloc(Transform.class)).origin, convexbody.getWorldTransform(Stack.alloc(Transform.class)).origin);
|
float squareMot0 = tmp.lengthSquared();
|
if (squareMot0 < convexbody.getCcdSquareMotionThreshold()) {
|
return 1f;
|
}
|
|
Transform tmpTrans = Stack.alloc(Transform.class);
|
|
//const btVector3& from = convexbody->m_worldTransform.getOrigin();
|
//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
|
//todo: only do if the motion exceeds the 'radius'
|
|
Transform triInv = triBody.getWorldTransform(Stack.alloc(Transform.class));
|
triInv.inverse();
|
|
Transform convexFromLocal = Stack.alloc(Transform.class);
|
convexFromLocal.mul(triInv, convexbody.getWorldTransform(tmpTrans));
|
|
Transform convexToLocal = Stack.alloc(Transform.class);
|
convexToLocal.mul(triInv, convexbody.getInterpolationWorldTransform(tmpTrans));
|
|
if (triBody.getCollisionShape().isConcave()) {
|
Vector3f rayAabbMin = (Vector3f) Stack.alloc(convexFromLocal.origin);
|
VectorUtil.setMin(rayAabbMin, convexToLocal.origin);
|
|
Vector3f rayAabbMax = (Vector3f) Stack.alloc(convexFromLocal.origin);
|
VectorUtil.setMax(rayAabbMax, convexToLocal.origin);
|
|
float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
|
|
tmp.set(ccdRadius0, ccdRadius0, ccdRadius0);
|
rayAabbMin.sub(tmp);
|
rayAabbMax.add(tmp);
|
|
float curHitFraction = 1f; // is this available?
|
LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, convexbody.getCcdSweptSphereRadius(), curHitFraction);
|
|
raycastCallback.hitFraction = convexbody.getHitFraction();
|
|
CollisionObject concavebody = triBody;
|
|
ConcaveShape triangleMesh = (ConcaveShape)concavebody.getCollisionShape();
|
|
if (triangleMesh != null) {
|
triangleMesh.processAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
|
}
|
|
if (raycastCallback.hitFraction < convexbody.getHitFraction()) {
|
convexbody.setHitFraction(raycastCallback.hitFraction);
|
return raycastCallback.hitFraction;
|
}
|
}
|
|
return 1f;
|
}
|
|
@Override
|
public void getAllContactManifolds(ObjectArrayList<PersistentManifold> manifoldArray) {
|
if (btConvexTriangleCallback.manifoldPtr != null) {
|
manifoldArray.add(btConvexTriangleCallback.manifoldPtr);
|
}
|
}
|
|
public void clearCache() {
|
btConvexTriangleCallback.clearCache();
|
}
|
|
////////////////////////////////////////////////////////////////////////////
|
|
private static class LocalTriangleSphereCastCallback extends TriangleCallback {
|
public final Transform ccdSphereFromTrans = new Transform();
|
public final Transform ccdSphereToTrans = new Transform();
|
public final Transform meshTransform = new Transform();
|
|
public float ccdSphereRadius;
|
public float hitFraction;
|
|
private final Transform ident = new Transform();
|
|
public LocalTriangleSphereCastCallback(Transform from, Transform to, float ccdSphereRadius, float hitFraction) {
|
this.ccdSphereFromTrans.set(from);
|
this.ccdSphereToTrans.set(to);
|
this.ccdSphereRadius = ccdSphereRadius;
|
this.hitFraction = hitFraction;
|
|
// JAVA NOTE: moved here from processTriangle
|
ident.setIdentity();
|
}
|
|
public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
|
// do a swept sphere for now
|
|
//btTransform ident;
|
//ident.setIdentity();
|
|
CastResult castResult = new CastResult();
|
castResult.fraction = hitFraction;
|
SphereShape pointShape = new SphereShape(ccdSphereRadius);
|
TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
|
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
|
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);
|
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
|
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
|
//local space?
|
|
if (convexCaster.calcTimeOfImpact(ccdSphereFromTrans, ccdSphereToTrans, ident, ident, castResult)) {
|
if (hitFraction > castResult.fraction) {
|
hitFraction = castResult.fraction;
|
}
|
}
|
}
|
}
|
|
////////////////////////////////////////////////////////////////////////////
|
|
public static class CreateFunc extends CollisionAlgorithmCreateFunc {
|
private final ObjectPool<ConvexConcaveCollisionAlgorithm> pool = ObjectPool.get(ConvexConcaveCollisionAlgorithm.class);
|
|
@Override
|
public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
|
ConvexConcaveCollisionAlgorithm algo = pool.get();
|
algo.init(ci, body0, body1, false);
|
return algo;
|
}
|
|
@Override
|
public void releaseCollisionAlgorithm(CollisionAlgorithm algo) {
|
pool.release((ConvexConcaveCollisionAlgorithm)algo);
|
}
|
}
|
|
public static class SwappedCreateFunc extends CollisionAlgorithmCreateFunc {
|
private final ObjectPool<ConvexConcaveCollisionAlgorithm> pool = ObjectPool.get(ConvexConcaveCollisionAlgorithm.class);
|
|
@Override
|
public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
|
ConvexConcaveCollisionAlgorithm algo = pool.get();
|
algo.init(ci, body0, body1, true);
|
return algo;
|
}
|
|
@Override
|
public void releaseCollisionAlgorithm(CollisionAlgorithm algo) {
|
pool.release((ConvexConcaveCollisionAlgorithm)algo);
|
}
|
}
|
|
}
|
