RobotNet/RobotNet.MapManager/Services/ServerHelper.cs

using RobotNet.MapManager.Data;
using RobotNet.MapShares.Dtos;
using RobotNet.MapShares.Enums;

namespace RobotNet.MapManager.Services;

public class ServerHelper
{
    public static Edge CreateEdge(Map map, Guid startNodeId, Guid endNodeId, TrajectoryDegree trajectoryDegree, double cpX1 = 0, double cpY1 = 0, double cpX2 = 0, double cpY2 = 0)
    {
        return new Edge()
        {
            MapId = map.Id,
            StartNodeId = startNodeId,
            EndNodeId = endNodeId,
            TrajectoryDegree = trajectoryDegree,
            DirectionAllowed = map.EdgeDirectionAllowedDefault,
            RotationAllowed = map.EdgeRotationAllowedDefault,
            MaxSpeed = trajectoryDegree == TrajectoryDegree.One ? map.EdgeStraightMaxSpeedDefault : map.EdgeCurveMaxSpeedDefault,
            MaxRotationSpeed = map.EdgeMaxRotationSpeedDefault,
            MaxHeight = map.EdgeMaxHeightDefault,
            MinHeight = map.EdgeMinHeightDefault,
            Actions = "[]",
            AllowedDeviationXy = map.EdgeAllowedDeviationXyDefault,
            AllowedDeviationTheta = map.EdgeAllowedDeviationThetaDefault,
            ControlPoint1X = cpX1,
            ControlPoint1Y = cpY1,
            ControlPoint2X = cpX2,
            ControlPoint2Y = cpY2,
        };
    }

    public static Node CreateNode(Map map, double x, double y)
    {
        return new Node()
        {
            MapId = map.Id,
            Name = map.NodeNameAutoGenerate ? $"{map.NodeNameTemplateDefault}{++map.NodeCount}" : string.Empty,
            X = x,
            Y = y,
            Theta = 0,
            AllowedDeviationXy = map.NodeAllowedDeviationXyDefault,
            AllowedDeviationTheta = map.NodeAllowedDeviationThetaDefault,
            Actions = "[]",
        };
    }

    public static EdgeDto CreateEdgeDto(Edge edge, Node startNode, Node endNode)
    {
        return new EdgeDto()
        {
            Id = edge.Id,
            MapId = edge.MapId,
            StartNodeId = edge.StartNodeId,
            EndNodeId = edge.EndNodeId,
            TrajectoryDegree = edge.TrajectoryDegree,
            ControlPoint1X = edge.ControlPoint1X,
            ControlPoint1Y = edge.ControlPoint1Y,
            ControlPoint2X = edge.ControlPoint2X,
            ControlPoint2Y = edge.ControlPoint2Y,
            DirectionAllowed = edge.DirectionAllowed,
            RotationAllowed = edge.RotationAllowed,
            MaxSpeed = edge.MaxSpeed,
            MaxRotationSpeed = edge.MaxRotationSpeed,
            MaxHeight = edge.MaxHeight,
            MinHeight = edge.MinHeight,
            Actions = edge.Actions,
            AllowedDeviationXy = edge.AllowedDeviationXy,
            AllowedDeviationTheta = edge.AllowedDeviationTheta,
            StartNode = new NodeDto()
            {
                Id = startNode.Id,
                Name = startNode.Name,
                MapId = startNode.MapId,
                Theta = startNode.Theta,
                X = startNode.X,
                Y = startNode.Y,
                Actions = startNode.Actions,
                AllowedDeviationXy = startNode.AllowedDeviationXy,
                AllowedDeviationTheta = startNode.AllowedDeviationTheta,
            },
            EndNode = new NodeDto()
            {
                Id = endNode.Id,
                Name = endNode.Name,
                MapId = endNode.MapId,
                Theta = endNode.Theta,
                X = endNode.X,
                Y = endNode.Y,
                AllowedDeviationXy = endNode.AllowedDeviationXy,
                AllowedDeviationTheta = endNode.AllowedDeviationTheta,
                Actions = endNode.Actions,
            },
        };
    }

    public static NodeDto CreateNodeDto(Node node)
    {
        return new NodeDto()
        {
            Id = node.Id,
            Name = node.Name,
            MapId = node.MapId,
            Theta = node.Theta,
            X = node.X,
            Y = node.Y,
            AllowedDeviationXy = node.AllowedDeviationXy,
            AllowedDeviationTheta = node.AllowedDeviationTheta,
            Actions = node.Actions,
        };
    }

    public static Edge? GetClosesEdge(double x, double y, IEnumerable<Node> nodes, IEnumerable<Edge> edges, double allowDistance)
    {
        double minDistance = double.MaxValue;
        Edge? edgeResult = null;
        foreach (var edge in edges)
        {
            if (edge is not null)
            {
                var startNode = nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
                var endNode = nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
                if (startNode is null || endNode is null) continue;
                var distance = DistanceToSegment(x, y, startNode.X, startNode.Y, endNode.X, endNode.Y);
                if (distance < minDistance)
                {
                    minDistance = distance;
                    edgeResult = edge;
                }
            }
        }
        if (minDistance > allowDistance) return null;
        return edgeResult;
    }

    private static double DistanceToSegment(double x, double y, double x1, double y1, double x2, double y2)
    {
        double dx = x2 - x1;
        double dy = y2 - y1;

        double segmentLengthSquared = dx * dx + dy * dy;

        double t;
        if (segmentLengthSquared == 0)
        {
            t = 0;
        }
        else
        {
            t = ((x - x1) * dx + (y - y1) * dy) / segmentLengthSquared;
            t = Math.Max(0, Math.Min(1, t));
        }

        double nearestX = x1 + t * dx;
        double nearestY = y1 + t * dy;

        double distanceSquared = (x - nearestX) * (x - nearestX) + (y - nearestY) * (y - nearestY);

        return Math.Sqrt(distanceSquared);
    }
}