static int ccdCollide(dGeomID o1, dGeomID o2, int flags,
dContactGeom *contact, int /*skip*/,
void *obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void *obj2, ccd_support_fn supp2, ccd_center_fn cen2)
{
ccd_t ccd;
int res;
ccd_real_t depth;
ccd_vec3_t dir, pos;
int max_contacts = (flags & 0xffff);
if (max_contacts < 1)
return 0;
CCD_INIT(&ccd);
ccd.support1 = supp1;
ccd.support2 = supp2;
ccd.center1 = cen1;
ccd.center2 = cen2;
ccd.max_iterations = 500;
ccd.mpr_tolerance = 1E-6;
if (flags & CONTACTS_UNIMPORTANT){
if (ccdMPRIntersect(obj1, obj2, &ccd)){
return 1;
}else{
return 0;
}
}
res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
if (res == 0){
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = contact->side2 = -1;
contact->depth = depth;
contact->pos[0] = ccdVec3X(&pos);
contact->pos[1] = ccdVec3Y(&pos);
contact->pos[2] = ccdVec3Z(&pos);
ccdVec3Scale(&dir, -1.);
contact->normal[0] = ccdVec3X(&dir);
contact->normal[1] = ccdVec3Y(&dir);
contact->normal[2] = ccdVec3Z(&dir);
return 1;
}
return 0;
}
void cylcyl(void)
{
fprintf(stdout, "%s:\n", __func__);
ccd_t ccd;
CCD_CYL(cyl1);
CCD_CYL(cyl2);
ccd_vec3_t axis;
cyl1.radius = 0.35;
cyl1.height = 0.5;
cyl2.radius = 0.5;
cyl2.height = 1.;
CCD_INIT(&ccd);
ccd.support1 = ccdSupport;
ccd.support2 = ccdSupport;
ccd.center1 = ccdObjCenter;
ccd.center2 = ccdObjCenter;
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&cyl1.pos, 0.3, 0.1, 0.1);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&axis, 0., 1., 1.);
ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis);
ccdVec3Set(&cyl2.pos, 0., 0., 0.);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&axis, 0., 1., 1.);
ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis);
ccdVec3Set(&cyl2.pos, -0.2, 0.7, 0.2);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&axis, 0.567, 1.2, 1.);
ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis);
ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&axis, -4.567, 1.2, 0.);
ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 3., &axis);
ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
fprintf(stdout, "\n----\n\n");
}
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
unsigned int max_iterations, FCL_REAL tolerance,
FCL_REAL* res)
{
ccd_t ccd;
ccd_real_t dist;
CCD_INIT(&ccd);
ccd.support1 = supp1;
ccd.support2 = supp2;
ccd.max_iterations = max_iterations;
ccd.dist_tolerance = tolerance;
dist = ccdGJKDist(obj1, obj2, &ccd);
*res = dist;
if(dist < 0) return false;
else return true;
}
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
unsigned int max_iterations, FCL_REAL tolerance,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
ccd_t ccd;
int res;
ccd_real_t depth;
ccd_vec3_t dir, pos;
CCD_INIT(&ccd);
ccd.support1 = supp1;
ccd.support2 = supp2;
ccd.center1 = cen1;
ccd.center2 = cen2;
ccd.max_iterations = max_iterations;
ccd.mpr_tolerance = tolerance;
if(!contact_points)
{
return ccdMPRIntersect(obj1, obj2, &ccd);
}
res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
if(res == 0)
{
contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
*penetration_depth = depth;
normal->setValue(-ccdVec3X(&dir), -ccdVec3Y(&dir), -ccdVec3Z(&dir));
return true;
}
return false;
}
static void boxbox(void)
{
fprintf(stdout, "%s:\n", __func__);
ccd_t ccd;
CCD_BOX(box1);
CCD_BOX(box2);
ccd_vec3_t axis;
ccd_quat_t rot;
box1.x = box1.y = box1.z = 1.;
box2.x = 0.5;
box2.y = 1.;
box2.z = 1.5;
bench_num = 1;
CCD_INIT(&ccd);
ccd.support1 = ccdSupport;
ccd.support2 = ccdSupport;
ccd.center1 = ccdObjCenter;
ccd.center2 = ccdObjCenter;
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
ccdVec3Set(&box1.pos, -0.3, 0.5, 1.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
box2.x = box2.y = box2.z = 1.;
ccdVec3Set(&axis, 0., 0., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&box1.pos, 0., 0., 0.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
box2.x = box2.y = box2.z = 1.;
ccdVec3Set(&axis, 0., 0., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&box1.pos, -0.5, 0., 0.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
box2.x = box2.y = box2.z = 1.;
ccdVec3Set(&axis, 0., 0., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&box1.pos, -0.5, 0.5, 0.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
ccdVec3Set(&axis, 0., 1., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
ccdVec3Set(&axis, 0., 1., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&axis, 1., 1., 1.);
ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis);
ccdQuatMul(&box1.quat, &rot);
ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
box2.x = 0.2; box2.y = 0.5; box2.z = 1.;
box2.x = box2.y = box2.z = 1.;
ccdVec3Set(&axis, 0., 0., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&axis, 1., 0., 0.);
ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis);
ccdQuatMul(&box1.quat, &rot);
ccdVec3Set(&box1.pos, -1.3, 0., 0.);
ccdVec3Set(&box2.pos, 0., 0., 0.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
fprintf(stdout, "\n----\n\n");
}
void boxcyl(void)
{
fprintf(stdout, "%s:\n", __func__);
ccd_t ccd;
CCD_BOX(box);
CCD_CYL(cyl);
ccd_vec3_t axis;
box.x = 0.5;
box.y = 1.;
box.z = 1.5;
cyl.radius = 0.4;
cyl.height = 0.7;
CCD_INIT(&ccd);
ccd.support1 = ccdSupport;
ccd.support2 = ccdSupport;
ccd.center1 = ccdObjCenter;
ccd.center2 = ccdObjCenter;
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&cyl.pos, .6, 0., 0.);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&cyl.pos, .6, 0.6, 0.);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&cyl.pos, .6, 0.6, 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&axis, 0., 1., 0.);
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis);
ccdVec3Set(&cyl.pos, .6, 0.6, 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&axis, 0.67, 1.1, 0.12);
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis);
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&axis, -0.1, 2.2, -1.);
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis);
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
ccdVec3Set(&axis, 1., 1., 0.);
ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis);
ccdVec3Set(&box.pos, .6, 0., 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&axis, -0.1, 2.2, -1.);
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis);
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
ccdVec3Set(&axis, 1., 1., 0.);
ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis);
ccdVec3Set(&box.pos, .9, 0.8, 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
fprintf(stdout, "\n----\n\n");
}
