159 lines
8.4 KiB
C#
159 lines
8.4 KiB
C#
using RobotNet.MapShares;
|
|
using RobotNet.MapShares.Dtos;
|
|
using RobotNet.RobotShares.Dtos;
|
|
|
|
namespace RobotNet.RobotManager.Services.Traffic;
|
|
|
|
public class TrafficMath
|
|
{
|
|
public static bool Edge1IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
|
|
{
|
|
if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.One) return true;
|
|
|
|
double dx = edge.X2 - edge.X1;
|
|
double dy = edge.Y2 - edge.Y1;
|
|
var length = Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));
|
|
if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;
|
|
|
|
double distance = Math.Abs(dy * centerCircle.X - dx * centerCircle.Y + (edge.X2 * edge.Y1 - edge.X1 * edge.Y2)) / length;
|
|
if (distance > radius) return false;
|
|
|
|
double a = dx * dx + dy * dy;
|
|
double b = 2 * (dx * (edge.X1 - centerCircle.X) + dy * (edge.Y1 - centerCircle.Y));
|
|
double c = (edge.X1 - centerCircle.X) * (edge.X1 - centerCircle.X) + (edge.Y1 - centerCircle.Y) * (edge.Y1 - centerCircle.Y) - radius * radius;
|
|
|
|
double delta = b * b - 4 * a * c;
|
|
if (delta < 0) return false;
|
|
|
|
double t1 = (-b + Math.Sqrt(delta)) / (2 * a);
|
|
double t2 = (-b - Math.Sqrt(delta)) / (2 * a);
|
|
|
|
return (t1 >= 0 && t1 <= 1) || (t2 >= 0 && t2 <= 1);
|
|
}
|
|
|
|
public static bool Edge2IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
|
|
{
|
|
if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.Two) return true;
|
|
|
|
var length = Math.Sqrt(Math.Pow(edge.X2 - edge.X1, 2) + Math.Pow(edge.Y2 - edge.Y1, 2));
|
|
if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;
|
|
|
|
double step = 0.1 / length;
|
|
for (double t = 0; t <= 1.001; t += step)
|
|
{
|
|
(double x1, double y1) = CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
|
|
if (Math.Sqrt(Math.Pow(x1 - centerCircle.X, 2) + Math.Pow(y1 - centerCircle.Y, 2)) < radius) return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public static bool Edge3IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
|
|
{
|
|
if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.Three) return true;
|
|
|
|
var length = Math.Sqrt(Math.Pow(edge.X2 - edge.X1, 2) + Math.Pow(edge.Y2 - edge.Y1, 2));
|
|
if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;
|
|
|
|
double step = 0.1 / length;
|
|
for (double t = 0; t <= 1.001; t += step)
|
|
{
|
|
(double x1, double y1) = CurveDegreeThree(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
|
|
if (Math.Sqrt(Math.Pow(x1 - centerCircle.X, 2) + Math.Pow(y1 - centerCircle.Y, 2)) < radius) return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public static bool EdgeIntersectCircle(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
|
|
{
|
|
if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One) return Edge1IntersectEdge(centerCircle, radius, edge);
|
|
else if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two) return Edge2IntersectEdge(centerCircle, radius, edge);
|
|
else if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Three) return Edge3IntersectEdge(centerCircle, radius, edge);
|
|
return true;
|
|
}
|
|
|
|
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 CalTurningRadius(AgentModel model, double offsetLength, double offsetWidth)
|
|
{
|
|
var navigationPointX = offsetLength - model.NavigationPointX;
|
|
var navigationPointY = offsetWidth - model.NavigationPointY;
|
|
var length = model.Length + 2 * offsetLength;
|
|
var width = model.Width + 2 * offsetWidth;
|
|
double d1 = Math.Sqrt(navigationPointX * navigationPointX + navigationPointY * navigationPointY);
|
|
double d2 = Math.Sqrt((length - navigationPointX) * (length - navigationPointX) + navigationPointY * navigationPointY);
|
|
double d3 = Math.Sqrt(navigationPointX * navigationPointX + (width - navigationPointY) * (width - navigationPointY));
|
|
double d4 = Math.Sqrt((length - navigationPointX) * (length - navigationPointX) + (width - navigationPointY) * (width - navigationPointY));
|
|
|
|
return Math.Max(Math.Max(d1, d2), Math.Max(d3, d4));
|
|
}
|
|
|
|
public static TrafficLockedShapeDto CalRobotRectShape(AgentModel model, double offsetLength, double offsetWidth, double x, double y, double theta)
|
|
{
|
|
double x1 = model.NavigationPointX - offsetLength;
|
|
double y1 = model.NavigationPointY - offsetWidth;
|
|
double x2 = model.Length + model.NavigationPointX + offsetLength;
|
|
double y2 = model.NavigationPointY - offsetWidth;
|
|
double x3 = model.Length + model.NavigationPointX + offsetLength;
|
|
double y3 = model.Width + model.NavigationPointY + offsetWidth;
|
|
double x4 = model.NavigationPointX - offsetLength;
|
|
double y4 = model.Width + model.NavigationPointY + offsetWidth;
|
|
|
|
return new()
|
|
{
|
|
Type = TrafficLockedShapeType.Rectangle,
|
|
X1 = x1 * Math.Cos(theta) - y1 * Math.Sin(theta) + x,
|
|
Y1 = x1 * Math.Sin(theta) + y1 * Math.Cos(theta) + y,
|
|
X2 = x2 + Math.Cos(theta) - y2 * Math.Sin(theta) + x,
|
|
Y2 = x2 * Math.Sin(theta) + y2 * Math.Cos(theta) + y,
|
|
X3 = x3 + Math.Cos(theta) - y3 * Math.Sin(theta) + x,
|
|
Y3 = x3 * Math.Sin(theta) + y3 * Math.Cos(theta) + y,
|
|
X4 = x4 + Math.Cos(theta) - y4 * Math.Sin(theta) + x,
|
|
Y4 = x4 * Math.Sin(theta) + y4 * Math.Cos(theta) + y
|
|
};
|
|
}
|
|
|
|
public static bool RectIntersectCircle(TrafficNodeDto centerCircle, double radius, double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
|
|
{
|
|
double xMin = Math.Min(Math.Min(x1, x2), Math.Min(x3, x4));
|
|
double xMax = Math.Max(Math.Max(x1, x2), Math.Max(x3, x4));
|
|
double yMin = Math.Min(Math.Min(y1, y2), Math.Min(y3, y4));
|
|
double yMax = Math.Max(Math.Max(y1, y2), Math.Max(y3, y4));
|
|
|
|
double xClosest = Math.Max(xMin, Math.Min(centerCircle.X, xMax));
|
|
double yClosest = Math.Max(yMin, Math.Min(centerCircle.Y, yMax));
|
|
|
|
double distance = Math.Sqrt((centerCircle.X - xClosest) * (centerCircle.X - xClosest) + (centerCircle.Y - yClosest) * (centerCircle.Y - yClosest));
|
|
return distance <= radius;
|
|
}
|
|
|
|
public static bool RectIntersectRect(TrafficLockedShapeDto rect1, TrafficLockedShapeDto rect2)
|
|
{
|
|
double x1Min = Math.Min(Math.Min(rect1.X1, rect1.X2), Math.Min(rect1.X3, rect1.X4));
|
|
double x1Max = Math.Max(Math.Max(rect1.X1, rect1.X2), Math.Max(rect1.X3, rect1.X4));
|
|
double y1Min = Math.Min(Math.Min(rect1.Y1, rect1.Y2), Math.Min(rect1.Y3, rect1.Y4));
|
|
double y1Max = Math.Max(Math.Max(rect1.Y1, rect1.Y2), Math.Max(rect1.Y3, rect1.Y4));
|
|
|
|
double x2Min = Math.Min(Math.Min(rect2.X1, rect2.X2), Math.Min(rect2.X3, rect2.X4));
|
|
double x2Max = Math.Max(Math.Max(rect2.X1, rect2.X2), Math.Max(rect2.X3, rect2.X4));
|
|
double y2Min = Math.Min(Math.Min(rect2.Y1, rect2.Y2), Math.Min(rect2.Y3, rect2.Y4));
|
|
double y2Max = Math.Max(Math.Max(rect2.Y1, rect2.Y2), Math.Max(rect2.Y3, rect2.Y4));
|
|
|
|
bool xOverlap = x1Min <= x2Max && x2Min <= x1Max;
|
|
bool yOverlap = y1Min <= y2Max && y2Min <= y1Max;
|
|
|
|
return xOverlap && yOverlap;
|
|
}
|
|
}
|