Esempio n. 1
0
 /// <summary> Returns a linear combination of the vectors into result.</summary>
 public static void LinComb(
     double fA, ref Vector3d vA, double fB, ref Vector3d vB, double fC, ref Vector3d vC, out Vector3d result)
 {
     result.X = fA * vA.X + fB * vB.X + fC * vC.X;
     result.Y = fA * vA.Y + fB * vB.Y + fC * vC.Y;
     result.Z = fA * vA.Z + fB * vB.Z + fC * vC.Z;
 }
Esempio n. 2
0
        /// <summary>
        /// Squared distance from point to (filled) triangle
        /// Implementation based on the paper, Distance Between Point and Triangle in 3D.
        /// David Eberly, Geometric Tools, LLC
        /// </summary>
        /// <param name="pt">Point to measure distance from</param>
        /// <param name="fs">ptClosest = (1-fs-ft)*ptTri0 + fs*ptTri1 + ft*ptTri2</param>
        /// <param name="ft">ptClosest = (1-fs-ft)*ptTri0 + fs*ptTri1 + ft*ptTri2</param>
        /// <returns>Squared distance</returns>
        public double SqrDist(ref Vector3d pt, out double fs, out double ft)
        {
            var vD = new Vector3d
            {
                X = Tri.Pt0.X - pt.X,
                Y = Tri.Pt0.Y - pt.Y,
                Z = Tri.Pt0.Z - pt.Z
            };

            double fd = v01.X * vD.X + v01.Y * vD.Y + v01.Z * vD.Z;     // Dot(vE0, vD);
            double fe = v02.X * vD.X + v02.Y * vD.Y + v02.Z * vD.Z;     // Dot(vE1, vD);
            double ff = vD.X * vD.X + vD.Y * vD.Y + vD.Z * vD.Z;        // Dot(vD, vD);

            fs = fb * fe - fc * fd;
            ft = fb * fd - fa * fe;

            double result;
            if (fs + ft <= fDet)
            {
                if (fs < 0)
                {
                    if (ft < 0)
                    {
                        // Region 4
                        if (fe < 0)  // minimum on edge s=0
                        {
                            fs = 0;
                            if (fe >= 0)
                            {
                                ft = 0;
                                result = ff;
                            }
                            else if (-fe >= fc)
                            {
                                ft = 1;
                                result = fc + 2 * fe + ff;
                            }
                            else
                            {
                                ft = -fe / fc;
                                // Calculate the square distance
                                result = (fc * ft + 2 * fe) * ft + ff;
                            }
                        }
                        else            // minimum on edge t=0
                        {
                            ft = 0;
                            if (fd >= 0)
                            {
                                fs = 0;
                                result = ff;
                            }
                            else if (-fd >= fa)
                            {
                                fs = 1;
                                result = fa + 2 * fd + ff;
                            }
                            else
                            {
                                fs = -fd / fa;
                                result = (fa * fs + 2 * fd) * fs + ff;
                            }
                        }
                    }
                    else
                    {
                        // Region 3
                        fs = 0;                 // minimum on edge s=0
                        if (fe >= 0)
                        {
                            ft = 0;
                            result = ff;
                        }
                        else if (-fe >= fc)
                        {
                            ft = 1;
                            result = fc + 2 * fe + ff;
                        }
                        else
                        {
                            ft = -fe / fc;
                            // Calculate the square distance
                            result = (fc * ft + 2 * fe) * ft + ff;
                        };

                    }
                }
                else if (ft < 0)
                {
                    // Region 5
                    ft = 0;
                    if (fd >= 0)
                    {
                        fs = 0;
                        // Calculate the square distance
                        result = ff;
                    }
                    else if (-fd >= fa)
                    {
                        fs = 1;
                        // Calculate the square distance
                        result = fa + 2 * fd + ff;
                    }
                    else
                    {
                        fs = -fd / fa;
                        // Calculate the square distance
                        result = (fa * fs + 2 * fd) * fs + ff;
                    };
                }
                else
                {
                    // Region 0,  Inside the triangle
                    // First check that the triangle is not degenerated
                    if (fDet != 0)
                    {
                        double finvDet = 1 / fDet;
                        fs = fs * finvDet;
                        ft = ft * finvDet;
                        // Calculate the square distance
                        result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;
                    }
                    else
                    {
                        // Degenerated, calculate distance to line
                        if ((fa == 0) && (fc == 0))  // Triangle is a point
                        {
                            fs = 0;
                            ft = 0;
                            result = ff;
                        }
                        else if ((fa == 0)) // Edge s = 0
                        {
                            fs = 0;
                            if (fe >= 0)
                            {
                                ft = 0;
                                result = ff;
                            }
                            else if (-fe >= fc)
                            {
                                ft = 1;
                                result = fc + 2 * fe + ff;
                            }
                            else
                            {
                                ft = -fe / fc;
                                // Calculate the square distance
                                result = (fc * ft + 2 * fe) * ft + ff;
                            }
                        }
                        else if ((fc == 0)) // Edge t = 0
                        {
                            ft = 0;
                            if (fd >= 0)
                            {
                                fs = 0;
                                result = ff;
                            }
                            else if (-fd >= fa)
                            {
                                fs = 1;
                                result = fa + 2 * fd + ff;
                            }
                            else
                            {
                                fs = -fd / fa;
                                result = (fa * fs + 2 * fd) * fs + ff;
                            }
                        }
                        else  // All edges are in one line
                        {
                            double fTmp0 = fa + fd;
                            double fTmp1 = fb + fe;
                            if (fTmp1 < fTmp0)         // minimum on edge s+t=1
                            {
                                double fnum = fTmp0 - fTmp1;
                                double fden = fa - 2 * fb + fc;
                                if (fnum >= fden)
                                {
                                    ft = 1;
                                    fs = 0;
                                    // Calculate the square distance
                                    result = fc + 2 * fe + ff;
                                }
                                else
                                {
                                    ft = fnum / fden;
                                    fs = 1 - ft;
                                    // Calculate the square distance
                                    result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;

                                };
                            }
                            else
                            {                         // minimum on edge t=0
                                if (fTmp0 <= 0)
                                {
                                    fs = 1;
                                    ft = 0;
                                    // Calculate the square distance
                                    result = fa + 2 * fd + ff;
                                }
                                else if (fd >= 0)
                                {
                                    fs = 0;
                                    ft = 0;
                                    // Calculate the square distance
                                    result = ff;
                                }
                                else
                                {
                                    fs = -fd / fa;
                                    ft = 0;
                                    // Calculate the square distance
                                    result = (fa * fs + 2 * fd) * fs + ff;
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                if (fs < 0)
                {
                    // Region 2
                    double fTmp0 = fb + fd;
                    double fTmp1 = fc + fe;
                    if (fTmp1 > fTmp0)         // minimum on edge s+t=1
                    {
                        double fnum = fTmp1 - fTmp0;
                        double fden = fa - 2 * fb + fc;
                        if (fnum >= fden)
                        {
                            fs = 1;
                            ft = 0;
                            // Calculate the square distance
                            result = fa + 2 * fd + ff;
                        }
                        else
                        {
                            fs = fnum / fden;
                            ft = 1 - fs;
                            // Calculate the square distance
                            result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;
                        };
                    }
                    else
                    {                         // minimum on edge s=0
                        if (fTmp1 <= 0)
                        {
                            ft = 1;
                            fs = 0;
                            // Calculate the square distance
                            result = fc + 2 * fe + ff;
                        }
                        else if (fe >= 0)
                        {
                            ft = 0;
                            fs = 0;
                            // Calculate the square distance
                            result = ff;
                        }
                        else
                        {
                            ft = -fe / fc;
                            fs = 0;
                            // Calculate the square distance
                            result = (fc * ft + 2 * fe) * ft + ff;
                        };
                    };
                }
                else if (ft < 0)
                {
                    // Region 6
                    double fTmp0 = fa + fd;
                    double fTmp1 = fb + fe;
                    if (fTmp1 < fTmp0)         // minimum on edge s+t=1
                    {
                        double fnum = fTmp0 - fTmp1;
                        double fden = fa - 2 * fb + fc;
                        if (fnum >= fden)
                        {
                            ft = 1;
                            fs = 0;
                            // Calculate the square distance
                            result = fc + 2 * fe + ff;
                        }
                        else
                        {
                            ft = fnum / fden;
                            fs = 1 - ft;
                            // Calculate the square distance
                            result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;

                        };
                    }
                    else
                    {                         // minimum on edge t=0
                        if (fTmp0 <= 0)
                        {
                            fs = 1;
                            ft = 0;
                            // Calculate the square distance
                            result = fa + 2 * fd + ff;
                        }
                        else if (fd >= 0)
                        {
                            fs = 0;
                            ft = 0;
                            // Calculate the square distance
                            result = ff;
                        }
                        else
                        {
                            fs = -fd / fa;
                            ft = 0;
                            // Calculate the square distance
                            result = (fa * fs + 2 * fd) * fs + ff;
                        };
                    };
                }
                else
                {
                    // Region 1
                    double fnum = fc + fe - fb - fd;
                    if (fnum <= 0)
                    {
                        fs = 0;
                        ft = 1;
                        // Calculate the square distance
                        result = fc + 2 * fe + ff;

                    }
                    else
                    {
                        double fden = fa - 2 * fb + fc;
                        if (fnum >= fden)
                        {
                            fs = 1;
                            ft = 0;
                            // Calculate the square distance
                            result = fa + 2 * fd + ff;
                        }
                        else
                        {
                            fs = fnum / fden;
                            ft = 1 - fs;
                            // Calculate the square distance
                            result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;
                        }
                    }
                }
            }
            // Ensure that Result is non-negative (it may be < 0 due to numerical noise)
            if (result < 0)
                result = 0;
            return result;
        }
Esempio n. 3
0
 /// <summary>
 /// Squared distance from point to (filled) triangle
 /// </summary>
 /// <param name="pt">Point to measure distance from</param>
 /// <param name="ptTri0">Triangle corner</param>
 /// <param name="ptTri1">Triangle corner</param>
 /// <param name="ptTri2">Triangle corner</param>
 /// <param name="ptClosest">Closest point on the triangle</param>
 /// <returns>Squared distance</returns>
 public double SqrDist(ref Vector3d pt, out Vector3d ptClosest)
 {
     double fs, ft;
     var sqrDist = SqrDist(ref pt, out fs, out ft);
     ptClosest.X = Tri.Pt0.X + fs * v01.X + ft * v02.X;
     ptClosest.Y = Tri.Pt0.Y + fs * v01.Y + ft * v02.Y;
     ptClosest.Z = Tri.Pt0.Z + fs * v01.Z + ft * v02.Z;
     return sqrDist;
 }