//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)); }