_ccd_inline ccd_real_t __ccdVec3PointSegmentDist2(const ccd_vec3_t *P, const ccd_vec3_t *x0, const ccd_vec3_t *b, ccd_vec3_t *witness) { // The computation comes from solving equation of segment: // S(t) = x0 + t.d // where - x0 is initial point of segment // - d is direction of segment from x0 (|d| > 0) // - t belongs to <0, 1> interval // // Than, distance from a segment to some point P can be expressed: // D(t) = |x0 + t.d - P|^2 // which is distance from any point on segment. Minimization // of this function brings distance from P to segment. // Minimization of D(t) leads to simple quadratic equation that's // solving is straightforward. // // Bonus of this method is witness point for free. ccd_real_t dist, t; ccd_vec3_t d, a; // direction of segment ccdVec3Sub2(&d, b, x0); // precompute vector from P to x0 ccdVec3Sub2(&a, x0, P); t = -CCD_REAL(1.) * ccdVec3Dot(&a, &d); t /= ccdVec3Len2(&d); if (t < CCD_ZERO || ccdIsZero(t)){ dist = ccdVec3Dist2(x0, P); if (witness) ccdVec3Copy(witness, x0); }else if (t > CCD_ONE || ccdEq(t, CCD_ONE)){ dist = ccdVec3Dist2(b, P); if (witness) ccdVec3Copy(witness, b); }else{ if (witness){ ccdVec3Copy(witness, &d); ccdVec3Scale(witness, t); ccdVec3Add(witness, x0); dist = ccdVec3Dist2(witness, P); }else{ // recycling variables ccdVec3Scale(&d, t); ccdVec3Add(&d, &a); dist = ccdVec3Len2(&d); } } return dist; }
static void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) { const ccd_sphere_t *s = (const ccd_sphere_t *)obj; ccd_vec3_t dir; ccdVec3Copy(&dir, _dir); ccdQuatRotVec(&dir, &s->o.rot_inv); ccdVec3Copy(v, &dir); ccdVec3Scale(v, s->radius); ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir))); // transform support vertex ccdQuatRotVec(v, &s->o.rot); ccdVec3Add(v, &s->o.pos); }
static void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) { const ccd_cap_t *o = (const ccd_cap_t *)obj; ccd_vec3_t dir, pos1, pos2; ccdVec3Copy(&dir, _dir); ccdQuatRotVec(&dir, &o->o.rot_inv); ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height); ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height); ccdVec3Copy(v, &dir); ccdVec3Scale(v, o->radius); ccdVec3Add(&pos1, v); ccdVec3Add(&pos2, v); if (ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2)){ ccdVec3Copy(v, &pos1); }else{ ccdVec3Copy(v, &pos2); } // transform support vertex ccdQuatRotVec(v, &o->o.rot); ccdVec3Add(v, &o->o.pos); }
static void findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *portal, ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) { (void)(obj1); (void)(obj2); (void)(ccd); /* ccd_vec3_t vec; ccd_real_t k; */ /* Origin lies on v0-v1 segment.*/ /* Depth is distance to v1, direction also and position must be*/ /* computed*/ ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1); ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2); ccdVec3Scale(pos, CCD_REAL(0.5)); /* ccdVec3Sub2(&vec, &ccdSimplexPoint(portal, 1)->v, &ccdSimplexPoint(portal, 0)->v); k = CCD_SQRT(ccdVec3Len2(&ccdSimplexPoint(portal, 0)->v)); k /= CCD_SQRT(ccdVec3Len2(&vec)); ccdVec3Scale(&vec, -k); ccdVec3Add(pos, &vec); */ ccdVec3Copy(dir, &ccdSimplexPoint(portal, 1)->v); *depth = CCD_SQRT(ccdVec3Len2(dir)); ccdVec3Normalize(dir); }
static void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_sphere_t* s = static_cast<const ccd_sphere_t*>(obj); ccd_vec3_t dir; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &s->rot_inv); ccdVec3Copy(v, &dir); ccdVec3Scale(v, s->radius); ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir))); // transform support vertex ccdQuatRotVec(v, &s->rot); ccdVec3Add(v, &s->pos); }
static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_cap_t* o = static_cast<const ccd_cap_t*>(obj); ccd_vec3_t dir, pos1, pos2; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &o->rot_inv); ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height); ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height); ccdVec3Copy(v, &dir); ccdVec3Scale(v, o->radius); ccdVec3Add(&pos1, v); ccdVec3Add(&pos2, v); if(ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2)) ccdVec3Copy(v, &pos1); else ccdVec3Copy(v, &pos2); // transform support vertex ccdQuatRotVec(v, &o->rot); ccdVec3Add(v, &o->pos); }
static int ccdCollide(dGeomID o1, dGeomID o2, int flags, dContactGeom *contact, int /*skip*/, void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, void *obj2, ccd_support_fn supp2, ccd_center_fn cen2) { ccd_t ccd; int res; ccd_real_t depth; ccd_vec3_t dir, pos; int max_contacts = (flags & 0xffff); if (max_contacts < 1) return 0; CCD_INIT(&ccd); ccd.support1 = supp1; ccd.support2 = supp2; ccd.center1 = cen1; ccd.center2 = cen2; ccd.max_iterations = 500; ccd.mpr_tolerance = 1E-6; if (flags & CONTACTS_UNIMPORTANT){ if (ccdMPRIntersect(obj1, obj2, &ccd)){ return 1; }else{ return 0; } } res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); if (res == 0){ contact->g1 = o1; contact->g2 = o2; contact->side1 = contact->side2 = -1; contact->depth = depth; contact->pos[0] = ccdVec3X(&pos); contact->pos[1] = ccdVec3Y(&pos); contact->pos[2] = ccdVec3Z(&pos); ccdVec3Scale(&dir, -1.); contact->normal[0] = ccdVec3X(&dir); contact->normal[1] = ccdVec3Y(&dir); contact->normal[2] = ccdVec3Z(&dir); return 1; } return 0; }
static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *portal, ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) { // Touching contact on portal's v1 - so depth is zero and direction // is unimportant and pos can be guessed *depth = CCD_REAL(0.); ccdVec3Copy(dir, ccd_vec3_origin); ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1); ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2); ccdVec3Scale(pos, 0.5); }
static void findPos(const void *obj1, const void *obj2, const ccd_t *ccd, const ccd_simplex_t *portal, ccd_vec3_t *pos) { ccd_vec3_t dir; size_t i; ccd_real_t b[4], sum, inv; ccd_vec3_t vec, p1, p2; (void)(obj1); (void)(obj2); (void)(ccd); (void)(portal); portalDir(portal, &dir); /* use barycentric coordinates of tetrahedron to find origin*/ ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v, &ccdSimplexPoint(portal, 2)->v); b[0] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v); ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v, &ccdSimplexPoint(portal, 2)->v); b[1] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v); ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 0)->v, &ccdSimplexPoint(portal, 1)->v); b[2] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v); ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v, &ccdSimplexPoint(portal, 1)->v); b[3] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v); sum = b[0] + b[1] + b[2] + b[3]; if (ccdIsZero(sum) || sum < CCD_ZERO) { b[0] = CCD_REAL(0.); ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v, &ccdSimplexPoint(portal, 3)->v); b[1] = ccdVec3Dot(&vec, &dir); ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v, &ccdSimplexPoint(portal, 1)->v); b[2] = ccdVec3Dot(&vec, &dir); ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v, &ccdSimplexPoint(portal, 2)->v); b[3] = ccdVec3Dot(&vec, &dir); sum = b[1] + b[2] + b[3]; } inv = CCD_REAL(1.) / sum; ccdVec3Copy(&p1, ccd_vec3_origin); ccdVec3Copy(&p2, ccd_vec3_origin); for (i = 0; i < 4; i++) { ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v1); ccdVec3Scale(&vec, b[i]); ccdVec3Add(&p1, &vec); ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v2); ccdVec3Scale(&vec, b[i]); ccdVec3Add(&p2, &vec); } ccdVec3Scale(&p1, inv); ccdVec3Scale(&p2, inv); ccdVec3Copy(pos, &p1); ccdVec3Add(pos, &p2); ccdVec3Scale(pos, 0.5); }
static int discoverPortal(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *portal) { ccd_vec3_t dir, va, vb; ccd_real_t dot; int cont; /* vertex 0 is center of portal*/ findOrigin(obj1, obj2, ccd, ccdSimplexPointW(portal, 0)); ccdSimplexSetSize(portal, 1); if (ccdVec3Eq(&ccdSimplexPoint(portal, 0)->v, ccd_vec3_origin)) { /* Portal's center lies on origin (0,0,0) => we know that objects*/ /* intersect but we would need to know penetration info.*/ /* So move center little bit...*/ ccdVec3Set(&va, CCD_EPS * CCD_REAL(10.), CCD_ZERO, CCD_ZERO); ccdVec3Add(&ccdSimplexPointW(portal, 0)->v, &va); } /* vertex 1 = support in direction of origin*/ ccdVec3Copy(&dir, &ccdSimplexPoint(portal, 0)->v); ccdVec3Scale(&dir, CCD_REAL(-1.)); ccdVec3Normalize(&dir); __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 1)); ccdSimplexSetSize(portal, 2); /* test if origin isn't outside of v1*/ dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &dir); if (ccdIsZero(dot) || dot < CCD_ZERO) return -1; /* vertex 2*/ ccdVec3Cross(&dir, &ccdSimplexPoint(portal, 0)->v, &ccdSimplexPoint(portal, 1)->v); if (ccdIsZero(ccdVec3Len2(&dir))) { if (ccdVec3Eq(&ccdSimplexPoint(portal, 1)->v, ccd_vec3_origin)) { /* origin lies on v1*/ return 1; } else { /* origin lies on v0-v1 segment*/ return 2; } } ccdVec3Normalize(&dir); __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 2)); dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &dir); if (ccdIsZero(dot) || dot < CCD_ZERO) return -1; ccdSimplexSetSize(portal, 3); /* vertex 3 direction*/ ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v, &ccdSimplexPoint(portal, 0)->v); ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v, &ccdSimplexPoint(portal, 0)->v); ccdVec3Cross(&dir, &va, &vb); ccdVec3Normalize(&dir); /* it is better to form portal faces to be oriented "outside" origin*/ dot = ccdVec3Dot(&dir, &ccdSimplexPoint(portal, 0)->v); if (dot > CCD_ZERO) { ccdSimplexSwap(portal, 1, 2); ccdVec3Scale(&dir, CCD_REAL(-1.)); } while (ccdSimplexSize(portal) < 4) { __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 3)); dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &dir); if (ccdIsZero(dot) || dot < CCD_ZERO) return -1; cont = 0; /* test if origin is outside (v1, v0, v3) - set v2 as v3 and*/ /* continue*/ ccdVec3Cross(&va, &ccdSimplexPoint(portal, 1)->v, &ccdSimplexPoint(portal, 3)->v); dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v); if (dot < CCD_ZERO && !ccdIsZero(dot)) { ccdSimplexSet(portal, 2, ccdSimplexPoint(portal, 3)); cont = 1; } if (!cont) { /* test if origin is outside (v3, v0, v2) - set v1 as v3 and*/ /* continue*/ ccdVec3Cross(&va, &ccdSimplexPoint(portal, 3)->v, &ccdSimplexPoint(portal, 2)->v); dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v); if (dot < CCD_ZERO && !ccdIsZero(dot)) { ccdSimplexSet(portal, 1, ccdSimplexPoint(portal, 3)); cont = 1; } } if (cont) { ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v, &ccdSimplexPoint(portal, 0)->v); ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v, &ccdSimplexPoint(portal, 0)->v); ccdVec3Cross(&dir, &va, &vb); ccdVec3Normalize(&dir); } else { ccdSimplexSetSize(portal, 4); } } return 0; }
ccd_real_t ccdVec3PointTriDist2(const ccd_vec3_t *P, const ccd_vec3_t *x0, const ccd_vec3_t *B, const ccd_vec3_t *C, ccd_vec3_t *witness) { // Computation comes from analytic expression for triangle (x0, B, C) // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and // Then equation for distance is: // D(s, t) = | T(s, t) - P |^2 // This leads to minimization of quadratic function of two variables. // The solution from is taken only if s is between 0 and 1, t is // between 0 and 1 and t + s < 1, otherwise distance from segment is // computed. ccd_vec3_t d1, d2, a; ccd_real_t u, v, w, p, q, r, d; ccd_real_t s, t, dist, dist2; ccd_vec3_t witness2; ccdVec3Sub2(&d1, B, x0); ccdVec3Sub2(&d2, C, x0); ccdVec3Sub2(&a, x0, P); u = ccdVec3Dot(&a, &a); v = ccdVec3Dot(&d1, &d1); w = ccdVec3Dot(&d2, &d2); p = ccdVec3Dot(&a, &d1); q = ccdVec3Dot(&a, &d2); r = ccdVec3Dot(&d1, &d2); d = w * v - r * r; if (ccdIsZero(d)){ // To avoid division by zero for zero (or near zero) area triangles s = t = -1.; }else{ s = (q * r - w * p) / d; t = (-s * r - q) / w; } if ((ccdIsZero(s) || s > CCD_ZERO) && (ccdEq(s, CCD_ONE) || s < CCD_ONE) && (ccdIsZero(t) || t > CCD_ZERO) && (ccdEq(t, CCD_ONE) || t < CCD_ONE) && (ccdEq(t + s, CCD_ONE) || t + s < CCD_ONE)){ if (witness){ ccdVec3Scale(&d1, s); ccdVec3Scale(&d2, t); ccdVec3Copy(witness, x0); ccdVec3Add(witness, &d1); ccdVec3Add(witness, &d2); dist = ccdVec3Dist2(witness, P); }else{ dist = s * s * v; dist += t * t * w; dist += CCD_REAL(2.) * s * t * r; dist += CCD_REAL(2.) * s * p; dist += CCD_REAL(2.) * t * q; dist += u; } }else{ dist = __ccdVec3PointSegmentDist2(P, x0, B, witness); dist2 = __ccdVec3PointSegmentDist2(P, x0, C, &witness2); if (dist2 < dist){ dist = dist2; if (witness) ccdVec3Copy(witness, &witness2); } dist2 = __ccdVec3PointSegmentDist2(P, B, C, &witness2); if (dist2 < dist){ dist = dist2; if (witness) ccdVec3Copy(witness, &witness2); } } return dist; }