QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

417
srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/MathExtensions.cs Normal file
View File

@@ -0,0 +1,417 @@
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];
}
}