87 lines
3.2 KiB
C#
87 lines
3.2 KiB
C#
using RobotNet.MapShares;
|
|
using RobotNet.MapShares.Dtos;
|
|
using RobotNet.MapShares.Enums;
|
|
using RobotNet.RobotShares.Enums;
|
|
using RobotNet.Shares;
|
|
|
|
namespace RobotNet.RobotManager.Services.Planner.Space;
|
|
|
|
public class MapCompute
|
|
{
|
|
public static double DistanceToCurveEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
|
|
{
|
|
double dMin = Math.Sqrt(Math.Pow(x - startNode.X, 2) + Math.Pow(y - startNode.Y, 2));
|
|
var length = MapEditorHelper.GetEdgeLength(new()
|
|
{
|
|
X1 = startNode.X,
|
|
Y1 = startNode.Y,
|
|
X2 = endNode.X,
|
|
Y2 = endNode.Y,
|
|
ControlPoint1X = edge.ControlPoint1X,
|
|
ControlPoint1Y = edge.ControlPoint1Y,
|
|
ControlPoint2X = edge.ControlPoint2X,
|
|
ControlPoint2Y = edge.ControlPoint2Y,
|
|
TrajectoryDegree = edge.TrajectoryDegree,
|
|
});
|
|
double step = 0.1 / (length == 0 ? 0.1 : length);
|
|
|
|
for (double t = 0; t <= 1; t += step)
|
|
{
|
|
(double curveX, double curveY) = MapEditorHelper.Curve(t, new()
|
|
{
|
|
TrajectoryDegree = edge.TrajectoryDegree,
|
|
ControlPoint1X = edge.ControlPoint1X,
|
|
ControlPoint1Y = edge.ControlPoint1Y,
|
|
ControlPoint2X = edge.ControlPoint2X,
|
|
ControlPoint2Y= edge.ControlPoint2Y,
|
|
X1 = startNode.X,
|
|
Y1 = startNode.Y,
|
|
X2 = endNode.X,
|
|
Y2 = endNode.Y,
|
|
});
|
|
double d = Math.Sqrt(Math.Pow(x - curveX, 2) + Math.Pow(y - curveY, 2));
|
|
if (d < dMin) dMin = d;
|
|
}
|
|
|
|
return dMin;
|
|
}
|
|
|
|
public static MessageResult<double> DistanceToEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
|
|
{
|
|
if (edge.TrajectoryDegree == TrajectoryDegree.One)
|
|
{
|
|
double time = 0;
|
|
var edgeLengthSquared = Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2);
|
|
if (edgeLengthSquared > 0)
|
|
{
|
|
time = Math.Max(0, Math.Min(1, ((x - startNode.X) * (endNode.X - startNode.X) + (y - startNode.Y) * (endNode.Y - startNode.Y)) / edgeLengthSquared));
|
|
}
|
|
|
|
double nearestX = startNode.X + time * (endNode.X - startNode.X);
|
|
double nearestY = startNode.Y + time * (endNode.Y - startNode.Y);
|
|
|
|
return new(true) { Data = Math.Sqrt(Math.Pow(x - nearestX, 2) + Math.Pow(y - nearestY, 2)) };
|
|
}
|
|
else
|
|
{
|
|
return new(true) { Data = DistanceToCurveEdge(x, y, edge, startNode, endNode) };
|
|
}
|
|
}
|
|
|
|
public static Direction GetNodeDirection(RobotDirection robotDirection) =>
|
|
robotDirection switch
|
|
{
|
|
RobotDirection.FORWARD => Direction.FORWARD,
|
|
RobotDirection.BACKWARD => Direction.BACKWARD,
|
|
_ => Direction.NONE
|
|
};
|
|
|
|
public static RobotDirection GetRobotDirection(Direction nodeDirection) =>
|
|
nodeDirection switch
|
|
{
|
|
Direction.FORWARD => RobotDirection.FORWARD,
|
|
Direction.BACKWARD => RobotDirection.BACKWARD,
|
|
_ => RobotDirection.NONE
|
|
};
|
|
}
|