static int refinePortal(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *portal) { ccd_vec3_t dir; ccd_support_t v4; while (1) { /* compute direction outside the portal (from v0 throught v1,v2,v3*/ /* face)*/ portalDir(portal, &dir); /* test if origin is inside the portal*/ if (portalEncapsulesOrigin(portal, &dir)) return 0; /* get next support point*/ __ccdSupport(obj1, obj2, &dir, ccd, &v4); /* test if v4 can expand portal to contain origin and if portal*/ /* expanding doesn't reach given tolerance*/ if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir, ccd)) { return -1; } /* v1-v2-v3 triangle must be rearranged to face outside Minkowski*/ /* difference (direction from v0).*/ expandPortal(portal, &v4); } return -1; }
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); } }
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); }