34 lines
2.0 KiB
C#
34 lines
2.0 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
|
|
/// <summary>旋转矩形与轴对齐栅格的分离轴相交判定。</summary>
|
|
internal static class OrientedRectangleCellIntersection
|
|
{
|
|
/// <summary>任一投影轴没有严格分离时返回 true；擦边按相交处理。</summary>
|
|
public static bool Intersects(VehicleFootprint rectangle, double cellMinX, double cellMaxX, double cellMinY, double cellMaxY)
|
|
{
|
|
if (rectangle == null || cellMaxX < cellMinX || cellMaxY < cellMinY) return false;
|
|
double cellCenterX = (cellMinX + cellMaxX) / 2d;
|
|
double cellCenterY = (cellMinY + cellMaxY) / 2d;
|
|
double cellHalfX = (cellMaxX - cellMinX) / 2d;
|
|
double cellHalfY = (cellMaxY - cellMinY) / 2d;
|
|
|
|
return !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLongitudinalX, rectangle.AxisLongitudinalY) &&
|
|
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLateralX, rectangle.AxisLateralY) &&
|
|
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 1d, 0d) &&
|
|
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 0d, 1d);
|
|
}
|
|
|
|
private static bool HasStrictSeparation(VehicleFootprint rectangle, double cellCenterX, double cellCenterY, double cellHalfX, double cellHalfY,
|
|
double axisX, double axisY)
|
|
{
|
|
double rectangleCenter = rectangle.CenterX * axisX + rectangle.CenterY * axisY;
|
|
double cellCenter = cellCenterX * axisX + cellCenterY * axisY;
|
|
double rectangleRadius = rectangle.HalfLengthMeters * Math.Abs(rectangle.AxisLongitudinalX * axisX + rectangle.AxisLongitudinalY * axisY) +
|
|
rectangle.HalfWidthMeters * Math.Abs(rectangle.AxisLateralX * axisX + rectangle.AxisLateralY * axisY);
|
|
double cellRadius = cellHalfX * Math.Abs(axisX) + cellHalfY * Math.Abs(axisY);
|
|
return Math.Abs(rectangleCenter - cellCenter) > rectangleRadius + cellRadius;
|
|
}
|
|
}
|