Hiho 126 GeoHash二·附近目标

Hiho 126
两点间的距离不能直接用公式，精度不够，要优化一下。把一个点旋转到零轴上。
Length = r * Acos(Cos(x1) * Cos(x2) * Cos(y1-y2) - Sin(x1) * Sin(x2))
=>
Length = r * Acos(Cos(x1-x2) * Cos(y1-y2));

using System;
using System.Collections.Generic;
using System.Text;

namespace Hiho
{
    class _126
    {
        public struct Point
        {
            public double X, Y;
            public Point(double x, double y)
            {
                X = x;
                Y = y;
            }
        }
        private static Dictionary<string, List<Point>> dictionary = new Dictionary<string, List<Point>>();
        private static HashSet<string> visit =null;
        private static List<char> Base32 = new List<char>() { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'j', 'k', 'm', 'n', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z' };
        static string GeoEncode(double latitude, double longitude, int precision)
        {
            double[] latitudeInterval = { -90, 90 };
            double[] longitudeInterval = { -180, 180 };
            int length = precision * 5;
            StringBuilder geohash = new StringBuilder();
            int bits = 0;
            for (int i = 1; i <= length; ++i)
            {
                if (i % 2 == 1)
                {
                    double mid = (longitudeInterval[0] + longitudeInterval[1]) / 2;
                    if (longitude > mid)
                    {

                        bits = bits * 2 + 1;
                        longitudeInterval[0] = mid;
                    }
                    else
                    {

                        bits = bits * 2;
                        longitudeInterval[1] = mid;
                    }
                }
                else
                {
                    double mid = (latitudeInterval[0] + latitudeInterval[1]) / 2;
                    if (latitude > mid)
                    {
                        bits = bits * 2 + 1;
                        latitudeInterval[0] = mid;
                    }
                    else
                    {
                        bits = bits * 2;
                        latitudeInterval[1] = mid;
                    }
                }
                if (i % 5 == 0)
                {
                    geohash.Append(Base32[bits]);
                    bits = 0;
                }
            }
            return geohash.ToString();
        }
        static void GeoDecode(string geohash, out double lat, out double lon)
        {
            bool odd = true;
            double[] latitudeInterval = { -90, 90 };
            double[] longitudeInterval = { -180, 180 };
            for (int i = 0; i < geohash.Length; ++i)
            {
                int bits = Base32.IndexOf(geohash[i]);
                for (int j = 4; j >= 0; --j)
                {
                    int bit = (bits >> j) & 1;
                    if (odd)
                    {
                        double mid = (longitudeInterval[0] + longitudeInterval[1]) / 2;
                        longitudeInterval[1 - bit] = mid;
                    }
                    else
                    {
                        double mid = (latitudeInterval[0] + latitudeInterval[1]) / 2;
                        latitudeInterval[1 - bit] = mid;
                    }
                    odd = !odd;
                }
            }
            lat = (latitudeInterval[0] + latitudeInterval[1]) / 2;
            lon = (longitudeInterval[0] + longitudeInterval[1]) / 2;
        }
        static double ToRadian(double angle)
        {
            return angle * Math.PI / 180.0;
        }
        static int CountNum(string geohash, double X2, double Y2)
        {
            if (visit.Contains(geohash)) return 0;
            visit.Add(geohash);
            if (dictionary.ContainsKey(geohash))
            {
                List<Point> points = dictionary[geohash];
                int count = 0;
                double r = 6000000;
                foreach (Point p in points)
                {
                    //double d = r * Math.Acos(Math.Cos(ToRadian(p.X)) * Math.Cos(ToRadian(X2)) * Math.Cos(ToRadian(p.Y) - ToRadian(Y2)) + Math.Sin(ToRadian(p.X)) * Math.Sin(ToRadian(X2)));
                    double d = r * Math.Acos(Math.Cos(ToRadian(p.X)-ToRadian(X2)) * Math.Cos(ToRadian(p.Y)-ToRadian(Y2)));
                    if (d <= 500)
                        ++count;
                }
                return count;
            }
            else return 0;
        }
        static void Main(string[] args)
        {
            string[] lineArray = Console.ReadLine().Split(' ');
            int N = int.Parse(lineArray[0]), M = int.Parse(lineArray[1]), digit = 6;
            for (int i = 0; i < N; ++i)
            {
                lineArray = Console.ReadLine().Split(' ');
                double X = double.Parse(lineArray[0]), Y = double.Parse(lineArray[1]);
                string encodeStr = GeoEncode(X, Y, digit);
                if (dictionary.ContainsKey(encodeStr))
                    dictionary[encodeStr].Add(new Point(X, Y));
                else
                    dictionary.Add(encodeStr, new List<Point>() { new Point(X, Y) });
            }
            double dx = 575 * 180 / (2 * Math.PI * 6000000), dy = 1150 * 360/ (2 * Math.PI * 6000000);
            for (int i = 0; i < M; ++i)
            {
                lineArray = Console.ReadLine().Split(' ');
                double X = double.Parse(lineArray[0]), Y = double.Parse(lineArray[1]);
                visit = new HashSet<string>();
                int ans = CountNum(GeoEncode(X, Y, digit), X, Y)
                    + CountNum(GeoEncode(X + dx, Y - dy, digit), X, Y)
                    + CountNum(GeoEncode(X + dx, Y, digit), X, Y)
                    + CountNum(GeoEncode(X + dx, Y + dy, digit), X, Y)
                    + CountNum(GeoEncode(X, Y - dy, digit), X, Y)
                    + CountNum(GeoEncode(X, Y + dy, digit), X, Y)
                    + CountNum(GeoEncode(X - dx, Y - dy, digit), X, Y)
                    + CountNum(GeoEncode(X - dx, Y, digit), X, Y)
                    + CountNum(GeoEncode(X - dx, Y + dy, digit), X, Y);
                Console.WriteLine(ans);
            }
        }
    }
}