Esempio n. 1
0
    static Vector3 closestPtPointTetrahedron(ref int size)
    {
        float   eps = (1e-4f);
        Vector3 a   = Q[0];
        Vector3 b   = Q[1];
        Vector3 c   = Q[2];
        Vector3 d   = Q[3];

        //degenerated
        Vector3 ab       = (b - a);
        Vector3 ac       = (c - a);
        Vector3 n        = Vector3.Normalize(Vector3.Cross(ab, ac));
        float   signDist = Vector3.Dot(n, (d - a));

        if (eps > Mathf.Abs(signDist))
        {
            size = 3;
            return(closestPtPointTriangle(ref size));
        }

        BoolV4 bIsOutside4 = PointOutsideOfPlane4(a, b, c, d);

        if (bIsOutside4.BAllEq(false))
        {
            //All inside
            return(Vector3.zero);
        }

        Index   indices = new Index(); indices.Reset();
        Vector3 closest = getClosestPtPointTriangle(bIsOutside4, ref indices, ref size);

        Vector3 q0 = Q[indices.x]; Vector3 q1 = Q[indices.y]; Vector3 q2 = Q[indices.z];
        int     b0 = B[indices.x]; int b1 = B[indices.y]; int b2 = B[indices.z];

        Q[0] = q0; Q[1] = q1; Q[2] = q2;
        B[0] = b0; B[1] = b1; B[2] = b2;

        return(closest);
    }
Esempio n. 2
0
    static Vector3 getClosestPtPointTriangle(BoolV4 bIsOutside4, ref Index indices, ref int size)
    {
        float bestSqDist = float.MaxValue;

        Index _indices = new Index(); _indices.Reset();

        Vector3 result = Vector3.zero;

        if (bIsOutside4.x)
        {
            //use the original indices, size, v and w
            result     = closestPtPointTriangleBaryCentric(Q[0], Q[1], Q[2], ref indices, ref size);
            bestSqDist = Vector3.Dot(result, result);
        }

        if (bIsOutside4.y)
        {
            int _size = 3;
            _indices.x = 0; _indices.y = 2; _indices.z = 3;
            Vector3 q = closestPtPointTriangleBaryCentric(Q[0], Q[2], Q[3], ref _indices, ref _size);

            float sqDist = Vector3.Dot(q, q);
            bool  con    = bestSqDist > sqDist;
            if (con)
            {
                result     = q;
                bestSqDist = sqDist;

                indices.x = _indices.x;
                indices.y = _indices.y;
                indices.z = _indices.z;

                size = _size;
            }
        }

        if (bIsOutside4.z)
        {
            int _size = 3;

            _indices.x = 0; _indices.y = 3; _indices.z = 1;

            Vector3 q      = closestPtPointTriangleBaryCentric(Q[0], Q[3], Q[1], ref _indices, ref _size);
            float   sqDist = Vector3.Dot(q, q);
            bool    con    = bestSqDist > sqDist;
            if (con)
            {
                result     = q;
                bestSqDist = sqDist;

                indices.x = _indices.x;
                indices.y = _indices.y;
                indices.z = _indices.z;

                size = _size;
            }
        }

        if (bIsOutside4.w)
        {
            int _size = 3;
            _indices.x = 1; _indices.y = 3; _indices.z = 2;
            Vector3 q = closestPtPointTriangleBaryCentric(Q[1], Q[3], Q[2], ref _indices, ref _size);

            float sqDist = Vector3.Dot(q, q);
            bool  con    = bestSqDist > sqDist;

            if (con)
            {
                result     = q;
                bestSqDist = sqDist;

                indices.x = _indices.x;
                indices.y = _indices.y;
                indices.z = _indices.z;

                size = _size;
            }
        }

        return(result);
    }