260 lines
11 KiB
C#
260 lines
11 KiB
C#
using RobotNet.MapShares.Dtos;
|
|
using System.ComponentModel.DataAnnotations;
|
|
using System.Text.RegularExpressions;
|
|
|
|
namespace RobotNet.MapShares;
|
|
|
|
public partial class MapEditorHelper
|
|
{
|
|
[GeneratedRegex(@"^[a-zA-Z0-9\-_]*$")]
|
|
private static partial Regex MyRegex();
|
|
private static readonly Regex NameRegex = MyRegex();
|
|
|
|
public static (bool IsSuccess, string returnStr) NameChecking(string name)
|
|
{
|
|
if (string.IsNullOrEmpty(name)) return (false, "Tên không được để trống.");
|
|
if (name.Length < 3 || name.Length > 127) return (false, "Tên gồm từ 3 đến 127 ký tự");
|
|
if (!NameRegex.IsMatch(name)) return (false, "Tên chỉ chứa các ký tự thường (a - z), các ký tự hoa (A - Z), chữ số và ký tự '-'");
|
|
return (true, "");
|
|
}
|
|
|
|
public static IEnumerable<string> NameValidation(string name)
|
|
{
|
|
if (string.IsNullOrEmpty(name)) yield return "Tên không được để trống.";
|
|
if (name.Length < 3 || name.Length > 127) yield return "Tên gồm từ 3 đến 127 ký tự";
|
|
if (!NameRegex.IsMatch(name)) yield return "Tên chỉ chứa các ký tự thường (a - z), các ký tự hoa (A - Z), chữ số và ký tự '-'";
|
|
}
|
|
|
|
public static IEnumerable<string> RobotIdValidation(string id)
|
|
{
|
|
if (string.IsNullOrEmpty(id) || string.IsNullOrWhiteSpace(id)) yield return "Id không được để trống.";
|
|
if (id is not null && (id.Length < 1 || id.Length > 127)) yield return "Id gồm từ 1 đến 127 ký tự";
|
|
if (id is not null && !NameRegex.IsMatch(id)) yield return "Id chỉ chứa các ký tự thường (a - z), kĩ tự in hoa (A - Z), chữ số và ký tự '-'";
|
|
}
|
|
|
|
public static (double X, double Y) CaculateControlPoint(double x1, double y1, double x2, double y2, double angle, double length)
|
|
{
|
|
double dx = x2 - x1;
|
|
double dy = y2 - y1;
|
|
double a = Math.Atan2(dy, dx) + angle * Math.PI / 180;
|
|
double d = length * Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));
|
|
return (x1 + Math.Cos(a) * d, y1 + Math.Sin(a) * d);
|
|
}
|
|
|
|
public static (double X, double Y)? CaculateIntersectionPoint(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
|
|
{
|
|
if (x1 == x2 && x3 == x4)
|
|
{
|
|
if (x1 == x3)
|
|
{
|
|
return (x1, y1);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
if (y1 == y2)
|
|
{
|
|
return (x3, y1);
|
|
}
|
|
|
|
if (x3 == x4)
|
|
{
|
|
return (x3, y1);
|
|
}
|
|
|
|
double mAB = (y2 - y1) / (x2 - x1);
|
|
double mCD = (y4 - y3) / (x4 - x3);
|
|
|
|
if (mAB == mCD)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
double x = (mAB * x1 - mCD * x3 + y3 - y1) / (mAB - mCD);
|
|
double y = mAB * (x - x1) + y1;
|
|
|
|
return (x, y);
|
|
}
|
|
|
|
public static (double PX1, double PY1, double PX2, double PY2) CaculateDoubleControlPoint(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
|
|
{
|
|
var (X, Y) = CaculateIntersection(x1, y1, x2, y2, x3, y3);
|
|
var cp2 = CaculateIntersection(x4, y4, x3, y3, x2, y2);
|
|
|
|
return (X, Y, cp2.X, cp2.Y);
|
|
}
|
|
|
|
private static (double X, double Y) CaculateIntersection(double x1, double y1, double x2, double y2, double x3, double y3)
|
|
{
|
|
double length = Math.Sqrt(Math.Pow(x3 - x2, 2) + Math.Pow(y3 - y2, 2));
|
|
var angle = Math.Atan2(y2 - y1, x2 - x1);
|
|
|
|
var x = x2 + length * Math.Cos(angle) * Math.Sqrt(2) / 2;
|
|
var y = y2 + length * Math.Sin(angle) * Math.Sqrt(2) / 2;
|
|
return (x, y);
|
|
}
|
|
|
|
public static NodeDto? GetClosesNode(double x, double y, List<NodeDto> nodes)
|
|
{
|
|
NodeDto? finalNode = null;
|
|
double minDistance = 99;
|
|
foreach (var node in nodes)
|
|
{
|
|
var distance = Math.Sqrt(Math.Pow(node.X - x, 2) + Math.Pow(node.Y - y, 2));
|
|
if (distance < minDistance)
|
|
{
|
|
minDistance = distance;
|
|
finalNode = node;
|
|
}
|
|
}
|
|
if (minDistance <= 0.35 && finalNode is not null) return finalNode;
|
|
return null;
|
|
}
|
|
|
|
public static bool NodeInScanZone(double xRef, double yRef, double x1, double y1, double x2, double y2)
|
|
{
|
|
double Xmin = Math.Min(x1, x2);
|
|
double Xmax = Math.Max(x1, x2);
|
|
double Ymin = Math.Min(y1, y2);
|
|
double Ymax = Math.Max(y1, y2);
|
|
|
|
return (Xmin <= xRef && xRef <= Xmax) && (Ymin <= yRef && yRef <= Ymax);
|
|
}
|
|
|
|
public static bool IsPointInside(double x, double y, ZoneDto zone)
|
|
{
|
|
int crossings = 0;
|
|
|
|
crossings += DoesRayCross(zone.X1, zone.Y1, zone.X2, zone.Y2, x, y) ? 1 : 0;
|
|
crossings += DoesRayCross(zone.X2, zone.Y2, zone.X3, zone.Y3, x, y) ? 1 : 0;
|
|
crossings += DoesRayCross(zone.X3, zone.Y3, zone.X4, zone.Y4, x, y) ? 1 : 0;
|
|
crossings += DoesRayCross(zone.X4, zone.Y4, zone.X1, zone.Y1, x, y) ? 1 : 0;
|
|
|
|
return crossings % 2 == 1;
|
|
}
|
|
|
|
public static bool DoesRayCross(double x1, double y1, double x2, double y2, double x, double y)
|
|
{
|
|
if (Math.Min(y1, y2) <= y && y < Math.Max(y1, y2))
|
|
{
|
|
double xIntersect = x1 + (y - y1) * (x2 - x1) / (y2 - y1);
|
|
return x < xIntersect;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public static double CalculateQuadrilateralArea(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
|
|
{
|
|
return 0.5 * Math.Abs(x1 * y2 + x2 * y3 + x3 * y4 + x4 * y1 - (y1 * x2 + y2 * x3 + y3 * x4 + y4 * x1));
|
|
}
|
|
|
|
public static (double x, double y) CurveDegreeTwo(double t, double x1, double y1, double controlPointX, double controlPointY, double x2, double y2)
|
|
{
|
|
var x = (1 - t) * (1 - t) * x1 + 2 * t * (1 - t) * controlPointX + t * t * x2;
|
|
var y = (1 - t) * (1 - t) * y1 + 2 * t * (1 - t) * controlPointY + t * t * y2;
|
|
return (x, y);
|
|
}
|
|
|
|
public static (double x, double y) CurveDegreeThree(double t, double x1, double y1, double controlPoint1X, double controlPoint1Y, double controlPoint2X, double controlPoint2Y, double x2, double y2)
|
|
{
|
|
var x = Math.Pow(1 - t, 3) * x1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2X + Math.Pow(t, 3) * x2; ;
|
|
var y = Math.Pow(1 - t, 3) * y1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2Y + Math.Pow(t, 3) * y2;
|
|
return (x, y);
|
|
}
|
|
|
|
public static (double x, double y) Curve(double t, EdgeCaculatorModel edge)
|
|
{
|
|
if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
|
|
{
|
|
return (edge.X1 + t * (edge.X2 - edge.X1), edge.Y1 + t * (edge.Y2 - edge.Y1));
|
|
}
|
|
else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
|
|
{
|
|
return CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
|
|
}
|
|
else
|
|
{
|
|
return CurveDegreeThree(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
|
|
}
|
|
}
|
|
|
|
public static double GetEdgeLength(EdgeCaculatorModel edge)
|
|
{
|
|
if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
|
|
{
|
|
return Math.Round(Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2)), 3);
|
|
}
|
|
else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
|
|
{
|
|
var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
|
|
if (length == 0) return 0;
|
|
double step = 0.1 / length;
|
|
double distance = 0;
|
|
|
|
for (double t = step; t <= 1.001; t += step)
|
|
{
|
|
(double x1, double y1) = CurveDegreeTwo(t - step, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
|
|
(double x2, double y2) = CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
|
|
distance += Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2));
|
|
}
|
|
|
|
return Math.Round(distance, 3);
|
|
}
|
|
else
|
|
{
|
|
var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
|
|
if (length == 0) return 0;
|
|
double step = 0.1 / length;
|
|
double distance = 0;
|
|
for (double t = step; t <= 1.001; t += step)
|
|
{
|
|
var sTime = t - step;
|
|
(var sx, var sy) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
|
|
sTime = t;
|
|
(var ex, var ey) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
|
|
|
|
distance += Math.Sqrt(Math.Pow(sx - ex, 2) + Math.Pow(sy - ey, 2));
|
|
}
|
|
return Math.Round(distance, 3);
|
|
}
|
|
}
|
|
|
|
public static NodeDto GetNearByNode(NodeDto orginNode, NodeDto node2, EdgeDto edge, [Range(0, 1)] double ratio)
|
|
{
|
|
NodeDto Start = edge.StartNodeId == orginNode.Id ? orginNode : node2;
|
|
NodeDto End = edge.StartNodeId == orginNode.Id ? node2 : orginNode;
|
|
var localRatio = edge.StartNodeId == orginNode.Id ? ratio : 1 - ratio;
|
|
bool isReverse = edge.StartNodeId != orginNode.Id && edge.TrajectoryDegree == Enums.TrajectoryDegree.Three;
|
|
var (x, y) = Curve(localRatio, new()
|
|
{
|
|
X1 = Start.X,
|
|
Y1 = Start.Y,
|
|
X2 = End.X,
|
|
Y2 = End.Y,
|
|
ControlPoint1X = !isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
|
|
ControlPoint1Y = !isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y,
|
|
ControlPoint2X = !isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
|
|
ControlPoint2Y = !isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
|
|
TrajectoryDegree = edge.TrajectoryDegree,
|
|
});
|
|
return new() { X = x, Y = y };
|
|
}
|
|
|
|
public static double GetAngle(NodeDto originNode, NodeDto Node1, NodeDto 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);
|
|
}
|
|
}
|