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