/*
|
* 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.collision.shapes.ConvexShape;
|
import com.bulletphysics.linearmath.MatrixUtil;
|
import com.bulletphysics.linearmath.Transform;
|
import com.bulletphysics.linearmath.VectorUtil;
|
import cz.advel.stack.Stack;
|
import javax.vecmath.Vector3f;
|
|
/**
|
* SubsimplexConvexCast implements Gino van den Bergens' paper
|
* "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
|
* GJK based Ray Cast, optimized version
|
* Objects should not start in overlap, otherwise results are not defined.
|
*
|
* @author jezek2
|
*/
|
public class SubsimplexConvexCast extends ConvexCast {
|
|
//protected final BulletStack stack = BulletStack.get();
|
|
// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
|
// See discussion about this here http://www.bulletphysics.com/phpBB2/viewtopic.php?t=565
|
//#ifdef BT_USE_DOUBLE_PRECISION
|
//#define MAX_ITERATIONS 64
|
//#else
|
//#define MAX_ITERATIONS 32
|
//#endif
|
|
private static final int MAX_ITERATIONS = 32;
|
|
private SimplexSolverInterface simplexSolver;
|
private ConvexShape convexA;
|
private ConvexShape convexB;
|
|
public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, SimplexSolverInterface simplexSolver) {
|
this.convexA = shapeA;
|
this.convexB = shapeB;
|
this.simplexSolver = simplexSolver;
|
}
|
|
public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
|
Vector3f tmp = Stack.alloc(Vector3f.class);
|
|
simplexSolver.reset();
|
|
Vector3f linVelA = Stack.alloc(Vector3f.class);
|
Vector3f linVelB = Stack.alloc(Vector3f.class);
|
linVelA.sub(toA.origin, fromA.origin);
|
linVelB.sub(toB.origin, fromB.origin);
|
|
float lambda = 0f;
|
|
Transform interpolatedTransA = (Transform) Stack.alloc(fromA);
|
Transform interpolatedTransB = (Transform) Stack.alloc(fromB);
|
|
// take relative motion
|
Vector3f r = Stack.alloc(Vector3f.class);
|
r.sub(linVelA, linVelB);
|
|
Vector3f v = Stack.alloc(Vector3f.class);
|
|
tmp.negate(r);
|
MatrixUtil.transposeTransform(tmp, tmp, fromA.basis);
|
Vector3f supVertexA = convexA.localGetSupportingVertex(tmp, Stack.alloc(Vector3f.class));
|
fromA.transform(supVertexA);
|
|
MatrixUtil.transposeTransform(tmp, r, fromB.basis);
|
Vector3f supVertexB = convexB.localGetSupportingVertex(tmp, Stack.alloc(Vector3f.class));
|
fromB.transform(supVertexB);
|
|
v.sub(supVertexA, supVertexB);
|
|
int maxIter = MAX_ITERATIONS;
|
|
Vector3f n = Stack.alloc(Vector3f.class);
|
n.set(0f, 0f, 0f);
|
boolean hasResult = false;
|
Vector3f c = Stack.alloc(Vector3f.class);
|
|
float lastLambda = lambda;
|
|
float dist2 = v.lengthSquared();
|
//#ifdef BT_USE_DOUBLE_PRECISION
|
// btScalar epsilon = btScalar(0.0001);
|
//#else
|
float epsilon = 0.0001f;
|
//#endif
|
Vector3f w = Stack.alloc(Vector3f.class), p = Stack.alloc(Vector3f.class);
|
float VdotR;
|
|
while ((dist2 > epsilon) && (maxIter--) != 0) {
|
tmp.negate(v);
|
MatrixUtil.transposeTransform(tmp, tmp, interpolatedTransA.basis);
|
convexA.localGetSupportingVertex(tmp, supVertexA);
|
interpolatedTransA.transform(supVertexA);
|
|
MatrixUtil.transposeTransform(tmp, v, interpolatedTransB.basis);
|
convexB.localGetSupportingVertex(tmp, supVertexB);
|
interpolatedTransB.transform(supVertexB);
|
|
w.sub(supVertexA, supVertexB);
|
|
float VdotW = v.dot(w);
|
|
if (lambda > 1f) {
|
return false;
|
}
|
|
if (VdotW > 0f) {
|
VdotR = v.dot(r);
|
|
if (VdotR >= -(BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
|
return false;
|
}
|
else {
|
lambda = lambda - VdotW / VdotR;
|
|
// interpolate to next lambda
|
// x = s + lambda * r;
|
VectorUtil.setInterpolate3(interpolatedTransA.origin, fromA.origin, toA.origin, lambda);
|
VectorUtil.setInterpolate3(interpolatedTransB.origin, fromB.origin, toB.origin, lambda);
|
//m_simplexSolver->reset();
|
// check next line
|
w.sub(supVertexA, supVertexB);
|
lastLambda = lambda;
|
n.set(v);
|
hasResult = true;
|
}
|
}
|
simplexSolver.addVertex(w, supVertexA , supVertexB);
|
if (simplexSolver.closest(v)) {
|
dist2 = v.lengthSquared();
|
hasResult = true;
|
// todo: check this normal for validity
|
//n.set(v);
|
//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
|
//printf("DIST2=%f\n",dist2);
|
//printf("numverts = %i\n",m_simplexSolver->numVertices());
|
}
|
else {
|
dist2 = 0f;
|
}
|
}
|
|
//int numiter = MAX_ITERATIONS - maxIter;
|
// printf("number of iterations: %d", numiter);
|
|
// don't report a time of impact when moving 'away' from the hitnormal
|
|
result.fraction = lambda;
|
if (n.lengthSquared() >= BulletGlobals.SIMD_EPSILON * BulletGlobals.SIMD_EPSILON) {
|
result.normal.normalize(n);
|
}
|
else {
|
result.normal.set(0f, 0f, 0f);
|
}
|
|
// don't report time of impact for motion away from the contact normal (or causes minor penetration)
|
if (result.normal.dot(r) >= -result.allowedPenetration)
|
return false;
|
|
Vector3f hitA = Stack.alloc(Vector3f.class);
|
Vector3f hitB = Stack.alloc(Vector3f.class);
|
simplexSolver.compute_points(hitA,hitB);
|
result.hitPoint.set(hitB);
|
return true;
|
}
|
|
}
|
