mysimulation/server/tso.common/WorldGeometry/Utils/TriangleSet.cs

using FSO.Common.Model;
using FSO.Common.WorldGeometry;
using System;
using System.Collections.Generic;
using System.Linq;

namespace FSO.SimAntics.Model.Routing
{
    /// <summary>
    /// (copied from VMObstacleSet)
    /// A k-d Tree for looking up rectangle intersections
    /// Ideally much faster lookups for routing rectangular cover, avatar and object movement. O(log n) vs O(n)
    /// 
    /// Concerns:
    ///  - Tree balancing: a random insert may be the best solution, as algorithms for this can be quite complex.
    ///  - Should be mutable, as rect cover routing will add new entries. We also may want to add or remove elements from a "static" set.
    ///  - Tree cloning: wall and object sets would be nice, but for routing ideally we want to add new free-rects to the set dynamically. This means we need to clone the tree.
    ///  - Return true if ANY found, or return ALL found. First useful for routing, second for checking collision validity.
    /// </summary>
    public class BaseTriangleSet
    {
        public VMObstacleSetNode[] Nodes;
        protected List<int> FreeList = new List<int>();
        protected int PoolInd = 0;
        public int Root = -1;
        public int Count;

        public BaseTriangleSet()
        {
            InitNodes(64);
        }

        public BaseTriangleSet(BaseTriangleSet last)
        {
            if (last.Root != -1)
            {
                Count = last.Count;
                Nodes = (VMObstacleSetNode[])last.Nodes.Clone();
                Root = last.Root;
                FreeList = last.FreeList.ToList();
                PoolInd = last.PoolInd;
            }
            else
            {
                InitNodes(64);
            }
        }

        public BaseTriangleSet(IEnumerable<BaseMeshTriangle> obstacles)
        {
            InitNodes(obstacles.Count());
            foreach (var obstacle in obstacles)
                Add(obstacle);
        }

        private void InitNodes(int capacity)
        {
            if (Nodes == null)
            {
                Nodes = new VMObstacleSetNode[capacity];
            }
            else
            {
                Array.Resize(ref Nodes, capacity);
            }
        }

        private int GetNode()
        {
            if (FreeList.Count > 0)
            {
                var free = FreeList.Last();
                FreeList.RemoveAt(FreeList.Count - 1);
                return free;
            }
            else
            {
                return PoolInd++;
            }
        }

        private int GetNode(IntersectRectDimension dir, BaseMeshTriangle rect)
        {
            var ind = GetNode();
            Nodes[ind] = new VMObstacleSetNode()
            {
                Dimension = dir,
                Rect = rect,
                LeftChild = -1,
                RightChild = -1,
                Index = ind,

                x1 = rect.x1,
                x2 = rect.x2,
                y1 = rect.y1,
                y2 = rect.y2
            };
            return ind;
        }

        private void Reclaim(int index)
        {
            if (index == PoolInd - 1) PoolInd--;
            else FreeList.Add(index);
        }

        public void Add(BaseMeshTriangle rect)
        {
            if (PoolInd >= Nodes.Length && FreeList.Count == 0) InitNodes(Nodes.Length * 2);
            Count++;
            if (Root == -1)
            {
                Root = GetNode(IntersectRectDimension.Left, rect);
            }
            else
            {
                AddAsChild(ref Nodes[Root], rect);
            }
        }

        private void AddAsChild(ref VMObstacleSetNode node, BaseMeshTriangle rect)
        {
            bool rightSide = false;
            switch (node.Dimension)
            {
                case IntersectRectDimension.Top:
                    rightSide = rect.y1 > node.Rect.y1; break;
                case IntersectRectDimension.Left:
                    rightSide = rect.x1 > node.Rect.x1; break;
                case IntersectRectDimension.Bottom:
                    rightSide = rect.y2 > node.Rect.y2; break;
                case IntersectRectDimension.Right:
                    rightSide = rect.x2 > node.Rect.x2; break;
            }
            if (rightSide)
            {
                if (node.RightChild != -1) AddAsChild(ref Nodes[node.RightChild], rect);
                else
                {
                    node.RightChild = GetNode((IntersectRectDimension)(((int)node.Dimension + 1) % 4), rect);
                }
            }
            else
            {
                if (node.LeftChild != -1) AddAsChild(ref Nodes[node.LeftChild], rect);
                else
                {
                    node.LeftChild = GetNode((IntersectRectDimension)(((int)node.Dimension + 1) % 4), rect);
                }
            }
        }

        public void RecursiveReAdd(VMObstacleSetNode node)
        {
            Count--;
            Reclaim(node.Index);
            Add(node.Rect);
            if (node.LeftChild != -1) RecursiveReAdd(Nodes[node.LeftChild]);
            if (node.RightChild != -1) RecursiveReAdd(Nodes[node.RightChild]);
        }

        public bool SearchForIntersect(BaseMeshTriangle rect)
        {
            if (Root == -1) return false;
            else
            {
                return SearchForIntersect(ref Nodes[Root], rect);
            }
        }

        public bool SearchForIntersect(ref VMObstacleSetNode node, BaseMeshTriangle rect)
        {
            if (node.Intersects(rect)) return true;
            //search in child nodes.
            int dontSearch = 0;
            switch (node.Dimension)
            {
                case IntersectRectDimension.Top:
                    dontSearch = (rect.y2 <= node.y1) ? 2 : 0; break; //if true, do not have to search right (where top greater)
                case IntersectRectDimension.Left:
                    dontSearch = (rect.x2 <= node.x1) ? 2 : 0; break; //if true, do not have to search right (where left greater)
                case IntersectRectDimension.Bottom:
                    dontSearch = (rect.y1 >= node.y2) ? 1 : 0; break; //if true, do not have to search left (where bottom less)
                case IntersectRectDimension.Right:
                    dontSearch = (rect.x1 >= node.x2) ? 1 : 0; break; //if true, do not have to search left (where right less)
            }

            //may need to search both :'( won't happen often with our small rectangles over large space though.
            return ((dontSearch != 1 && node.LeftChild != -1 && SearchForIntersect(ref Nodes[node.LeftChild], rect))
                || (dontSearch != 2 && node.RightChild != -1 && SearchForIntersect(ref Nodes[node.RightChild], rect)));
        }

        public List<BaseMeshTriangle> AllIntersect(BaseMeshTriangle rect)
        {
            var result = new List<BaseMeshTriangle>();
            if (Root == -1) return result;
            else
            {
                AllIntersect(ref Nodes[Root], rect, result);
                return result;
            }
        }

        public void AllIntersect(ref VMObstacleSetNode node, BaseMeshTriangle rect, List<BaseMeshTriangle> result)
        {
            if (node.Intersects(rect)) result.Add(node.Rect);
            //search in child nodes.
            int dontSearch = 0;
            switch (node.Dimension)
            {
                case IntersectRectDimension.Top:
                    dontSearch = (rect.y2 <= node.y1) ? 2 : 0; break; //if true, do not have to search right (where top greater)
                case IntersectRectDimension.Left:
                    dontSearch = (rect.x2 <= node.x1) ? 2 : 0; break; //if true, do not have to search right (where left greater)
                case IntersectRectDimension.Bottom:
                    dontSearch = (rect.y1 >= node.y2) ? 1 : 0; break; //if true, do not have to search left (where bottom less)
                case IntersectRectDimension.Right:
                    dontSearch = (rect.x1 >= node.x2) ? 1 : 0; break; //if true, do not have to search left (where right less)
            }

            //may need to search both :'( won't happen often with our small rectangles over large space though.
            //if (node.LeftChild != -1) AllIntersect(ref Nodes[node.LeftChild], rect, result);
            //if (node.RightChild != -1) AllIntersect(ref Nodes[node.RightChild], rect, result);

            if (dontSearch != 1 && node.LeftChild != -1) AllIntersect(ref Nodes[node.LeftChild], rect, result);
            if (dontSearch != 2 && node.RightChild != -1) AllIntersect(ref Nodes[node.RightChild], rect, result);
        }

        public List<BaseMeshTriangle> OnEdge(BaseMeshTriangle rect)
        {
            var result = new List<BaseMeshTriangle>();
            if (Root == -1) return result;
            else
            {
                OnEdge(ref Nodes[Root], rect, result);
                return result;
            }
        }

        public void OnEdge(ref VMObstacleSetNode node, BaseMeshTriangle rect, List<BaseMeshTriangle> result)
        {
            if (node.OnEdge(rect)) result.Add(node.Rect);
            //search in child nodes.
            //binary search to find equal opposing edges.
            int dontSearch = 0;
            switch (node.Dimension)
            {
                case IntersectRectDimension.Top:
                    dontSearch = (rect.y2 < node.y1) ? 2 : 0; break; //if true, do not have to search right (where top greater)
                case IntersectRectDimension.Left:
                    dontSearch = (rect.x2 < node.x1) ? 2 : 0; break; //if true, do not have to search right (where left greater)
                case IntersectRectDimension.Bottom:
                    dontSearch = (rect.y1 > node.y2) ? 1 : 0; break; //if true, do not have to search left (where bottom less)
                case IntersectRectDimension.Right:
                    dontSearch = (rect.x1 > node.x2) ? 1 : 0; break; //if true, do not have to search left (where right less)
            }

            //may need to search both :'( won't happen often with our small rectangles over large space though.
            //if (node.LeftChild != -1) OnEdge(ref Nodes[node.LeftChild], rect, result);
            //if (node.RightChild != -1) OnEdge(ref Nodes[node.RightChild], rect, result);

            if (dontSearch != 1 && node.LeftChild != -1) OnEdge(ref Nodes[node.LeftChild], rect, result);
            if (dontSearch != 2 && node.RightChild != -1) OnEdge(ref Nodes[node.RightChild], rect, result);
        }

        public static BaseTriangleSet RoughBalanced(List<BaseMeshTriangle> input)
        {
            //roughly attempts to balance the set. 
            //...currently by random shuffle. at least it's deterministic?
            var rand = new Random(1);
            for (int i = 1; i < input.Count; i++)
            {
                var swap = input[i - 1];
                var ind = rand.Next(input.Count - i) + i;
                input[i - 1] = input[ind];
                input[ind] = swap;
            }

            return new BaseTriangleSet(input);
        }

        /*
        public bool Delete(VMEntityObstacle rect)
        {
            if (Root == -1) return false;
            else
            {
                var result = Delete(ref Nodes[Root], rect, ref Nodes[Root]);
                if (result) { Count--; }
                return result;
            }
        }
        */

        /*
        public bool Delete(ref VMObstacleSetNode node, VMEntityObstacle rect, ref VMObstacleSetNode parent)
        {
            if (rect.Parent == (node.Rect as VMEntityObstacle).Parent)
            {
                if (parent.Index == node.Index)
                {
                    Root = -1;
                }
                else
                {
                    if (parent.LeftChild == node.Index) parent.LeftChild = -1;
                    if (parent.RightChild == node.Index) parent.RightChild = -1;
                }
                if (node.LeftChild != -1) RecursiveReAdd(Nodes[node.LeftChild]);
                if (node.RightChild != -1) RecursiveReAdd(Nodes[node.RightChild]);
                Reclaim(node.Index);
                return true;
            }
            //search in child nodes.
            //binary search to find equal opposing edges.

            bool rightSide = false;
            switch (node.Dimension)
            {
                case IntersectRectDimension.Top:
                    rightSide = rect.y1 > node.y1; break;
                case IntersectRectDimension.Left:
                    rightSide = rect.x1 > node.x1; break;
                case IntersectRectDimension.Bottom:
                    rightSide = rect.y2 > node.y2; break;
                case IntersectRectDimension.Right:
                    rightSide = rect.x2 > node.x2; break;
            }

            return ((rightSide && node.RightChild != -1 && Delete(ref Nodes[node.RightChild], rect, ref node))
            || (!rightSide && node.LeftChild != -1 && Delete(ref Nodes[node.LeftChild], rect, ref node)));
        }
        */
    }

    public struct VMObstacleSetNode
    {
        public int LeftChild;
        public int RightChild;
        public IntersectRectDimension Dimension;
        public BaseMeshTriangle Rect;
        public int Index;

        public float x1;
        public float x2;
        public float y1;
        public float y2;

        public bool Intersects(BaseMeshTriangle other)
        {
            return !((other.x1 >= x2 || other.x2 <= x1) || (other.y1 >= y2 || other.y2 <= y1));
        }

        public bool OnEdge(BaseMeshTriangle other)
        {
            return (x2 == other.x1) || (x1 == other.x2) || (y1 == other.y2) || (y2 == other.y1);
        }
    }
}