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 pConf(ccd_box_t *box1, ccd_box_t *box2, const ccd_vec3_t *v)
{
fprintf(stdout, "# box1.pos: [%lf %lf %lf]\n",
ccdVec3X(&box1->pos), ccdVec3Y(&box1->pos), ccdVec3Z(&box1->pos));
fprintf(stdout, "# box1->quat: [%lf %lf %lf %lf]\n",
box1->quat.q[0], box1->quat.q[1], box1->quat.q[2], box1->quat.q[3]);
fprintf(stdout, "# box2->pos: [%lf %lf %lf]\n",
ccdVec3X(&box2->pos), ccdVec3Y(&box2->pos), ccdVec3Z(&box2->pos));
fprintf(stdout, "# box2->quat: [%lf %lf %lf %lf]\n",
box2->quat.q[0], box2->quat.q[1], box2->quat.q[2], box2->quat.q[3]);
fprintf(stdout, "# sep: [%lf %lf %lf]\n",
ccdVec3X(v), ccdVec3Y(v), ccdVec3Z(v));
fprintf(stdout, "#\n");
}
static void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
const ccd_cyl_t *cyl = (const ccd_cyl_t *)obj;
ccd_vec3_t dir;
double zdist, rad;
ccdVec3Copy(&dir, _dir);
ccdQuatRotVec(&dir, &cyl->o.rot_inv);
zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
zdist = sqrt(zdist);
if (ccdIsZero(zdist)){
ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
}else{
rad = cyl->radius / zdist;
ccdVec3Set(v, rad * ccdVec3X(&dir),
rad * ccdVec3Y(&dir),
ccdSign(ccdVec3Z(&dir)) * cyl->height);
}
// transform support vertex
ccdQuatRotVec(v, &cyl->o.rot);
ccdVec3Add(v, &cyl->o.pos);
}
static void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
{
const ccd_cone_t* cone = static_cast<const ccd_cone_t*>(obj);
ccd_vec3_t dir;
ccdVec3Copy(&dir, dir_);
ccdQuatRotVec(&dir, &cone->rot_inv);
double zdist, len, rad;
zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
len = zdist + dir.v[2] * dir.v[2];
zdist = sqrt(zdist);
len = sqrt(len);
double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height);
if(dir.v[2] > len * sin_a)
ccdVec3Set(v, 0., 0., cone->height);
else if(zdist > 0)
{
rad = cone->radius / zdist;
ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height);
}
else
ccdVec3Set(v, 0, 0, -cone->height);
// transform support vertex
ccdQuatRotVec(v, &cone->rot);
ccdVec3Add(v, &cone->pos);
}
/** Support functions */
static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
{
const ccd_box_t* o = static_cast<const ccd_box_t*>(obj);
ccd_vec3_t dir;
ccdVec3Copy(&dir, dir_);
ccdQuatRotVec(&dir, &o->rot_inv);
ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
ccdSign(ccdVec3Y(&dir)) * o->dim[1],
ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
ccdQuatRotVec(v, &o->rot);
ccdVec3Add(v, &o->pos);
}
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 ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
const ccd_box_t *o = (const ccd_box_t *)obj;
ccd_vec3_t dir;
ccdVec3Copy(&dir, _dir);
ccdQuatRotVec(&dir, &o->o.rot_inv);
ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
ccdSign(ccdVec3Y(&dir)) * o->dim[1],
ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
// transform support vertex
ccdQuatRotVec(v, &o->o.rot);
ccdVec3Add(v, &o->o.pos);
}
void CollisionObject::Support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec)
{
// assume that obj_t is user-defined structure that holds info about
// object (in this case box: x, y, z, pos, quat - dimensions of box,
// position and rotation)
CollisionObject *obj = (CollisionObject *)_obj;
ccd_vec3_t dir;
// apply rotation on direction vector
ccdVec3Copy(&dir, _dir);
// compute support point in specified direction
ccdVec3Set(vec, ccdSign(ccdVec3X(&dir)) * obj->_colShape.GetHalfSize().x,
ccdSign(ccdVec3Y(&dir)) * obj->_colShape.GetHalfSize().y,
ccdSign(ccdVec3Z(&dir)) * obj->_colShape.GetHalfSize().z);
// transform support point according to position and rotation of object
ccdVec3Add(vec, &obj->_pos);
}