Esempio n. 1
0
        public bool ClosestPtPointTetrahedron(FPVector p, FPVector a, FPVector b, FPVector c, FPVector d,
                                              ref SubSimplexClosestResult finalResult)
        {
            tempResult.Reset();

            // Start out assuming point inside all halfspaces, so closest to itself
            finalResult.ClosestPointOnSimplex = p;
            finalResult.UsedVertices.Reset();
            finalResult.UsedVertices.UsedVertexA = true;
            finalResult.UsedVertices.UsedVertexB = true;
            finalResult.UsedVertices.UsedVertexC = true;
            finalResult.UsedVertices.UsedVertexD = true;

            int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d);
            int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b);
            int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c);
            int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a);

            if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
            {
                finalResult.Degenerate = true;
                return(false);
            }

            if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
            {
                return(false);
            }

            FP bestSqDist = FP.MaxValue;

            // If point outside face abc then compute closest point on abc
            if (pointOutsideABC != 0)
            {
                ClosestPtPointTriangle(p, a, b, c, ref tempResult);
                FPVector q = tempResult.ClosestPointOnSimplex;

                FP sqDist = ((FPVector)(q - p)).sqrMagnitude;
                // Update best closest point if (squared) distance is less than current best
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.ClosestPointOnSimplex = q;
                    //convert result bitmask!
                    finalResult.UsedVertices.Reset();
                    finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
                    finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB;
                    finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;
                    finalResult.SetBarycentricCoordinates(
                        tempResult.BarycentricCoords[VertexA],
                        tempResult.BarycentricCoords[VertexB],
                        tempResult.BarycentricCoords[VertexC],
                        0);
                }
            }

            // Repeat test for face acd
            if (pointOutsideACD != 0)
            {
                ClosestPtPointTriangle(p, a, c, d, ref tempResult);
                FPVector q = tempResult.ClosestPointOnSimplex;
                //convert result bitmask!

                FP sqDist = ((FPVector)(q - p)).sqrMagnitude;
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.ClosestPointOnSimplex = q;
                    finalResult.UsedVertices.Reset();
                    finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
                    finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB;
                    finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC;
                    finalResult.SetBarycentricCoordinates(
                        tempResult.BarycentricCoords[VertexA],
                        0,
                        tempResult.BarycentricCoords[VertexB],
                        tempResult.BarycentricCoords[VertexC]);
                }
            }
            // Repeat test for face adb

            if (pointOutsideADB != 0)
            {
                ClosestPtPointTriangle(p, a, d, b, ref tempResult);
                FPVector q = tempResult.ClosestPointOnSimplex;
                //convert result bitmask!

                FP sqDist = ((FPVector)(q - p)).sqrMagnitude;
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.ClosestPointOnSimplex = q;
                    finalResult.UsedVertices.Reset();
                    finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
                    finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;
                    finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC;
                    finalResult.SetBarycentricCoordinates(
                        tempResult.BarycentricCoords[VertexA],
                        tempResult.BarycentricCoords[VertexC],
                        0,
                        tempResult.BarycentricCoords[VertexB]);
                }
            }
            // Repeat test for face bdc

            if (pointOutsideBDC != 0)
            {
                ClosestPtPointTriangle(p, b, d, c, ref tempResult);
                FPVector q = tempResult.ClosestPointOnSimplex;
                //convert result bitmask!
                FP sqDist = ((FPVector)(q - p)).sqrMagnitude;
                if (sqDist < bestSqDist)
                {
                    bestSqDist = sqDist;
                    finalResult.ClosestPointOnSimplex = q;
                    finalResult.UsedVertices.Reset();
                    finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA;
                    finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;
                    finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;

                    finalResult.SetBarycentricCoordinates(
                        0,
                        tempResult.BarycentricCoords[VertexA],
                        tempResult.BarycentricCoords[VertexC],
                        tempResult.BarycentricCoords[VertexB]);
                }
            }

            //help! we ended up full !

            if (finalResult.UsedVertices.UsedVertexA &&
                finalResult.UsedVertices.UsedVertexB &&
                finalResult.UsedVertices.UsedVertexC &&
                finalResult.UsedVertices.UsedVertexD)
            {
                return(true);
            }

            return(true);
        }
Esempio n. 2
0
        public bool ClosestPtPointTriangle(FPVector p, FPVector a, FPVector b, FPVector c,
                                           ref SubSimplexClosestResult result)
        {
            result.UsedVertices.Reset();

            FP v, w;

            // Check if P in vertex region outside A
            FPVector ab = b - a;
            FPVector ac = c - a;
            FPVector ap = p - a;
            FP       d1 = FPVector.Dot(ab, ap);
            FP       d2 = FPVector.Dot(ac, ap);

            if (d1 <= FP.Zero && d2 <= FP.Zero)
            {
                result.ClosestPointOnSimplex    = a;
                result.UsedVertices.UsedVertexA = true;
                result.SetBarycentricCoordinates(1, 0, 0, 0);
                return(true); // a; // barycentric coordinates (1,0,0)
            }

            // Check if P in vertex region outside B
            FPVector bp = p - b;
            FP       d3 = FPVector.Dot(ab, bp);
            FP       d4 = FPVector.Dot(ac, bp);

            if (d3 >= FP.Zero && d4 <= d3)
            {
                result.ClosestPointOnSimplex    = b;
                result.UsedVertices.UsedVertexB = true;
                result.SetBarycentricCoordinates(0, 1, 0, 0);

                return(true); // b; // barycentric coordinates (0,1,0)
            }
            // Check if P in edge region of AB, if so return projection of P onto AB
            FP vc = d1 * d4 - d3 * d2;

            if (vc <= FP.Zero && d1 >= FP.Zero && d3 <= FP.Zero)
            {
                v = d1 / (d1 - d3);
                result.ClosestPointOnSimplex    = a + v * ab;
                result.UsedVertices.UsedVertexA = true;
                result.UsedVertices.UsedVertexB = true;
                result.SetBarycentricCoordinates(1 - v, v, 0, 0);
                return(true);
                //return a + v * ab; // barycentric coordinates (1-v,v,0)
            }

            // Check if P in vertex region outside C
            FPVector cp = p - c;
            FP       d5 = FPVector.Dot(ab, cp);
            FP       d6 = FPVector.Dot(ac, cp);

            if (d6 >= FP.Zero && d5 <= d6)
            {
                result.ClosestPointOnSimplex    = c;
                result.UsedVertices.UsedVertexC = true;
                result.SetBarycentricCoordinates(0, 0, 1, 0);
                return(true);//c; // barycentric coordinates (0,0,1)
            }

            // Check if P in edge region of AC, if so return projection of P onto AC
            FP vb = d5 * d2 - d1 * d6;

            if (vb <= FP.Zero && d2 >= FP.Zero && d6 <= FP.Zero)
            {
                w = d2 / (d2 - d6);
                result.ClosestPointOnSimplex    = a + w * ac;
                result.UsedVertices.UsedVertexA = true;
                result.UsedVertices.UsedVertexC = true;
                result.SetBarycentricCoordinates(1 - w, 0, w, 0);
                return(true);
                //return a + w * ac; // barycentric coordinates (1-w,0,w)
            }

            // Check if P in edge region of BC, if so return projection of P onto BC
            FP va = d3 * d6 - d5 * d4;

            if (va <= FP.Zero && (d4 - d3) >= FP.Zero && (d5 - d6) >= FP.Zero)
            {
                w = (d4 - d3) / ((d4 - d3) + (d5 - d6));

                result.ClosestPointOnSimplex    = b + w * (c - b);
                result.UsedVertices.UsedVertexB = true;
                result.UsedVertices.UsedVertexC = true;
                result.SetBarycentricCoordinates(0, 1 - w, w, 0);
                return(true);
                // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
            }

            // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
            FP denom = FP.One / (va + vb + vc);

            v = vb * denom;
            w = vc * denom;

            result.ClosestPointOnSimplex    = a + ab * v + ac * w;
            result.UsedVertices.UsedVertexA = true;
            result.UsedVertices.UsedVertexB = true;
            result.UsedVertices.UsedVertexC = true;
            result.SetBarycentricCoordinates(1 - v - w, v, w, 0);

            return(true);
        }