Exemplo n.º 1
0
static int refinePortal(const void *obj1, const void *obj2,
                        const ccd_t *ccd, ccd_simplex_t *portal)
{
    ccd_vec3_t dir;
    ccd_support_t v4;

    while (1) {
        /* compute direction outside the portal (from v0 throught v1,v2,v3*/
        /* face)*/
        portalDir(portal, &dir);

        /* test if origin is inside the portal*/
        if (portalEncapsulesOrigin(portal, &dir))
            return 0;

        /* get next support point*/
        __ccdSupport(obj1, obj2, &dir, ccd, &v4);

        /* test if v4 can expand portal to contain origin and if portal*/
        /* expanding doesn't reach given tolerance*/
        if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
                || portalReachTolerance(portal, &v4, &dir, ccd)) {
            return -1;
        }

        /* v1-v2-v3 triangle must be rearranged to face outside Minkowski*/
        /* difference (direction from v0).*/
        expandPortal(portal, &v4);
    }

    return -1;
}
Exemplo n.º 2
0
static void findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd,
                       ccd_simplex_t *portal,
                       ccd_real_t *depth, ccd_vec3_t *pdir, ccd_vec3_t *pos)
{
    ccd_vec3_t dir;
    ccd_support_t v4;

    while (1){
        // compute portal direction and obtain next support point
        portalDir(portal, &dir);
        __ccdSupport(obj1, obj2, &dir, ccd, &v4);

        // reached tolerance -> find penetration info
        if (portalReachTolerance(portal, &v4, &dir, ccd)){
            *depth = ccdVec3PointTriDist2(ccd_vec3_origin,
                                          &ccdSimplexPoint(portal, 1)->v,
                                          &ccdSimplexPoint(portal, 2)->v,
                                          &ccdSimplexPoint(portal, 3)->v,
                                          pdir);
            *depth = CCD_SQRT(*depth);
            ccdVec3Normalize(pdir);

            // barycentric coordinates:
            findPos(obj1, obj2, ccd, portal, pos);

            return;
        }

        expandPortal(portal, &v4);
    }
}
Exemplo n.º 3
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;
}