Пример #1
0
        public bool ClosestPtPointTetrahedron(TSVector p, TSVector a, TSVector b, TSVector c, TSVector d, ref SubSimplexClosestResult finalResult)
        {
            this.tempResult.Reset();
            finalResult.ClosestPointOnSimplex = p;
            finalResult.UsedVertices.Reset();
            finalResult.UsedVertices.UsedVertexA = true;
            finalResult.UsedVertices.UsedVertexB = true;
            finalResult.UsedVertices.UsedVertexC = true;
            finalResult.UsedVertices.UsedVertexD = true;
            int  num  = this.PointOutsideOfPlane(p, a, b, c, d);
            int  num2 = this.PointOutsideOfPlane(p, a, c, d, b);
            int  num3 = this.PointOutsideOfPlane(p, a, d, b, c);
            int  num4 = this.PointOutsideOfPlane(p, b, d, c, a);
            bool flag = num < 0 || num2 < 0 || num3 < 0 || num4 < 0;
            bool result;

            if (flag)
            {
                finalResult.Degenerate = true;
                result = false;
            }
            else
            {
                bool flag2 = num == 0 && num2 == 0 && num3 == 0 && num4 == 0;
                if (flag2)
                {
                    result = false;
                }
                else
                {
                    FP   y     = FP.MaxValue;
                    bool flag3 = num != 0;
                    if (flag3)
                    {
                        this.ClosestPtPointTriangle(p, a, b, c, ref this.tempResult);
                        TSVector closestPointOnSimplex = this.tempResult.ClosestPointOnSimplex;
                        FP       sqrMagnitude          = (closestPointOnSimplex - p).sqrMagnitude;
                        bool     flag4 = sqrMagnitude < y;
                        if (flag4)
                        {
                            y = sqrMagnitude;
                            finalResult.ClosestPointOnSimplex = closestPointOnSimplex;
                            finalResult.UsedVertices.Reset();
                            finalResult.UsedVertices.UsedVertexA = this.tempResult.UsedVertices.UsedVertexA;
                            finalResult.UsedVertices.UsedVertexB = this.tempResult.UsedVertices.UsedVertexB;
                            finalResult.UsedVertices.UsedVertexC = this.tempResult.UsedVertices.UsedVertexC;
                            finalResult.SetBarycentricCoordinates(this.tempResult.BarycentricCoords[0], this.tempResult.BarycentricCoords[1], this.tempResult.BarycentricCoords[2], 0);
                        }
                    }
                    bool flag5 = num2 != 0;
                    if (flag5)
                    {
                        this.ClosestPtPointTriangle(p, a, c, d, ref this.tempResult);
                        TSVector closestPointOnSimplex2 = this.tempResult.ClosestPointOnSimplex;
                        FP       sqrMagnitude2          = (closestPointOnSimplex2 - p).sqrMagnitude;
                        bool     flag6 = sqrMagnitude2 < y;
                        if (flag6)
                        {
                            y = sqrMagnitude2;
                            finalResult.ClosestPointOnSimplex = closestPointOnSimplex2;
                            finalResult.UsedVertices.Reset();
                            finalResult.UsedVertices.UsedVertexA = this.tempResult.UsedVertices.UsedVertexA;
                            finalResult.UsedVertices.UsedVertexC = this.tempResult.UsedVertices.UsedVertexB;
                            finalResult.UsedVertices.UsedVertexD = this.tempResult.UsedVertices.UsedVertexC;
                            finalResult.SetBarycentricCoordinates(this.tempResult.BarycentricCoords[0], 0, this.tempResult.BarycentricCoords[1], this.tempResult.BarycentricCoords[2]);
                        }
                    }
                    bool flag7 = num3 != 0;
                    if (flag7)
                    {
                        this.ClosestPtPointTriangle(p, a, d, b, ref this.tempResult);
                        TSVector closestPointOnSimplex3 = this.tempResult.ClosestPointOnSimplex;
                        FP       sqrMagnitude3          = (closestPointOnSimplex3 - p).sqrMagnitude;
                        bool     flag8 = sqrMagnitude3 < y;
                        if (flag8)
                        {
                            y = sqrMagnitude3;
                            finalResult.ClosestPointOnSimplex = closestPointOnSimplex3;
                            finalResult.UsedVertices.Reset();
                            finalResult.UsedVertices.UsedVertexA = this.tempResult.UsedVertices.UsedVertexA;
                            finalResult.UsedVertices.UsedVertexD = this.tempResult.UsedVertices.UsedVertexB;
                            finalResult.UsedVertices.UsedVertexB = this.tempResult.UsedVertices.UsedVertexC;
                            finalResult.SetBarycentricCoordinates(this.tempResult.BarycentricCoords[0], this.tempResult.BarycentricCoords[2], 0, this.tempResult.BarycentricCoords[1]);
                        }
                    }
                    bool flag9 = num4 != 0;
                    if (flag9)
                    {
                        this.ClosestPtPointTriangle(p, b, d, c, ref this.tempResult);
                        TSVector closestPointOnSimplex4 = this.tempResult.ClosestPointOnSimplex;
                        FP       sqrMagnitude4          = (closestPointOnSimplex4 - p).sqrMagnitude;
                        bool     flag10 = sqrMagnitude4 < y;
                        if (flag10)
                        {
                            finalResult.ClosestPointOnSimplex = closestPointOnSimplex4;
                            finalResult.UsedVertices.Reset();
                            finalResult.UsedVertices.UsedVertexB = this.tempResult.UsedVertices.UsedVertexA;
                            finalResult.UsedVertices.UsedVertexD = this.tempResult.UsedVertices.UsedVertexB;
                            finalResult.UsedVertices.UsedVertexC = this.tempResult.UsedVertices.UsedVertexC;
                            finalResult.SetBarycentricCoordinates(0, this.tempResult.BarycentricCoords[0], this.tempResult.BarycentricCoords[2], this.tempResult.BarycentricCoords[1]);
                        }
                    }
                    bool flag11 = finalResult.UsedVertices.UsedVertexA && finalResult.UsedVertices.UsedVertexB && finalResult.UsedVertices.UsedVertexC && finalResult.UsedVertices.UsedVertexD;
                    result = (!flag11 || true);
                }
            }
            return(result);
        }
Пример #2
0
        public bool ClosestPtPointTriangle(TSVector p, TSVector a, TSVector b, TSVector c, ref SubSimplexClosestResult result)
        {
            result.UsedVertices.Reset();
            TSVector tSVector  = b - a;
            TSVector tSVector2 = c - a;
            TSVector vector    = p - a;
            FP       x         = TSVector.Dot(tSVector, vector);
            FP       fP        = TSVector.Dot(tSVector2, vector);
            bool     flag      = x <= FP.Zero && fP <= FP.Zero;
            bool     result2;

            if (flag)
            {
                result.ClosestPointOnSimplex    = a;
                result.UsedVertices.UsedVertexA = true;
                result.SetBarycentricCoordinates(1, 0, 0, 0);
                result2 = true;
            }
            else
            {
                TSVector vector2 = p - b;
                FP       fP2     = TSVector.Dot(tSVector, vector2);
                FP       fP3     = TSVector.Dot(tSVector2, vector2);
                bool     flag2   = fP2 >= FP.Zero && fP3 <= fP2;
                if (flag2)
                {
                    result.ClosestPointOnSimplex    = b;
                    result.UsedVertices.UsedVertexB = true;
                    result.SetBarycentricCoordinates(0, 1, 0, 0);
                    result2 = true;
                }
                else
                {
                    FP   fP4   = x * fP3 - fP2 * fP;
                    bool flag3 = fP4 <= FP.Zero && x >= FP.Zero && fP2 <= FP.Zero;
                    if (flag3)
                    {
                        FP fP5 = x / (x - fP2);
                        result.ClosestPointOnSimplex    = a + fP5 * tSVector;
                        result.UsedVertices.UsedVertexA = true;
                        result.UsedVertices.UsedVertexB = true;
                        result.SetBarycentricCoordinates(1 - fP5, fP5, 0, 0);
                        result2 = true;
                    }
                    else
                    {
                        TSVector vector3 = p - c;
                        FP       x2      = TSVector.Dot(tSVector, vector3);
                        FP       fP6     = TSVector.Dot(tSVector2, vector3);
                        bool     flag4   = fP6 >= FP.Zero && x2 <= fP6;
                        if (flag4)
                        {
                            result.ClosestPointOnSimplex    = c;
                            result.UsedVertices.UsedVertexC = true;
                            result.SetBarycentricCoordinates(0, 0, 1, 0);
                            result2 = true;
                        }
                        else
                        {
                            FP   fP7   = x2 * fP - x * fP6;
                            bool flag5 = fP7 <= FP.Zero && fP >= FP.Zero && fP6 <= FP.Zero;
                            if (flag5)
                            {
                                FP fP8 = fP / (fP - fP6);
                                result.ClosestPointOnSimplex    = a + fP8 * tSVector2;
                                result.UsedVertices.UsedVertexA = true;
                                result.UsedVertices.UsedVertexC = true;
                                result.SetBarycentricCoordinates(1 - fP8, 0, fP8, 0);
                                result2 = true;
                            }
                            else
                            {
                                FP   x3    = fP2 * fP6 - x2 * fP3;
                                bool flag6 = x3 <= FP.Zero && fP3 - fP2 >= FP.Zero && x2 - fP6 >= FP.Zero;
                                if (flag6)
                                {
                                    FP fP8 = (fP3 - fP2) / (fP3 - fP2 + (x2 - fP6));
                                    result.ClosestPointOnSimplex    = b + fP8 * (c - b);
                                    result.UsedVertices.UsedVertexB = true;
                                    result.UsedVertices.UsedVertexC = true;
                                    result.SetBarycentricCoordinates(0, 1 - fP8, fP8, 0);
                                    result2 = true;
                                }
                                else
                                {
                                    FP y   = FP.One / (x3 + fP7 + fP4);
                                    FP fP5 = fP7 * y;
                                    FP fP8 = fP4 * y;
                                    result.ClosestPointOnSimplex    = a + tSVector * fP5 + tSVector2 * fP8;
                                    result.UsedVertices.UsedVertexA = true;
                                    result.UsedVertices.UsedVertexB = true;
                                    result.UsedVertices.UsedVertexC = true;
                                    result.SetBarycentricCoordinates(1 - fP5 - fP8, fP5, fP8, 0);
                                    result2 = true;
                                }
                            }
                        }
                    }
                }
            }
            return(result2);
        }