Assets/Scripts/OptimizationLogic/Core/Geometry2D.cs - master - GeekNerd/ServerModelingProject

Assets/Scripts/OptimizationLogic/Core/Geometry2D.cs
254 строки9 KB
Gorney-Alex
Init
30 май 2026, 19:05
30 май 2026, 19:053cdcc49
using System;
using System.Collections.Generic;
using UnityEngine;

namespace OptimizationLogic.Core
{
    public static class Geometry2D
    {
        public static float SignedArea(IReadOnlyList<Vector2> polygon)
        {
            if (polygon == null || polygon.Count < 3) return 0f;

            double sum = 0.0;
            for (int i = 0; i < polygon.Count; i++)
            {
                Vector2 a = polygon[i];
                Vector2 b = polygon[(i + 1) % polygon.Count];
                sum += (double)a.x * b.y - (double)b.x * a.y;
            }
            return (float)(0.5 * sum);
        }

        // Ray casting point-in-polygon (works for simple polygons; boundary counts as inside).
        public static bool IsPointInPolygon(Vector2 point, IReadOnlyList<Vector2> polygon)
        {
            if (polygon == null || polygon.Count < 3) return false;

            bool inside = false;
            for (int i = 0, j = polygon.Count - 1; i < polygon.Count; j = i++)
            {
                Vector2 pi = polygon[i];
                Vector2 pj = polygon[j];

                // Check if point is on segment
                if (DistancePointToSegment(point, pj, pi) <= 1e-6f)
                    return true;

                bool intersect = ((pi.y > point.y) != (pj.y > point.y))
                                 && (point.x < (pj.x - pi.x) * (point.y - pi.y) / (pj.y - pi.y + 1e-12f) + pi.x);
                if (intersect) inside = !inside;
            }

            return inside;
        }

        public static Rect ComputeBoundingBox(IReadOnlyList<Vector2> points)
        {
            if (points == null || points.Count == 0) return new Rect();

            float minX = points[0].x, maxX = points[0].x;
            float minY = points[0].y, maxY = points[0].y;

            for (int i = 1; i < points.Count; i++)
            {
                Vector2 p = points[i];
                if (p.x < minX) minX = p.x;
                if (p.x > maxX) maxX = p.x;
                if (p.y < minY) minY = p.y;
                if (p.y > maxY) maxY = p.y;
            }

            return Rect.MinMaxRect(minX, minY, maxX, maxY);
        }

        public static float DistancePointToSegment(Vector2 p, Vector2 a, Vector2 b)
        {
            Vector2 ab = b - a;
            float abSqr = ab.sqrMagnitude;
            if (abSqr <= 1e-12f) return Vector2.Distance(p, a);

            float t = Mathf.Clamp01(Vector2.Dot(p - a, ab) / abSqr);
            Vector2 closest = a + t * ab;
            return Vector2.Distance(p, closest);
        }

        public static float DistanceRectToSegment(Rect rect, Vector2 a, Vector2 b)
        {
            // 0 if segment intersects rect
            if (SegmentIntersectsRect(a, b, rect)) return 0f;

            Vector2 p1 = new(rect.xMin, rect.yMin);
            Vector2 p2 = new(rect.xMax, rect.yMin);
            Vector2 p3 = new(rect.xMax, rect.yMax);
            Vector2 p4 = new(rect.xMin, rect.yMax);

            float d = float.PositiveInfinity;
            d = Mathf.Min(d, DistancePointToSegment(p1, a, b));
            d = Mathf.Min(d, DistancePointToSegment(p2, a, b));
            d = Mathf.Min(d, DistancePointToSegment(p3, a, b));
            d = Mathf.Min(d, DistancePointToSegment(p4, a, b));

            // Also consider distance from segment endpoints to rect
            d = Mathf.Min(d, DistancePointToRect(a, rect));
            d = Mathf.Min(d, DistancePointToRect(b, rect));

            return d;
        }

        public static float DistancePointToRect(Vector2 p, Rect rect)
        {
            float dx = 0f;
            if (p.x < rect.xMin) dx = rect.xMin - p.x;
            else if (p.x > rect.xMax) dx = p.x - rect.xMax;

            float dy = 0f;
            if (p.y < rect.yMin) dy = rect.yMin - p.y;
            else if (p.y > rect.yMax) dy = p.y - rect.yMax;

            return Mathf.Sqrt(dx * dx + dy * dy);
        }

        public static bool SegmentIntersectsRect(Vector2 a, Vector2 b, Rect rect)
        {
            if (rect.Contains(a) || rect.Contains(b)) return true;

            Vector2 r1 = new(rect.xMin, rect.yMin);
            Vector2 r2 = new(rect.xMax, rect.yMin);
            Vector2 r3 = new(rect.xMax, rect.yMax);
            Vector2 r4 = new(rect.xMin, rect.yMax);

            return SegmentsIntersect(a, b, r1, r2)
                   || SegmentsIntersect(a, b, r2, r3)
                   || SegmentsIntersect(a, b, r3, r4)
                   || SegmentsIntersect(a, b, r4, r1);
        }

        public static bool SegmentsIntersect(Vector2 p1, Vector2 p2, Vector2 q1, Vector2 q2)
        {
            float o1 = Orientation(p1, p2, q1);
            float o2 = Orientation(p1, p2, q2);
            float o3 = Orientation(q1, q2, p1);
            float o4 = Orientation(q1, q2, p2);

            if (o1 * o2 < 0f && o3 * o4 < 0f) return true;

            // Collinear cases
            if (Mathf.Abs(o1) < 1e-6f && OnSegment(p1, p2, q1)) return true;
            if (Mathf.Abs(o2) < 1e-6f && OnSegment(p1, p2, q2)) return true;
            if (Mathf.Abs(o3) < 1e-6f && OnSegment(q1, q2, p1)) return true;
            if (Mathf.Abs(o4) < 1e-6f && OnSegment(q1, q2, p2)) return true;

            return false;
        }

        private static float Orientation(Vector2 a, Vector2 b, Vector2 c)
        {
            // cross((b-a),(c-a))
            return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
        }

        private static bool OnSegment(Vector2 a, Vector2 b, Vector2 p)
        {
            return p.x >= Mathf.Min(a.x, b.x) - 1e-6f && p.x <= Mathf.Max(a.x, b.x) + 1e-6f
                   && p.y >= Mathf.Min(a.y, b.y) - 1e-6f && p.y <= Mathf.Max(a.y, b.y) + 1e-6f;
        }

        public static float DistanceBetweenRects(Rect a, Rect b)
        {
            float dx = 0f;
            if (a.xMax < b.xMin) dx = b.xMin - a.xMax;
            else if (b.xMax < a.xMin) dx = a.xMin - b.xMax;

            float dy = 0f;
            if (a.yMax < b.yMin) dy = b.yMin - a.yMax;
            else if (b.yMax < a.yMin) dy = a.yMin - b.yMax;

            return Mathf.Sqrt(dx * dx + dy * dy);
        }

        public static bool Intersects(Rect a, Rect b)
        {
            return a.Overlaps(b);
        }

        // SAT for oriented rectangles.
        public static bool Intersects(OrientedRect a, OrientedRect b)
        {
            Vector2[] axes = { a.Right, a.Up, b.Right, b.Up };
            Vector2[] aCorners = a.GetCorners();
            Vector2[] bCorners = b.GetCorners();

            for (int i = 0; i < axes.Length; i++)
            {
                Vector2 axis = axes[i];
                ProjectOntoAxis(aCorners, axis, out float aMin, out float aMax);
                ProjectOntoAxis(bCorners, axis, out float bMin, out float bMax);
                if (aMax < bMin || bMax < aMin) return false;
            }

            return true;
        }

        public static bool Intersects(Rect a, OrientedRect b)
        {
            OrientedRect aObb = new OrientedRect(a.center, a.size, 0f);
            return Intersects(aObb, b);
        }

        private static void ProjectOntoAxis(Vector2[] corners, Vector2 axis, out float min, out float max)
        {
            float len = axis.magnitude;
            if (len <= 1e-12f)
            {
                min = max = 0f;
                return;
            }
            Vector2 n = axis / len;

            float p0 = Vector2.Dot(corners[0], n);
            min = max = p0;
            for (int i = 1; i < corners.Length; i++)
            {
                float p = Vector2.Dot(corners[i], n);
                if (p < min) min = p;
                if (p > max) max = p;
            }
        }

        public static float DistanceRectToCircle(Rect rect, Vector2 center)
        {
            float cx = Mathf.Clamp(center.x, rect.xMin, rect.xMax);
            float cy = Mathf.Clamp(center.y, rect.yMin, rect.yMax);
            return Vector2.Distance(center, new Vector2(cx, cy));
        }

        public static Vector2 SafeNormalize(Vector2 v, Vector2 fallback)
        {
            float m = v.magnitude;
            if (m <= 1e-6f) return fallback;
            return v / m;
        }

        public static int NormalizeRotationToCardinal(float rotationDeg)
        {
            float r = Mathf.Repeat(rotationDeg, 360f);
            int idx = Mathf.RoundToInt(r / 90f) % 4;
            if (idx < 0) idx += 4;
            return idx * 90;
        }

        public static Vector2 FrontVectorFromRotation(float rotationDeg)
        {
            int r = NormalizeRotationToCardinal(rotationDeg);
            return r switch
            {
                0 => Vector2.up,
                90 => Vector2.right,
                180 => Vector2.down,
                270 => Vector2.left,
                _ => Vector2.up
            };
        }
    }
}