418 lines
16 KiB
C#
418 lines
16 KiB
C#
using RobotNet10.GlobalPathPlanner.Model;
|
|
using System.ComponentModel.DataAnnotations;
|
|
|
|
namespace RobotNet10.GlobalPathPlanner;
|
|
|
|
public class MathExtensions
|
|
{
|
|
public static GlobalNode BezierPoint([Range(0, 1)] double t, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
|
{
|
|
t = Math.Clamp(t, 0.0, 1.0);
|
|
if (edge.Degree == 1)
|
|
{
|
|
return new()
|
|
{
|
|
X = startNode.X + t * (endNode.X - startNode.X),
|
|
Y = startNode.Y + t * (endNode.Y - startNode.Y)
|
|
};
|
|
}
|
|
else if (edge.Degree == 2)
|
|
{
|
|
return new()
|
|
{
|
|
X = (1 - t) * (1 - t) * startNode.X + 2 * t * (1 - t) * edge.ControlPoint1X + t * t * endNode.X,
|
|
Y = (1 - t) * (1 - t) * startNode.Y + 2 * t * (1 - t) * edge.ControlPoint1Y + t * t * endNode.Y
|
|
};
|
|
}
|
|
else if (edge.Degree == 3)
|
|
{
|
|
return new()
|
|
{
|
|
X = Math.Pow(1 - t, 3) * startNode.X + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2X + Math.Pow(t, 3) * endNode.X,
|
|
Y = Math.Pow(1 - t, 3) * startNode.Y + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2Y + Math.Pow(t, 3) * endNode.Y,
|
|
};
|
|
}
|
|
return endNode;
|
|
}
|
|
|
|
public static double GetEdgeLength(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
|
{
|
|
var lineLength = Math.Sqrt(Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2));
|
|
if (edge.Degree == 1)
|
|
{
|
|
return lineLength;
|
|
}
|
|
else if (edge.Degree == 2)
|
|
{
|
|
if (lineLength <= 0) return 0;
|
|
double step = 0.1 / lineLength;
|
|
double distance = 0;
|
|
|
|
for (double t = step; t <= 1.001; t += step)
|
|
{
|
|
var timePoint = BezierPoint(t - step, startNode, endNode, edge);
|
|
var lastTimePoint = BezierPoint(t, startNode, endNode, 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 = 0.1 / lineLength;
|
|
double distance = 0;
|
|
for (double t = step; t <= 1.001; t += step)
|
|
{
|
|
var sTime = t - step;
|
|
var timePoint = BezierPoint(1 - sTime, startNode, endNode, edge);
|
|
sTime = t;
|
|
var lastTimePoint = BezierPoint(1 - sTime, startNode, endNode, 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 GetEdgesLength(GlobalEdge[] edges, GlobalNode[] Nodes)
|
|
{
|
|
if (edges.Length == 0) return -1;
|
|
double distance = 0;
|
|
for (int i = 0; i < edges.Length; i++)
|
|
{
|
|
var edge = edges[i];
|
|
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) return 999;
|
|
distance += GetEdgeLength(startNode, endNode, edge);
|
|
}
|
|
return distance;
|
|
}
|
|
|
|
// 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 DistanceToQuadraticBezier(GlobalNode nodeRef, GlobalNode startNode, GlobalNode endNode, GlobalEdge 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
|
|
|
|
double ax = startNode.X - 2 * edge.ControlPoint1X + endNode.X;
|
|
double ay = startNode.Y - 2 * edge.ControlPoint1Y + endNode.Y;
|
|
double bx = 2 * (edge.ControlPoint1X - startNode.X);
|
|
double by = 2 * (edge.ControlPoint1Y - startNode.Y);
|
|
double cx = startNode.X - nodeRef.X;
|
|
double cy = startNode.Y - 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
|
|
// Khai triển: c·b + (2c·a + b·b)·t + 3a·b·t² + 2a·a·t³ = 0
|
|
// Vậy: A = 2a·a, B = 3a·b, C = 2c·a + b·b, D = 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);
|
|
|
|
double minDist = double.MaxValue;
|
|
|
|
// Kiểm tra khoảng cách tại các điểm tới hạn
|
|
foreach (double t in roots)
|
|
{
|
|
if (t >= 0 && t <= 1)
|
|
{
|
|
GlobalNode p = BezierPoint(t, startNode, endNode, edge);
|
|
double dist = nodeRef.DistanceTo(p);
|
|
minDist = Math.Min(minDist, dist);
|
|
}
|
|
}
|
|
|
|
// Kiểm tra khoảng cách tại 2 đầu mút
|
|
minDist = Math.Min(minDist, nodeRef.DistanceTo(startNode));
|
|
minDist = Math.Min(minDist, nodeRef.DistanceTo(endNode));
|
|
|
|
return minDist;
|
|
}
|
|
|
|
// 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
|
|
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
|
|
// Math.Pow(negative, 1.0/3) trả về NaN trong C#, cần xử lý riêng
|
|
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];
|
|
}
|
|
|
|
// 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];
|
|
}
|
|
|
|
// Phương pháp lấy mẫu - Đơn giản nhưng chậm hơn
|
|
public static double DistanceToCubicBezier(GlobalNode nodeRef, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
|
{
|
|
double bestT = 0;
|
|
double minDistance = Math.Sqrt(Math.Pow(nodeRef.X - startNode.X, 2) + Math.Pow(nodeRef.Y - startNode.Y, 2));
|
|
var length = GetEdgeLength(startNode, endNode, edge);
|
|
double step = 0.1 / (length == 0 ? 0.1 : length);
|
|
|
|
// Bước 1: Lấy mẫu thô
|
|
for (double t = 0; t <= 1; t += step)
|
|
{
|
|
GlobalNode p = BezierPoint(t, startNode, endNode, 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, startNode, endNode, edge));
|
|
double d1 = nodeRef.DistanceTo(BezierPoint(bestT, startNode, endNode, edge));
|
|
double d2 = nodeRef.DistanceTo(BezierPoint(t2, startNode, endNode, 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;
|
|
}
|
|
|
|
public static double DistanceToEdge(GlobalNode nodeRef, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
|
|
{
|
|
if (edge.Degree == 2)
|
|
{
|
|
return DistanceToQuadraticBezier(nodeRef, startNode, endNode, edge);
|
|
}
|
|
else if (edge.Degree == 3)
|
|
{
|
|
return DistanceToCubicBezier(nodeRef, startNode, endNode, edge);
|
|
}
|
|
else
|
|
{
|
|
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, ((nodeRef.X - startNode.X) * (endNode.X - startNode.X) + (nodeRef.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 Math.Sqrt(Math.Pow(nodeRef.X - nearestX, 2) + Math.Pow(nodeRef.Y - nearestY, 2));
|
|
}
|
|
}
|
|
|
|
public static double GetAngle(GlobalNode originNode, GlobalNode Node1, GlobalNode Node2)
|
|
{
|
|
double BA_x = Node1.X - originNode.X;
|
|
double BA_y = Node1.Y - originNode.Y;
|
|
double BC_x = Node2.X - originNode.X;
|
|
double BC_y = Node2.Y - originNode.Y;
|
|
// 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 GetStartAngle(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge, double ratio)
|
|
{
|
|
GlobalNode NearNode = BezierPoint(ratio, startNode, endNode, edge);
|
|
return Math.Atan2(NearNode.Y - startNode.Y, NearNode.X - startNode.X) * 180 / Math.PI;
|
|
}
|
|
|
|
public static double GetEndAngle(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge, double ratio)
|
|
{
|
|
GlobalNode NearNode = BezierPoint(ratio, startNode, endNode, edge);
|
|
return Math.Atan2(endNode.Y - NearNode.Y, endNode.X - NearNode.X) * 180 / Math.PI;
|
|
}
|
|
|
|
public static Orientation[] GetOrientations(Orientation currentDirection, GlobalNode[] nodes, GlobalEdge[] edges, double ratio, double changeOrientationAngle)
|
|
{
|
|
Orientation[] Orientations = new Orientation[nodes.Length];
|
|
if (nodes.Length > 0) Orientations[0] = currentDirection;
|
|
if (nodes.Length > 2)
|
|
{
|
|
for (int i = 1; i < nodes.Length - 1; i++)
|
|
{
|
|
GlobalNode startNode = BezierPoint(1 - ratio, nodes[i - 1], nodes[i], edges[i - 1]);
|
|
GlobalNode endNode = BezierPoint(ratio, nodes[i], nodes[i + 1], edges[i]);
|
|
var angle = GetAngle(nodes[i], startNode, endNode);
|
|
if (angle < changeOrientationAngle) Orientations[i] = Orientations[i - 1] == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
|
|
else Orientations[i] = Orientations[i - 1];
|
|
}
|
|
}
|
|
if (nodes.Length > 1) Orientations[^1] = Orientations[^2];
|
|
return Orientations;
|
|
}
|
|
|
|
public static Orientation GetOrientationStart(GlobalNode nodeRef, GlobalNode nearNode, GlobalEdge edge, double InNodeAngle, double ratio, double changeOrientationAngle)
|
|
{
|
|
GlobalNode NearNode = BezierPoint(ratio, nodeRef, nearNode, edge);
|
|
|
|
var RobotNearNode = new GlobalNode()
|
|
{
|
|
X = nodeRef.X + Math.Cos(InNodeAngle * Math.PI / 180),
|
|
Y = nodeRef.Y + Math.Sin(InNodeAngle * Math.PI / 180),
|
|
};
|
|
var angle = GetAngle(nodeRef, NearNode, RobotNearNode);
|
|
return angle > changeOrientationAngle ? Orientation.BACKWARD : Orientation.FORWARD;
|
|
}
|
|
|
|
public static Orientation GetOrientationEnd(GlobalNode nodeRef, GlobalNode nearNode, GlobalEdge edge, double InNodeAngle, double ratio, double changeOrientationAngle)
|
|
{
|
|
GlobalNode NearNode = BezierPoint(1 - ratio, nearNode, nodeRef, edge);
|
|
|
|
var RobotNearNode = new GlobalNode()
|
|
{
|
|
X = nodeRef.X + Math.Cos(InNodeAngle * Math.PI / 180),
|
|
Y = nodeRef.Y + Math.Sin(InNodeAngle * Math.PI / 180),
|
|
};
|
|
var angle = GetAngle(nodeRef, NearNode, RobotNearNode);
|
|
return angle > changeOrientationAngle ? Orientation.FORWARD : Orientation.BACKWARD;
|
|
}
|
|
|
|
public static GlobalEdge[] GetEdgesPlanning(GlobalNode[] path, GlobalEdge[] edges, GlobalEdge? closesEdge)
|
|
{
|
|
var EdgesPlanning = new List<GlobalEdge>();
|
|
for (int i = 0; i < path.Length - 1; i++)
|
|
{
|
|
var edge = edges.FirstOrDefault(e => e.StartNodeId == path[i].Id && e.EndNodeId == path[i + 1].Id);
|
|
if (edge is null)
|
|
{
|
|
if (i != 0) return [];
|
|
EdgesPlanning.Add(new GlobalEdge()
|
|
{
|
|
Id = closesEdge is null ? Guid.NewGuid() : closesEdge.Id,
|
|
StartNodeId = path[i].Id,
|
|
EndNodeId = path[i + 1].Id,
|
|
Degree = 1,
|
|
});
|
|
continue;
|
|
}
|
|
EdgesPlanning.Add(new()
|
|
{
|
|
Id = edge.Id,
|
|
StartNodeId = path[i].Id,
|
|
EndNodeId = path[i + 1].Id,
|
|
Degree = edge.Degree,
|
|
ControlPoint1X = edge.ControlPoint1X,
|
|
ControlPoint1Y = edge.ControlPoint1Y,
|
|
ControlPoint2X = edge.ControlPoint2X,
|
|
ControlPoint2Y = edge.ControlPoint2Y
|
|
});
|
|
}
|
|
return [.. EdgesPlanning];
|
|
}
|
|
}
|