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