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 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);
}
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);
}
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);
}
void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3)
{
ccd_triangle_t* o = new ccd_triangle_t;
Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
ccdVec3Set(&o->c, center[0], center[1], center[2]);
ccdVec3Set(&o->pos, 0., 0., 0.);
ccdQuatSet(&o->rot, 0., 0., 0., 1.);
ccdQuatInvert2(&o->rot_inv, &o->rot);
return o;
}
void cylcyl(void)
{
fprintf(stdout, "%s:\n", __func__);
ccd_t ccd;
CCD_CYL(cyl1);
CCD_CYL(cyl2);
ccd_vec3_t axis;
cyl1.radius = 0.35;
cyl1.height = 0.5;
cyl2.radius = 0.5;
cyl2.height = 1.;
CCD_INIT(&ccd);
ccd.support1 = ccdSupport;
ccd.support2 = ccdSupport;
ccd.center1 = ccdObjCenter;
ccd.center2 = ccdObjCenter;
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&cyl1.pos, 0.3, 0.1, 0.1);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&axis, 0., 1., 1.);
ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis);
ccdVec3Set(&cyl2.pos, 0., 0., 0.);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&axis, 0., 1., 1.);
ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis);
ccdVec3Set(&cyl2.pos, -0.2, 0.7, 0.2);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&axis, 0.567, 1.2, 1.);
ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 4., &axis);
ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
ccdVec3Set(&axis, -4.567, 1.2, 0.);
ccdQuatSetAngleAxis(&cyl2.quat, M_PI / 3., &axis);
ccdVec3Set(&cyl2.pos, 0.6, -0.7, 0.2);
runBench(&cyl1, &cyl2, &ccd);
runBench(&cyl2, &cyl1, &ccd);
fprintf(stdout, "\n----\n\n");
}
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 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 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);
}
void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf)
{
ccd_triangle_t* o = new ccd_triangle_t;
Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
ccdVec3Set(&o->c, center[0], center[1], center[2]);
const Quaternion3f& q = tf.getQuatRotation();
const Vec3f& T = tf.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
ccdQuatInvert2(&o->rot_inv, &o->rot);
return o;
}
/** Basic shape to ccd shape */
static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
{
const Quaternion3f& q = tf.getQuatRotation();
const Vec3f& T = tf.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
ccdQuatInvert2(&o->rot_inv, &o->rot);
}
/** 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 ccdGeomToObj(const dGeomID g, ccd_obj_t *o)
{
const dReal *ode_pos;
dQuaternion ode_rot;
ode_pos = dGeomGetPosition(g);
dGeomGetQuaternion(g, ode_rot);
ccdVec3Set(&o->pos, ode_pos[0], ode_pos[1], ode_pos[2]);
ccdQuatSet(&o->rot, ode_rot[1], ode_rot[2], ode_rot[3], ode_rot[0]);
ccdQuatInvert2(&o->rot_inv, &o->rot);
}
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);
}
void boxcyl(void)
{
fprintf(stdout, "%s:\n", __func__);
ccd_t ccd;
CCD_BOX(box);
CCD_CYL(cyl);
ccd_vec3_t axis;
box.x = 0.5;
box.y = 1.;
box.z = 1.5;
cyl.radius = 0.4;
cyl.height = 0.7;
CCD_INIT(&ccd);
ccd.support1 = ccdSupport;
ccd.support2 = ccdSupport;
ccd.center1 = ccdObjCenter;
ccd.center2 = ccdObjCenter;
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&cyl.pos, .6, 0., 0.);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&cyl.pos, .6, 0.6, 0.);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&cyl.pos, .6, 0.6, 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&axis, 0., 1., 0.);
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 3., &axis);
ccdVec3Set(&cyl.pos, .6, 0.6, 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&axis, 0.67, 1.1, 0.12);
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 4., &axis);
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&axis, -0.1, 2.2, -1.);
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis);
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
ccdVec3Set(&axis, 1., 1., 0.);
ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis);
ccdVec3Set(&box.pos, .6, 0., 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
ccdVec3Set(&axis, -0.1, 2.2, -1.);
ccdQuatSetAngleAxis(&cyl.quat, M_PI / 5., &axis);
ccdVec3Set(&cyl.pos, .6, 0., 0.5);
ccdVec3Set(&axis, 1., 1., 0.);
ccdQuatSetAngleAxis(&box.quat, -M_PI / 4., &axis);
ccdVec3Set(&box.pos, .9, 0.8, 0.5);
runBench(&box, &cyl, &ccd);
runBench(&cyl, &box, &ccd);
fprintf(stdout, "\n----\n\n");
}
static void boxbox(void)
{
fprintf(stdout, "%s:\n", __func__);
ccd_t ccd;
CCD_BOX(box1);
CCD_BOX(box2);
ccd_vec3_t axis;
ccd_quat_t rot;
box1.x = box1.y = box1.z = 1.;
box2.x = 0.5;
box2.y = 1.;
box2.z = 1.5;
bench_num = 1;
CCD_INIT(&ccd);
ccd.support1 = ccdSupport;
ccd.support2 = ccdSupport;
ccd.center1 = ccdObjCenter;
ccd.center2 = ccdObjCenter;
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
ccdVec3Set(&box1.pos, -0.3, 0.5, 1.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
box2.x = box2.y = box2.z = 1.;
ccdVec3Set(&axis, 0., 0., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&box1.pos, 0., 0., 0.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
box2.x = box2.y = box2.z = 1.;
ccdVec3Set(&axis, 0., 0., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&box1.pos, -0.5, 0., 0.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
box2.x = box2.y = box2.z = 1.;
ccdVec3Set(&axis, 0., 0., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&box1.pos, -0.5, 0.5, 0.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
ccdVec3Set(&axis, 0., 1., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
ccdVec3Set(&axis, 0., 1., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&axis, 1., 1., 1.);
ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis);
ccdQuatMul(&box1.quat, &rot);
ccdVec3Set(&box1.pos, -0.5, 0.1, 0.4);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
box1.x = box1.y = box1.z = 1.;
box2.x = 0.2; box2.y = 0.5; box2.z = 1.;
box2.x = box2.y = box2.z = 1.;
ccdVec3Set(&axis, 0., 0., 1.);
ccdQuatSetAngleAxis(&box1.quat, M_PI / 4., &axis);
ccdVec3Set(&axis, 1., 0., 0.);
ccdQuatSetAngleAxis(&rot, M_PI / 4., &axis);
ccdQuatMul(&box1.quat, &rot);
ccdVec3Set(&box1.pos, -1.3, 0., 0.);
ccdVec3Set(&box2.pos, 0., 0., 0.);
runBench(&box1, &box2, &ccd);
runBench(&box2, &box1, &ccd);
fprintf(stdout, "\n----\n\n");
}
static int discoverPortal(const void *obj1, const void *obj2,
const ccd_t *ccd, ccd_simplex_t *portal)
{
ccd_vec3_t dir, va, vb;
ccd_real_t dot;
int cont;
/* vertex 0 is center of portal*/
findOrigin(obj1, obj2, ccd, ccdSimplexPointW(portal, 0));
ccdSimplexSetSize(portal, 1);
if (ccdVec3Eq(&ccdSimplexPoint(portal, 0)->v, ccd_vec3_origin)) {
/* Portal's center lies on origin (0,0,0) => we know that objects*/
/* intersect but we would need to know penetration info.*/
/* So move center little bit...*/
ccdVec3Set(&va, CCD_EPS * CCD_REAL(10.), CCD_ZERO, CCD_ZERO);
ccdVec3Add(&ccdSimplexPointW(portal, 0)->v, &va);
}
/* vertex 1 = support in direction of origin*/
ccdVec3Copy(&dir, &ccdSimplexPoint(portal, 0)->v);
ccdVec3Scale(&dir, CCD_REAL(-1.));
ccdVec3Normalize(&dir);
__ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 1));
ccdSimplexSetSize(portal, 2);
/* test if origin isn't outside of v1*/
dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &dir);
if (ccdIsZero(dot) || dot < CCD_ZERO)
return -1;
/* vertex 2*/
ccdVec3Cross(&dir, &ccdSimplexPoint(portal, 0)->v,
&ccdSimplexPoint(portal, 1)->v);
if (ccdIsZero(ccdVec3Len2(&dir))) {
if (ccdVec3Eq(&ccdSimplexPoint(portal, 1)->v, ccd_vec3_origin)) {
/* origin lies on v1*/
return 1;
} else {
/* origin lies on v0-v1 segment*/
return 2;
}
}
ccdVec3Normalize(&dir);
__ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 2));
dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &dir);
if (ccdIsZero(dot) || dot < CCD_ZERO)
return -1;
ccdSimplexSetSize(portal, 3);
/* vertex 3 direction*/
ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v,
&ccdSimplexPoint(portal, 0)->v);
ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v,
&ccdSimplexPoint(portal, 0)->v);
ccdVec3Cross(&dir, &va, &vb);
ccdVec3Normalize(&dir);
/* it is better to form portal faces to be oriented "outside" origin*/
dot = ccdVec3Dot(&dir, &ccdSimplexPoint(portal, 0)->v);
if (dot > CCD_ZERO) {
ccdSimplexSwap(portal, 1, 2);
ccdVec3Scale(&dir, CCD_REAL(-1.));
}
while (ccdSimplexSize(portal) < 4) {
__ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 3));
dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &dir);
if (ccdIsZero(dot) || dot < CCD_ZERO)
return -1;
cont = 0;
/* test if origin is outside (v1, v0, v3) - set v2 as v3 and*/
/* continue*/
ccdVec3Cross(&va, &ccdSimplexPoint(portal, 1)->v,
&ccdSimplexPoint(portal, 3)->v);
dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v);
if (dot < CCD_ZERO && !ccdIsZero(dot)) {
ccdSimplexSet(portal, 2, ccdSimplexPoint(portal, 3));
cont = 1;
}
if (!cont) {
/* test if origin is outside (v3, v0, v2) - set v1 as v3 and*/
/* continue*/
ccdVec3Cross(&va, &ccdSimplexPoint(portal, 3)->v,
&ccdSimplexPoint(portal, 2)->v);
dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v);
if (dot < CCD_ZERO && !ccdIsZero(dot)) {
ccdSimplexSet(portal, 1, ccdSimplexPoint(portal, 3));
cont = 1;
}
}
if (cont) {
ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v,
&ccdSimplexPoint(portal, 0)->v);
ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v,
&ccdSimplexPoint(portal, 0)->v);
ccdVec3Cross(&dir, &va, &vb);
ccdVec3Normalize(&dir);
} else {
ccdSimplexSetSize(portal, 4);
}
}
return 0;
}
