59 lines
2.0 KiB
C#
59 lines
2.0 KiB
C#
using System;
|
||
|
||
namespace NavisworksTransport.Utils
|
||
{
|
||
public static class CornerTurnCheckHelper
|
||
{
|
||
/// <summary>
|
||
/// 基于标准转角通过公式：L(θ) = (C₁-W·cosθ)/sinθ + (C₂-W·sinθ)/cosθ
|
||
/// 在 θ ∈ (0, π/2) 上搜索最大可通过长度。
|
||
/// </summary>
|
||
public static bool CanPass(double length, double width, double entryWidth, double exitWidth,
|
||
out double margin, out double maxLength)
|
||
{
|
||
margin = 0;
|
||
maxLength = 0;
|
||
|
||
if (length <= 0 || width <= 0 || entryWidth <= 0 || exitWidth <= 0)
|
||
return false;
|
||
|
||
if (width > entryWidth || width > exitWidth)
|
||
return false;
|
||
|
||
maxLength = FindRequiredLength(width, entryWidth, exitWidth);
|
||
margin = maxLength - length;
|
||
return length <= maxLength;
|
||
}
|
||
|
||
/// <summary> 给定角度 θ，计算该角度下能通过的最大模块长度 </summary>
|
||
private static double L(double theta, double width, double c1, double c2)
|
||
{
|
||
var sin = Math.Sin(theta);
|
||
var cos = Math.Cos(theta);
|
||
return (c1 - width * cos) / sin + (c2 - width * sin) / cos;
|
||
}
|
||
|
||
/// <summary> 三分法搜索 θ ∈ (0, π/2) 上 L(θ) 的最小值（即最大可通过长度） </summary>
|
||
private static double FindRequiredLength(double width, double c1, double c2)
|
||
{
|
||
double lo = 1e-6;
|
||
double hi = Math.PI / 2 - 1e-6;
|
||
|
||
// 三分搜索
|
||
for (int i = 0; i < 80; i++)
|
||
{
|
||
var m1 = lo + (hi - lo) / 3;
|
||
var m2 = hi - (hi - lo) / 3;
|
||
var f1 = L(m1, width, c1, c2);
|
||
var f2 = L(m2, width, c1, c2);
|
||
if (f1 < f2)
|
||
hi = m2;
|
||
else
|
||
lo = m1;
|
||
}
|
||
|
||
return L((lo + hi) / 2, width, c1, c2);
|
||
}
|
||
}
|
||
}
|