//Look at the GJK_EPA.h header file for documentation and instructions
bool SteerLib::GJK_EPA::intersect(float& return_penetration_depth, Util::Vector& return_penetration_vector, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
std::vector<Util::Vector> simplex;
bool collision = gjk(_shapeA, _shapeB, simplex);
if(collision)
epa(return_penetration_depth, return_penetration_vector,simplex,_shapeA, _shapeB);
/*else{
return_penetration_depth = 0;
return_penetration_vector.zero();
}*/
return collision;
}
void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
{
if (m_convexB1)
{
m_simplexSolver->reset();
btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = transA;
input.m_transformB = transB;
gjk.getClosestPoints(input,pointCollector,0);
} else
{
//convex versus plane
const btConvexShape* convexShape = m_convexA;
const btStaticPlaneShape* planeShape = m_planeShape;
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform convexWorldTransform = transA;
btTransform convexInPlaneTrans;
convexInPlaneTrans= transB.inverse() * convexWorldTransform;
btTransform planeInConvex;
planeInConvex= convexWorldTransform.inverse() * transB;
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
pointCollector.addContactPoint(
normalOnSurfaceB,
vtxInPlaneWorld,
distance);
}
}
bool btGjkConvexCast::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
m_simplexSolver->reset();
/// compute linear velocity for this interval, to interpolate
//assume no rotation/angular velocity, assert here?
btVector3 linVelA,linVelB;
linVelA = toA.getOrigin()-fromA.getOrigin();
linVelB = toB.getOrigin()-fromB.getOrigin();
btScalar radius = btScalar(0.001);
btScalar lambda = btScalar(0.);
btVector3 v(1,0,0);
int maxIter = MAX_ITERATIONS;
btVector3 n;
n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
bool hasResult = false;
btVector3 c;
btVector3 r = (linVelA-linVelB);
btScalar lastLambda = lambda;
//btScalar epsilon = btScalar(0.001);
int numIter = 0;
//first solution, using GJK
btTransform identityTrans;
identityTrans.setIdentity();
// result.drawCoordSystem(sphereTr);
btPointCollector pointCollector;
btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.getClosestPoints(input,pointCollector,0);
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
if (hasResult)
{
btScalar dist;
dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
}
btScalar dLambda = btScalar(0.);
btScalar projectedLinearVelocity = r.dot(n);
dLambda = dist / (projectedLinearVelocity);
lambda = lambda - dLambda;
if (lambda > btScalar(1.))
return false;
if (lambda < btScalar(0.))
return false;
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
break;
}
lastLambda = lambda;
//interpolate to next lambda
result.DebugDraw( lambda );
input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
gjk.getClosestPoints(input,pointCollector,0);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < btScalar(0.))
{
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal=n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
} else
{
//??
return false;
}
}
//is n normalized?
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (n.dot(r)>=-result.m_allowedPenetration)
return false;
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
}
bool btContinuousConvexCollision::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
m_simplexSolver->reset();
/// compute linear and angular velocity for this interval, to interpolate
btVector3 linVelA,angVelA,linVelB,angVelB;
btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
btScalar boundingRadiusB = m_convexB->getAngularMotionDisc();
btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
btVector3 relLinVel = (linVelB-linVelA);
btScalar relLinVelocLength = (linVelB-linVelA).length();
if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
return false;
btScalar radius = btScalar(0.001);
btScalar lambda = btScalar(0.);
btVector3 v(1,0,0);
int maxIter = MAX_ITERATIONS;
btVector3 n;
n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
bool hasResult = false;
btVector3 c;
btScalar lastLambda = lambda;
//btScalar epsilon = btScalar(0.001);
int numIter = 0;
//first solution, using GJK
btTransform identityTrans;
identityTrans.setIdentity();
btSphereShape raySphere(btScalar(0.0));
raySphere.setMargin(btScalar(0.));
// result.drawCoordSystem(sphereTr);
btPointCollector pointCollector1;
{
btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.getClosestPoints(input,pointCollector1,0);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if (hasResult)
{
btScalar dist;
dist = pointCollector1.m_distance;
n = pointCollector1.m_normalOnBInWorld;
btScalar projectedLinearVelocity = relLinVel.dot(n);
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
}
btScalar dLambda = btScalar(0.);
projectedLinearVelocity = relLinVel.dot(n);
//calculate safe moving fraction from distance / (linear+rotational velocity)
//btScalar clippedDist = GEN_min(angularConservativeRadius,dist);
//btScalar clippedDist = dist;
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
return false;
dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > btScalar(1.))
return false;
if (lambda < btScalar(0.))
return false;
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
break;
}
lastLambda = lambda;
//interpolate to next lambda
btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
result.DebugDraw( lambda );
btPointCollector pointCollector;
btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = interpolatedTransA;
input.m_transformB = interpolatedTransB;
gjk.getClosestPoints(input,pointCollector,0);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < btScalar(0.))
{
//degenerate ?!
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal=n;//.setValue(1,1,1);// = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
} else
{
//??
return false;
}
}
if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
return false;
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
/*
//todo:
//if movement away from normal, discard result
btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
if (result.m_fraction < btScalar(1.))
{
if (move.dot(result.m_normal) <= btScalar(0.))
{
}
}
*/
}
/* compute intersection of two convex polyhedrons */
TRI* cvi (double *va, int nva, double *pa, int npa, double *vb, int nvb, double *pb, int npb, CVIKIND kind, int *m, double **pv, int *nv)
{
double e [6], p [3], q [3], eps, d, *nl, *pt, *nn, *yy;
PFV *pfv, *v, *w, *z;
int i, j, k, n;
TRI *tri, *t;
/* initialize */
eps = GEOMETRIC_EPSILON;
tri = t = NULL;
pfv = NULL;
yy = NULL;
/* compute closest points */
d = gjk (va, nva, vb, nvb, p, q);
if (d > GEOMETRIC_EPSILON) { *m = 0; return NULL; }
/* push 'p' deeper inside only if regularized intersection is sought */
if (kind == REGULARIZED && !refine_point (pa, npa, pb, npb, p, &eps)) { *m = 0; return NULL; }
/* vertices extents for a later sanity check */
vertices_extents (va, nva, vb, nvb, eps, e);
/* translate base points of planes so that
* p = q = 0; compute new normals 'yy' */
ERRMEM (yy = malloc (sizeof (double [3]) * (npa+npb)));
for (i = 0, nl = pa, pt = pa + 3, nn = yy;
i < npa; i ++, nl += 6, pt += 6, nn += 3)
{
SUB (pt, p, q); /* q => translated point of current plane */
d = - DOT (nl, q); /* d => zero offset */
if (d > -GEOMETRIC_EPSILON) d = -eps; /* regularisation (tiny swelling) */
DIV (nl, -d, nn); /* <nn, x> <= 1 (yy stores vertices of polar polygon) */
}
for (i = 0, nl = pb, pt = pb + 3; i < npb;
i ++, nl += 6, pt += 6, nn += 3)
{
SUB (pt, p, q);
d = - DOT (nl, q);
if (d > -GEOMETRIC_EPSILON) d = -eps; /* regularisation (tiny swelling) */
DIV (nl, -d, nn);
}
/* compute and polarise convex
* hull of new normals 'yy' */
if (!(tri = hull (yy, npa+npb, &i))) goto error; /* tri = cv (polar (a) U polar (b)) */
if (!(pfv = TRI_Polarise (tri, i, &j))) goto error; /* pfv = polar (tri) => pfv = a * b */
/* normals in 'pfv' point to 'yy'; triangulate
* polar faces and set 'a' or 'b' flags */
/* count all face vertices */
for (k = n = 0; k < j; k ++)
{
v = &pfv [k]; n ++;
for (w = v->n; w != v; w = w->n) n ++;
}
/* there is (number of face vertices - 2) triangles per face,
* hence there is n - j * 2 triangles in total; vertex
* memory in 'pfv' is placed after n PFV items */
#if GEOMDEBUG
ASSERT_DEBUG (n - j*2 > 3, "Inconsitent polar faces => too few vertices in some faces");
#else
if (n - j*2 <= 3) goto error;
#endif
ERRMEM (tri = realloc (tri, sizeof (TRI) * (n-j*2) + sizeof (double [3]) * i)); /* allocate space for triangles and vertices */
pt = (double*) (tri + (n - j*2)); /* this is where output vertices begin */
nn = (double*) (pfv + n); /* this is where coords begin in 'pfv' block */
memcpy (pt, nn, sizeof (double [3]) * i); /* copy vertex data */
if (pv) *pv = pt;
if (nv) *nv = i;
/* shift point coords to the old 'zero' */
for (k = 0, nl = pt; k < i; k ++, nl += 3)
{
ADD (nl, p, nl); /* 'nl' used as a point */
if (nl[0] < e [0] || nl[1] < e [1] || nl[2] < e [2] ||
nl[0] > e [3] || nl[1] > e [4] || nl[2] > e [5])
{
#if GEOMDEBUG
printf ("CVI HAS GONE INSANE FOR THE INPUT:\n"), dump_input (va, nva, pa, npa, vb, nvb, pb, npb);
#endif
goto error;
}
}
for (k = 0, t = tri; k < j; k ++)
{
v = &pfv [k]; /* fixed vertex 'v' */
for (w = v->n, z = w->n; z != v; w = w->n, z = z->n, t ++) /* remaining vertices 'w' & 'z' */
{
COPY (v->nl, t->out); /* copy normal */
NORMALIZE (t->out);
t->ver [0] = pt + (v->coord - nn); /* map vertices */
t->ver [1] = pt + (w->coord - nn);
t->ver [2] = pt + (z->coord - nn);
t->flg = ((v->nl - yy) / 3) + 1; /* first 'npa' entries in 'yy' come from 'a' => positive 1-based index in 'a' */
if (t->flg > npa) t->flg = -(t->flg - npa); /* last 'npb' ones come from 'b' => negative 1-based index in 'b' */
}
}
goto done;
error:
if (tri) free (tri);
tri = t = NULL;
done:
free (yy);
free (pfv);
(*m) = (t - tri);
return tri;
}
/* generate test */
static void gen ()
{
double d [3], move [3];
switch (mode)
{
case GJK_CONVEX_CONVEX:
{
asize = bsize = 0;
while (asize < minim) asize = rand () % limit;
while (bsize < minim) bsize = rand () % limit;
SETRAND (move, 1.0);
for (int n = 0; n < asize; n ++)
{ SETRAND (apoint [n], 0.75);
ADD (apoint [n], move, apoint [n]); }
for (int n = 0; n < bsize; n ++)
{ SETRAND (bpoint [n], 0.75);
SUB (bpoint [n], move, bpoint [n]); }
free (a);
free (b);
a = hull ((double*)apoint, asize, &alength);
b = hull ((double*)bpoint, bsize, &blength);
double *va, *vb;
int nva, nvb;
va = TRI_Vertices (a, alength, &nva);
vb = TRI_Vertices (b, blength, &nvb);
gjk (va, nva, vb, nvb, p, q);
free (va);
free (vb);
}
break;
case GJK_CONVEX_SPHERE:
{
asize = bsize = 0;
while (asize < minim) asize = rand () % limit;
SETRAND (move, 1.0);
for (int n = 0; n < asize; n ++)
{ SETRAND (apoint [n], 0.75);
ADD (apoint [n], move, apoint [n]); }
free (a);
a = hull ((double*)apoint, asize, &alength);
SETRAND (center, 1.0);
SUB (center, move, center);
radius = 0.75 * DRAND ();
double *va;
int nva;
va = TRI_Vertices (a, alength, &nva);
gjk_convex_sphere (va, nva, center, radius, p, q);
free (va);
}
break;
case GJK_CONVEX_POINT:
{
asize = bsize = 0;
while (asize < minim) asize = rand () % limit;
SETRAND (move, 1.0);
for (int n = 0; n < asize; n ++)
{ SETRAND (apoint [n], 0.75);
ADD (apoint [n], move, apoint [n]); }
free (a);
a = hull ((double*)apoint, asize, &alength);
SETRAND (center, 1.0);
SUB (center, move, center);
radius = 0.0;
double *va;
int nva;
va = TRI_Vertices (a, alength, &nva);
COPY (center, p);
gjk_convex_point (va, nva, p, q);
free (va);
}
break;
case GJK_CONVEX_ELLIP:
{
asize = bsize = 0;
while (asize < minim) asize = rand () % limit;
SETRAND (move, 1.0);
for (int n = 0; n < asize; n ++)
{ SETRAND (apoint [n], 0.75);
ADD (apoint [n], move, apoint [n]); }
free (a);
a = hull ((double*)apoint, asize, &alength);
SETRAND (el1_center, 1.0);
SUB (el1_center, move, el1_center);
el1_sca [0] = 0.75 * DRAND ();
el1_sca [1] = 0.75 * DRAND ();
el1_sca [2] = 0.75 * DRAND ();
EXPMAP (el1_sca, el1_rot);
double *va;
int nva;
va = TRI_Vertices (a, alength, &nva);
gjk_convex_ellip (va, nva, el1_center, el1_sca, el1_rot, p, q);
free (va);
}
break;
case GJK_SPHERE_ELLIP:
{
SETRAND (move, 1.0);
SETRAND (center, 1.0);
radius = 0.75 * DRAND ();
SETRAND (el1_center, 1.0);
SUB (el1_center, move, el1_center);
el1_sca [0] = 0.75 * DRAND ();
el1_sca [1] = 0.75 * DRAND ();
el1_sca [2] = 0.75 * DRAND ();
EXPMAP (el1_sca, el1_rot);
gjk_sphere_ellip (center, radius, el1_center, el1_sca, el1_rot, p, q);
}
break;
case GJK_ELLIP_ELLIP:
{
SETRAND (move, 1.0);
SETRAND (el1_center, 1.0);
el1_sca [0] = 0.75 * DRAND ();
el1_sca [1] = 0.75 * DRAND ();
el1_sca [2] = 0.75 * DRAND ();
EXPMAP (el1_sca, el1_rot);
SETRAND (el2_center, 1.0);
SUB (el2_center, move, el2_center);
el2_sca [0] = 0.75 * DRAND ();
el2_sca [1] = 0.75 * DRAND ();
el2_sca [2] = 0.75 * DRAND ();
EXPMAP (el2_sca, el2_rot);
gjk_ellip_ellip (el1_center, el1_sca, el1_rot, el2_center, el2_sca, el2_rot, p, q);
}
break;
case GJK_ELLIP_POINT:
{
SETRAND (move, 1.0);
SETRAND (el1_center, 1.0);
el1_sca [0] = 0.75 * DRAND ();
el1_sca [1] = 0.75 * DRAND ();
el1_sca [2] = 0.75 * DRAND ();
EXPMAP (el1_sca, el1_rot);
SETRAND (center, 1.0);
SUB (center, move, center);
radius = 0.0;
COPY (center, p);
gjk_ellip_point (el1_center, el1_sca, el1_rot, p, q);
}
break;
}
SUB (p, q, d);
printf ("|p-q|=%g\n", LEN (d));
}
