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; }
static void runBench(const void *o1, const void *o2, const ccd_t *ccd) { ccd_real_t depth; ccd_vec3_t dir, pos; size_t i; const struct timespec *timer; cuTimerStart(); for (i = 0; i < cycles; i++){ ccdMPRPenetration(o1, o2, ccd, &depth, &dir, &pos); } timer = cuTimerStop(); fprintf(stdout, "%02d: %ld %ld\n", bench_num, (long)timer->tv_sec, (long)timer->tv_nsec); fflush(stdout); bench_num++; }
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; }