RobotNet/RobotNet.RobotManager/Services/Simulation/Algorithm/PurePursuit.cs

using RobotNet.RobotManager.Services.Simulation.Models;
using RobotNet.RobotShares.Enums;

namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
public class PurePursuit
{
    private double MaxAngularVelocity = 1.5;
    private double LookaheadDistance = 0.5;
    private List<NavigationNode> Waypoints_Value = [];
    public int OnNodeIndex = 0;
    public int GoalIndex = 0;
    public NavigationNode? Goal;

    public PurePursuit WithLookheadDistance(double distance)
    {
        LookaheadDistance = distance;
        return this;
    }

    public PurePursuit WithMaxAngularVelocity(double vel)
    {
        MaxAngularVelocity = vel;
        return this;
    }

    public PurePursuit WithPath(NavigationNode[] path)
    {
        Waypoints_Value = [.. path];
        return this;
    }

    public void UpdatePath(NavigationNode[] path)
    {
        Waypoints_Value = [.. path];
    }

    public void UpdateGoal(NavigationNode goal)
    {
        Goal = goal;
        GoalIndex = Waypoints_Value.IndexOf(Goal);
    }

    public (NavigationNode node, int index) GetOnNode(double x, double y)
    {
        double minDistance = double.MaxValue;
        NavigationNode onNode = Waypoints_Value[0];
        int index = 0;
        for (int i = 1; i < Waypoints_Value.Count; i++)
        {
            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
            if (distance < minDistance)
            {
                onNode = Waypoints_Value[i];
                minDistance = distance;
                index = i;
            }
        }
        return (onNode, index);
    }

    private (NavigationNode? node, int index) OnNode(double x, double y)
    {
        if (Waypoints_Value is null || Waypoints_Value.Count == 0) return (null, 0);
        double minDistance = double.MaxValue;
        int index = 0;
        NavigationNode? onNode = null;
        if(Goal is null) return (onNode, index);
        for (int i = OnNodeIndex; i < Waypoints_Value.IndexOf(Goal); i ++)
        {
            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
            if (distance < minDistance)
            {
                onNode = Waypoints_Value[i];
                minDistance = distance;
                index = i;
            }
        }
        return (onNode, index);
    }

    public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
    {
        if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
        NavigationNode? lookaheadStartPt = null;
        var (onNode, index) = OnNode(X_Ref, Y_Ref);
        if (onNode is null || Goal is null) return 0;
        OnNodeIndex = index;
        double lookDistance = 0;
        for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
        {
            lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
            if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
            {
                lookaheadStartPt = Waypoints_Value[i];
                break;
            }
        }
        lookaheadStartPt ??= Goal;
        if (onNode.Direction == RobotDirection.BACKWARD)
        {
            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
            else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
            Angle_Ref += Math.PI;
            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
        }
        var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;

        if (Math.Abs(distance) > Math.PI)
        {
            double minDistance;
            if (distance + Math.PI == 0.0) minDistance = 0.0;
            else
            {
                double data = (distance + Math.PI) / (2 * Math.PI);
                if (data < 0) data = Math.Round(data + 0.5);
                else data = Math.Round(data - 0.5);
                minDistance = distance + Math.PI - data * (2 * Math.PI);
                double checker = 0;
                if (minDistance != 0.0)
                {
                    checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
                }
                if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
                {
                    minDistance = 0.0;
                }
                else if (distance + Math.PI < 0.0)
                {
                    minDistance += Math.PI * 2;
                }
            }
            if (minDistance == 0.0 && distance + Math.PI > 0.0)
            {
                minDistance = Math.PI * 2;
            }
            distance = minDistance - Math.PI;
        }

        var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
        if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
        {
            if (AngularVelocity < 0.0)
            {
                AngularVelocity = -1.0;
            }
            else if (AngularVelocity > 0.0)
            {
                AngularVelocity = 1.0;
            }
            else if (AngularVelocity == 0.0)
            {
                AngularVelocity = 0.0;
            }
            AngularVelocity *= MaxAngularVelocity;
        }
        return AngularVelocity;
    }

}