Exemple #1
0
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);
}
Exemple #3
0
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);
}
Exemple #4
0
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);
}
Exemple #6
0
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;
}
Exemple #7
0
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);
}
Exemple #9
0
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);
}
Exemple #10
0
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);
}
Exemple #11
0
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;
}
Exemple #12
0
/** 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);
}
Exemple #13
0
/** 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);
}
Exemple #17
0
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");
}
Exemple #18
0
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");
}
Exemple #19
0
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;
}