static void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
const ccd_cap_t *o = (const ccd_cap_t *)obj;
ccd_vec3_t dir, pos1, pos2;
ccdVec3Copy(&dir, _dir);
ccdQuatRotVec(&dir, &o->o.rot_inv);
ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
ccdVec3Copy(v, &dir);
ccdVec3Scale(v, o->radius);
ccdVec3Add(&pos1, v);
ccdVec3Add(&pos2, v);
if (ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2)){
ccdVec3Copy(v, &pos1);
}else{
ccdVec3Copy(v, &pos2);
}
// transform support vertex
ccdQuatRotVec(v, &o->o.rot);
ccdVec3Add(v, &o->o.pos);
}
static void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v)
{
const ccd_convex_t *c = (const ccd_convex_t *)obj;
ccd_vec3_t dir, p;
ccd_real_t maxdot, dot;
size_t i;
dReal *curp;
ccdVec3Copy(&dir, _dir);
ccdQuatRotVec(&dir, &c->o.rot_inv);
maxdot = -CCD_REAL_MAX;
curp = c->convex->points;
for (i = 0; i < c->convex->pointcount; i++, curp += 3){
ccdVec3Set(&p, curp[0], curp[1], curp[2]);
dot = ccdVec3Dot(&dir, &p);
if (dot > maxdot){
ccdVec3Copy(v, &p);
maxdot = dot;
}
}
// transform support vertex
ccdQuatRotVec(v, &c->o.rot);
ccdVec3Add(v, &c->o.pos);
}
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 supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
{
const ccd_triangle_t* tri = static_cast<const ccd_triangle_t*>(obj);
ccd_vec3_t dir, p;
ccd_real_t maxdot, dot;
int i;
ccdVec3Copy(&dir, dir_);
ccdQuatRotVec(&dir, &tri->rot_inv);
maxdot = -CCD_REAL_MAX;
for(i = 0; i < 3; ++i)
{
ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]);
dot = ccdVec3Dot(&dir, &p);
if(dot > maxdot)
{
ccdVec3Copy(v, &tri->p[i]);
maxdot = dot;
}
}
// transform support vertex
ccdQuatRotVec(v, &tri->rot);
ccdVec3Add(v, &tri->pos);
}
static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
{
const ccd_convex_t* c = (const ccd_convex_t*)obj;
ccd_vec3_t dir, p;
ccd_real_t maxdot, dot;
int i;
Vec3f* curp;
const Vec3f& center = c->convex->center;
ccdVec3Copy(&dir, dir_);
ccdQuatRotVec(&dir, &c->rot_inv);
maxdot = -CCD_REAL_MAX;
curp = c->convex->points;
for(i = 0; i < c->convex->num_points; ++i, curp += 1)
{
ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]);
dot = ccdVec3Dot(&dir, &p);
if(dot > maxdot)
{
ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]);
maxdot = dot;
}
}
// transform support vertex
ccdQuatRotVec(v, &c->rot);
ccdVec3Add(v, &c->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);
}
static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
{
const ccd_cap_t* o = static_cast<const ccd_cap_t*>(obj);
ccd_vec3_t dir, pos1, pos2;
ccdVec3Copy(&dir, dir_);
ccdQuatRotVec(&dir, &o->rot_inv);
ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
ccdVec3Copy(v, &dir);
ccdVec3Scale(v, o->radius);
ccdVec3Add(&pos1, v);
ccdVec3Add(&pos2, v);
if(ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2))
ccdVec3Copy(v, &pos1);
else
ccdVec3Copy(v, &pos2);
// transform support vertex
ccdQuatRotVec(v, &o->rot);
ccdVec3Add(v, &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 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 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);
}
static void centerTriangle(const void* obj, ccd_vec3_t* c)
{
const ccd_triangle_t *o = static_cast<const ccd_triangle_t*>(obj);
ccdVec3Copy(c, &o->c);
ccdQuatRotVec(c, &o->rot);
ccdVec3Add(c, &o->pos);
}
static void centerConvex(const void* obj, ccd_vec3_t* c)
{
const ccd_convex_t *o = static_cast<const ccd_convex_t*>(obj);
ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]);
ccdQuatRotVec(c, &o->rot);
ccdVec3Add(c, &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);
}