Задача на нахождение наикратчайшего расстояния между квадратами

 static float GetClosestDistanceToHero(GameObject obj)
        {
            if (HERO.hero == null) return float.MaxValue;
            return GetClosestDistBetwin_SQUARE_rectangles(HERO.hero, obj);
        }
        static float GetClosestDistBetwin_SQUARE_rectangles(GameObject o1, GameObject o2)
        {
            //это наборы точек двух квадратов
            var points_1 = GetBarrierVertexes(o1);
            var points_2 = GetBarrierVertexes(o2);
 
            float closestPoints = float.MaxValue;
            if (points_1 == null | points_2 == null) return closestPoints;
 
 
            float CLOSEST = float.MaxValue;
 
            for (int i = 0; i < points_1.Count; ++i)
            {
                Vector2 segmentONE_1, segmentONE_2;
                //точки первого отрезка
                segmentONE_1 = points_1[i];
                int one_2 = (i == points_1.Count - 1) ? 0 : i + 1;
                segmentONE_2 = points_1[one_2];
 
                for (int i2 = 0; i2 < points_2.Count; ++i2)
                {
                    Vector2 segmentTWO_1, segmentTWO_2;
 
                    //точки второго отрезка
                    segmentTWO_1 = points_2[i2];
                    int two_2 = (i2 == points_2.Count - 1) ? 0 : i2 + 1;
                    segmentTWO_2 = points_2[two_2];
 
 
                    //найти кратчайшее расстояние между отрезками
                    float d = GetClosestDistBetwinTwo_SEGMENTS(segmentONE_1, segmentONE_2, segmentTWO_1, segmentTWO_2);
                    if (d < CLOSEST) CLOSEST = d;
                }
            }
 
            return CLOSEST;
        }
        static float GetClosestDistBetwinTwo_SEGMENTS(Vector2 segmentONE_1, Vector2 segmentONE_2, Vector2 segmentTWO_1, Vector2 segmentTWO_2)
        {
            float va1 = GetClosestDistBetwin_SEGMENT_AND_POINT(segmentONE_1, segmentONE_2, segmentTWO_1);
            float va2 = GetClosestDistBetwin_SEGMENT_AND_POINT(segmentONE_1, segmentONE_2, segmentTWO_2);
 
            if (va1 <= va2) return va1; else return va2;
        }
        static float GetClosestDistBetwin_SEGMENT_AND_POINT(Vector2 segmentONE_1, Vector2 segmentONE_2, Vector2 point)
        {
            float distance = float.MaxValue;
 
            double A = GetDistBetwin_POINTS(segmentONE_1, point);
            double B = GetDistBetwin_POINTS(segmentONE_2, point);
            double C = GetDistBetwin_POINTS(segmentONE_1, segmentONE_2);
 
            double skobki = Math.Pow(((B * B - A * A + C * C) / (2 * C)), 2);
 
            double h = Math.Sqrt(B * B - skobki);
            return (float)h;
        }
        static float GetDistBetwin_POINTS(Vector2 p1, Vector2 p2)
        {
            double pow1 = Math.Pow((double)(p1.X - p2.X), 2);
            double pow2 = Math.Pow((double)(p1.Y - p2.Y), 2);
            float d = (float)Math.Sqrt(pow1 + pow2);
            return d;
        }