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