Initial commit

srcs/RobotNet10/Commons/RobotNet10.Common/KDTTree.cs

@@ -0,0 +1,299 @@
+using RobotNet10.Common.Models;
+
+namespace RobotNet10.Common;
+
+/// <summary>
+/// KD-Tree implementation for efficient 2D spatial search of nodes.
+/// Thread-safe for read operations after construction.
+/// Time complexity: O(n log n) build, O(log n) search average case.
+/// </summary>
+public class KDTree
+{
+    private readonly KDTreeNode? _root;
+    private readonly List<KDTreeData> _nodes;
+
+    /// <summary>
+    /// Initializes a new KD-Tree from a collection of nodes.
+    /// </summary>
+    /// <param name="nodes">The nodes to index. Original collection is not modified.</param>
+    /// <exception cref="ArgumentNullException">Thrown when nodes is null.</exception>
+    public KDTree(IEnumerable<KDTreeData> nodes)
+    {
+        ArgumentNullException.ThrowIfNull(nodes);
+
+        // Create a copy to avoid mutating input
+        _nodes = [.. nodes];
+
+        if (_nodes.Count == 0)
+        {
+            _root = null;
+            return;
+        }
+
+        _root = BuildTree(0, _nodes.Count - 1, 0);
+    }
+
+    /// <summary>
+    /// Gets the total number of nodes in the tree.
+    /// </summary>
+    public int Count => _nodes.Count;
+
+    /// <summary>
+    /// Builds the KD-Tree using index-based recursion to avoid memory allocation overhead.
+    /// Time complexity: O(n log n)
+    /// </summary>
+    private KDTreeNode? BuildTree(int start, int end, int depth)
+    {
+        if (start > end)
+            return null;
+
+        int axis = depth % 2;
+
+        // Use QuickSelect to find median without full sort
+        int medianIndex = QuickSelect(start, end, (start + end) / 2, axis);
+
+        return new KDTreeNode(
+            node: _nodes[medianIndex],
+            axis: axis,
+            left: BuildTree(start, medianIndex - 1, depth + 1),
+            right: BuildTree(medianIndex + 1, end, depth + 1)
+        );
+    }
+
+    /// <summary>
+    /// QuickSelect algorithm to find the k-th smallest element.
+    /// Time complexity: O(n) average, O(n²) worst case.
+    /// </summary>
+    private int QuickSelect(int left, int right, int k, int axis)
+    {
+        while (left < right)
+        {
+            int pivotIndex = Partition(left, right, axis);
+
+            if (pivotIndex == k)
+                return k;
+            else if (k < pivotIndex)
+                right = pivotIndex - 1;
+            else
+                left = pivotIndex + 1;
+        }
+        return left;
+    }
+
+    /// <summary>
+    /// Partitions the array for QuickSelect using median-of-three pivot selection.
+    /// This is the CORRECTED version that handles all edge cases properly.
+    /// </summary>
+    private int Partition(int left, int right, int axis)
+    {
+        // Handle small subarrays
+        if (right - left < 2)
+        {
+            if (right > left && CompareNodes(_nodes[right], _nodes[left], axis) < 0)
+                Swap(left, right);
+            return left;
+        }
+
+        // Use median-of-three pivot selection for better performance
+        int mid = left + (right - left) / 2; // Overflow-safe
+
+        // Sort left, mid, right to get median as pivot
+        if (CompareNodes(_nodes[mid], _nodes[left], axis) < 0)
+            Swap(left, mid);
+        if (CompareNodes(_nodes[right], _nodes[left], axis) < 0)
+            Swap(left, right);
+        if (CompareNodes(_nodes[right], _nodes[mid], axis) < 0)
+            Swap(mid, right);
+
+        // Now: nodes[left] <= nodes[mid] <= nodes[right]
+        // Use mid as pivot and hide it at right-1
+        KDTreeData pivot = _nodes[mid];
+        Swap(mid, right - 1);
+
+        // Partition with pivot at right-1
+        int i = left + 1;      // Start after left (which is already <= pivot)
+        int j = right - 2;     // Start before pivot position
+
+        while (i <= j)
+        {
+            // Find element >= pivot from left
+            while (i <= j && CompareNodes(_nodes[i], pivot, axis) < 0)
+                i++;
+
+            // Find element <= pivot from right
+            while (i <= j && CompareNodes(_nodes[j], pivot, axis) > 0)
+                j--;
+
+            if (i < j)
+            {
+                Swap(i, j);
+                i++;
+                j--;
+            }
+            else
+            {
+                break;
+            }
+        }
+
+        // Put pivot in its final position
+        Swap(i, right - 1);
+        return i;
+    }
+
+    /// <summary>
+    /// Compares two nodes along the specified axis.
+    /// </summary>
+    private static int CompareNodes(KDTreeData a, KDTreeData b, int axis)
+    {
+        return axis == 0 ? a.X.CompareTo(b.X) : a.Y.CompareTo(b.Y);
+    }
+
+    /// <summary>
+    /// Swaps two elements in the nodes list using tuple deconstruction.
+    /// </summary>
+    private void Swap(int i, int j)
+    {
+        if (i != j)
+        {
+            (_nodes[j], _nodes[i]) = (_nodes[i], _nodes[j]);
+        }
+    }
+
+    /// <summary>
+    /// Finds the nearest node to the given coordinates within the specified distance limit.
+    /// Time complexity: O(log n) average, O(n) worst case.
+    /// </summary>
+    /// <param name="x">X coordinate</param>
+    /// <param name="y">Y coordinate</param>
+    /// <param name="limitDistance">Maximum search distance</param>
+    /// <returns>The nearest node within limit, or null if none found</returns>
+    /// <exception cref="ArgumentOutOfRangeException">Thrown when limitDistance is negative.</exception>
+    public KDTreeData? FindNearest(double x, double y, double limitDistance)
+    {
+        ArgumentOutOfRangeException.ThrowIfNegative(limitDistance);
+
+        if (_root == null)
+            return null;
+
+        double limitDistSquared = limitDistance * limitDistance;
+        var result = FindNearestRecursive(_root, x, y, null, double.MaxValue, limitDistSquared);
+
+        return result.BestNode;
+    }
+
+    /// <summary>
+    /// Recursively finds the nearest node using squared distances to avoid sqrt operations.
+    /// </summary>
+    private static SearchResult FindNearestRecursive(
+        KDTreeNode? node,
+        double x,
+        double y,
+        KDTreeData? bestNode,
+        double bestDistSquared,
+        double limitDistSquared)
+    {
+        if (node == null)
+            return new SearchResult(bestNode, bestDistSquared);
+
+        // Calculate squared distance (avoid sqrt for performance)
+        double dx = node.Node.X - x;
+        double dy = node.Node.Y - y;
+        double distSquared = dx * dx + dy * dy;
+
+        // Update best if this node is closer and within limit
+        if (distSquared < bestDistSquared && distSquared <= limitDistSquared)
+        {
+            bestNode = node.Node;
+            bestDistSquared = distSquared;
+        }
+
+        // Determine which side to search first
+        double delta = node.Axis == 0 ? x - node.Node.X : y - node.Node.Y;
+        KDTreeNode? nearSide = delta < 0 ? node.Left : node.Right;
+        KDTreeNode? farSide = delta < 0 ? node.Right : node.Left;
+
+        // Search near side
+        var result = FindNearestRecursive(nearSide, x, y, bestNode, bestDistSquared, limitDistSquared);
+        bestNode = result.BestNode;
+        bestDistSquared = result.BestDistSquared;
+
+        // Only search far side if it could contain a closer point
+        double deltaSquared = delta * delta;
+        if (deltaSquared < bestDistSquared)
+        {
+            result = FindNearestRecursive(farSide, x, y, bestNode, bestDistSquared, limitDistSquared);
+            bestNode = result.BestNode;
+            bestDistSquared = result.BestDistSquared;
+        }
+
+        return new SearchResult(bestNode, bestDistSquared);
+    }
+
+    /// <summary>
+    /// Finds all nodes within the specified radius from the given coordinates.
+    /// </summary>
+    /// <param name="x">X coordinate</param>
+    /// <param name="y">Y coordinate</param>
+    /// <param name="radius">Search radius</param>
+    /// <returns>List of all nodes within the radius</returns>
+    public List<KDTreeData> FindInRadius(double x, double y, double radius)
+    {
+        ArgumentOutOfRangeException.ThrowIfNegative(radius);
+
+        if (_root == null)
+            return [];
+
+        var result = new List<KDTreeData>();
+        double radiusSquared = radius * radius;
+        FindInRadiusRecursive(_root, x, y, radiusSquared, result);
+        return result;
+    }
+
+    /// <summary>
+    /// Recursively finds all nodes within radius.
+    /// </summary>
+    private static void FindInRadiusRecursive(
+        KDTreeNode? node,
+        double x,
+        double y,
+        double radiusSquared,
+        List<KDTreeData> result)
+    {
+        if (node == null)
+            return;
+
+        double dx = node.Node.X - x;
+        double dy = node.Node.Y - y;
+        double distSquared = dx * dx + dy * dy;
+
+        if (distSquared <= radiusSquared)
+            result.Add(node.Node);
+
+        double delta = node.Axis == 0 ? x - node.Node.X : y - node.Node.Y;
+        double deltaSquared = delta * delta;
+
+        // Search both sides if sphere intersects splitting plane
+        if (delta < 0)
+        {
+            FindInRadiusRecursive(node.Left, x, y, radiusSquared, result);
+            if (deltaSquared <= radiusSquared)
+                FindInRadiusRecursive(node.Right, x, y, radiusSquared, result);
+        }
+        else
+        {
+            FindInRadiusRecursive(node.Right, x, y, radiusSquared, result);
+            if (deltaSquared <= radiusSquared)
+                FindInRadiusRecursive(node.Left, x, y, radiusSquared, result);
+        }
+    }
+
+    /// <summary>
+    /// Internal struct to return search results without allocations.
+    /// </summary>
+    private readonly struct SearchResult(KDTreeData? bestNode, double bestDistSquared)
+    {
+        public readonly KDTreeData? BestNode = bestNode;
+        public readonly double BestDistSquared = bestDistSquared;
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.Common/Models/KDTreeNode.cs

@@ -0,0 +1,33 @@
+namespace RobotNet10.Common.Models;
+
+/// <summary>
+/// Represents a node in the KD-Tree structure.
+/// Immutable to prevent structural corruption.
+/// </summary>
+/// <remarks>
+/// Initializes a new KD-Tree node.
+/// </remarks>
+public class KDTreeNode(KDTreeData node, int axis, KDTreeNode? left = null, KDTreeNode? right = null)
+{
+    /// <summary>
+    /// The spatial node stored at this tree node.
+    /// </summary>
+    public KDTreeData Node { get; } = node ?? throw new ArgumentNullException(nameof(node));
+
+    /// <summary>
+    /// Left child (contains points with smaller values along the split axis).
+    /// </summary>
+    public KDTreeNode? Left { get; } = left;
+
+    /// <summary>
+    /// Right child (contains points with larger values along the split axis).
+    /// </summary>
+    public KDTreeNode? Right { get; } = right;
+
+    /// <summary>
+    /// The axis used for splitting: 0 for X, 1 for Y.
+    /// </summary>
+    public int Axis { get; } = axis;
+}
+
+public record KDTreeData(string Id, double X, double Y);

srcs/RobotNet10/Commons/RobotNet10.Common/Models/SpaceEdge.cs

@@ -0,0 +1,15 @@
+namespace RobotNet10.Common.Models;
+
+public class SpaceEdge
+{
+    public Guid Id { get; set; }
+    public double StartX { get; set; }
+    public double StartY { get; set; }
+    public double EndX { get; set; }
+    public double EndY { get; set; }
+    public int Degree { get; set; }
+    public double ControlPoint1X { get; set; }
+    public double ControlPoint1Y { get; set; }
+    public double ControlPoint2X { get; set; }
+    public double ControlPoint2Y { get; set; }
+}

srcs/RobotNet10/Commons/RobotNet10.Common/Models/SpaceNode.cs

@@ -0,0 +1,11 @@
+namespace RobotNet10.Common.Models;
+
+public record SpaceNode(double X, double Y)
+{
+    public double DistanceTo(SpaceNode other)
+    {
+        double dx = X - other.X;
+        double dy = Y - other.Y;
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.Common/RobotNet10.Common.csproj

@@ -0,0 +1,13 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net10.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+
+	<ItemGroup>
+		<PackageReference Include="NLog" Version="6.1.1" />
+		<PackageReference Include="NLog.Web.AspNetCore" Version="6.1.2" />
+	</ItemGroup>
+</Project>

srcs/RobotNet10/Commons/RobotNet10.Common/SpaceCompute.cs

@@ -0,0 +1,365 @@
+using RobotNet10.Common.Models;
+using System.ComponentModel.DataAnnotations;
+
+namespace RobotNet10.Common;
+
+public class SpaceCompute
+{
+    public static SpaceNode BezierPoint([Range(0, 1)] double t, SpaceEdge edge)
+    {
+        t = Math.Clamp(t, 0.0, 1.0);
+        if (edge.Degree == 1) return new SpaceNode(edge.StartX + t * (edge.EndX - edge.StartX), edge.StartY + t * (edge.EndY - edge.StartY));
+        else if (edge.Degree == 2)
+        {
+            return new((1 - t) * (1 - t) * edge.StartX + 2 * t * (1 - t) * edge.ControlPoint1X + t * t * edge.EndX,
+                       (1 - t) * (1 - t) * edge.StartY + 2 * t * (1 - t) * edge.ControlPoint1Y + t * t * edge.EndY);
+        }
+        else if (edge.Degree == 3)
+        {
+            return new(Math.Pow(1 - t, 3) * edge.StartX + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2X + Math.Pow(t, 3) * edge.EndX,
+                       Math.Pow(1 - t, 3) * edge.StartY + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2Y + Math.Pow(t, 3) * edge.EndY);
+        }
+        return new(edge.EndX, edge.EndY);
+    }
+
+    // Giải phương trình bậc 2: ax² + bx + c = 0
+    public static double[] SolveQuadratic(double a, double b, double c)
+    {
+        var roots = new List<double>();
+
+        if (Math.Abs(a) < 1e-10)
+        {
+            if (Math.Abs(b) > 1e-10)
+            {
+                roots.Add(-c / b);
+            }
+            return [.. roots];
+        }
+
+        double discriminant = b * b - 4 * a * c;
+
+        if (discriminant >= 0)
+        {
+            double sqrtD = Math.Sqrt(discriminant);
+            roots.Add((-b + sqrtD) / (2 * a));
+            roots.Add((-b - sqrtD) / (2 * a));
+        }
+
+        return [.. roots];
+    }
+
+    // Giải phương trình bậc 3: ax³ + bx² + cx + d = 0
+    public static double[] SolveCubic(double a, double b, double c, double d)
+    {
+        if (Math.Abs(a) < 1e-10)
+        {
+            // Phương trình bậc 2
+            return SolveQuadratic(b, c, d);
+        }
+
+        // Chuẩn hóa về dạng x³ + px + q = 0 (depressed cubic)
+        b /= a;
+        c /= a;
+        d /= a;
+
+        double p = (3 * c - b * b) / 3;
+        double q = (2 * b * b * b - 9 * b * c + 27 * d) / 27;
+
+        double discriminant = q * q / 4 + p * p * p / 27;
+
+        var roots = new List<double>();
+
+        if (discriminant >= 0)
+        {
+            // Một nghiệm thực
+            double sqrtD = Math.Sqrt(discriminant);
+            double term1 = -q / 2 + sqrtD;
+            double term2 = -q / 2 - sqrtD;
+            
+            // Tính căn bậc 3, xử lý số âm
+            double u = term1 >= 0 
+                ? Math.Pow(term1, 1.0 / 3) 
+                : -Math.Pow(-term1, 1.0 / 3);
+            double v = term2 >= 0 
+                ? Math.Pow(term2, 1.0 / 3) 
+                : -Math.Pow(-term2, 1.0 / 3);
+            
+            double root = u + v - b / 3;
+            
+            if (!double.IsNaN(root) && !double.IsInfinity(root))
+            {
+                roots.Add(root);
+            }
+        }
+        else
+        {
+            // Ba nghiệm thực (trường hợp lượng giác)
+            double r = Math.Sqrt(-p * p * p / 27);
+            
+            if (r > 1e-10)
+            {
+                double acosArg = -q / (2 * r);
+                // Clamp acosArg vào [-1, 1] để tránh NaN
+                acosArg = Math.Max(-1.0, Math.Min(1.0, acosArg));
+                double phi = Math.Acos(acosArg);
+                double temp = 2 * Math.Pow(r, 1.0 / 3);
+
+                roots.Add(temp * Math.Cos(phi / 3) - b / 3);
+                roots.Add(temp * Math.Cos((phi + 2 * Math.PI) / 3) - b / 3);
+                roots.Add(temp * Math.Cos((phi + 4 * Math.PI) / 3) - b / 3);
+            }
+        }
+
+        return [.. roots];
+    }
+
+
+    // Phương pháp chính xác hơn sử dụng giải phương trình bậc 3
+    public static (double distance, double time) DistanceToQuadraticBezier(SpaceNode nodeRef, SpaceEdge edge)
+    {
+        // Đạo hàm của hàm khoảng cách bình phương theo t
+        // Giải phương trình bậc 3: d/dt[|P(t) - G|²] = 0
+
+        // Quadratic Bezier: P(t) = (1-t)²P₀ + 2t(1-t)P₁ + t²P₂
+        // Đạo hàm: P'(t) = 2(1-t)(P₁-P₀) + 2t(P₂-P₁)
+        // Khoảng cách bình phương: D(t) = |P(t) - G|²
+        // Đạo hàm: D'(t) = 2(P(t) - G) · P'(t) = 0
+        
+        // Viết lại: P(t) = P₀ + 2t(P₁-P₀) + t²(P₂-2P₁+P₀)
+        // P'(t) = 2(P₁-P₀) + 2t(P₂-2P₁+P₀)
+        // (P(t) - G) · P'(t) = 0
+        
+        double ax = edge.StartX - 2 * edge.ControlPoint1X + edge.EndX;
+        double ay = edge.StartY - 2 * edge.ControlPoint1Y + edge.EndY;
+        double bx = 2 * (edge.ControlPoint1X - edge.StartX);
+        double by = 2 * (edge.ControlPoint1Y - edge.StartY);
+        double cx = edge.StartX - nodeRef.X;
+        double cy = edge.StartY - nodeRef.Y;
+
+        // Hệ số của phương trình bậc 3: At³ + Bt² + Ct + D = 0
+        // Từ: (P(t) - G) · P'(t) = 0
+        // 
+        // Với: P(t) = P₀ + bt + at², P'(t) = b + 2at
+        //      c = P₀ - G
+        //      (c + bt + at²) · (b + 2at) = 0
+        //
+        // Khai triển tích vô hướng:
+        // c·b + c·2at + bt·b + bt·2at + at²·b + at²·2at = 0
+        // = c·b + 2c·a·t + b·b·t + 2a·b·t² + a·b·t² + 2a·a·t³ = 0
+        // = c·b + (2c·a + b·b)·t + 3a·b·t² + 2a·a·t³ = 0
+        //
+        // Vậy: A = 2a·a = 2(aₓ² + aᵧ²)
+        //      B = 3a·b = 3(aₓbₓ + aᵧbᵧ)
+        //      C = 2c·a + b·b = 2(cₓaₓ + cᵧaᵧ) + (bₓ² + bᵧ²)
+        //      D = c·b = cₓbₓ + cᵧbᵧ
+        double A = 2 * (ax * ax + ay * ay);
+        double B = 3 * (ax * bx + ay * by);
+        double C = 2 * (ax * cx + ay * cy) + (bx * bx + by * by);
+        double D = bx * cx + by * cy;
+
+        // Tìm các nghiệm của phương trình bậc 3
+        var roots = SolveCubic(A, B, C, D);
+
+        // Tính khoảng cách tại 2 đầu mút
+        double distStart = nodeRef.DistanceTo(new(edge.StartX, edge.StartY));
+        double distEnd = nodeRef.DistanceTo(new(edge.EndX, edge.EndY));
+        
+        // Khởi tạo với giá trị tại 2 đầu mút
+        double minDist = Math.Min(distStart, distEnd);
+        double time = distStart < distEnd ? 0 : 1;
+
+        // Kiểm tra khoảng cách tại các điểm tới hạn (nghiệm của đạo hàm)
+        foreach (double t in roots)
+        {
+            // Bỏ qua NaN và Infinity
+            if (double.IsNaN(t) || double.IsInfinity(t)) continue;
+            
+            // Chỉ xét nghiệm trong khoảng [0, 1]
+            if (t >= 0 && t <= 1)
+            {
+                SpaceNode p = BezierPoint(t, edge);
+                double dist = nodeRef.DistanceTo(p);
+                
+                // Cập nhật minDist và time nếu tìm thấy khoảng cách nhỏ hơn
+                if (dist < minDist)
+                {
+                    minDist = dist;
+                    time = t;
+                }
+            }
+        }
+
+        // Kiểm tra lại khoảng cách tại 2 đầu mút (chỉ cập nhật nếu nhỏ hơn)
+        // Lưu ý: Không ghi đè nếu minDist đã được cập nhật từ nghiệm trong (0,1)
+        if (distStart < minDist)
+        {
+            minDist = distStart;
+            time = 0;
+        }
+        if (distEnd < minDist)
+        {
+            minDist = distEnd;
+            time = 1;
+        }
+
+        return (minDist, time);
+    }
+
+    // Phương pháp lấy mẫu - Đơn giản nhưng chậm hơn
+    public static (double distance, double time) DistanceToCubicBezier(SpaceNode nodeRef, SpaceEdge edge)
+    {
+        double bestT = 0;
+        double minDistance = Math.Sqrt(Math.Pow(nodeRef.X - edge.StartX, 2) + Math.Pow(nodeRef.Y - edge.StartY, 2));
+        var length = GetEdgeLength(edge, 0.3);
+        double step = 0.3 / (length == 0 ? 0.1 : length);
+
+        // Bước 1: Lấy mẫu thô
+        for (double t = 0; t <= 1; t += step)
+        {
+            SpaceNode p = BezierPoint(t, edge);
+            double dist = nodeRef.DistanceTo(p);
+
+            if (dist < minDistance)
+            {
+                minDistance = dist;
+                bestT = t;
+            }
+        }
+
+        // Bước 2: Tối ưu hóa chính xác hơn
+        double epsilon = 1e-6;
+        step = 0.01;
+
+        for (int iter = 0; iter < 10; iter++)
+        {
+            double t1 = Math.Max(0, bestT - step);
+            double t2 = Math.Min(1, bestT + step);
+
+            double d0 = nodeRef.DistanceTo(BezierPoint(t1, edge));
+            double d1 = nodeRef.DistanceTo(BezierPoint(bestT, edge));
+            double d2 = nodeRef.DistanceTo(BezierPoint(t2, edge));
+
+            if (d0 < d1)
+            {
+                bestT = t1;
+                minDistance = d0;
+            }
+            else if (d2 < d1)
+            {
+                bestT = t2;
+                minDistance = d2;
+            }
+            else
+            {
+                step *= 0.5;
+            }
+
+            if (step < epsilon) break;
+        }
+
+        return (minDistance, bestT);
+    }
+
+    public static (double x, double y, double distance, double time) GetProjectionOnEdge(double x, double y, SpaceEdge edge)
+    {
+        if (edge.Degree == 2)
+        {
+            (double distance, double time) = DistanceToQuadraticBezier(new(x, y), edge);
+            var node = BezierPoint(time, edge);
+            return (node.X, node.Y, distance, time);
+        }
+        else if (edge.Degree == 3)
+        {
+            (double distance, var time) = DistanceToCubicBezier(new(x, y), edge);
+            var node = BezierPoint(time, edge);
+            return (node.X, node.Y, distance, time);
+        }
+        else
+        {
+            double time = 0;
+            var edgeLengthSquared = Math.Pow(edge.StartX - edge.EndX, 2) + Math.Pow(edge.StartY - edge.EndY, 2);
+            if (edgeLengthSquared > 0)
+            {
+                time = Math.Max(0, Math.Min(1, ((x - edge.StartX) * (edge.EndX - edge.StartX) + (y - edge.StartY) * (edge.EndY - edge.StartY)) / edgeLengthSquared));
+            }
+
+            double nearestX = edge.StartX + time * (edge.EndX - edge.StartX);
+            double nearestY = edge.StartY + time * (edge.EndY - edge.StartY);
+
+            return (nearestX, nearestY, Math.Sqrt(Math.Pow(x - nearestX, 2) + Math.Pow(y - nearestY, 2)), time);
+        }
+    }
+
+    public static double GetEdgeLength(SpaceEdge edge, double resolution)
+    {
+        var lineLength = Math.Sqrt(Math.Pow(edge.StartX - edge.EndX, 2) + Math.Pow(edge.StartY - edge.EndY, 2));
+        if (edge.Degree == 1)
+        {
+            return lineLength;
+        }
+        else if (edge.Degree == 2)
+        {
+            if (lineLength <= 0) return 0;
+            double step = resolution / lineLength;
+            double distance = 0;
+
+            for (double t = step; t <= 1.001; t += step)
+            {
+                var timePoint = BezierPoint(t - step, edge);
+                var lastTimePoint = BezierPoint(t, edge);
+                distance += Math.Sqrt(Math.Pow(timePoint.X - lastTimePoint.X, 2) + Math.Pow(timePoint.Y - lastTimePoint.Y, 2));
+            }
+
+            return Math.Round(distance, 3);
+        }
+        else
+        {
+            if (lineLength <= 0) return 0;
+            double step = resolution / lineLength;
+            double distance = 0;
+            for (double t = step; t <= 1.001; t += step)
+            {
+                var sTime = t - step;
+                var timePoint = BezierPoint(1 - sTime,  edge);
+                sTime = t;
+                var lastTimePoint = BezierPoint(1 - sTime, edge);
+
+                distance += Math.Sqrt(Math.Pow(timePoint.X - lastTimePoint.X, 2) + Math.Pow(timePoint.Y - lastTimePoint.Y, 2));
+            }
+            return Math.Round(distance, 3);
+        }
+    }
+
+    public static double GetVectorAngle(double originNodeX, double originNodeY, double vector1X, double vector1Y, double vector2X, double vector2Y)
+    {
+        double BA_x = vector1X - originNodeX;
+        double BA_y = vector1Y - originNodeY;
+        double BC_x = vector2X - originNodeX;
+        double BC_y = vector2Y - originNodeY;
+        // Tính độ dài của các vector AB và BC
+        double lengthAB = Math.Sqrt(BA_x * BA_x + BA_y * BA_y);
+        double lengthBC = Math.Sqrt(BC_x * BC_x + BC_y * BC_y);
+        // Tính tích vô hướng của AB và BC
+        double dotProduct = BA_x * BC_x + BA_y * BC_y;
+        if (lengthAB * lengthBC == 0) return 0;
+        if (dotProduct / (lengthAB * lengthBC) > 1) return 0;
+        if (dotProduct / (lengthAB * lengthBC) < -1) return 180;
+        return Math.Acos(dotProduct / (lengthAB * lengthBC)) * (180.0 / Math.PI);
+    }
+
+    public static double NormalizeDegreeAngle(double angle)
+    {
+        angle = angle % 360;
+        if (angle > 180) angle -= 360;
+        else if (angle < -180) angle += 360;
+        return angle;
+    }
+
+    public static double NormalizeRadianAngle(double angle)
+    {
+        angle = angle % (2 * Math.PI);
+        if (angle > Math.PI) angle -= (2 * Math.PI);
+        else if (angle < -Math.PI) angle += (2 * Math.PI);
+        return angle;
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.Common/WatchThread.cs

@@ -0,0 +1,135 @@
+using Microsoft.Extensions.Logging;
+using System.Diagnostics;
+
+namespace RobotNet10.Common;
+
+public class WatchThread<T>(int Interval, Action Callback, ILogger<T>? Logger, ThreadPriority Priority = ThreadPriority.Highest) : IDisposable where T : class
+{
+    public bool Disposed;
+
+    private Thread? Thread;
+    private CancellationTokenSource? ThreadCts;
+
+    private long NextDueTime;
+    private readonly Lock Lock = new();
+    private void Handler(CancellationToken cancellationToken)
+    {
+        while (!cancellationToken.IsCancellationRequested)
+        {
+            try
+            {
+                bool shouldRun = false;
+                lock (Lock)
+                {
+                    if (Disposed) return;
+                    long now = GetCurrentTimeMs();
+                    if (now >= NextDueTime)
+                    {
+                        shouldRun = true;
+                        long scheduledTime = NextDueTime;
+                        NextDueTime += Interval;
+
+                        if (now - scheduledTime > Interval / 2)
+                        {
+                            NextDueTime = now + Interval;
+                            if (Logger is not null && Logger.IsEnabled(LogLevel.Warning)) Logger.LogWarning("WatchThread Warning: Elapsed time {peak}ms exceeds interval {Interval}ms.", now - scheduledTime + Interval, Interval);
+                        }
+                    }
+                }
+
+                if (shouldRun)
+                {
+                    try { Callback.Invoke(); }
+                    catch (Exception ex) { if (Logger is not null && Logger.IsEnabled(LogLevel.Error)) Logger.LogError("Callback error: {ex}", ex.Message); }
+                }
+
+                lock (Lock)
+                {
+                    if (Disposed) return;
+                    long now = GetCurrentTimeMs();
+                    long delay = NextDueTime - now;
+                    if (delay < 0) delay = 0;
+                    Thread.Sleep((int)delay);
+                }
+            }
+            catch (Exception ex)
+            {
+                if (Logger is not null && Logger.IsEnabled(LogLevel.Error)) Logger.LogError("WatchThread Error: {ex}", ex.Message);
+                Thread.Sleep(Interval);
+            }
+        }
+    }
+
+    private static long GetCurrentTimeMs()
+    {
+        return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
+    }
+
+    public void Start()
+    {
+        lock (Lock)
+        {
+            if (!Disposed)
+            {
+                if (Thread?.IsAlive == true) return;
+
+                NextDueTime = GetCurrentTimeMs() + Interval;
+                ThreadCts = new CancellationTokenSource();
+                Thread = new Thread(() => Handler(ThreadCts.Token))
+                {
+                    Priority = Priority,
+                    IsBackground = false,
+                    Name = $"WatchThread-{typeof(T).Name}"
+                };
+                Thread.Start();
+            }
+            else throw new ObjectDisposedException(nameof(WatchThread<>));
+        }
+    }
+
+    public void Stop()
+    {
+        Thread? threadToJoin;
+        lock (Lock)
+        {
+            if (Thread == null) return;
+            ThreadCts?.Cancel();
+            threadToJoin = Thread;
+        }
+
+        // If Stop() is called from within the Callback (same thread), skip Join to avoid deadlock
+        if (threadToJoin != null && threadToJoin != Thread.CurrentThread)
+        {
+            if (!threadToJoin.Join(TimeSpan.FromSeconds(5)))
+            {
+                Logger?.LogWarning("Thread did not stop gracefully");
+            }
+        }
+
+        lock (Lock)
+        {
+            ThreadCts?.Dispose();
+            ThreadCts = null;
+            Thread = null;
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (Disposed) return;
+
+        Disposed = true; 
+        if (disposing) Stop();
+    }
+
+    ~WatchThread()
+    {
+        Dispose(false);
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.Common/WatchThreadAsync.cs

@@ -0,0 +1,135 @@
+using Microsoft.Extensions.Logging;
+using System.Diagnostics;
+
+namespace RobotNet10.Common;
+
+public class WatchThreadAsync<T>(int Interval, Func<Task> Callback, ILogger<T>? Logger, ThreadPriority Priority = ThreadPriority.Highest) : IDisposable where T : class
+{
+    public bool Disposed;
+
+    private Thread? Thread;
+    private CancellationTokenSource? ThreadCts;
+
+    private long NextDueTime;
+    private readonly Lock Lock = new();
+    private async Task Handler(CancellationToken cancellationToken)
+    {
+        while (!cancellationToken.IsCancellationRequested)
+        {
+            try
+            {
+                bool shouldRun = false;
+                lock (Lock)
+                {
+                    if (Disposed) return;
+                    long now = GetCurrentTimeMs();
+                    if (now >= NextDueTime)
+                    {
+                        shouldRun = true;
+                        long scheduledTime = NextDueTime;
+                        NextDueTime += Interval;
+
+                        if (now - scheduledTime > Interval / 2)
+                        {
+                            NextDueTime = now + Interval;
+                            if (Logger is not null && Logger.IsEnabled(LogLevel.Warning)) Logger.LogWarning("WatchThreadAsync Warning: Elapsed time {peak}ms exceeds interval {Interval}ms.", now - scheduledTime + Interval, Interval);
+                        }
+                    }
+                }
+
+                if (shouldRun)
+                {
+                    try { await Callback.Invoke(); }
+                    catch (Exception ex) { if (Logger is not null && Logger.IsEnabled(LogLevel.Error)) Logger.LogError("Callback error: {ex}", ex.Message); }
+                }
+
+                lock (Lock)
+                {
+                    if (Disposed) return;
+                    long now = GetCurrentTimeMs();
+                    long delay = NextDueTime - now;
+                    if (delay < 0) delay = 0;
+                    Thread.Sleep((int)delay);
+                }
+            }
+            catch (Exception ex)
+            {
+                if (Logger is not null && Logger.IsEnabled(LogLevel.Error)) Logger.LogError("WatchThreadAsync Error: {ex}", ex.Message);
+                Thread.Sleep(Interval);
+            }
+        }
+    }
+
+    private static long GetCurrentTimeMs()
+    {
+        return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
+    }
+
+    public void Start()
+    {
+        lock (Lock)
+        {
+            if (!Disposed)
+            {
+                if (Thread?.IsAlive == true) return;
+
+                NextDueTime = GetCurrentTimeMs() + Interval;
+                ThreadCts = new CancellationTokenSource();
+                Thread = new Thread(async () => await Handler(ThreadCts.Token))
+                {
+                    Priority = Priority,
+                    IsBackground = false,
+                    Name = $"WatchThreadAsync-{typeof(T).Name}"
+                };
+                Thread.Start();
+            }
+            else throw new ObjectDisposedException(nameof(WatchThreadAsync<>));
+        }
+    }
+
+    public void Stop()
+    {
+        Thread? threadToJoin;
+        lock (Lock)
+        {
+            if (Thread == null) return;
+            ThreadCts?.Cancel();
+            threadToJoin = Thread;
+        }
+
+        // If Stop() is called from within the Callback (same thread), skip Join to avoid deadlock
+        if (threadToJoin != null && threadToJoin != Thread.CurrentThread)
+        {
+            if (!threadToJoin.Join(TimeSpan.FromSeconds(5)))
+            {
+                Logger?.LogWarning("Thread did not stop gracefully");
+            }
+        }
+
+        lock (Lock)
+        {
+            ThreadCts?.Dispose();
+            ThreadCts = null;
+            Thread = null;
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (Disposed) return;
+
+        Disposed = true; 
+        if (disposing) Stop();
+    }
+
+    ~WatchThreadAsync()
+    {
+        Dispose(false);
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.Common/WatchTimer.cs

@@ -0,0 +1,112 @@
+using Microsoft.Extensions.Logging;
+using System.Diagnostics;
+
+namespace RobotNet10.Common;
+
+public class WatchTimer<T>(int Interval, Action Callback, ILogger<T>? Logger) : IDisposable where T : class
+{
+    private Timer? Timer;
+    public bool Disposed;
+
+    private long NextDueTime;
+    private readonly Lock Lock = new();
+
+    private void Handler(object? state)
+    {
+        try
+        {
+            bool shouldRun = false;
+            lock (Lock)
+            {
+                if (Disposed) return;
+                long now = GetCurrentTimeMs();
+                if (now >= NextDueTime)
+                {
+                    shouldRun = true;
+                    long scheduledTime = NextDueTime;
+                    NextDueTime += Interval;
+
+                    if (now - scheduledTime > Interval / 2)
+                    {
+                        NextDueTime = now + Interval;
+                        if(Logger is not null && Logger.IsEnabled(LogLevel.Warning)) Logger.LogWarning("WatchTimer Warning: Elapsed time {peak}ms exceeds interval {Interval}ms.", now - scheduledTime + Interval, Interval);
+                    }
+                }
+            }
+
+            if (shouldRun)
+            {
+                try { Callback.Invoke(); }
+                catch (Exception ex) { if (Logger is not null && Logger.IsEnabled(LogLevel.Error)) Logger.LogError("Callback error: {ex}", ex.Message); }
+            }
+
+            lock (Lock)
+            {
+                if (Disposed) return;
+                long now = GetCurrentTimeMs();
+                long delay = NextDueTime - now;
+                if (delay < 0) delay = 0;
+                Timer?.Change(delay, Timeout.Infinite);
+            }
+        }
+        catch (Exception ex)
+        {
+            if(Logger is not null && Logger.IsEnabled(LogLevel.Error)) Logger.LogError("WatchTimer Error: {ex}", ex.Message);
+            Timer?.Change(Interval, Timeout.Infinite);
+        }
+    }
+
+    public void Start()
+    {
+        if (!Disposed)
+        {
+            lock (Lock)
+            {
+                NextDueTime = GetCurrentTimeMs() + Interval;
+                Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
+                Timer.Change(Interval, Timeout.Infinite);
+            }
+        }
+        else throw new ObjectDisposedException(nameof(WatchTimer<>));
+    }
+
+    public void Stop()
+    {
+        if (Disposed) return;
+
+        if (Timer != null)
+        {
+            lock (Lock)
+            {
+                Timer.Change(Timeout.Infinite, Timeout.Infinite);
+                Timer.Dispose();
+                Timer = null;
+            }
+        }
+    }
+
+    private static long GetCurrentTimeMs()
+    {
+        return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (Disposed) return;
+
+        if (disposing) Stop();
+
+        Disposed = true;
+    }
+
+    ~WatchTimer()
+    {
+        Dispose(false);
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.Common/WatchTimerAsync.cs

@@ -0,0 +1,114 @@
+using Microsoft.Extensions.Logging;
+using System.Diagnostics;
+
+namespace RobotNet10.Common;
+
+public class WatchTimerAsync<T>(int interval, Func<Task> Callback, ILogger<T>? Logger) : IDisposable where T : class
+{
+    private Timer? Timer;
+    public bool Disposed;
+
+    private long NextDueTime;
+    private readonly Lock Lock = new();
+
+    public int Interval => interval;
+
+    private async void Handler(object? state)
+    {
+        try
+        {
+            bool shouldRun = false;
+            lock (Lock)
+            {
+                if (Disposed) return;
+                long now = GetCurrentTimeMs();
+                if (now >= NextDueTime)
+                {
+                    shouldRun = true;
+                    long scheduledTime = NextDueTime;
+                    NextDueTime += interval;
+
+                    if (now - scheduledTime > interval / 2)
+                    {
+                        NextDueTime = now + interval;
+                        if (Logger is not null && Logger.IsEnabled(LogLevel.Warning)) Logger.LogWarning("WatchTimerAsync Warning: Elapsed time {peak}ms exceeds interval {interval}ms.", now - scheduledTime + interval, interval);
+                    }
+                }
+            }
+
+            if (shouldRun)
+            {
+                try { await Callback.Invoke(); }
+                catch (Exception ex) { if (Logger is not null && Logger.IsEnabled(LogLevel.Error)) Logger.LogError("Callback error: {ex}", ex.Message); }
+            }
+
+            lock (Lock)
+            {
+                if (Disposed) return;
+                long now = GetCurrentTimeMs();
+                long delay = NextDueTime - now;
+                if (delay < 0) delay = 0;
+                Timer?.Change(delay, Timeout.Infinite);
+            }
+        }
+        catch (Exception ex)
+        {
+            if (Logger is not null && Logger.IsEnabled(LogLevel.Error)) Logger.LogError("WatchTimerAsync Error: {ex}", ex.Message);
+            Timer?.Change(interval, Timeout.Infinite);
+        }
+    }
+
+    public void Start()
+    {
+        if (!Disposed)
+        {
+            lock (Lock)
+            {
+                NextDueTime = GetCurrentTimeMs() + interval;
+                Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
+                Timer.Change(interval, Timeout.Infinite);
+            }
+        }
+        else throw new ObjectDisposedException(nameof(WatchTimerAsync<>));
+    }
+
+    public void Stop()
+    {
+        if (Disposed) return;
+
+        if (Timer != null)
+        {
+            lock (Lock)
+            {
+                Timer.Change(Timeout.Infinite, Timeout.Infinite);
+                Timer.Dispose();
+                Timer = null;
+            }
+        }
+    }
+
+    private static long GetCurrentTimeMs()
+    {
+        return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (Disposed) return;
+
+        if (disposing) Stop();
+
+        Disposed = true;
+    }
+
+    ~WatchTimerAsync()
+    {
+        Dispose(false);
+    }
+}