410 lines
14 KiB
C#
410 lines
14 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Numerics;
|
||
|
||
namespace NavisworksTransport.Utils.CoordinateSystem
|
||
{
|
||
public sealed class ObjectPassageProjectionOptimizationRequest
|
||
{
|
||
public ObjectPassageProjectionOptimizationRequest(
|
||
double sizeX,
|
||
double sizeY,
|
||
double sizeZ,
|
||
Vector3 hostPathForward,
|
||
Vector3 hostUp,
|
||
double areaRelativeTieTolerance = 0.01,
|
||
Quaternion? baselineHostRotation = null)
|
||
{
|
||
SizeX = sizeX;
|
||
SizeY = sizeY;
|
||
SizeZ = sizeZ;
|
||
HostPathForward = hostPathForward;
|
||
HostUp = hostUp;
|
||
AreaRelativeTieTolerance = areaRelativeTieTolerance;
|
||
BaselineHostRotation = Quaternion.Normalize(baselineHostRotation ?? Quaternion.Identity);
|
||
}
|
||
|
||
public double SizeX { get; }
|
||
|
||
public double SizeY { get; }
|
||
|
||
public double SizeZ { get; }
|
||
|
||
public Vector3 HostPathForward { get; }
|
||
|
||
public Vector3 HostUp { get; }
|
||
|
||
public double AreaRelativeTieTolerance { get; }
|
||
|
||
public Quaternion BaselineHostRotation { get; }
|
||
|
||
/// <summary>
|
||
/// 门型偏好：true=宽>高，false=高>宽
|
||
/// </summary>
|
||
public bool RequireWidthLarger { get; set; }
|
||
|
||
/// <summary>
|
||
/// 物体下方物流车辆高度（模型单位）
|
||
/// </summary>
|
||
public double VehicleHeightModelUnits { get; set; }
|
||
}
|
||
|
||
public sealed class ObjectPassageProjectionOptimizationResult
|
||
{
|
||
private ObjectPassageProjectionOptimizationResult(
|
||
bool success,
|
||
LocalEulerRotationCorrection correction,
|
||
ObjectPassageProjectionScore score,
|
||
string errorMessage,
|
||
double? computedLiftOffsetModelUnits = null,
|
||
double? aabbSx = null, double? aabbSy = null, double? aabbSz = null)
|
||
{
|
||
Success = success;
|
||
Correction = correction;
|
||
Score = score;
|
||
ErrorMessage = errorMessage;
|
||
ComputedLiftOffsetModelUnits = computedLiftOffsetModelUnits;
|
||
AabbSx = aabbSx; AabbSy = aabbSy; AabbSz = aabbSz;
|
||
}
|
||
|
||
public bool Success { get; }
|
||
|
||
public LocalEulerRotationCorrection Correction { get; }
|
||
|
||
public ObjectPassageProjectionScore Score { get; }
|
||
|
||
public string ErrorMessage { get; }
|
||
|
||
/// <summary>
|
||
/// 自动调整后计算出的上下偏移（模型单位），使物体底面贴合物流车高度。仅地面路径有效。
|
||
/// </summary>
|
||
public double? ComputedLiftOffsetModelUnits { get; }
|
||
|
||
/// <summary>
|
||
/// 优化器实测的 AABB 尺寸（模型单位）。
|
||
/// </summary>
|
||
public double? AabbSx { get; }
|
||
public double? AabbSy { get; }
|
||
public double? AabbSz { get; }
|
||
|
||
public static ObjectPassageProjectionOptimizationResult CreateSuccess(
|
||
LocalEulerRotationCorrection correction,
|
||
ObjectPassageProjectionScore score,
|
||
double? computedLiftOffsetModelUnits = null,
|
||
double? aabbSx = null, double? aabbSy = null, double? aabbSz = null)
|
||
{
|
||
return new ObjectPassageProjectionOptimizationResult(true, correction, score, null, computedLiftOffsetModelUnits, aabbSx, aabbSy, aabbSz);
|
||
}
|
||
|
||
public static ObjectPassageProjectionOptimizationResult CreateFailure(string errorMessage)
|
||
{
|
||
return new ObjectPassageProjectionOptimizationResult(
|
||
false,
|
||
LocalEulerRotationCorrection.Zero,
|
||
ObjectPassageProjectionScore.Invalid,
|
||
errorMessage);
|
||
}
|
||
}
|
||
|
||
public struct ObjectPassageProjectionScore
|
||
{
|
||
public static readonly ObjectPassageProjectionScore Invalid =
|
||
new ObjectPassageProjectionScore(double.PositiveInfinity, double.PositiveInfinity);
|
||
|
||
public ObjectPassageProjectionScore(double widthAcrossPath, double heightAlongHostUp)
|
||
{
|
||
WidthAcrossPath = widthAcrossPath;
|
||
HeightAlongHostUp = heightAlongHostUp;
|
||
Area = widthAcrossPath * heightAlongHostUp;
|
||
}
|
||
|
||
public double WidthAcrossPath { get; }
|
||
|
||
public double HeightAlongHostUp { get; }
|
||
|
||
public double Area { get; }
|
||
}
|
||
|
||
public static class ObjectPassageProjectionOptimizer
|
||
{
|
||
private const double Epsilon = 1e-9;
|
||
|
||
public static ObjectPassageProjectionOptimizationResult Optimize(
|
||
ObjectPassageProjectionOptimizationRequest request)
|
||
{
|
||
if (!TryValidateRequest(request, out string errorMessage))
|
||
{
|
||
return ObjectPassageProjectionOptimizationResult.CreateFailure(errorMessage);
|
||
}
|
||
|
||
return OptimizeWithEvaluator(request, correction => Evaluate(request, correction));
|
||
}
|
||
|
||
public static ObjectPassageProjectionOptimizationResult OptimizeWithEvaluator(
|
||
ObjectPassageProjectionOptimizationRequest request,
|
||
Func<LocalEulerRotationCorrection, ObjectPassageProjectionScore> evaluator)
|
||
{
|
||
if (!TryValidateRequest(request, out string errorMessage))
|
||
{
|
||
return ObjectPassageProjectionOptimizationResult.CreateFailure(errorMessage);
|
||
}
|
||
|
||
if (evaluator == null)
|
||
{
|
||
return ObjectPassageProjectionOptimizationResult.CreateFailure("自动调整评分器为空。");
|
||
}
|
||
|
||
LocalEulerRotationCorrection bestCorrection = LocalEulerRotationCorrection.Zero;
|
||
ObjectPassageProjectionScore bestScore = ObjectPassageProjectionScore.Invalid;
|
||
foreach (double xDegrees in CandidateAngles())
|
||
{
|
||
foreach (double yDegrees in CandidateAngles())
|
||
{
|
||
foreach (double zDegrees in CandidateAngles())
|
||
{
|
||
var correction = new LocalEulerRotationCorrection(xDegrees, yDegrees, zDegrees);
|
||
ObjectPassageProjectionScore score = evaluator(correction);
|
||
if (IsBetter(score, bestScore, request.AreaRelativeTieTolerance))
|
||
{
|
||
bestScore = score;
|
||
bestCorrection = correction;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
RefineByPatternSearch(request, evaluator, ref bestCorrection, ref bestScore);
|
||
|
||
if (double.IsInfinity(bestScore.Area))
|
||
{
|
||
return ObjectPassageProjectionOptimizationResult.CreateFailure("优化器未找到有效姿态。");
|
||
}
|
||
|
||
return ObjectPassageProjectionOptimizationResult.CreateSuccess(bestCorrection, bestScore);
|
||
}
|
||
|
||
public static ObjectPassageProjectionScore Evaluate(
|
||
ObjectPassageProjectionOptimizationRequest request,
|
||
LocalEulerRotationCorrection correction)
|
||
{
|
||
if (!TryCreateFrame(request, out Vector3 hostSide, out Vector3 hostUp))
|
||
{
|
||
throw new InvalidOperationException("路径方向退化，无法计算通行截面。");
|
||
}
|
||
|
||
Quaternion correctionRotation = correction.CreateQuaternion(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
|
||
Quaternion rotation = Quaternion.Normalize(correctionRotation * request.BaselineHostRotation);
|
||
Vector3 rotatedHostX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, rotation));
|
||
Vector3 rotatedHostY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, rotation));
|
||
Vector3 rotatedHostZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, rotation));
|
||
|
||
double width = ProjectExtent(
|
||
request.SizeX,
|
||
request.SizeY,
|
||
request.SizeZ,
|
||
rotatedHostX,
|
||
rotatedHostY,
|
||
rotatedHostZ,
|
||
hostSide);
|
||
|
||
double height = ProjectExtent(
|
||
request.SizeX,
|
||
request.SizeY,
|
||
request.SizeZ,
|
||
rotatedHostX,
|
||
rotatedHostY,
|
||
rotatedHostZ,
|
||
hostUp);
|
||
|
||
return new ObjectPassageProjectionScore(width, height);
|
||
}
|
||
|
||
private static bool TryValidateRequest(
|
||
ObjectPassageProjectionOptimizationRequest request,
|
||
out string errorMessage)
|
||
{
|
||
if (request == null)
|
||
{
|
||
errorMessage = "自动调整请求为空。";
|
||
return false;
|
||
}
|
||
|
||
if (request.SizeX <= Epsilon || request.SizeY <= Epsilon || request.SizeZ <= Epsilon)
|
||
{
|
||
errorMessage = "物体尺寸无效。";
|
||
return false;
|
||
}
|
||
|
||
if (!TryCreateFrame(request, out _, out _))
|
||
{
|
||
errorMessage = "路径方向退化，无法计算通行截面。";
|
||
return false;
|
||
}
|
||
|
||
errorMessage = null;
|
||
return true;
|
||
}
|
||
|
||
private static bool TryCreateFrame(
|
||
ObjectPassageProjectionOptimizationRequest request,
|
||
out Vector3 hostSide,
|
||
out Vector3 hostUp)
|
||
{
|
||
hostSide = Vector3.Zero;
|
||
hostUp = Vector3.Zero;
|
||
|
||
if (request == null || request.HostUp.LengthSquared() < Epsilon)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
hostUp = Vector3.Normalize(request.HostUp);
|
||
Vector3 hostPathForward = request.HostPathForward - hostUp * Vector3.Dot(request.HostPathForward, hostUp);
|
||
if (hostPathForward.LengthSquared() < Epsilon)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
hostPathForward = Vector3.Normalize(hostPathForward);
|
||
hostSide = Vector3.Cross(hostPathForward, hostUp);
|
||
if (hostSide.LengthSquared() < Epsilon)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
hostSide = Vector3.Normalize(hostSide);
|
||
return true;
|
||
}
|
||
|
||
private static bool IsBetter(
|
||
ObjectPassageProjectionScore candidate,
|
||
ObjectPassageProjectionScore currentBest,
|
||
double areaRelativeTieTolerance)
|
||
{
|
||
if (double.IsInfinity(currentBest.Area))
|
||
{
|
||
return true;
|
||
}
|
||
|
||
double tolerance = Math.Max(0.0, areaRelativeTieTolerance) * Math.Max(1.0, currentBest.Area);
|
||
if (candidate.Area < currentBest.Area - tolerance)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
if (Math.Abs(candidate.Area - currentBest.Area) <= tolerance)
|
||
{
|
||
if (candidate.HeightAlongHostUp < currentBest.HeightAlongHostUp - Epsilon)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
if (Math.Abs(candidate.HeightAlongHostUp - currentBest.HeightAlongHostUp) <= Epsilon &&
|
||
candidate.WidthAcrossPath < currentBest.WidthAcrossPath - Epsilon)
|
||
{
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private static double ProjectExtent(
|
||
double sizeX,
|
||
double sizeY,
|
||
double sizeZ,
|
||
Vector3 rotatedHostX,
|
||
Vector3 rotatedHostY,
|
||
Vector3 rotatedHostZ,
|
||
Vector3 targetAxis)
|
||
{
|
||
return Math.Abs(Vector3.Dot(rotatedHostX, targetAxis)) * sizeX +
|
||
Math.Abs(Vector3.Dot(rotatedHostY, targetAxis)) * sizeY +
|
||
Math.Abs(Vector3.Dot(rotatedHostZ, targetAxis)) * sizeZ;
|
||
}
|
||
|
||
private static double[] CandidateAngles()
|
||
{
|
||
return new[]
|
||
{
|
||
0.0,
|
||
90.0,
|
||
180.0,
|
||
270.0,
|
||
45.0,
|
||
135.0,
|
||
225.0,
|
||
315.0
|
||
};
|
||
}
|
||
|
||
private static void RefineByPatternSearch(
|
||
ObjectPassageProjectionOptimizationRequest request,
|
||
Func<LocalEulerRotationCorrection, ObjectPassageProjectionScore> evaluator,
|
||
ref LocalEulerRotationCorrection bestCorrection,
|
||
ref ObjectPassageProjectionScore bestScore)
|
||
{
|
||
double stepDegrees = 22.5;
|
||
while (stepDegrees >= 1.0)
|
||
{
|
||
bool improved = false;
|
||
foreach (var delta in PatternSearchDeltas(stepDegrees))
|
||
{
|
||
var candidate = new LocalEulerRotationCorrection(
|
||
NormalizeDegrees(bestCorrection.XDegrees + delta.X),
|
||
NormalizeDegrees(bestCorrection.YDegrees + delta.Y),
|
||
NormalizeDegrees(bestCorrection.ZDegrees + delta.Z));
|
||
ObjectPassageProjectionScore score = evaluator(candidate);
|
||
if (IsBetter(score, bestScore, request.AreaRelativeTieTolerance))
|
||
{
|
||
bestCorrection = candidate;
|
||
bestScore = score;
|
||
improved = true;
|
||
}
|
||
}
|
||
|
||
if (!improved)
|
||
{
|
||
stepDegrees *= 0.5;
|
||
}
|
||
}
|
||
}
|
||
|
||
private static (double X, double Y, double Z)[] PatternSearchDeltas(double stepDegrees)
|
||
{
|
||
return new[]
|
||
{
|
||
( stepDegrees, 0.0, 0.0),
|
||
(-stepDegrees, 0.0, 0.0),
|
||
(0.0, stepDegrees, 0.0),
|
||
(0.0, -stepDegrees, 0.0),
|
||
(0.0, 0.0, stepDegrees),
|
||
(0.0, 0.0, -stepDegrees),
|
||
( stepDegrees, stepDegrees, 0.0),
|
||
( stepDegrees, -stepDegrees, 0.0),
|
||
(-stepDegrees, stepDegrees, 0.0),
|
||
(-stepDegrees, -stepDegrees, 0.0),
|
||
( stepDegrees, 0.0, stepDegrees),
|
||
( stepDegrees, 0.0, -stepDegrees),
|
||
(-stepDegrees, 0.0, stepDegrees),
|
||
(-stepDegrees, 0.0, -stepDegrees),
|
||
(0.0, stepDegrees, stepDegrees),
|
||
(0.0, stepDegrees, -stepDegrees),
|
||
(0.0, -stepDegrees, stepDegrees),
|
||
(0.0, -stepDegrees, -stepDegrees)
|
||
};
|
||
}
|
||
|
||
private static double NormalizeDegrees(double degrees)
|
||
{
|
||
double normalized = degrees % 360.0;
|
||
if (normalized < 0.0)
|
||
{
|
||
normalized += 360.0;
|
||
}
|
||
|
||
return Math.Abs(normalized) < Epsilon ? 0.0 : normalized;
|
||
}
|
||
}
|
||
}
|