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 findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *portal, ccd_real_t *depth, ccd_vec3_t *pdir, ccd_vec3_t *pos) { ccd_vec3_t dir; ccd_support_t v4; while (1){ // compute portal direction and obtain next support point portalDir(portal, &dir); __ccdSupport(obj1, obj2, &dir, ccd, &v4); // reached tolerance -> find penetration info if (portalReachTolerance(portal, &v4, &dir, ccd)){ *depth = ccdVec3PointTriDist2(ccd_vec3_origin, &ccdSimplexPoint(portal, 1)->v, &ccdSimplexPoint(portal, 2)->v, &ccdSimplexPoint(portal, 3)->v, pdir); *depth = CCD_SQRT(*depth); ccdVec3Normalize(pdir); // barycentric coordinates: findPos(obj1, obj2, ccd, portal, pos); return; } expandPortal(portal, &v4); } }
_ccd_inline void portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir) { ccd_vec3_t v2v1, v3v1; ccdVec3Sub2(&v2v1, &ccdSimplexPoint(portal, 2)->v, &ccdSimplexPoint(portal, 1)->v); ccdVec3Sub2(&v3v1, &ccdSimplexPoint(portal, 3)->v, &ccdSimplexPoint(portal, 1)->v); ccdVec3Cross(dir, &v2v1, &v3v1); ccdVec3Normalize(dir); }
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; }