AbstractedSheep.ShuttleTrackerWorld.Coordinate.ClosestPoint C# (CSharp) Method

        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));
        }

        static void Main(string[] args)
        {
            World world = World.GenerateWorld(JsonHelper.ParseNetlink("http://shuttles.rpi.edu/displays/netlink.js"));
            EtaCalculator etaCalc = new EtaCalculator(world);

            Coordinate ep1 = new Coordinate(37760814, -77030853);
            Coordinate ep2 = new Coordinate(40366681, -123921572);
            Coordinate c = new Coordinate(55891119, -92376489);
            c = c.ClosestPoint(ep1, ep2);
            Console.WriteLine(c.ToString());

            Console.WriteLine("Press any key to continue...");
            Console.ReadKey();
            //while (true)
            //{
            //	etaCalc.Recalculate();
            //}
        }