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 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 findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd,
ccd_simplex_t *portal,
ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
{
(void)(obj1);
(void)(obj2);
(void)(ccd);
/*
ccd_vec3_t vec;
ccd_real_t k;
*/
/* Origin lies on v0-v1 segment.*/
/* Depth is distance to v1, direction also and position must be*/
/* computed*/
ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1);
ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2);
ccdVec3Scale(pos, CCD_REAL(0.5));
/*
ccdVec3Sub2(&vec, &ccdSimplexPoint(portal, 1)->v,
&ccdSimplexPoint(portal, 0)->v);
k = CCD_SQRT(ccdVec3Len2(&ccdSimplexPoint(portal, 0)->v));
k /= CCD_SQRT(ccdVec3Len2(&vec));
ccdVec3Scale(&vec, -k);
ccdVec3Add(pos, &vec);
*/
ccdVec3Copy(dir, &ccdSimplexPoint(portal, 1)->v);
*depth = CCD_SQRT(ccdVec3Len2(dir));
ccdVec3Normalize(dir);
}
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 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);
}
_ccd_inline ccd_real_t __ccdVec3PointSegmentDist2(const ccd_vec3_t *P,
const ccd_vec3_t *x0,
const ccd_vec3_t *b,
ccd_vec3_t *witness)
{
// The computation comes from solving equation of segment:
// S(t) = x0 + t.d
// where - x0 is initial point of segment
// - d is direction of segment from x0 (|d| > 0)
// - t belongs to <0, 1> interval
//
// Than, distance from a segment to some point P can be expressed:
// D(t) = |x0 + t.d - P|^2
// which is distance from any point on segment. Minimization
// of this function brings distance from P to segment.
// Minimization of D(t) leads to simple quadratic equation that's
// solving is straightforward.
//
// Bonus of this method is witness point for free.
ccd_real_t dist, t;
ccd_vec3_t d, a;
// direction of segment
ccdVec3Sub2(&d, b, x0);
// precompute vector from P to x0
ccdVec3Sub2(&a, x0, P);
t = -CCD_REAL(1.) * ccdVec3Dot(&a, &d);
t /= ccdVec3Len2(&d);
if (t < CCD_ZERO || ccdIsZero(t)){
dist = ccdVec3Dist2(x0, P);
if (witness)
ccdVec3Copy(witness, x0);
}else if (t > CCD_ONE || ccdEq(t, CCD_ONE)){
dist = ccdVec3Dist2(b, P);
if (witness)
ccdVec3Copy(witness, b);
}else{
if (witness){
ccdVec3Copy(witness, &d);
ccdVec3Scale(witness, t);
ccdVec3Add(witness, x0);
dist = ccdVec3Dist2(witness, P);
}else{
// recycling variables
ccdVec3Scale(&d, t);
ccdVec3Add(&d, &a);
dist = ccdVec3Len2(&d);
}
}
return dist;
}
static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd,
ccd_simplex_t *portal,
ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
{
// Touching contact on portal's v1 - so depth is zero and direction
// is unimportant and pos can be guessed
*depth = CCD_REAL(0.);
ccdVec3Copy(dir, ccd_vec3_origin);
ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1);
ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2);
ccdVec3Scale(pos, 0.5);
}
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 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 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);
}
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 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);
}
void CollisionObject::Center(const void *_obj, ccd_vec3_t *center)
{
CollisionObject *obj = (CollisionObject *)_obj;
ccd_vec3_t cnt = GLMtoCCD(obj->GetCollissionShape().GetCenter());
ccdVec3Copy(center, &cnt);
}
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);
}
static void centerShape(const void* obj, ccd_vec3_t* c)
{
const ccd_obj_t *o = static_cast<const ccd_obj_t*>(obj);
ccdVec3Copy(c, &o->pos);
}
static void findPos(const void *obj1, const void *obj2, const ccd_t *ccd,
const ccd_simplex_t *portal, ccd_vec3_t *pos)
{
ccd_vec3_t dir;
size_t i;
ccd_real_t b[4], sum, inv;
ccd_vec3_t vec, p1, p2;
(void)(obj1);
(void)(obj2);
(void)(ccd);
(void)(portal);
portalDir(portal, &dir);
/* use barycentric coordinates of tetrahedron to find origin*/
ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v,
&ccdSimplexPoint(portal, 2)->v);
b[0] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v);
ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v,
&ccdSimplexPoint(portal, 2)->v);
b[1] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v);
ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 0)->v,
&ccdSimplexPoint(portal, 1)->v);
b[2] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v);
ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v,
&ccdSimplexPoint(portal, 1)->v);
b[3] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v);
sum = b[0] + b[1] + b[2] + b[3];
if (ccdIsZero(sum) || sum < CCD_ZERO) {
b[0] = CCD_REAL(0.);
ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v,
&ccdSimplexPoint(portal, 3)->v);
b[1] = ccdVec3Dot(&vec, &dir);
ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v,
&ccdSimplexPoint(portal, 1)->v);
b[2] = ccdVec3Dot(&vec, &dir);
ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v,
&ccdSimplexPoint(portal, 2)->v);
b[3] = ccdVec3Dot(&vec, &dir);
sum = b[1] + b[2] + b[3];
}
inv = CCD_REAL(1.) / sum;
ccdVec3Copy(&p1, ccd_vec3_origin);
ccdVec3Copy(&p2, ccd_vec3_origin);
for (i = 0; i < 4; i++) {
ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v1);
ccdVec3Scale(&vec, b[i]);
ccdVec3Add(&p1, &vec);
ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v2);
ccdVec3Scale(&vec, b[i]);
ccdVec3Add(&p2, &vec);
}
ccdVec3Scale(&p1, inv);
ccdVec3Scale(&p2, inv);
ccdVec3Copy(pos, &p1);
ccdVec3Add(pos, &p2);
ccdVec3Scale(pos, 0.5);
}
ccd_real_t ccdVec3PointTriDist2(const ccd_vec3_t *P,
const ccd_vec3_t *x0, const ccd_vec3_t *B,
const ccd_vec3_t *C,
ccd_vec3_t *witness)
{
// Computation comes from analytic expression for triangle (x0, B, C)
// T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
// Then equation for distance is:
// D(s, t) = | T(s, t) - P |^2
// This leads to minimization of quadratic function of two variables.
// The solution from is taken only if s is between 0 and 1, t is
// between 0 and 1 and t + s < 1, otherwise distance from segment is
// computed.
ccd_vec3_t d1, d2, a;
ccd_real_t u, v, w, p, q, r, d;
ccd_real_t s, t, dist, dist2;
ccd_vec3_t witness2;
ccdVec3Sub2(&d1, B, x0);
ccdVec3Sub2(&d2, C, x0);
ccdVec3Sub2(&a, x0, P);
u = ccdVec3Dot(&a, &a);
v = ccdVec3Dot(&d1, &d1);
w = ccdVec3Dot(&d2, &d2);
p = ccdVec3Dot(&a, &d1);
q = ccdVec3Dot(&a, &d2);
r = ccdVec3Dot(&d1, &d2);
d = w * v - r * r;
if (ccdIsZero(d)){
// To avoid division by zero for zero (or near zero) area triangles
s = t = -1.;
}else{
s = (q * r - w * p) / d;
t = (-s * r - q) / w;
}
if ((ccdIsZero(s) || s > CCD_ZERO)
&& (ccdEq(s, CCD_ONE) || s < CCD_ONE)
&& (ccdIsZero(t) || t > CCD_ZERO)
&& (ccdEq(t, CCD_ONE) || t < CCD_ONE)
&& (ccdEq(t + s, CCD_ONE) || t + s < CCD_ONE)){
if (witness){
ccdVec3Scale(&d1, s);
ccdVec3Scale(&d2, t);
ccdVec3Copy(witness, x0);
ccdVec3Add(witness, &d1);
ccdVec3Add(witness, &d2);
dist = ccdVec3Dist2(witness, P);
}else{
dist = s * s * v;
dist += t * t * w;
dist += CCD_REAL(2.) * s * t * r;
dist += CCD_REAL(2.) * s * p;
dist += CCD_REAL(2.) * t * q;
dist += u;
}
}else{
dist = __ccdVec3PointSegmentDist2(P, x0, B, witness);
dist2 = __ccdVec3PointSegmentDist2(P, x0, C, &witness2);
if (dist2 < dist){
dist = dist2;
if (witness)
ccdVec3Copy(witness, &witness2);
}
dist2 = __ccdVec3PointSegmentDist2(P, B, C, &witness2);
if (dist2 < dist){
dist = dist2;
if (witness)
ccdVec3Copy(witness, &witness2);
}
}
return dist;
}
static void ccdCenter(const void *obj, ccd_vec3_t *c)
{
const ccd_obj_t *o = (const ccd_obj_t *)obj;
ccdVec3Copy(c, &o->pos);
}
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;
}
