/*
|
* 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.narrowphase;
|
|
import com.bulletphysics.BulletGlobals;
|
import com.bulletphysics.BulletStats;
|
import com.bulletphysics.collision.shapes.ConvexShape;
|
import com.bulletphysics.linearmath.IDebugDraw;
|
import com.bulletphysics.linearmath.MatrixUtil;
|
import com.bulletphysics.linearmath.Transform;
|
import cz.advel.stack.Stack;
|
import cz.advel.stack.StaticAlloc;
|
import javax.vecmath.Vector3f;
|
|
/**
|
* GjkPairDetector uses GJK to implement the {@link DiscreteCollisionDetectorInterface}.
|
*
|
* @author jezek2
|
*/
|
public class GjkPairDetector extends DiscreteCollisionDetectorInterface {
|
|
//protected final BulletStack stack = BulletStack.get();
|
|
// must be above the machine epsilon
|
private static final float REL_ERROR2 = 1.0e-6f;
|
|
private final Vector3f cachedSeparatingAxis = new Vector3f();
|
private ConvexPenetrationDepthSolver penetrationDepthSolver;
|
private SimplexSolverInterface simplexSolver;
|
private ConvexShape minkowskiA;
|
private ConvexShape minkowskiB;
|
private boolean ignoreMargin;
|
|
// some debugging to fix degeneracy problems
|
public int lastUsedMethod;
|
public int curIter;
|
public int degenerateSimplex;
|
public int catchDegeneracies;
|
|
public void init(ConvexShape objectA, ConvexShape objectB, SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver penetrationDepthSolver) {
|
this.cachedSeparatingAxis.set(0f, 0f, 1f);
|
this.ignoreMargin = false;
|
this.lastUsedMethod = -1;
|
this.catchDegeneracies = 1;
|
|
this.penetrationDepthSolver = penetrationDepthSolver;
|
this.simplexSolver = simplexSolver;
|
this.minkowskiA = objectA;
|
this.minkowskiB = objectB;
|
}
|
|
@StaticAlloc
|
public void getClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw, boolean swapResults) {
|
Vector3f tmp = Stack.alloc(Vector3f.class);
|
|
float distance = 0f;
|
Vector3f normalInB = Stack.alloc(Vector3f.class);
|
normalInB.set(0f, 0f, 0f);
|
Vector3f pointOnA = Stack.alloc(Vector3f.class), pointOnB = Stack.alloc(Vector3f.class);
|
Transform localTransA = (Transform) Stack.alloc(input.transformA);
|
Transform localTransB = (Transform) Stack.alloc(input.transformB);
|
Vector3f positionOffset = Stack.alloc(Vector3f.class);
|
positionOffset.add(localTransA.origin, localTransB.origin);
|
positionOffset.scale(0.5f);
|
localTransA.origin.sub(positionOffset);
|
localTransB.origin.sub(positionOffset);
|
|
float marginA = minkowskiA.getMargin();
|
float marginB = minkowskiB.getMargin();
|
|
BulletStats.gNumGjkChecks++;
|
|
// for CCD we don't use margins
|
if (ignoreMargin) {
|
marginA = 0f;
|
marginB = 0f;
|
}
|
|
curIter = 0;
|
int gGjkMaxIter = 1000; // this is to catch invalid input, perhaps check for #NaN?
|
cachedSeparatingAxis.set(0f, 1f, 0f);
|
|
boolean isValid = false;
|
boolean checkSimplex = false;
|
boolean checkPenetration = true;
|
degenerateSimplex = 0;
|
|
lastUsedMethod = -1;
|
|
{
|
float squaredDistance = BulletGlobals.SIMD_INFINITY;
|
float delta = 0f;
|
|
float margin = marginA + marginB;
|
|
simplexSolver.reset();
|
|
Vector3f seperatingAxisInA = Stack.alloc(Vector3f.class);
|
Vector3f seperatingAxisInB = Stack.alloc(Vector3f.class);
|
|
Vector3f pInA = Stack.alloc(Vector3f.class);
|
Vector3f qInB = Stack.alloc(Vector3f.class);
|
|
Vector3f pWorld = Stack.alloc(Vector3f.class);
|
Vector3f qWorld = Stack.alloc(Vector3f.class);
|
Vector3f w = Stack.alloc(Vector3f.class);
|
|
Vector3f tmpPointOnA = Stack.alloc(Vector3f.class), tmpPointOnB = Stack.alloc(Vector3f.class);
|
Vector3f tmpNormalInB = Stack.alloc(Vector3f.class);
|
|
for (;;) //while (true)
|
{
|
seperatingAxisInA.negate(cachedSeparatingAxis);
|
MatrixUtil.transposeTransform(seperatingAxisInA, seperatingAxisInA, input.transformA.basis);
|
|
seperatingAxisInB.set(cachedSeparatingAxis);
|
MatrixUtil.transposeTransform(seperatingAxisInB, seperatingAxisInB, input.transformB.basis);
|
|
minkowskiA.localGetSupportingVertexWithoutMargin(seperatingAxisInA, pInA);
|
minkowskiB.localGetSupportingVertexWithoutMargin(seperatingAxisInB, qInB);
|
|
pWorld.set(pInA);
|
localTransA.transform(pWorld);
|
|
qWorld.set(qInB);
|
localTransB.transform(qWorld);
|
|
w.sub(pWorld, qWorld);
|
|
delta = cachedSeparatingAxis.dot(w);
|
|
// potential exit, they don't overlap
|
if ((delta > 0f) && (delta * delta > squaredDistance * input.maximumDistanceSquared)) {
|
checkPenetration = false;
|
break;
|
}
|
|
// exit 0: the new point is already in the simplex, or we didn't come any closer
|
if (simplexSolver.inSimplex(w)) {
|
degenerateSimplex = 1;
|
checkSimplex = true;
|
break;
|
}
|
// are we getting any closer ?
|
float f0 = squaredDistance - delta;
|
float f1 = squaredDistance * REL_ERROR2;
|
|
if (f0 <= f1) {
|
if (f0 <= 0f) {
|
degenerateSimplex = 2;
|
}
|
checkSimplex = true;
|
break;
|
}
|
// add current vertex to simplex
|
simplexSolver.addVertex(w, pWorld, qWorld);
|
|
// calculate the closest point to the origin (update vector v)
|
if (!simplexSolver.closest(cachedSeparatingAxis)) {
|
degenerateSimplex = 3;
|
checkSimplex = true;
|
break;
|
}
|
|
if (cachedSeparatingAxis.lengthSquared() < REL_ERROR2) {
|
degenerateSimplex = 6;
|
checkSimplex = true;
|
break;
|
}
|
|
float previousSquaredDistance = squaredDistance;
|
squaredDistance = cachedSeparatingAxis.lengthSquared();
|
|
// redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
|
// are we getting any closer ?
|
if (previousSquaredDistance - squaredDistance <= BulletGlobals.FLT_EPSILON * previousSquaredDistance) {
|
simplexSolver.backup_closest(cachedSeparatingAxis);
|
checkSimplex = true;
|
break;
|
}
|
|
// degeneracy, this is typically due to invalid/uninitialized worldtransforms for a CollisionObject
|
if (curIter++ > gGjkMaxIter) {
|
//#if defined(DEBUG) || defined (_DEBUG)
|
if (BulletGlobals.DEBUG) {
|
System.err.printf("btGjkPairDetector maxIter exceeded:%i\n", curIter);
|
System.err.printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
|
cachedSeparatingAxis.x,
|
cachedSeparatingAxis.y,
|
cachedSeparatingAxis.z,
|
squaredDistance,
|
minkowskiA.getShapeType(),
|
minkowskiB.getShapeType());
|
}
|
//#endif
|
break;
|
|
}
|
|
boolean check = (!simplexSolver.fullSimplex());
|
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
|
|
if (!check) {
|
// do we need this backup_closest here ?
|
simplexSolver.backup_closest(cachedSeparatingAxis);
|
break;
|
}
|
}
|
|
if (checkSimplex) {
|
simplexSolver.compute_points(pointOnA, pointOnB);
|
normalInB.sub(pointOnA, pointOnB);
|
float lenSqr = cachedSeparatingAxis.lengthSquared();
|
// valid normal
|
if (lenSqr < 0.0001f) {
|
degenerateSimplex = 5;
|
}
|
if (lenSqr > BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
|
float rlen = 1f / (float) Math.sqrt(lenSqr);
|
normalInB.scale(rlen); // normalize
|
float s = (float) Math.sqrt(squaredDistance);
|
|
assert (s > 0f);
|
|
tmp.scale((marginA / s), cachedSeparatingAxis);
|
pointOnA.sub(tmp);
|
|
tmp.scale((marginB / s), cachedSeparatingAxis);
|
pointOnB.add(tmp);
|
|
distance = ((1f / rlen) - margin);
|
isValid = true;
|
|
lastUsedMethod = 1;
|
}
|
else {
|
lastUsedMethod = 2;
|
}
|
}
|
|
boolean catchDegeneratePenetrationCase =
|
(catchDegeneracies != 0 && penetrationDepthSolver != null && degenerateSimplex != 0 && ((distance + margin) < 0.01f));
|
|
//if (checkPenetration && !isValid)
|
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) {
|
// penetration case
|
|
// if there is no way to handle penetrations, bail out
|
if (penetrationDepthSolver != null) {
|
// Penetration depth case.
|
BulletStats.gNumDeepPenetrationChecks++;
|
|
boolean isValid2 = penetrationDepthSolver.calcPenDepth(
|
simplexSolver,
|
minkowskiA, minkowskiB,
|
localTransA, localTransB,
|
cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
|
debugDraw/*,input.stackAlloc*/);
|
|
if (isValid2) {
|
tmpNormalInB.sub(tmpPointOnB, tmpPointOnA);
|
|
float lenSqr = tmpNormalInB.lengthSquared();
|
if (lenSqr > (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
|
tmpNormalInB.scale(1f / (float) Math.sqrt(lenSqr));
|
tmp.sub(tmpPointOnA, tmpPointOnB);
|
float distance2 = -tmp.length();
|
// only replace valid penetrations when the result is deeper (check)
|
if (!isValid || (distance2 < distance)) {
|
distance = distance2;
|
pointOnA.set(tmpPointOnA);
|
pointOnB.set(tmpPointOnB);
|
normalInB.set(tmpNormalInB);
|
isValid = true;
|
lastUsedMethod = 3;
|
}
|
else {
|
|
}
|
}
|
else {
|
//isValid = false;
|
lastUsedMethod = 4;
|
}
|
}
|
else {
|
lastUsedMethod = 5;
|
}
|
|
}
|
}
|
}
|
|
if (isValid) {
|
//#ifdef __SPU__
|
// //spu_printf("distance\n");
|
//#endif //__CELLOS_LV2__
|
|
tmp.add(pointOnB, positionOffset);
|
output.addContactPoint(
|
normalInB,
|
tmp,
|
distance);
|
//printf("gjk add:%f",distance);
|
}
|
}
|
|
public void setMinkowskiA(ConvexShape minkA) {
|
minkowskiA = minkA;
|
}
|
|
public void setMinkowskiB(ConvexShape minkB) {
|
minkowskiB = minkB;
|
}
|
|
public void setCachedSeperatingAxis(Vector3f seperatingAxis) {
|
cachedSeparatingAxis.set(seperatingAxis);
|
}
|
|
public void setPenetrationDepthSolver(ConvexPenetrationDepthSolver penetrationDepthSolver) {
|
this.penetrationDepthSolver = penetrationDepthSolver;
|
}
|
|
/**
|
* Don't use setIgnoreMargin, it's for Bullet's internal use.
|
*/
|
public void setIgnoreMargin(boolean ignoreMargin) {
|
this.ignoreMargin = ignoreMargin;
|
}
|
|
}
|
