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