Пример #1
0
_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;
}
Пример #2
0
_ccd_inline void portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir)
{
    ccd_vec3_t v2v1, v3v1;

    ccdVec3Sub2(&v2v1, &ccdSimplexPoint(portal, 2)->v,
                &ccdSimplexPoint(portal, 1)->v);
    ccdVec3Sub2(&v3v1, &ccdSimplexPoint(portal, 3)->v,
                &ccdSimplexPoint(portal, 1)->v);
    ccdVec3Cross(dir, &v2v1, &v3v1);
    ccdVec3Normalize(dir);
}
Пример #3
0
_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd,
                            ccd_support_t *center)
{
    ccd->center1(obj1, &center->v1);
    ccd->center2(obj2, &center->v2);
    ccdVec3Sub2(&center->v, &center->v1, &center->v2);
}
Пример #4
0
_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal,
                                     const ccd_support_t *v4,
                                     const ccd_vec3_t *dir,
                                     const ccd_t *ccd)
{
    ccd_vec3_t vec;
    ccd_real_t dot;

    ccdVec3Sub2(&vec, &v4->v, &ccdSimplexPoint(portal, 3)->v);
    dot = ccdVec3Dot(&vec, dir);

    return ccdEq(dot, ccd->mpr_tolerance) || dot < ccd->mpr_tolerance;
}
Пример #5
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;
}
Пример #6
0
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;
}