AbstractedSheep.ShuttleTrackerWorld.Point3D.DistanceTo C# (CSharp) Method

        public double DistanceTo(Point3D p)
        {
            return Math.Sqrt(Math.Pow(p.X - this.X, 2) + Math.Pow(p.Y - this.Y, 2) + Math.Pow(p.Z - this.Z, 2));
        }

        /// <summary>
        /// Calculates the closest Coordinate to a line created by two other Coordinates. The resulting Coordinate is not garunteed
        /// to fall between the two endpoint Coordinates.
        /// </summary>
        /// <param name="endpoint1">A Coordinate on the line.</param>
        /// <param name="endpoint2">Another Coordinate on the line.</param>
        /// <returns>The closest Coordinate on the line to this Coordinate.</returns>
        public Coordinate ClosestPoint(Coordinate endpoint1, Coordinate endpoint2)
        {
            double  R       = 3956;
            Point3D ep1cart = new Point3D(
                R * Math.Cos(DegToRad(endpoint1.latitude)) * Math.Cos(DegToRad(endpoint1.longitude)),
                R * Math.Cos(DegToRad(endpoint1.latitude)) * Math.Sin(DegToRad(endpoint1.longitude)),
                R * Math.Sin(DegToRad(endpoint1.latitude)));
            Point3D ep2cart = new Point3D(
                R * Math.Cos(DegToRad(endpoint2.latitude)) * Math.Cos(DegToRad(endpoint2.longitude)),
                R * Math.Cos(DegToRad(endpoint2.latitude)) * Math.Sin(DegToRad(endpoint2.longitude)),
                R * Math.Sin(DegToRad(endpoint2.latitude)));
            Point3D ptcart = new Point3D(
                R * Math.Cos(DegToRad(this.latitude)) * Math.Cos(DegToRad(this.longitude)),
                R * Math.Cos(DegToRad(this.latitude)) * Math.Sin(DegToRad(this.longitude)),
                R * Math.Sin(DegToRad(this.latitude)));

            Point3D origin = new Point3D(0, 0, 0);

            double d          = ((ptcart - ep1cart).CrossProduct(ptcart - ep2cart)).Magnitude() / (ep2cart - ep1cart).Magnitude();
            double hypotenuse = ptcart.DistanceTo(ep1cart);
            double theta      = Math.Asin(d / hypotenuse);
            double adjacent   = d / Math.Tan(theta);

            Point3D closestcart = ep1cart.MoveTowards(ep2cart, adjacent);
            Point3D surfacecart = origin.MoveTowards(closestcart, R);

            return(new Coordinate(
                       (int)(RadToDeg(Math.Asin(surfacecart.Z / R)) * 1E6),
                       (int)(RadToDeg(Math.Atan2(surfacecart.Y, surfacecart.X)) * 1E6)));
        }