Added ray casting (#5)

QTree.MonoGame.Common/DynamicQuadTree.cs

@@ -1,6 +1,7 @@
 using Microsoft.Xna.Framework;
 using QTree.MonoGame.Common.Extensions;
 using QTree.MonoGame.Common.Interfaces;
+using QTree.MonoGame.Common.RayCasting;
 using System;
 using System.Collections.Generic;
 
@@ -290,5 +291,8 @@ private void Split()
 
             IsSplit = true;
         }
+
+        protected override bool DoesRayIntersectQuad(QTreeRay ray) =>
+            _bounds.HasValue && (_bounds.Value.Contains(ray.Start) || _bounds.Value.IntersectsRayFast(ray));
     }
 }

QTree.MonoGame.Common/Extensions/RectangleExtensions.cs

@@ -1,4 +1,5 @@
 using Microsoft.Xna.Framework;
+using QTree.MonoGame.Common.RayCasting;
 
 namespace QTree.MonoGame.Common.Extensions
 {
@@ -20,5 +21,54 @@ public static void Split(this Rectangle rectangle,
             bottomLeft = new Rectangle(rectangle.X, bottomY, halfWidth, halfHeight);
             bottomRight = new Rectangle(rightX, bottomY, halfWidth, halfHeight);
         }
+
+        private static Vector2[] GetEdges(this Rectangle rectangle) =>
+        [
+            new Vector2(rectangle.Left, rectangle.Top),
+            new Vector2(rectangle.Right, rectangle.Top),
+            new Vector2(rectangle.Left, rectangle.Bottom),
+            new Vector2(rectangle.Right, rectangle.Bottom),
+        ];
+
+        public static bool IntersectsRay(this Rectangle rectangle, QTreeRay ray, out Vector2 intersection)
+        {
+            intersection = Vector2.Zero;
+            var foundIntersection = false;
+            var minDistance = float.MaxValue;
+            var edges = rectangle.GetEdges();
+            for (var i = 0; i < edges.Length; i++)
+            {
+                var p1 = edges[i];
+                var p2 = edges[(i + 1) % edges.Length];
+
+                if (ray.IntersectsLine(p1, p2, out var newIntersection))
+                {
+                    foundIntersection = true;
+                    var distance = Vector2.Distance(ray.Start, newIntersection);
+                    if (distance < minDistance)
+                    {
+                        minDistance = distance;
+                        intersection = newIntersection;
+                    }
+                }
+            }
+            return foundIntersection;
+        }
+
+        public static bool IntersectsRayFast(this Rectangle rectangle, QTreeRay ray)
+        {
+            var edges = rectangle.GetEdges();
+            for (var i = 0; i < edges.Length; i++)
+            {
+                var p1 = edges[i];
+                var p2 = edges[(i + 1) % edges.Length];
+
+                if (ray.IntersectsLine(p1, p2, out _))
+                {
+                   return true;
+                }
+            }
+            return false;
+        }
     }
 }

QTree.MonoGame.Common/QTree.MonoGame.Common.csproj

@@ -1,4 +1,4 @@
-<Project Sdk="Microsoft.NET.Sdk">
+<Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <TargetFramework>net8.0</TargetFramework>
     <Company>$(Authors)</Company>

QTree.MonoGame.Common/QuadTree.cs

@@ -2,6 +2,7 @@
 using QTree.MonoGame.Common.Exceptions;
 using QTree.MonoGame.Common.Extensions;
 using QTree.MonoGame.Common.Interfaces;
+using QTree.MonoGame.Common.RayCasting;
 using System;
 using System.Collections.Generic;
 
@@ -261,5 +262,8 @@ private void Split()
 
             IsSplit = true;
         }
+
+        protected override bool DoesRayIntersectQuad(QTreeRay ray) =>
+            _bounds.Contains(ray.Start) || _bounds.IntersectsRayFast(ray);
     }
 }

QTree.MonoGame.Common/QuadTreeBase.cs

@@ -1,17 +1,19 @@
 using Microsoft.Xna.Framework;
+using QTree.MonoGame.Common.Extensions;
 using QTree.MonoGame.Common.Interfaces;
+using QTree.MonoGame.Common.RayCasting;
 using System;
 using System.Collections.Generic;
 
 namespace QTree.MonoGame.Common
 {
-    public abstract class QuadTreeBase<T, TTree> : IQuadTree<T> where TTree : QuadTreeBase<T, TTree>
+    public abstract class QuadTreeBase<T, TTree>(int depth, int splitLimit, int depthLimit) : IQuadTree<T> where TTree : QuadTreeBase<T, TTree>
     {
-        protected int SplitLimit { get; }
-        protected int DepthLimit { get; }
-        protected int Depth { get; }
+        protected int SplitLimit { get; } = splitLimit;
+        protected int DepthLimit { get; } = depthLimit;
+        protected int Depth { get; } = depth;
 
-        protected List<IQuadTreeObject<T>> InternalObjects { get; }
+        protected List<IQuadTreeObject<T>> InternalObjects { get; } = [];
 
         protected bool IsSplit { get; set; }
 
@@ -20,14 +22,6 @@ public abstract class QuadTreeBase<T, TTree> : IQuadTree<T> where TTree : QuadTr
         protected TTree BL { get; set; }
         protected TTree BR { get; set; }
 
-        protected QuadTreeBase(int depth, int splitLimit, int depthLimit)
-        {
-            Depth = depth;
-            SplitLimit = splitLimit;
-            DepthLimit = depthLimit;
-            InternalObjects = new List<IQuadTreeObject<T>>();
-        }
-
         public void AddRange(params (int x, int y, int width, int height, T obj)[] objects)
         {
             foreach (var obj in objects)
@@ -112,6 +106,29 @@ public List<T> FindObject(int x, int y)
 
         public abstract List<T> FindObject(Point point);
 
+        public void RayCast(QTreeRay ray, Func<IQuadTreeObject<T>, Vector2, RaySearchOption> rayCastPredicate) => RayCastInternal(ray, rayCastPredicate);
+
+        protected abstract bool DoesRayIntersectQuad(QTreeRay ray);
+
+        protected bool RayCastInternal(QTreeRay ray, Func<IQuadTreeObject<T>, Vector2, RaySearchOption> rayCastPredicate)
+        {
+            if (!DoesRayIntersectQuad(ray)) return true;
+            foreach (var obj in InternalObjects)
+            {
+                if (obj.Bounds.IntersectsRay(ray, out var intersection) && rayCastPredicate(obj, intersection) == RaySearchOption.STOP)
+                {
+                    return false;
+                }
+            }
+
+            if (!IsSplit) return true;
+
+            return TL.RayCastInternal(ray, rayCastPredicate)
+                && TR.RayCastInternal(ray, rayCastPredicate)
+                && BL.RayCastInternal(ray, rayCastPredicate)
+                && BR.RayCastInternal(ray, rayCastPredicate);
+        }
+
         public int GetDepth()
         {
             return GetDepth(-1);

QTree.MonoGame.Common/QuadTreeObject.cs

@@ -3,18 +3,11 @@
 
 namespace QTree.MonoGame.Common
 {
-    public class QuadTreeObject<T> : IQuadTreeObject<T>
+    public class QuadTreeObject<T>(Rectangle bounds, T obj) : IQuadTreeObject<T>
     {
-        public Rectangle Bounds { get; }
-        public T Object { get; }
-        public QuadId Id { get; }
-
-        public QuadTreeObject(Rectangle bounds, T obj)
-        {
-            Id = new QuadId();
-            Bounds = bounds;
-            Object = obj;
-        }
+        public Rectangle Bounds { get; } = bounds;
+        public T Object { get; } = obj;
+        public QuadId Id { get; } = new QuadId();
 
         public QuadTreeObject(int x, int y, int width, int height, T obj)
             : this(new Rectangle(x, y, width, height), obj)

QTree.MonoGame.Common/RayCasting/QTreeRay.cs

@@ -0,0 +1,52 @@
+using Microsoft.Xna.Framework;
+using System;
+
+namespace QTree.MonoGame.Common.RayCasting
+{
+    public struct QTreeRay
+    {
+        public Vector2 Start;
+        public Vector2 Direction;
+
+        public QTreeRay(Vector2 start, Vector2 direction) 
+        { 
+            Start = start; 
+            Direction = direction;
+            Direction.Normalize();
+        }
+        public QTreeRay(Vector2 start, float angleInRadians)
+        {
+            Start = start;
+            Direction = new((float)Math.Cos(angleInRadians), (float)Math.Sin(angleInRadians));
+        }
+
+        public readonly bool IntersectsLine(Vector2 p1, Vector2 p2, out Vector2 intersection)
+        {
+            intersection = Vector2.Zero;
+            float x1 = p1.X, y1 = p1.Y;
+            float x2 = p2.X, y2 = p2.Y;
+            float x3 = Start.X, y3 = Start.Y;
+            float x4 = Start.X + Direction.X, y4 = Start.Y + Direction.Y;
+
+            float den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+            if (den == 0)
+            {
+                return false;
+            }
+
+            float t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / den;
+            float u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / den;
+
+            if (t >= 0 && t <= 1 && u >= 0)
+            {
+                intersection = new Vector2(x1 + t * (x2 - x1), y1 + t * (y2 - y1));
+                return true;
+            }
+            return false;
+        }
+
+        public static QTreeRay BetweenVectors(Vector2 start, Vector2 end) => 
+            new(start, end - start);
+
+    }
+}

QTree.MonoGame.Common/RayCasting/RaySearchOption.cs

@@ -0,0 +1,8 @@
+namespace QTree.MonoGame.Common.RayCasting
+{
+    public enum RaySearchOption
+    {
+        CONTINUE,
+        STOP,
+    }
+}

QTree.MonoGame.Test/DynamicQuadTreeTests.cs

@@ -1,6 +1,7 @@
 using Microsoft.VisualStudio.TestTools.UnitTesting;
 using Microsoft.Xna.Framework;
 using QTree.MonoGame.Common;
+using QTree.MonoGame.Common.RayCasting;
 using System;
 using System.Diagnostics;
 using System.Linq;
@@ -146,5 +147,47 @@ public void DepthTest()
             // assert
             Assert.AreEqual(5, sut.GetDepth());
         }
+
+        [DataTestMethod]
+        [DataRow(0f, 0f, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f)]
+        [DataRow(0f, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f)]
+        [DataRow(QUAD_SIZE - 5000f, 0f, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f)]
+        [DataRow(QUAD_SIZE, QUAD_SIZE, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f)]
+        public void RayCastTest(float startSearchX, float startSearchY, float objectCenterX, float objectCenterY)
+        {
+            // arrange
+            var size = 200;
+            var sut = new DynamicQuadTree<string>();
+            for (var i = 0; i < 1_000_000; i++)
+            {
+                sut.Add(_random.Next(QUAD_SIZE - size), _random.Next(QUAD_SIZE - size), size, size, string.Empty);
+            }
+            var position = new Vector2(objectCenterX, objectCenterY);
+            var expected = new Rectangle((int)position.X - size / 2, (int)position.Y - size / 2, size, size);
+            sut.Add(expected, "CORRECT");
+            var result = "";
+
+            // act
+            var timer = Stopwatch.StartNew();
+            var ray = QTreeRay.BetweenVectors(new Vector2(startSearchX, startSearchY), position);
+            sut.RayCast(ray, (treeObj, hit) =>
+            {
+                if (treeObj.Object == "CORRECT")
+                {
+                    result = treeObj.Object;
+                    return RaySearchOption.STOP;
+                }
+
+                Assert.IsFalse(result == "CORRECT");
+                return RaySearchOption.CONTINUE;
+            });
+
+            var time = timer.ElapsedMilliseconds;
+            timer.Stop();
+
+            // assert
+            Console.WriteLine($"Time: {time}");
+            Assert.IsTrue(result == "CORRECT");
+        }
     }
 }

QTree.MonoGame.Test/QuadTreeTests.cs

@@ -1,6 +1,7 @@
 using Microsoft.VisualStudio.TestTools.UnitTesting;
 using Microsoft.Xna.Framework;
 using QTree.MonoGame.Common;
+using QTree.MonoGame.Common.RayCasting;
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
@@ -176,5 +177,46 @@ public void DepthTest()
             // assert
             Assert.AreEqual(5, sut.GetDepth());
         }
+
+        [DataTestMethod]
+        [DataRow(0f, 0f, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f)]
+        [DataRow(0f, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f)]
+        [DataRow(QUAD_SIZE - 5000f, 0f, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f)]
+        [DataRow(QUAD_SIZE, QUAD_SIZE, QUAD_SIZE - 5000f, QUAD_SIZE - 5000f)]
+        public void RayCastTest(float startSearchX, float startSearchY, float objectCenterX, float objectCenterY)
+        {
+            // arrange
+            var size = 200;
+            var sut = new QuadTree<string>(new Rectangle(0, 0, QUAD_SIZE, QUAD_SIZE));
+            for (var i = 0; i < 1_000_000; i++)
+            {
+                sut.Add(_random.Next(QUAD_SIZE - size), _random.Next(QUAD_SIZE - size), size, size, string.Empty);
+            }
+            var position = new Vector2(objectCenterX, objectCenterY);
+            var expected = new Rectangle((int)position.X - size / 2, (int)position.Y - size / 2, size, size);
+            sut.Add(expected, "CORRECT");
+            var result = "";
+
+            // act
+            var timer = Stopwatch.StartNew();
+            var ray = QTreeRay.BetweenVectors(new Vector2(startSearchX, startSearchY), position);
+            sut.RayCast(ray, (treeObj, hit) =>
+            {
+                if (treeObj.Object == "CORRECT")
+                {
+                    result = treeObj.Object;
+                    return RaySearchOption.STOP;
+                }
+                Assert.IsFalse(result == "CORRECT");
+                return RaySearchOption.CONTINUE;
+            });
+
+            var time = timer.ElapsedMilliseconds;
+            timer.Stop();
+
+            // assert
+            Console.WriteLine($"Time: {time}");
+            Assert.IsTrue(result == "CORRECT");
+        }
     }
 }