354 lines
12 KiB
C#
354 lines
12 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
|
||
namespace NavisworksTransport
|
||
{
|
||
/// <summary>
|
||
/// 路径曲线化引擎
|
||
/// 实现基于圆弧过渡（Arc Fillet）的路径曲线化功能
|
||
/// </summary>
|
||
public static class PathCurveEngine
|
||
{
|
||
/// <summary>
|
||
/// 将值限制在指定范围内（.NET Framework 4.8 不支持 Math.Clamp）
|
||
/// </summary>
|
||
private static double Clamp(double value, double min, double max)
|
||
{
|
||
if (value < min) return min;
|
||
if (value > max) return max;
|
||
return value;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算圆弧切点和轨迹参数
|
||
/// </summary>
|
||
/// <param name="pPrev">前一个控制点</param>
|
||
/// <param name="pCurr">当前控制点</param>
|
||
/// <param name="pNext">下一个控制点</param>
|
||
/// <param name="turnRadius">转向半径（米）</param>
|
||
/// <returns>圆弧轨迹数据</returns>
|
||
public static ArcTrajectory CalculateFillet(
|
||
Point3D pPrev,
|
||
Point3D pCurr,
|
||
Point3D pNext,
|
||
double turnRadius)
|
||
{
|
||
// 1. 计算单位向量
|
||
Vector3D v1 = (pPrev - pCurr).Normalize();
|
||
Vector3D v2 = (pNext - pCurr).Normalize();
|
||
|
||
// 2. 计算夹角 α
|
||
double cosAlpha = GeometryHelper.DotProduct(
|
||
new Point3D(v1.X, v1.Y, v1.Z),
|
||
new Point3D(v2.X, v2.Y, v2.Z));
|
||
double angleRad = Math.Acos(Clamp(cosAlpha, -1.0, 1.0));
|
||
|
||
// 如果几乎共线，返回无效轨迹
|
||
if (angleRad < 0.01 || angleRad > Math.PI - 0.01)
|
||
{
|
||
return new ArcTrajectory
|
||
{
|
||
Ts = pCurr,
|
||
Te = pCurr,
|
||
ArcCenter = pCurr,
|
||
RequestedRadius = turnRadius,
|
||
ActualRadius = 0,
|
||
DeflectionAngle = angleRad,
|
||
ArcLength = 0
|
||
};
|
||
}
|
||
|
||
// 3. 计算切线长 Lt = R / tan(α/2)
|
||
double Lt = turnRadius / Math.Tan(angleRad / 2.0);
|
||
|
||
// 4. 安全截断检查
|
||
double seg1Length = (pPrev - pCurr).Length;
|
||
double seg2Length = (pNext - pCurr).Length;
|
||
double maxAllowedLt = Math.Min(seg1Length, seg2Length) * 0.45;
|
||
|
||
double actualRadius = turnRadius;
|
||
if (Lt > maxAllowedLt)
|
||
{
|
||
Lt = maxAllowedLt;
|
||
actualRadius = Lt * Math.Tan(angleRad / 2.0);
|
||
}
|
||
|
||
// 5. 计算切点
|
||
Point3D ts = pCurr + v1 * Lt;
|
||
Point3D te = pCurr + v2 * Lt;
|
||
|
||
// 6. 计算圆心 (使用角平分线方向)
|
||
Vector3D bisector = (v1 + v2).Normalize();
|
||
double distToCenter = actualRadius / Math.Sin(angleRad / 2.0);
|
||
Point3D arcCenter = pCurr + bisector * distToCenter;
|
||
|
||
// 7. 计算圆弧长度
|
||
double arcLength = actualRadius * angleRad;
|
||
|
||
return new ArcTrajectory
|
||
{
|
||
Ts = ts,
|
||
Te = te,
|
||
ArcCenter = arcCenter,
|
||
RequestedRadius = turnRadius,
|
||
ActualRadius = actualRadius,
|
||
DeflectionAngle = angleRad,
|
||
ArcLength = arcLength
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// Rodrigues旋转公式 - 绕任意轴旋转点
|
||
/// </summary>
|
||
/// <param name="point">要旋转的点</param>
|
||
/// <param name="center">旋转中心</param>
|
||
/// <param name="axis">旋转轴（单位向量）</param>
|
||
/// <param name="angle">旋转角度（弧度）</param>
|
||
/// <returns>旋转后的点</returns>
|
||
private static Point3D RotatePointAroundAxis(
|
||
Point3D point,
|
||
Point3D center,
|
||
Vector3D axis,
|
||
double angle)
|
||
{
|
||
// 将点转换为相对于中心的向量
|
||
Vector3D v = point - center;
|
||
|
||
// Rodrigues旋转公式
|
||
// v_rot = v * cos(θ) + (k × v) * sin(θ) + k * (k · v) * (1 - cos(θ))
|
||
Point3D kxv = GeometryHelper.CrossProduct(
|
||
new Point3D(axis.X, axis.Y, axis.Z),
|
||
new Point3D(v.X, v.Y, v.Z));
|
||
double kdv = GeometryHelper.DotProduct(
|
||
new Point3D(axis.X, axis.Y, axis.Z),
|
||
new Point3D(v.X, v.Y, v.Z));
|
||
|
||
Vector3D kxvVec = new Vector3D(kxv.X, kxv.Y, kxv.Z);
|
||
Vector3D vRot = v * Math.Cos(angle) + kxvVec * Math.Sin(angle) + axis * kdv * (1 - Math.Cos(angle));
|
||
|
||
return center + vRot;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 圆弧采样为离散点序列
|
||
/// </summary>
|
||
/// <param name="trajectory">圆弧轨迹</param>
|
||
/// <param name="samplingStep">采样步长（米）</param>
|
||
/// <returns>采样点列表</returns>
|
||
public static List<Point3D> SampleArc(
|
||
ArcTrajectory trajectory,
|
||
double samplingStep)
|
||
{
|
||
var points = new List<Point3D>();
|
||
|
||
// 如果圆弧长度很小，直接返回起止点
|
||
if (trajectory.ArcLength < 0.001)
|
||
{
|
||
points.Add(trajectory.Ts);
|
||
points.Add(trajectory.Te);
|
||
return points;
|
||
}
|
||
|
||
// 根据采样步长计算采样点数量
|
||
int sampleCount = Math.Max(2, (int)Math.Ceiling(trajectory.ArcLength / samplingStep));
|
||
|
||
// 计算起始和结束向量
|
||
Vector3D startVec = (trajectory.Ts - trajectory.ArcCenter).Normalize();
|
||
Vector3D endVec = (trajectory.Te - trajectory.ArcCenter).Normalize();
|
||
|
||
// 计算旋转轴（叉乘）
|
||
Point3D cross = GeometryHelper.CrossProduct(
|
||
new Point3D(startVec.X, startVec.Y, startVec.Z),
|
||
new Point3D(endVec.X, endVec.Y, endVec.Z));
|
||
Vector3D rotationAxis = new Vector3D(cross.X, cross.Y, cross.Z).Normalize();
|
||
|
||
// 等角度插值
|
||
for (int i = 0; i <= sampleCount; i++)
|
||
{
|
||
double t = i / (double)sampleCount;
|
||
double theta = t * trajectory.DeflectionAngle;
|
||
|
||
// Rodrigues旋转公式
|
||
Point3D sampledPoint = RotatePointAroundAxis(
|
||
trajectory.Ts,
|
||
trajectory.ArcCenter,
|
||
rotationAxis,
|
||
theta
|
||
);
|
||
|
||
points.Add(sampledPoint);
|
||
}
|
||
|
||
return points;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构建直线边
|
||
/// </summary>
|
||
/// <param name="p1">起始点</param>
|
||
/// <param name="p2">结束点</param>
|
||
/// <param name="samplingStep">采样步长（米）</param>
|
||
/// <returns>路径边</returns>
|
||
private static PathEdge BuildStraightEdge(
|
||
PathPoint p1,
|
||
PathPoint p2,
|
||
double samplingStep)
|
||
{
|
||
var edge = new PathEdge
|
||
{
|
||
StartPointId = p1.Id,
|
||
EndPointId = p2.Id,
|
||
SegmentType = PathSegmentType.Straight,
|
||
PhysicalLength = (p1.Position - p2.Position).Length
|
||
};
|
||
|
||
// 采样直线段
|
||
int sampleCount = Math.Max(2, (int)Math.Ceiling(edge.PhysicalLength / samplingStep));
|
||
edge.SampledPoints = new List<Point3D>();
|
||
|
||
for (int i = 0; i <= sampleCount; i++)
|
||
{
|
||
double t = i / (double)sampleCount;
|
||
Point3D sampledPoint = p1.Position + (p2.Position - p1.Position) * t;
|
||
edge.SampledPoints.Add(sampledPoint);
|
||
}
|
||
|
||
return edge;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构建包含圆弧的边
|
||
/// </summary>
|
||
/// <param name="p1">起始点</param>
|
||
/// <param name="p2">结束点</param>
|
||
/// <param name="arcTrajectory">圆弧轨迹</param>
|
||
/// <param name="samplingStep">采样步长（米）</param>
|
||
/// <returns>路径边</returns>
|
||
private static PathEdge BuildEdgeWithArc(
|
||
PathPoint p1,
|
||
PathPoint p2,
|
||
ArcTrajectory arcTrajectory,
|
||
double samplingStep)
|
||
{
|
||
var edge = new PathEdge
|
||
{
|
||
StartPointId = p1.Id,
|
||
EndPointId = p2.Id,
|
||
SegmentType = PathSegmentType.Arc,
|
||
Trajectory = arcTrajectory,
|
||
PhysicalLength = arcTrajectory.ArcLength
|
||
};
|
||
|
||
// 采样圆弧段
|
||
edge.SampledPoints = SampleArc(arcTrajectory, samplingStep);
|
||
|
||
return edge;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 生成采样点序列（用于延迟加载）
|
||
/// </summary>
|
||
/// <param name="edge">路径边</param>
|
||
/// <param name="samplingStep">采样步长（米）</param>
|
||
/// <returns>采样点列表</returns>
|
||
public static List<Point3D> GenerateSampledPoints(PathEdge edge, double samplingStep)
|
||
{
|
||
if (edge.SegmentType == PathSegmentType.Straight)
|
||
{
|
||
// 直线段采样
|
||
int sampleCount = Math.Max(2, (int)Math.Ceiling(edge.PhysicalLength / samplingStep));
|
||
var points = new List<Point3D>();
|
||
|
||
for (int i = 0; i <= sampleCount; i++)
|
||
{
|
||
double t = i / (double)sampleCount;
|
||
Point3D p1 = edge.Trajectory?.Ts ?? edge.Trajectory?.Te ?? new Point3D();
|
||
Point3D p2 = edge.Trajectory?.Te ?? new Point3D();
|
||
Point3D sampledPoint = p1 + (p2 - p1) * t;
|
||
points.Add(sampledPoint);
|
||
}
|
||
|
||
return points;
|
||
}
|
||
else if (edge.SegmentType == PathSegmentType.Arc && edge.Trajectory != null)
|
||
{
|
||
// 圆弧段采样
|
||
return SampleArc(edge.Trajectory, samplingStep);
|
||
}
|
||
|
||
return new List<Point3D>();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 对路径应用曲线化处理
|
||
/// </summary>
|
||
/// <param name="route">待处理的路径</param>
|
||
/// <param name="samplingStep">采样步长（米）</param>
|
||
public static void ApplyCurvatureToRoute(PathRoute route, double samplingStep)
|
||
{
|
||
if (route == null)
|
||
throw new ArgumentNullException(nameof(route));
|
||
|
||
route.Edges.Clear();
|
||
var sortedPoints = route.Points.OrderBy(p => p.Index).ToList();
|
||
|
||
if (sortedPoints.Count < 2)
|
||
{
|
||
route.IsCurved = false;
|
||
return;
|
||
}
|
||
|
||
// 处理每一段路径
|
||
for (int i = 0; i < sortedPoints.Count - 1; i++)
|
||
{
|
||
var p1 = sortedPoints[i];
|
||
var p2 = sortedPoints[i + 1];
|
||
|
||
// 判断是否为转折点（需要圆弧过渡）
|
||
bool needsArc = (i > 0 && i < sortedPoints.Count - 1);
|
||
|
||
if (needsArc)
|
||
{
|
||
var p0 = sortedPoints[i - 1];
|
||
var p2Next = sortedPoints[i + 2];
|
||
|
||
// 获取转向半径（优先使用自定义值）
|
||
double radius = p1.CustomTurnRadius ?? route.TurnRadius;
|
||
|
||
// 计算圆弧
|
||
var arcTraj = CalculateFillet(p0.Position, p1.Position, p2Next.Position, radius);
|
||
|
||
// 创建Edge（包含圆弧段）
|
||
var edge = BuildEdgeWithArc(p1, p2, arcTraj, samplingStep);
|
||
route.Edges.Add(edge);
|
||
}
|
||
else
|
||
{
|
||
// 直线边
|
||
var edge = BuildStraightEdge(p1, p2, samplingStep);
|
||
route.Edges.Add(edge);
|
||
}
|
||
}
|
||
|
||
route.IsCurved = true;
|
||
RecalculateRouteLength(route);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 重新计算路径总长度
|
||
/// </summary>
|
||
/// <param name="route">路径</param>
|
||
public static void RecalculateRouteLength(PathRoute route)
|
||
{
|
||
if (route == null || route.Edges == null)
|
||
{
|
||
route.TotalLength = 0;
|
||
return;
|
||
}
|
||
|
||
route.TotalLength = route.Edges.Sum(e => e.PhysicalLength);
|
||
}
|
||
}
|
||
} |