_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 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 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 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 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; }