Initial commit
This commit is contained in:
365
srcs/RobotNet10/Commons/RobotNet10.Common/SpaceCompute.cs
Normal file
365
srcs/RobotNet10/Commons/RobotNet10.Common/SpaceCompute.cs
Normal file
@@ -0,0 +1,365 @@
|
||||
using RobotNet10.Common.Models;
|
||||
using System.ComponentModel.DataAnnotations;
|
||||
|
||||
namespace RobotNet10.Common;
|
||||
|
||||
public class SpaceCompute
|
||||
{
|
||||
public static SpaceNode BezierPoint([Range(0, 1)] double t, SpaceEdge edge)
|
||||
{
|
||||
t = Math.Clamp(t, 0.0, 1.0);
|
||||
if (edge.Degree == 1) return new SpaceNode(edge.StartX + t * (edge.EndX - edge.StartX), edge.StartY + t * (edge.EndY - edge.StartY));
|
||||
else if (edge.Degree == 2)
|
||||
{
|
||||
return new((1 - t) * (1 - t) * edge.StartX + 2 * t * (1 - t) * edge.ControlPoint1X + t * t * edge.EndX,
|
||||
(1 - t) * (1 - t) * edge.StartY + 2 * t * (1 - t) * edge.ControlPoint1Y + t * t * edge.EndY);
|
||||
}
|
||||
else if (edge.Degree == 3)
|
||||
{
|
||||
return new(Math.Pow(1 - t, 3) * edge.StartX + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2X + Math.Pow(t, 3) * edge.EndX,
|
||||
Math.Pow(1 - t, 3) * edge.StartY + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2Y + Math.Pow(t, 3) * edge.EndY);
|
||||
}
|
||||
return new(edge.EndX, edge.EndY);
|
||||
}
|
||||
|
||||
// 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];
|
||||
}
|
||||
|
||||
// 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 (depressed cubic)
|
||||
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
|
||||
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];
|
||||
}
|
||||
|
||||
|
||||
// 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 distance, double time) DistanceToQuadraticBezier(SpaceNode nodeRef, SpaceEdge 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
|
||||
|
||||
// Quadratic Bezier: P(t) = (1-t)²P₀ + 2t(1-t)P₁ + t²P₂
|
||||
// Đạo hàm: P'(t) = 2(1-t)(P₁-P₀) + 2t(P₂-P₁)
|
||||
// Khoảng cách bình phương: D(t) = |P(t) - G|²
|
||||
// Đạo hàm: D'(t) = 2(P(t) - G) · P'(t) = 0
|
||||
|
||||
// Viết lại: P(t) = P₀ + 2t(P₁-P₀) + t²(P₂-2P₁+P₀)
|
||||
// P'(t) = 2(P₁-P₀) + 2t(P₂-2P₁+P₀)
|
||||
// (P(t) - G) · P'(t) = 0
|
||||
|
||||
double ax = edge.StartX - 2 * edge.ControlPoint1X + edge.EndX;
|
||||
double ay = edge.StartY - 2 * edge.ControlPoint1Y + edge.EndY;
|
||||
double bx = 2 * (edge.ControlPoint1X - edge.StartX);
|
||||
double by = 2 * (edge.ControlPoint1Y - edge.StartY);
|
||||
double cx = edge.StartX - nodeRef.X;
|
||||
double cy = edge.StartY - 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
|
||||
// (c + bt + at²) · (b + 2at) = 0
|
||||
//
|
||||
// Khai triển tích vô hướng:
|
||||
// c·b + c·2at + bt·b + bt·2at + at²·b + at²·2at = 0
|
||||
// = c·b + 2c·a·t + b·b·t + 2a·b·t² + a·b·t² + 2a·a·t³ = 0
|
||||
// = c·b + (2c·a + b·b)·t + 3a·b·t² + 2a·a·t³ = 0
|
||||
//
|
||||
// Vậy: A = 2a·a = 2(aₓ² + aᵧ²)
|
||||
// B = 3a·b = 3(aₓbₓ + aᵧbᵧ)
|
||||
// C = 2c·a + b·b = 2(cₓaₓ + cᵧaᵧ) + (bₓ² + bᵧ²)
|
||||
// D = c·b = cₓbₓ + 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);
|
||||
|
||||
// Tính khoảng cách tại 2 đầu mút
|
||||
double distStart = nodeRef.DistanceTo(new(edge.StartX, edge.StartY));
|
||||
double distEnd = nodeRef.DistanceTo(new(edge.EndX, edge.EndY));
|
||||
|
||||
// Khởi tạo với giá trị tại 2 đầu mút
|
||||
double minDist = Math.Min(distStart, distEnd);
|
||||
double time = distStart < distEnd ? 0 : 1;
|
||||
|
||||
// Kiểm tra khoảng cách tại các điểm tới hạn (nghiệm của đạo hàm)
|
||||
foreach (double t in roots)
|
||||
{
|
||||
// Bỏ qua NaN và Infinity
|
||||
if (double.IsNaN(t) || double.IsInfinity(t)) continue;
|
||||
|
||||
// Chỉ xét nghiệm trong khoảng [0, 1]
|
||||
if (t >= 0 && t <= 1)
|
||||
{
|
||||
SpaceNode p = BezierPoint(t, edge);
|
||||
double dist = nodeRef.DistanceTo(p);
|
||||
|
||||
// Cập nhật minDist và time nếu tìm thấy khoảng cách nhỏ hơn
|
||||
if (dist < minDist)
|
||||
{
|
||||
minDist = dist;
|
||||
time = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Kiểm tra lại khoảng cách tại 2 đầu mút (chỉ cập nhật nếu nhỏ hơn)
|
||||
// Lưu ý: Không ghi đè nếu minDist đã được cập nhật từ nghiệm trong (0,1)
|
||||
if (distStart < minDist)
|
||||
{
|
||||
minDist = distStart;
|
||||
time = 0;
|
||||
}
|
||||
if (distEnd < minDist)
|
||||
{
|
||||
minDist = distEnd;
|
||||
time = 1;
|
||||
}
|
||||
|
||||
return (minDist, time);
|
||||
}
|
||||
|
||||
// Phương pháp lấy mẫu - Đơn giản nhưng chậm hơn
|
||||
public static (double distance, double time) DistanceToCubicBezier(SpaceNode nodeRef, SpaceEdge edge)
|
||||
{
|
||||
double bestT = 0;
|
||||
double minDistance = Math.Sqrt(Math.Pow(nodeRef.X - edge.StartX, 2) + Math.Pow(nodeRef.Y - edge.StartY, 2));
|
||||
var length = GetEdgeLength(edge, 0.3);
|
||||
double step = 0.3 / (length == 0 ? 0.1 : length);
|
||||
|
||||
// Bước 1: Lấy mẫu thô
|
||||
for (double t = 0; t <= 1; t += step)
|
||||
{
|
||||
SpaceNode p = BezierPoint(t, 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, edge));
|
||||
double d1 = nodeRef.DistanceTo(BezierPoint(bestT, edge));
|
||||
double d2 = nodeRef.DistanceTo(BezierPoint(t2, 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, bestT);
|
||||
}
|
||||
|
||||
public static (double x, double y, double distance, double time) GetProjectionOnEdge(double x, double y, SpaceEdge edge)
|
||||
{
|
||||
if (edge.Degree == 2)
|
||||
{
|
||||
(double distance, double time) = DistanceToQuadraticBezier(new(x, y), edge);
|
||||
var node = BezierPoint(time, edge);
|
||||
return (node.X, node.Y, distance, time);
|
||||
}
|
||||
else if (edge.Degree == 3)
|
||||
{
|
||||
(double distance, var time) = DistanceToCubicBezier(new(x, y), edge);
|
||||
var node = BezierPoint(time, edge);
|
||||
return (node.X, node.Y, distance, time);
|
||||
}
|
||||
else
|
||||
{
|
||||
double time = 0;
|
||||
var edgeLengthSquared = Math.Pow(edge.StartX - edge.EndX, 2) + Math.Pow(edge.StartY - edge.EndY, 2);
|
||||
if (edgeLengthSquared > 0)
|
||||
{
|
||||
time = Math.Max(0, Math.Min(1, ((x - edge.StartX) * (edge.EndX - edge.StartX) + (y - edge.StartY) * (edge.EndY - edge.StartY)) / edgeLengthSquared));
|
||||
}
|
||||
|
||||
double nearestX = edge.StartX + time * (edge.EndX - edge.StartX);
|
||||
double nearestY = edge.StartY + time * (edge.EndY - edge.StartY);
|
||||
|
||||
return (nearestX, nearestY, Math.Sqrt(Math.Pow(x - nearestX, 2) + Math.Pow(y - nearestY, 2)), time);
|
||||
}
|
||||
}
|
||||
|
||||
public static double GetEdgeLength(SpaceEdge edge, double resolution)
|
||||
{
|
||||
var lineLength = Math.Sqrt(Math.Pow(edge.StartX - edge.EndX, 2) + Math.Pow(edge.StartY - edge.EndY, 2));
|
||||
if (edge.Degree == 1)
|
||||
{
|
||||
return lineLength;
|
||||
}
|
||||
else if (edge.Degree == 2)
|
||||
{
|
||||
if (lineLength <= 0) return 0;
|
||||
double step = resolution / lineLength;
|
||||
double distance = 0;
|
||||
|
||||
for (double t = step; t <= 1.001; t += step)
|
||||
{
|
||||
var timePoint = BezierPoint(t - step, edge);
|
||||
var lastTimePoint = BezierPoint(t, 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 = resolution / lineLength;
|
||||
double distance = 0;
|
||||
for (double t = step; t <= 1.001; t += step)
|
||||
{
|
||||
var sTime = t - step;
|
||||
var timePoint = BezierPoint(1 - sTime, edge);
|
||||
sTime = t;
|
||||
var lastTimePoint = BezierPoint(1 - sTime, 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 GetVectorAngle(double originNodeX, double originNodeY, double vector1X, double vector1Y, double vector2X, double vector2Y)
|
||||
{
|
||||
double BA_x = vector1X - originNodeX;
|
||||
double BA_y = vector1Y - originNodeY;
|
||||
double BC_x = vector2X - originNodeX;
|
||||
double BC_y = vector2Y - originNodeY;
|
||||
// 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 NormalizeDegreeAngle(double angle)
|
||||
{
|
||||
angle = angle % 360;
|
||||
if (angle > 180) angle -= 360;
|
||||
else if (angle < -180) angle += 360;
|
||||
return angle;
|
||||
}
|
||||
|
||||
public static double NormalizeRadianAngle(double angle)
|
||||
{
|
||||
angle = angle % (2 * Math.PI);
|
||||
if (angle > Math.PI) angle -= (2 * Math.PI);
|
||||
else if (angle < -Math.PI) angle += (2 * Math.PI);
|
||||
return angle;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user