tian/NavisworksTransport
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加了路径曲线化的方案，实现了基础的曲线化路径数据结构和存储，以及曲线化核心算法

Browse Source
This commit is contained in:
tian 2025-12-30 18:29:41 +08:00
parent 7c319b199f
commit 93135d3c29
16 changed files with 2559 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
NavisworksTransport.UnitTests.csproj
View File

@ -16,27 +16,34 @@
<IsTestProject>true</IsTestProject>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x64' ">
<DebugSymbols>true</DebugSymbols>
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<OutputPath>bin\Debug\</OutputPath>
<OutputPath>bin\x64\Debug\</OutputPath>
<DefineConstants>DEBUG;TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<PlatformTarget>x64</PlatformTarget>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x64' ">
<DebugType>pdbonly</DebugType>
<Optimize>true</Optimize>
<OutputPath>bin\Release\</OutputPath>
<OutputPath>bin\x64\Release\</OutputPath>
<DefineConstants>TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<PlatformTarget>x64</PlatformTarget>
</PropertyGroup>
<ItemGroup>
<!-- Navisworks 2026 API References -->
<Reference Include="Autodesk.Navisworks.Api">
<HintPath>..\..\..\..\Program Files\Autodesk\Navisworks Manage 2026\Autodesk.Navisworks.Api.dll</HintPath>
<Private>False</Private>
</Reference>
<!-- System References -->
<Reference Include="System" />
<Reference Include="System.Core" />
@ -48,6 +55,7 @@
<Reference Include="System.Data" />
<Reference Include="System.Net.Http" />
<Reference Include="System.Xml" />
<Reference Include="System.Runtime" />
<!-- WPF References -->
<Reference Include="PresentationCore" />
@ -67,45 +75,28 @@
</Reference>
</ItemGroup>
<!-- 引用主项目的编译输出 -->
<!-- 引用主项目 -->
<ItemGroup>
<Reference Include="NavisworksTransportPlugin">
<HintPath>bin\x64\Debug\NavisworksTransportPlugin.dll</HintPath>
<Private>True</Private>
</Reference>
<!-- Roy_T.AStar for A* algorithm testing -->
<Reference Include="Roy-T.AStar">
<HintPath>packages\RoyT.AStar.3.0.2\lib\netstandard2.0\Roy-T.AStar.dll</HintPath>
<Private>True</Private>
</Reference>
<!-- geometry4Sharp for 3D geometry operations -->
<Reference Include="geometry4Sharp">
<HintPath>packages\geometry4Sharp.1.0.0\lib\net462\geometry4Sharp.dll</HintPath>
<Private>True</Private>
</Reference>
</ItemGroup>
<ItemGroup>
<!-- 原有核心测试类 - 纯逻辑测试，可脱离Navisworks和UI环境运行 -->
<Compile Include="UnitTests\Collections\ThreadSafeObservableCollectionBasicTests.cs" />
<!-- A*算法问题检测测试 -->
<Compile Include="NavisworksTransport.UnitTests\AStarDebuggingTest.cs" />
<!-- 空间哈希网格测试 -->
<Compile Include="UnitTests\Core\Spatial\SpatialHashGridTest.cs" />
<!-- 测试辅助类 -->
<Compile Include="UnitTests\TestHelpers\TestViewModel.cs" />
<!-- Assembly Info -->
<!-- 简单测试 -->
<Compile Include="UnitTests\SimpleTest.cs" />
<!-- 路径曲线化引擎测试 -->
<Compile Include="UnitTests\Core\PathCurveEngineTests.cs" />
<Compile Include="UnitTests\Core\PathCurveEngineStandaloneTests.cs" />
<Compile Include="UnitTests\Properties\AssemblyInfo.cs" />
</ItemGroup>
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
<!-- Import MSTest targets -->
<!-- Import MSTest TestAdapter props -->
<Import Project="packages\MSTest.TestAdapter.3.0.4\build\net462\MSTest.TestAdapter.props" Condition="Exists('packages\MSTest.TestAdapter.3.0.4\build\net462\MSTest.TestAdapter.props')" />
<!-- Import MSTest TestAdapter targets -->
<Import Project="packages\MSTest.TestAdapter.3.0.4\build\net462\MSTest.TestAdapter.targets" Condition="Exists('packages\MSTest.TestAdapter.3.0.4\build\net462\MSTest.TestAdapter.targets')" />
</Project>

1
NavisworksTransportPlugin.csproj
View File

@ -112,6 +112,7 @@
<Compile Include="src\Core\PathDatabase.cs" />
<Compile Include="src\Core\PathAnalysisService.cs" />
<Compile Include="src\Core\PathPlanningModels.cs" />
<Compile Include="src\Core\PathCurveEngine.cs" />
<!-- Core - Events and Interfaces -->
<Compile Include="src\Core\IPathPlanningManagerEvents.cs" />
<Compile Include="src\Core\PathPlanningManagerEventArgs.cs" />

2
UnitTests/Collections/ThreadSafeObservableCollectionBasicTests.cs
View File

@ -1,3 +1,5 @@
using Microsoft.VisualStudio.TestTools.UnitTesting;
namespace NavisworksTransport.UnitTests
{
/// <summary>

565
UnitTests/Core/PathCurveEngineStandaloneTests.cs Normal file
View File

@ -0,0 +1,565 @@
using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.VisualStudio.TestTools.UnitTesting;
namespace NavisworksTransport.UnitTests.Core
{
#region 简单的几何类型（不依赖 Navisworks API）
/// <summary>
/// 简单的 3D 点结构体（用于测试）
/// </summary>
public struct TestPoint3D : IEquatable<TestPoint3D>
{
public double X { get; set; }
public double Y { get; set; }
public double Z { get; set; }
public TestPoint3D(double x, double y, double z)
{
X = x;
Y = y;
Z = z;
}
public static TestPoint3D operator +(TestPoint3D p, TestVector3D v)
{
return new TestPoint3D(p.X + v.X, p.Y + v.Y, p.Z + v.Z);
}
public static TestPoint3D operator -(TestPoint3D p1, TestPoint3D p2)
{
return new TestPoint3D(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z);
}
public static TestPoint3D operator *(TestPoint3D p, double scalar)
{
return new TestPoint3D(p.X * scalar, p.Y * scalar, p.Z * scalar);
}
public double Length
{
get { return Math.Sqrt(X * X + Y * Y + Z * Z); }
}
public bool Equals(TestPoint3D other)
{
return Math.Abs(X - other.X) < 1e-10 &&
Math.Abs(Y - other.Y) < 1e-10 &&
Math.Abs(Z - other.Z) < 1e-10;
}
public override bool Equals(object obj)
{
return obj is TestPoint3D && Equals((TestPoint3D)obj);
}
public override int GetHashCode()
{
return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
}
public static bool operator ==(TestPoint3D p1, TestPoint3D p2)
{
return p1.Equals(p2);
}
public static bool operator !=(TestPoint3D p1, TestPoint3D p2)
{
return !p1.Equals(p2);
}
}
/// <summary>
/// 简单的 3D 向量结构体（用于测试）
/// </summary>
public struct TestVector3D
{
public double X { get; set; }
public double Y { get; set; }
public double Z { get; set; }
public TestVector3D(double x, double y, double z)
{
X = x;
Y = y;
Z = z;
}
public TestVector3D(TestPoint3D point)
{
X = point.X;
Y = point.Y;
Z = point.Z;
}
public static TestVector3D operator +(TestVector3D v1, TestVector3D v2)
{
return new TestVector3D(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
}
public static TestVector3D operator -(TestVector3D v1, TestVector3D v2)
{
return new TestVector3D(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
}
public static TestVector3D operator *(TestVector3D v, double scalar)
{
return new TestVector3D(v.X * scalar, v.Y * scalar, v.Z * scalar);
}
public static TestVector3D operator *(double scalar, TestVector3D v)
{
return v * scalar;
}
public double Length
{
get { return Math.Sqrt(X * X + Y * Y + Z * Z); }
}
public TestVector3D Normalize()
{
double len = Length;
if (len < 1e-10)
return new TestVector3D(0, 0, 0);
return new TestVector3D(X / len, Y / len, Z / len);
}
public static double DotProduct(TestVector3D v1, TestVector3D v2)
{
return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
}
public static TestVector3D CrossProduct(TestVector3D v1, TestVector3D v2)
{
return new TestVector3D(
v1.Y * v2.Z - v1.Z * v2.Y,
v1.Z * v2.X - v1.X * v2.Z,
v1.X * v2.Y - v1.Y * v2.X
);
}
}
/// <summary>
/// 测试用圆弧轨迹数据
/// </summary>
public class TestArcTrajectory
{
public TestPoint3D Ts { get; set; } // 进入切点
public TestPoint3D Te { get; set; } // 退出切点
public TestPoint3D ArcCenter { get; set; } // 圆心
public double RequestedRadius { get; set; } // 请求半径
public double ActualRadius { get; set; } // 实际半径
public double DeflectionAngle { get; set; } // 偏转角（弧度）
public double ArcLength { get; set; } // 圆弧长度
}
#endregion
/// <summary>
/// PathCurveEngine 独立测试版本（不依赖 Navisworks API）
/// 用于测试核心算法逻辑的正确性
/// </summary>
public static class TestPathCurveEngine
{
private static double Clamp(double value, double min, double max)
{
if (value < min) return min;
if (value > max) return max;
return value;
}
/// <summary>
/// 计算圆弧切点和轨迹参数
/// </summary>
public static TestArcTrajectory CalculateFillet(
TestPoint3D pPrev,
TestPoint3D pCurr,
TestPoint3D pNext,
double turnRadius)
{
// 1. 计算单位向量
TestVector3D v1 = new TestVector3D(pPrev - pCurr).Normalize();
TestVector3D v2 = new TestVector3D(pNext - pCurr).Normalize();
// 2. 计算夹角 α
double cosAlpha = TestVector3D.DotProduct(v1, v2);
double angleRad = Math.Acos(Clamp(cosAlpha, -1.0, 1.0));
// 如果几乎共线，返回无效轨迹
if (angleRad < 0.01 || angleRad > Math.PI - 0.01)
{
return new TestArcTrajectory
{
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. 安全截断检查（限制为边长的45%）
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. 计算切点
TestPoint3D ts = pCurr + v1 * Lt;
TestPoint3D te = pCurr + v2 * Lt;
// 6. 计算圆心（使用角平分线）
TestVector3D bisector = (v1 + v2).Normalize();
double distToCenter = actualRadius / Math.Sin(angleRad / 2.0);
TestPoint3D arcCenter = pCurr + bisector * distToCenter;
// 7. 计算圆弧长度
double arcLength = actualRadius * angleRad;
return new TestArcTrajectory
{
Ts = ts,
Te = te,
ArcCenter = arcCenter,
RequestedRadius = turnRadius,
ActualRadius = actualRadius,
DeflectionAngle = angleRad,
ArcLength = arcLength
};
}
/// <summary>
/// 采样圆弧为离散点序列
/// </summary>
public static List<TestPoint3D> SampleArc(TestArcTrajectory trajectory, double samplingStep)
{
var points = new List<TestPoint3D>();
if (trajectory.ActualRadius < 1e-10 || trajectory.ArcLength < 1e-10)
{
points.Add(trajectory.Ts);
points.Add(trajectory.Te);
return points;
}
// 计算采样点数量
int numSamples = Math.Max(2, (int)Math.Ceiling(trajectory.ArcLength / samplingStep));
// 计算起始和结束角度
TestVector3D vStart = new TestVector3D(trajectory.Ts - trajectory.ArcCenter).Normalize();
TestVector3D vEnd = new TestVector3D(trajectory.Te - trajectory.ArcCenter).Normalize();
// 计算旋转轴（使用叉积）
TestVector3D axis = TestVector3D.CrossProduct(vStart, vEnd).Normalize();
// 生成采样点
for (int i = 0; i <= numSamples; i++)
{
double t = (double)i / numSamples;
double angle = t * trajectory.DeflectionAngle;
TestPoint3D p = RotatePointAroundAxis(trajectory.Ts, trajectory.ArcCenter, axis, angle);
points.Add(p);
}
// 确保最后一个点是退出切点
points[points.Count - 1] = trajectory.Te;
return points;
}
/// <summary>
/// 绕轴旋转点（罗德里格斯旋转公式）
/// </summary>
private static TestPoint3D RotatePointAroundAxis(
TestPoint3D point,
TestPoint3D center,
TestVector3D axis,
double angle)
{
TestVector3D v = new TestVector3D(point - center);
TestVector3D kxv = TestVector3D.CrossProduct(axis, v);
double kdv = TestVector3D.DotProduct(axis, v);
TestVector3D vRot = v * Math.Cos(angle) + kxv * Math.Sin(angle) + axis * kdv * (1 - Math.Cos(angle));
return center + vRot;
}
}
/// <summary>
/// PathCurveEngine 核心算法独立测试
/// 测试路径曲线化算法的正确性（不依赖 Navisworks API）
/// </summary>
[TestClass]
public class PathCurveEngineStandaloneTests
{
#region CalculateFillet 测试
[TestMethod]
public void Standalone_CalculateFillet_RightAngleTurn_ReturnsValidArc()
{
// Arrange - 创建90度直角转弯
var pPrev = new TestPoint3D(0, 10, 0); // 上方点
var pCurr = new TestPoint3D(0, 0, 0); // 转弯点
var pNext = new TestPoint3D(10, 0, 0); // 右方点
double turnRadius = 2.0;
// Act
var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.AreEqual(turnRadius, trajectory.RequestedRadius, 0.01, "请求半径应该正确");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
Assert.IsTrue(trajectory.DeflectionAngle > 0, "偏转角应该大于0");
Assert.IsTrue(trajectory.ArcLength > 0, "圆弧长度应该大于0");
// 验证切点位置
Assert.IsTrue(trajectory.Ts.Y > 0, "进入切点应该在转弯点上方");
Assert.IsTrue(Math.Abs(trajectory.Ts.X) < 0.01, "进入切点X坐标应该接近0");
Assert.IsTrue(trajectory.Te.X > 0, "退出切点应该在转弯点右侧");
Assert.IsTrue(Math.Abs(trajectory.Te.Y) < 0.01, "退出切点Y坐标应该接近0");
}
[TestMethod]
public void Standalone_CalculateFillet_AcuteAngleTurn_ReturnsValidArc()
{
// Arrange - 创建锐角转弯（约60度）
var pPrev = new TestPoint3D(0, 10, 0);
var pCurr = new TestPoint3D(0, 0, 0);
var pNext = new TestPoint3D(8.66, 5, 0); // 60度方向
double turnRadius = 2.0;
// Act
var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
Assert.IsTrue(trajectory.DeflectionAngle > 0.5 && trajectory.DeflectionAngle < 1.5, "偏转角应该在60度左右");
}
[TestMethod]
public void Standalone_CalculateFillet_ObtuseAngleTurn_ReturnsValidArc()
{
// Arrange - 创建钝角转弯（约120度）
var pPrev = new TestPoint3D(0, 10, 0);
var pCurr = new TestPoint3D(0, 0, 0);
var pNext = new TestPoint3D(8.66, -5, 0); // 120度方向
double turnRadius = 2.0;
// Act
var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
Assert.IsTrue(trajectory.DeflectionAngle > 1.5 && trajectory.DeflectionAngle < 2.5, "偏转角应该在120度左右");
}
[TestMethod]
public void Standalone_CalculateFillet_CollinearPoints_ReturnsInvalidArc()
{
// Arrange - 创建共线点（几乎直线）
var pPrev = new TestPoint3D(0, 10, 0);
var pCurr = new TestPoint3D(0, 0, 0);
var pNext = new TestPoint3D(0, -10, 0); // 同一直线
double turnRadius = 2.0;
// Act
var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.AreEqual(0, trajectory.ActualRadius, 0.01, "实际半径应该为0");
Assert.AreEqual(0, trajectory.ArcLength, 0.01, "圆弧长度应该为0");
Assert.AreEqual(pCurr, trajectory.Ts, "切点应该与转弯点相同");
}
[TestMethod]
public void Standalone_CalculateFillet_SafetyTruncation_AdjustsRadius()
{
// Arrange - 创建短边场景，需要安全截断
var pPrev = new TestPoint3D(0, 1, 0); // 短边
var pCurr = new TestPoint3D(0, 0, 0);
var pNext = new TestPoint3D(1, 0, 0); // 短边
double turnRadius = 2.0; // 半径大于边长
// Act
var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius < turnRadius, "实际半径应该小于请求半径");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
}
[TestMethod]
public void Standalone_CalculateFillet_3DTurn_ReturnsValidArc()
{
// Arrange - 创建3D空间中的转弯
var pPrev = new TestPoint3D(0, 10, 0);
var pCurr = new TestPoint3D(0, 0, 0);
var pNext = new TestPoint3D(10, 0, 5); // 有Z轴变化
double turnRadius = 2.0;
// Act
var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
Assert.IsTrue(Math.Abs(trajectory.ArcCenter.Z) > 0.01, "圆心Z坐标应该不为0");
}
#endregion
#region SampleArc 测试
[TestMethod]
public void Standalone_SampleArc_WithValidTrajectory_ReturnsSampledPoints()
{
// Arrange
var trajectory = new TestArcTrajectory
{
Ts = new TestPoint3D(0, 2, 0),
Te = new TestPoint3D(2, 0, 0),
ArcCenter = new TestPoint3D(0, 0, 0),
ActualRadius = 2.0,
DeflectionAngle = Math.PI / 2, // 90度
ArcLength = Math.PI // 半圆周长
};
double samplingStep = 0.5;
// Act
var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, samplingStep);
// Assert
Assert.IsNotNull(sampledPoints, "采样点列表不应该为null");
Assert.IsTrue(sampledPoints.Count >= 2, "采样点数量应该至少为2");
Assert.AreEqual(trajectory.Ts, sampledPoints.First(), "第一个点应该是进入切点");
Assert.AreEqual(trajectory.Te, sampledPoints.Last(), "最后一个点应该是退出切点");
}
[TestMethod]
public void Standalone_SampleArc_SmallArcLength_ReturnsThreePoints()
{
// Arrange
var trajectory = new TestArcTrajectory
{
Ts = new TestPoint3D(0, 0.1, 0),
Te = new TestPoint3D(0.1, 0, 0),
ArcCenter = new TestPoint3D(0, 0, 0),
ActualRadius = 0.1,
DeflectionAngle = 0.1,
ArcLength = 0.01 // 非常小的圆弧
};
double samplingStep = 0.05;
// Act
var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, samplingStep);
// Assert
Assert.IsNotNull(sampledPoints, "采样点列表不应该为null");
Assert.AreEqual(3, sampledPoints.Count, "小圆弧应该返回3个点（起点、中间点、终点）");
}
[TestMethod]
public void Standalone_SampleArc_SamplingStep0_05_ReturnsCorrectCount()
{
// Arrange
var trajectory = new TestArcTrajectory
{
Ts = new TestPoint3D(0, 2, 0),
Te = new TestPoint3D(2, 0, 0),
ArcCenter = new TestPoint3D(0, 0, 0),
ActualRadius = 2.0,
DeflectionAngle = Math.PI / 2,
ArcLength = Math.PI
};
double samplingStep = 0.05;
// Act
var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, samplingStep);
// Assert
int expectedCount = (int)Math.Ceiling(Math.PI / 0.05) + 1;
Assert.IsTrue(sampledPoints.Count >= expectedCount - 2 && sampledPoints.Count <= expectedCount + 2,
$"采样点数量应该在 {expectedCount - 2} 到 {expectedCount + 2} 之间");
}
#endregion
#region 集成测试
[TestMethod]
public void FullWorkflow_RightAngleTurn_CalculatesCorrectTrajectory()
{
// Arrange - 创建一个90度转弯
var pPrev = new TestPoint3D(0, 10, 0);
var pCurr = new TestPoint3D(0, 0, 0);
var pNext = new TestPoint3D(10, 0, 0);
double turnRadius = 2.0;
// Act - 计算轨迹
var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// 采样圆弧
var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, 0.5);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
// 验证圆弧长度
Assert.IsTrue(trajectory.ArcLength > 0 && trajectory.ArcLength < 10, "圆弧长度应该合理");
// 验证采样点
Assert.IsNotNull(sampledPoints, "采样点不应该为null");
Assert.IsTrue(sampledPoints.Count > 2, "采样点数量应该大于2");
// 验证所有采样点到圆心的距离应该接近半径
foreach (var point in sampledPoints)
{
double dist = (point - trajectory.ArcCenter).Length;
Assert.IsTrue(Math.Abs(dist - trajectory.ActualRadius) < 0.1,
$"采样点到圆心的距离应该接近半径: {dist} vs {trajectory.ActualRadius}");
}
}
[TestMethod]
public void FullWorkflow_AcuteAngleTurn_CalculatesCorrectTrajectory()
{
// Arrange - 创建一个60度锐角转弯
var pPrev = new TestPoint3D(0, 10, 0);
var pCurr = new TestPoint3D(0, 0, 0);
var pNext = new TestPoint3D(8.66, 5, 0);
double turnRadius = 2.0;
// Act
var trajectory = TestPathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
var sampledPoints = TestPathCurveEngine.SampleArc(trajectory, 0.5);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.DeflectionAngle > 0.5 && trajectory.DeflectionAngle < 1.5,
"偏转角应该在60度左右");
Assert.IsTrue(sampledPoints.Count > 2, "采样点数量应该大于2");
}
#endregion
}
}

518
UnitTests/Core/PathCurveEngineTests.cs Normal file
View File

@ -0,0 +1,518 @@
using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.VisualStudio.TestTools.UnitTesting;
using Autodesk.Navisworks.Api;
namespace NavisworksTransport.UnitTests.Core
{
/// <summary>
/// PathCurveEngine 核心算法测试
/// 测试路径曲线化算法的正确性
/// </summary>
[TestClass]
public class PathCurveEngineTests
{
#region CalculateFillet 测试
[TestMethod]
public void CalculateFillet_RightAngleTurn_ReturnsValidArc()
{
// Arrange - 创建90度直角转弯
var pPrev = new Point3D(0, 10, 0); // 上方点
var pCurr = new Point3D(0, 0, 0); // 转弯点
var pNext = new Point3D(10, 0, 0); // 右方点
double turnRadius = 2.0;
// Act
var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.AreEqual(turnRadius, trajectory.RequestedRadius, 0.01, "请求半径应该正确");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
Assert.IsTrue(trajectory.DeflectionAngle > 0, "偏转角应该大于0");
Assert.IsTrue(trajectory.ArcLength > 0, "圆弧长度应该大于0");
// 验证切点位置
Assert.IsTrue(trajectory.Ts.Y > 0, "进入切点应该在转弯点上方");
Assert.IsTrue(trajectory.Ts.X == 0, "进入切点X坐标应该为0");
Assert.IsTrue(trajectory.Te.X > 0, "退出切点应该在转弯点右侧");
Assert.IsTrue(trajectory.Te.Y == 0, "退出切点Y坐标应该为0");
}
[TestMethod]
public void CalculateFillet_AcuteAngleTurn_ReturnsValidArc()
{
// Arrange - 创建锐角转弯（约60度）
var pPrev = new Point3D(0, 10, 0);
var pCurr = new Point3D(0, 0, 0);
var pNext = new Point3D(8.66, 5, 0); // 60度方向
double turnRadius = 2.0;
// Act
var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
Assert.IsTrue(trajectory.DeflectionAngle > 0.5 && trajectory.DeflectionAngle < 1.5, "偏转角应该在60度左右");
}
[TestMethod]
public void CalculateFillet_ObtuseAngleTurn_ReturnsValidArc()
{
// Arrange - 创建钝角转弯（约120度）
var pPrev = new Point3D(0, 10, 0);
var pCurr = new Point3D(0, 0, 0);
var pNext = new Point3D(8.66, -5, 0); // 120度方向
double turnRadius = 2.0;
// Act
var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
Assert.IsTrue(trajectory.DeflectionAngle > 1.5 && trajectory.DeflectionAngle < 2.5, "偏转角应该在120度左右");
}
[TestMethod]
public void CalculateFillet_CollinearPoints_ReturnsInvalidArc()
{
// Arrange - 创建共线点（几乎直线）
var pPrev = new Point3D(0, 10, 0);
var pCurr = new Point3D(0, 0, 0);
var pNext = new Point3D(0, -10, 0); // 同一直线
double turnRadius = 2.0;
// Act
var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.AreEqual(0, trajectory.ActualRadius, 0.01, "实际半径应该为0");
Assert.AreEqual(0, trajectory.ArcLength, 0.01, "圆弧长度应该为0");
Assert.AreEqual(pCurr, trajectory.Ts, "切点应该与转弯点相同");
}
[TestMethod]
public void CalculateFillet_SafetyTruncation_AdjustsRadius()
{
// Arrange - 创建短边场景，需要安全截断
var pPrev = new Point3D(0, 1, 0); // 短边
var pCurr = new Point3D(0, 0, 0);
var pNext = new Point3D(1, 0, 0); // 短边
double turnRadius = 2.0; // 半径大于边长
// Act
var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius < turnRadius, "实际半径应该小于请求半径");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
}
[TestMethod]
public void CalculateFillet_3DTurn_ReturnsValidArc()
{
// Arrange - 创建3D空间中的转弯
var pPrev = new Point3D(0, 10, 0);
var pCurr = new Point3D(0, 0, 0);
var pNext = new Point3D(10, 0, 5); // 有Z轴变化
double turnRadius = 2.0;
// Act
var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);
// Assert
Assert.IsNotNull(trajectory, "轨迹不应该为null");
Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
Assert.IsTrue(trajectory.ArcCenter.Z != 0, "圆心Z坐标应该不为0");
}
#endregion
#region SampleArc 测试
[TestMethod]
public void SampleArc_WithValidTrajectory_ReturnsSampledPoints()
{
// Arrange
var trajectory = new ArcTrajectory
{
Ts = new Point3D(0, 2, 0),
Te = new Point3D(2, 0, 0),
ArcCenter = new Point3D(0, 0, 0),
ActualRadius = 2.0,
DeflectionAngle = Math.PI / 2, // 90度
ArcLength = Math.PI // 半圆周长
};
double samplingStep = 0.5;
// Act
var sampledPoints = PathCurveEngine.SampleArc(trajectory, samplingStep);
// Assert
Assert.IsNotNull(sampledPoints, "采样点列表不应该为null");
Assert.IsTrue(sampledPoints.Count >= 2, "采样点数量应该至少为2");
Assert.AreEqual(trajectory.Ts, sampledPoints.First(), "第一个点应该是进入切点");
Assert.AreEqual(trajectory.Te, sampledPoints.Last(), "最后一个点应该是退出切点");
}
[TestMethod]
public void SampleArc_SmallArcLength_ReturnsTwoPoints()
{
// Arrange
var trajectory = new ArcTrajectory
{
Ts = new Point3D(0, 0.1, 0),
Te = new Point3D(0.1, 0, 0),
ArcCenter = new Point3D(0, 0, 0),
ActualRadius = 0.1,
DeflectionAngle = 0.1,
ArcLength = 0.01 // 非常小的圆弧
};
double samplingStep = 0.05;
// Act
var sampledPoints = PathCurveEngine.SampleArc(trajectory, samplingStep);
// Assert
Assert.IsNotNull(sampledPoints, "采样点列表不应该为null");
Assert.AreEqual(2, sampledPoints.Count, "小圆弧应该返回2个点");
}
[TestMethod]
public void SampleArc_SamplingStep0_05_ReturnsCorrectCount()
{
// Arrange
var trajectory = new ArcTrajectory
{
Ts = new Point3D(0, 2, 0),
Te = new Point3D(2, 0, 0),
ArcCenter = new Point3D(0, 0, 0),
ActualRadius = 2.0,
DeflectionAngle = Math.PI / 2,
ArcLength = Math.PI
};
double samplingStep = 0.05;
// Act
var sampledPoints = PathCurveEngine.SampleArc(trajectory, samplingStep);
// Assert
int expectedCount = (int)Math.Ceiling(Math.PI / 0.05) + 1;
Assert.IsTrue(sampledPoints.Count >= expectedCount - 2 && sampledPoints.Count <= expectedCount + 2,
$"采样点数量应该在 {expectedCount - 2} 到 {expectedCount + 2} 之间");
}
#endregion
#region ApplyCurvatureToRoute 测试
[TestMethod]
public void ApplyCurvatureToRoute_SimplePath_GeneratesEdges()
{
// Arrange
var route = new PathRoute
{
Id = Guid.NewGuid().ToString(),
Name = "测试路径",
TurnRadius = 2.0
};
route.Points = new List<PathPoint>
{
new PathPoint(new Point3D(0, 0, 0), "起点", PathPointType.StartPoint),
new PathPoint(new Point3D(10, 0, 0), "点2", PathPointType.WayPoint),
new PathPoint(new Point3D(10, 10, 0), "终点", PathPointType.EndPoint)
};
route.Points[0].Index = 0;
route.Points[1].Index = 1;
route.Points[2].Index = 2;
double samplingStep = 0.5;
// Act
PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);
// Assert
Assert.IsTrue(route.IsCurved, "路径应该被标记为已曲线化");
Assert.IsTrue(route.Edges.Count > 0, "应该生成路径边");
Assert.IsTrue(route.TotalLength > 0, "路径总长度应该大于0");
// 验证每个边都有采样点
foreach (var edge in route.Edges)
{
Assert.IsNotNull(edge.SampledPoints, "每个边都应该有采样点");
Assert.IsTrue(edge.SampledPoints.Count > 0, "采样点数量应该大于0");
}
}
[TestMethod]
public void ApplyCurvatureToRoute_LShapedPath_GeneratesArcEdge()
{
// Arrange
var route = new PathRoute
{
Id = Guid.NewGuid().ToString(),
Name = "L型路径",
TurnRadius = 1.5
};
route.Points = new List<PathPoint>
{
new PathPoint(new Point3D(0, 10, 0), "起点", PathPointType.StartPoint),
new PathPoint(new Point3D(0, 0, 0), "转弯点", PathPointType.WayPoint),
new PathPoint(new Point3D(10, 0, 0), "终点", PathPointType.EndPoint)
};
route.Points[0].Index = 0;
route.Points[1].Index = 1;
route.Points[2].Index = 2;
double samplingStep = 0.5;
// Act
PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);
// Assert
Assert.IsTrue(route.IsCurved, "路径应该被标记为已曲线化");
Assert.AreEqual(2, route.Edges.Count, "L型路径应该生成2条边");
// 第一条边是直线段
Assert.AreEqual(PathSegmentType.Straight, route.Edges[0].SegmentType, "第一条边应该是直线段");
// 第二条边包含圆弧
Assert.AreEqual(PathSegmentType.Arc, route.Edges[1].SegmentType, "第二条边应该是圆弧段");
Assert.IsNotNull(route.Edges[1].Trajectory, "圆弧边应该有轨迹数据");
Assert.IsTrue(route.Edges[1].Trajectory.ActualRadius > 0, "实际半径应该大于0");
}
[TestMethod]
public void ApplyCurvatureToRoute_ZShapedPath_GeneratesMultipleArcEdges()
{
// Arrange
var route = new PathRoute
{
Id = Guid.NewGuid().ToString(),
Name = "Z型路径",
TurnRadius = 1.5
};
route.Points = new List<PathPoint>
{
new PathPoint(new Point3D(0, 10, 0), "起点", PathPointType.StartPoint),
new PathPoint(new Point3D(0, 0, 0), "转弯点1", PathPointType.WayPoint),
new PathPoint(new Point3D(10, 0, 0), "转弯点2", PathPointType.WayPoint),
new PathPoint(new Point3D(10, 10, 0), "终点", PathPointType.EndPoint)
};
route.Points[0].Index = 0;
route.Points[1].Index = 1;
route.Points[2].Index = 2;
route.Points[3].Index = 3;
double samplingStep = 0.5;
// Act
PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);
// Assert
Assert.IsTrue(route.IsCurved, "路径应该被标记为已曲线化");
Assert.AreEqual(3, route.Edges.Count, "Z型路径应该生成3条边");
// 应该有两条圆弧边
int arcEdgeCount = route.Edges.Count(e => e.SegmentType == PathSegmentType.Arc);
Assert.IsTrue(arcEdgeCount >= 1, "应该至少有一条圆弧边");
}
[TestMethod]
public void ApplyCurvatureToRoute_NullRoute_DoesNotThrow()
{
// Arrange
PathRoute route = null;
double samplingStep = 0.5;
// Act & Assert
try
{
PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);
Assert.Fail("应该抛出ArgumentNullException");
}
catch (ArgumentNullException)
{
// Expected
}
}
[TestMethod]
public void ApplyCurvatureToRoute_LessThanTwoPoints_DoesNotCurve()
{
// Arrange
var route = new PathRoute
{
Id = Guid.NewGuid().ToString(),
Name = "单点路径",
TurnRadius = 2.0
};
route.Points = new List<PathPoint>
{
new PathPoint(new Point3D(0, 0, 0), "起点", PathPointType.StartPoint)
};
route.Points[0].Index = 0;
double samplingStep = 0.5;
// Act
PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);
// Assert
Assert.IsFalse(route.IsCurved, "少于2个点的路径不应该被曲线化");
Assert.AreEqual(0, route.Edges.Count, "不应该生成任何边");
}
#endregion
#region RecalculateRouteLength 测试
[TestMethod]
public void RecalculateRouteLength_StraightEdges_CalculatesCorrectly()
{
// Arrange
var route = new PathRoute
{
Id = Guid.NewGuid().ToString(),
Name = "测试路径"
};
route.Edges = new List<PathEdge>
{
new PathEdge
{
Id = Guid.NewGuid().ToString(),
SegmentType = PathSegmentType.Straight,
PhysicalLength = 10.0,
SampledPoints = new List<Point3D>()
},
new PathEdge
{
Id = Guid.NewGuid().ToString(),
SegmentType = PathSegmentType.Straight,
PhysicalLength = 5.0,
SampledPoints = new List<Point3D>()
}
};
// Act
PathCurveEngine.RecalculateRouteLength(route);
// Assert
Assert.AreEqual(15.0, route.TotalLength, 0.01, "路径总长度应该等于所有边长度之和");
}
[TestMethod]
public void RecalculateRouteLength_MixedEdgeTypes_CalculatesCorrectly()
{
// Arrange
var route = new PathRoute
{
Id = Guid.NewGuid().ToString(),
Name = "测试路径"
};
route.Edges = new List<PathEdge>
{
new PathEdge
{
Id = Guid.NewGuid().ToString(),
SegmentType = PathSegmentType.Straight,
PhysicalLength = 10.0,
SampledPoints = new List<Point3D>()
},
new PathEdge
{
Id = Guid.NewGuid().ToString(),
SegmentType = PathSegmentType.Arc,
PhysicalLength = Math.PI, // 半圆
SampledPoints = new List<Point3D>()
},
new PathEdge
{
Id = Guid.NewGuid().ToString(),
SegmentType = PathSegmentType.Straight,
PhysicalLength = 5.0,
SampledPoints = new List<Point3D>()
}
};
// Act
PathCurveEngine.RecalculateRouteLength(route);
// Assert
double expectedLength = 10.0 + Math.PI + 5.0;
Assert.AreEqual(expectedLength, route.TotalLength, 0.01, "路径总长度应该正确计算");
}
[TestMethod]
public void RecalculateRouteLength_NullRoute_DoesNotThrow()
{
// Arrange
PathRoute route = null;
// Act
PathCurveEngine.RecalculateRouteLength(route);
// Assert - 不应该抛出异常
Assert.IsTrue(true, "null路径不应该导致异常");
}
[TestMethod]
public void RecalculateRouteLength_EmptyEdges_SetsZeroLength()
{
// Arrange
var route = new PathRoute
{
Id = Guid.NewGuid().ToString(),
Name = "空路径",
Edges = new List<PathEdge>()
};
// Act
PathCurveEngine.RecalculateRouteLength(route);
// Assert
Assert.AreEqual(0, route.TotalLength, 0.01, "空边的路径长度应该为0");
}
#endregion
#region 集成测试
[TestMethod]
public void FullWorkflow_CreateCurvePath_CalculatesCorrectLength()
{
// Arrange - 创建一个包含转弯的路径
var route = new PathRoute
{
Id = Guid.NewGuid().ToString(),
Name = "完整测试路径",
TurnRadius = 2.0
};
route.Points = new List<PathPoint>
{
new PathPoint(new Point3D(0, 20, 0), "起点", PathPointType.StartPoint),
new PathPoint(new Point3D(0, 0, 0), "转弯点", PathPointType.WayPoint),
new PathPoint(new Point3D(20, 0, 0), "终点", PathPointType.EndPoint)
};
route.Points[0].Index = 0;
route.Points[1].Index = 1;
route.Points[2].Index = 2;
double samplingStep = 0.5;
// Act - 应用曲线化
PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);
// Assert
Assert.IsTrue(route.IsCurved, "路径应该被曲线化");
Assert.IsTrue(route.TotalLength > 0, "路径总长度应该大于0");
// 验证路径总长度小于直线距离（因为圆弧比直线短）
double straightDistance = 20 + 20; // 两条直线段
Assert.IsTrue(route.TotalLength < straightDistance, "曲线化后的路径长度应该小于直线距离");
}
#endregion
}
}

36
UnitTests/SimpleTest.cs Normal file
View File

@ -0,0 +1,36 @@
using Microsoft.VisualStudio.TestTools.UnitTesting;
namespace NavisworksTransport.UnitTests
{
[TestClass]
public class SimpleTest
{
[TestMethod]
public void HelloWorld_ShouldPass()
{
// Arrange
string expected = "Hello World";
// Act
string actual = "Hello World";
// Assert
Assert.AreEqual(expected, actual, "Hello World 测试应该通过");
}
[TestMethod]
public void SimpleMath_ShouldReturnCorrectSum()
{
// Arrange
int a = 2;
int b = 3;
int expected = 5;
// Act
int actual = a + b;
// Assert
Assert.AreEqual(expected, actual, "2 + 3 应该等于 5");
}
}
}

3
UnitTests/TestHelpers/TestViewModel.cs
View File

@ -1,3 +1,6 @@
using System;
using System.Threading.Tasks;
namespace NavisworksTransport.UnitTests
{
/// <summary>

8
config.toml.example
View File

@ -20,6 +20,14 @@ vehicle_height_meters = 2.0
# 安全间隙（米）
safety_margin_meters = 0.05
# 路径曲线化配置
# 路径默认转弯半径（米）- 表示路径允许的最大转弯半径
default_path_turn_radius = 2.5
# 圆弧采样步长（米）- 推荐值：0.02-0.1
arc_sampling_step = 0.05
[visualization]
# 地图边距比例（0-1之间）
margin_ratio = 0.1

614
doc/working/路径曲线化实施方案_20251230.md Normal file
View File

@ -0,0 +1,614 @@
无人物流车转弯路径曲线化功能 - 实施方案
1. 目标概述
实现基于圆弧过渡（Arc Fillet）的路径曲线化功能，替代现有的直线连接方式，确保仿真系统能准确检测车辆转弯时的扫掠路径（Swept Path）碰撞。
核心原理
控制点与物理点分离：用户操作的
PathPoint
作为控制点，实际物理路径由 PathEdge 表示
圆弧过渡法：在相邻路径段转折处插入切圆弧，使用进入切点 Ts 和退出切点 Te 连接
安全截断：当计算的切线长度超过线段长度45%时，自动缩减半径，防止圆弧过大
2. 数据结构改动
2.1 新增数据模型
[NEW] PathSegmentType 枚举
/// <summary>
/// 路径段类型
/// </summary>
public enum PathSegmentType
{
/// <summary>
/// 直线段
/// </summary>
Straight,
/// <summary>
/// 圆弧段
/// </summary>
Arc
}
[NEW] ArcTrajectory 类
/// <summary>
/// 圆弧轨迹数据
/// </summary>
[Serializable]
public class ArcTrajectory
{
/// <summary>
/// 进入切点
/// </summary>
public Point3D Ts { get; set; }
/// <summary>
/// 退出切点
/// </summary>
public Point3D Te { get; set; }
/// <summary>
/// 圆心位置
/// </summary>
public Point3D ArcCenter { get; set; }
/// <summary>
/// 请求半径（配置的转向半径）
/// </summary>
public double RequestedRadius { get; set; }
/// <summary>
/// 实际半径（安全截断后）
/// </summary>
public double ActualRadius { get; set; }
/// <summary>
/// 偏转角（弧度）
/// </summary>
public double DeflectionAngle { get; set; }
/// <summary>
/// 圆弧长度（米）
/// </summary>
public double ArcLength { get; set; }
}
[NEW] PathEdge 类
/// <summary>
/// 路径边 - 连接两个连续控制点的物理路径段
/// </summary>
[Serializable]
public class PathEdge
{
/// <summary>
/// 边唯一标识符
/// </summary>
public string Id { get; set; }
/// <summary>
/// 起始控制点ID
/// </summary>
public string StartPointId { get; set; }
/// <summary>
/// 结束控制点ID
/// </summary>
public string EndPointId { get; set; }
/// <summary>
/// 路径段类型
/// </summary>
public PathSegmentType SegmentType { get; set; }
/// <summary>
/// 圆弧轨迹数据（仅当 SegmentType == Arc 时有效）
/// </summary>
public ArcTrajectory Trajectory { get; set; }
/// <summary>
/// 边的物理长度（米）
/// 直线段：两点间距离；圆弧段：直线段长度 + 圆弧长度
/// </summary>
public double PhysicalLength { get; set; }
/// <summary>
/// 采样点序列（用于碰撞检测和动画）
/// </summary>
[XmlIgnore]
public List<Point3D> SampledPoints { get; set; }
}
2.2 修改现有数据模型
[MODIFY]
PathPoint.cs
新增属性：
/// <summary>
/// 自定义转向半径（米），用于局部急转弯场景
/// 为 null 时使用全局默认值
/// </summary>
public double? CustomTurnRadius { get; set; }
[MODIFY]
PathRoute.cs
新增属性：
/// <summary>
/// 路径边集合 - 存储物理路径段
/// </summary>
public List<PathEdge> Edges { get; set; } = new List<PathEdge>();
/// <summary>
/// 默认转向半径（米）
/// </summary>
public double TurnRadius { get; set; } = 1.5;
/// <summary>
/// 是否已曲线化
/// </summary>
public bool IsCurved { get; set; } = false;
修改方法：
UpdateTotalLength()
: 综合计算所有 Edges 的物理长度
Clone()
: 需要深拷贝 Edges 集合
2.3 数据库表结构扩展
[MODIFY]
PathDatabase.cs
新增表 PathEdges：
CREATE TABLE IF NOT EXISTS PathEdges (
Id TEXT PRIMARY KEY,
RouteId TEXT NOT NULL,
StartPointId TEXT NOT NULL,
EndPointId TEXT NOT NULL,
SegmentType INTEGER NOT NULL, -- 0=Straight, 1=Arc
PhysicalLength REAL NOT NULL,
-- 圆弧轨迹数据 (仅 SegmentType=1 时有效)
Ts_X REAL,
Ts_Y REAL,
Ts_Z REAL,
Te_X REAL,
Te_Y REAL,
Te_Z REAL,
ArcCenter_X REAL,
ArcCenter_Y REAL,
ArcCenter_Z REAL,
RequestedRadius REAL,
ActualRadius REAL,
DeflectionAngle REAL,
ArcLength REAL,
FOREIGN KEY (RouteId) REFERENCES PathRoutes(Id) ON DELETE CASCADE
);
修改表
PathRoutes
：
ALTER TABLE PathRoutes ADD COLUMN TurnRadius REAL DEFAULT 1.5;
ALTER TABLE PathRoutes ADD COLUMN IsCurved INTEGER DEFAULT 0;
修改表
PathPoints
：
ALTER TABLE PathPoints ADD COLUMN CustomTurnRadius REAL;
3. 核心算法实现
3.1 PathCurveEngine 类
位置: src/Core/PathCurveEngine.cs (新建)
3.1.1 CalculateFillet 方法
/// <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 = Vector3D.DotProduct(v1, v2);
double angleRad = Math.Acos(Math.Clamp(cosAlpha, -1.0, 1.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
};
}
3.1.2 SampleArc 方法
/// <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>();
// 根据采样步长计算采样点数量
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();
// 计算旋转轴（叉乘）
Vector3D rotationAxis = Vector3D.CrossProduct(startVec, endVec).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;
}
3.1.3 ApplyCurvatureToRoute 方法
/// <summary>
/// 对路径应用曲线化处理
/// </summary>
/// <param name="route">待处理的路径</param>
/// <param name="samplingStep">采样步长（米）</param>
public static void ApplyCurvatureToRoute(PathRoute route, double samplingStep)
{
route.Edges.Clear();
var sortedPoints = route.GetSortedPoints();
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 - 2);
if (needsArc)
{
var p0 = sortedPoints[i - 1];
var p2Next = sortedPoints[i + 2];
// 获取转向半径（优先使用自定义值）
double radius = p1.CustomTurnRadius ?? route.TurnRadius;
// 计算圆弧
var arcTraj = CalculateFillet(p0, p1, p2, 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;
route.RecalculateLength();
}
4. 配置文件扩展
[MODIFY]
SystemConfig.cs
在
PathEditingConfig
类中新增：
/// <summary>
/// 默认转向半径（米）
/// 推荐值：1.0-2.0
/// </summary>
public double DefaultTurnRadius { get; set; } = 1.5;
/// <summary>
/// 圆弧采样步长（米）
/// 推荐值：0.02-0.1
/// </summary>
public double ArcSamplingStep { get; set; } = 0.05;
配置文件示例 (TOML)
[PathEditing]
CellSizeMeters = 0.5
MaxHeightDiffMeters = 0.35
VehicleLengthMeters = 1.0
VehicleWidthMeters = 1.0
VehicleHeightMeters = 2.0
SafetyMarginMeters = 0.05
# 路径曲线化配置
DefaultTurnRadius = 1.5
ArcSamplingStep = 0.05
1. 业务流程集成
5.1 路径编辑完成流程
[MODIFY]
PathPlanningManager.cs
-
FinishEditing
方法
public bool FinishEditing()
{
try
{
if (_currentRoute == null)
return false;
// **新增：应用曲线化**
if (ConfigManager.Instance.Current.PathEditing.EnablePathCurving)
{
double samplingStep = ConfigManager.Instance.Current.PathEditing.ArcSamplingStep;
_currentRoute.TurnRadius = ConfigManager.Instance.Current.PathEditing.DefaultTurnRadius;
PathCurveEngine.ApplyCurvatureToRoute(_currentRoute, samplingStep);
LogManager.Info($"路径曲线化完成: {_currentRoute.Name}, 边数: {_currentRoute.Edges.Count}");
}
// 保存到数据库（包含Edges）
SaveRouteToDatabase(_currentRoute);
// ... 现有逻辑 ...
}
catch (Exception ex)
{
LogManager.Error($"完成编辑失败: {ex.Message}");
return false;
}
}
5.2 数据持久化
[MODIFY]
PathDatabase.cs
新增方法：
SavePathEdges(string routeId, List<PathEdge> edges)
LoadPathEdges(string routeId) : List<PathEdge>
DeletePathEdges(string routeId)
修改方法：
SavePathRoute(PathRoute route)
: 同时保存 Edges
GetAllPathRoutes()
: 同时加载 Edges
DeletePathRoute(string routeId)
: 级联删除 Edges
5.3 导出格式扩展
[MODIFY]
PathDataManager.cs
DELMIA XML 格式：
<Path name="路径1">
<Edges>
<Edge type="arc">
<Trajectory>
<Ts x="10.5" y="5.2" z="0.0"/>
<Te x="11.3" y="6.8" z="0.0"/>
<ArcCenter x="10.9" y="6.0" z="0.0"/>
<Radius>1.5</Radius>
</Trajectory>
</Edge>
</Edges>
</Path>
CSV 格式（扁平化采样点）：
RouteId,EdgeIndex,SegmentType,PointIndex,X,Y,Z
route_001,0,Arc,0,10.5,5.2,0.0
route_001,0,Arc,1,10.52,5.25,0.0
...
6. 碰撞检测与动画集成
6.1 碰撞检测
[MODIFY] PathAnimationManager.cs - 碰撞检测逻辑
// 从 Edges 获取采样点而非直接使用 Points
foreach (var edge in route.Edges)
{
if (edge.SampledPoints == null || edge.SampledPoints.Count == 0)
{
// 懒加载：若采样点未生成，现场生成
edge.SampledPoints = GenerateSampledPoints(edge, arcSamplingStep);
}
foreach (var point in edge.SampledPoints)
{
// 执行OBB碰撞检测 ...
}
}
6.2 动画生成
使用 edge.SampledPoints 代替原始控制点生成动画帧序列。
1. 验证计划
7.1 单元测试
新建测试文件: test/Core/PathCurveEngineTests.cs
测试用例：
基础几何计算
验证90度直角转弯的圆弧计算
验证锐角/钝角场景
验证3D空间旋转正确性
安全截断
验证半径过大时自动缩减
边界条件：最小夹角（~0°）和最大夹角（~180°）
采样精度
验证采样点数量符合步长要求
验证采样点均匀分布在圆弧上
运行命令（需确认实际测试框架）：
# 假设使用 xUnit
dotnet test NavisworksTransport.Tests --filter "FullyQualifiedName~PathCurveEngine"
7.2 集成测试
场景测试路径：
创建包含3个控制点的简单L型路径
设置转向半径 = 1.5m
完成编辑触发曲线化
验证：
Edges 数量= 2
第一个Edge为直线段，第二个Edge包含圆弧
TotalLength 正确（直线段 + 圆弧段）
重新加载路径验证持久化
7.3 手动验证
IMPORTANT
以下步骤需要用户在Navisworks中手动验证
测试步骤：
打开Navisworks，加载测试模型
进入路径规划模式，手工添加4个控制点形成Z字形路径
在配置文件中设置：
DefaultTurnRadius = 2.0
ArcSamplingStep = 0.05
EnablePathCurving = true
完成编辑，观察路径可视化是否显示平滑曲线（而非折线）
运行碰撞检测，检查转弯处的采样点密度
导出DELMIA XML，用文本编辑器验证圆弧轨迹参数存在
预期结果：
路径在转折点处显示圆弧过渡
碰撞检测能捕获内轮差区域的碰撞
XML包含完整的 <Trajectory> 节点
8. 风险与注意事项
8.1 性能影响
圆弧采样：每个转折点生成 ArcLength / SamplingStep 个采样点
缓解措施：默认步长0.05m平衡精度与性能，支持配置调整
8.2 数据兼容性
旧版路径数据库无 Edges 表
迁移策略：
检测表是否存在，不存在则创建
加载旧路径时，Edges为空视为"未曲线化"，用户完成编辑时自动曲线化
8.3 极端场景
非常小的转向半径 + 锐角：可能导致无法满足几何约束
处理：最小半径限制0.3m，警告日志记录
9. 实施顺序建议
✅ 数据结构 → 新增类和修改现有模型
✅ 配置管理 → 扩展SystemConfig和TOML
✅ 核心算法 → PathCurveEngine + 单元测试
✅ 数据库 → 表结构变更 + CRUD方法
✅ 业务集成 → FinishEditing + 数据持久化
✅ 导出扩展 → XML/CSV格式支持
✅ 碰撞检测 → 使用Edges采样点
✅ 验证测试 → 集成测试 + 手动验证
10. 后续优化方向
支持Clothoid曲线（回旋曲线）以实现更平滑的加速度变化
UI可视化：实时预览圆弧位置
性能优化：采样点缓存机制
物流车转弯路径曲线化功能实施任务规划
总体目标
实现基于圆弧过渡（Arc Fillet）的路径曲线化功能，替代现有的直线连接方式，满足仿真系统对物理真实性的要求。
实施阶段
[ ] 阶段1: 数据结构设计与扩展
设计 Edge（边）数据结构
PathEdge 类定义
PathSegmentType 枚举（直线/圆弧）
ArcTrajectory 圆弧轨迹数据
扩展 PathPoint 支持局部半径覆盖
新增 CustomTurnRadius 属性
重构 PathRoute 类
新增 Edges 集合
新增 TurnRadius 全局转向半径
更新长度计算逻辑
更新数据库表结构
设计 Edges 表结构
设计数据迁移策略
[ ] 阶段2: 曲线化算法实现
创建 PathCurveEngine 核心算法类
CalculateFillet 方法（圆弧切点计算）
安全截断逻辑实现
SampleArc 方法（圆弧采样）
实现路径曲线化主流程
ApplyCurvatureToRoute 方法
BuildEdgesFromPoints 方法
单元测试
基础几何计算测试
边界条件测试
[ ] 阶段3: 配置管理
添加曲线化相关配置项
DefaultTurnRadius (默认转向半径)
ArcSamplingStep (圆弧采样步长，默认0.05m)
EnablePathCurving (启用开关)
更新配置文件读取/保存逻辑
[ ] 阶段4: 集成到现有业务流程
修改 PathRoute 长度计算
直线段 + 圆弧段综合计算
集成到路径编辑完成流程
FinishEditing 触发曲线化
更新数据持久化
PathDatabase 保存/加载 Edges
PathDataManager 导出格式扩展
[ ] 阶段5: 碰撞检测与动画集成
碰撞检测使用曲线路径
采样点序列生成
集成到现有碰撞检测
PathAnimationManager 支持曲线路径
使用 Edges 生成动画帧
[ ] 阶段6: DELMIA 导出扩展
XML Tag Group 格式支持圆弧
导出 Ts, Te, ArcCenter
CSV 格式导出采样点序列
[ ] 阶段7: 测试与验证
功能测试
性能测试
边界场景测试

2
run-unit-tests.bat
View File

@ -29,7 +29,7 @@ if not exist "%MSBUILD_PATH%" (
echo Using MSBuild: "%MSBUILD_PATH%"
echo Building test project...
"%MSBUILD_PATH%" NavisworksTransport.UnitTests.csproj /p:Configuration=Debug /p:Platform=AnyCPU /verbosity:minimal
"%MSBUILD_PATH%" NavisworksTransport.UnitTests.csproj /p:Configuration=Release /p:Platform=x64 /verbosity:minimal
if errorlevel 1 (
echo Test project build failed!

14
src/Core/Config/SystemConfig.cs
View File

@ -87,6 +87,20 @@ namespace NavisworksTransport.Core.Config
/// 安全间隙（米）
/// </summary>
public double SafetyMarginMeters { get; set; } = 0.05;
// 路径曲线化配置
/// <summary>
/// 路径默认转弯半径（米）
/// 表示路径允许的最大转弯半径，车辆的最小转弯半径必须小于等于此值
/// </summary>
public double DefaultPathTurnRadius { get; set; } = 2.5;
/// <summary>
/// 圆弧采样步长（米）
/// 推荐值：0.02-0.1
/// </summary>
public double ArcSamplingStep { get; set; } = 0.05;
}
/// <summary>

354
src/Core/PathCurveEngine.cs Normal file
View File

@ -0,0 +1,354 @@
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);
}
}
}

63
src/Core/PathDataManager.cs
View File

@ -959,6 +959,69 @@ namespace NavisworksTransport
pointsElement.AppendChild(pointElement);
}
// 添加路径边
if (route.Edges != null && route.Edges.Count > 0)
{
var edgesElement = xmlDoc.CreateElement("Edges", _delmiaNamespace);
routeElement.AppendChild(edgesElement);
foreach (var edge in route.Edges)
{
var edgeElement = xmlDoc.CreateElement("Edge", _delmiaNamespace);
edgeElement.SetAttribute("id", edge.Id);
edgeElement.SetAttribute("type", edge.SegmentType.ToString().ToLower());
edgeElement.SetAttribute("startPointId", edge.StartPointId);
edgeElement.SetAttribute("endPointId", edge.EndPointId);
edgeElement.SetAttribute("physicalLength", edge.PhysicalLength.ToString("F6"));
// 圆弧轨迹数据
if (edge.SegmentType == PathSegmentType.Arc && edge.Trajectory != null)
{
var trajElement = xmlDoc.CreateElement("Trajectory", _delmiaNamespace);
// 进入切点
var tsElement = xmlDoc.CreateElement("Ts", _delmiaNamespace);
tsElement.SetAttribute("x", edge.Trajectory.Ts.X.ToString("F3"));
tsElement.SetAttribute("y", edge.Trajectory.Ts.Y.ToString("F3"));
tsElement.SetAttribute("z", edge.Trajectory.Ts.Z.ToString("F3"));
trajElement.AppendChild(tsElement);
// 退出切点
var teElement = xmlDoc.CreateElement("Te", _delmiaNamespace);
teElement.SetAttribute("x", edge.Trajectory.Te.X.ToString("F3"));
teElement.SetAttribute("y", edge.Trajectory.Te.Y.ToString("F3"));
teElement.SetAttribute("z", edge.Trajectory.Te.Z.ToString("F3"));
trajElement.AppendChild(teElement);
// 圆心
var centerElement = xmlDoc.CreateElement("ArcCenter", _delmiaNamespace);
centerElement.SetAttribute("x", edge.Trajectory.ArcCenter.X.ToString("F3"));
centerElement.SetAttribute("y", edge.Trajectory.ArcCenter.Y.ToString("F3"));
centerElement.SetAttribute("z", edge.Trajectory.ArcCenter.Z.ToString("F3"));
trajElement.AppendChild(centerElement);
// 半径
var radiusElement = xmlDoc.CreateElement("Radius", _delmiaNamespace);
radiusElement.InnerText = edge.Trajectory.ActualRadius.ToString("F3");
trajElement.AppendChild(radiusElement);
// 偏转角
var angleElement = xmlDoc.CreateElement("DeflectionAngle", _delmiaNamespace);
angleElement.InnerText = edge.Trajectory.DeflectionAngle.ToString("F6");
trajElement.AppendChild(angleElement);
// 圆弧长度
var arcLengthElement = xmlDoc.CreateElement("ArcLength", _delmiaNamespace);
arcLengthElement.InnerText = edge.Trajectory.ArcLength.ToString("F3");
trajElement.AppendChild(arcLengthElement);
edgeElement.AppendChild(trajElement);
}
edgesElement.AppendChild(edgeElement);
}
}
return routeElement;
}

214
src/Core/PathDatabase.cs
View File

@ -64,6 +64,8 @@ namespace NavisworksTransport
Name TEXT NOT NULL,
TotalLength REAL,
EstimatedTime REAL,
TurnRadius REAL DEFAULT 0.0, -- 实际使用时从配置文件获取默认值
IsCurved INTEGER DEFAULT 0,
MaxVehicleLength REAL,
MaxVehicleWidth REAL,
MaxVehicleHeight REAL,
@ -105,7 +107,7 @@ namespace NavisworksTransport
)
");
// 4. 路径点表
// 4. 路径点表（新增 CustomTurnRadius）
ExecuteNonQuery(@"
CREATE TABLE IF NOT EXISTS PathPoints (
Id TEXT PRIMARY KEY,
@ -116,6 +118,33 @@ namespace NavisworksTransport
Y REAL,
Z REAL,
Type INTEGER,
CustomTurnRadius REAL,
FOREIGN KEY(RouteId) REFERENCES PathRoutes(Id) ON DELETE CASCADE
)
");
// 5. 路径边表（新增）
ExecuteNonQuery(@"
CREATE TABLE IF NOT EXISTS PathEdges (
Id TEXT PRIMARY KEY,
RouteId TEXT NOT NULL,
StartPointId TEXT NOT NULL,
EndPointId TEXT NOT NULL,
SegmentType INTEGER NOT NULL,
PhysicalLength REAL NOT NULL,
Ts_X REAL,
Ts_Y REAL,
Ts_Z REAL,
Te_X REAL,
Te_Y REAL,
Te_Z REAL,
ArcCenter_X REAL,
ArcCenter_Y REAL,
ArcCenter_Z REAL,
RequestedRadius REAL,
ActualRadius REAL,
DeflectionAngle REAL,
ArcLength REAL,
FOREIGN KEY(RouteId) REFERENCES PathRoutes(Id) ON DELETE CASCADE
)
");
@ -123,6 +152,7 @@ namespace NavisworksTransport
// 创建索引
ExecuteNonQuery("CREATE INDEX IF NOT EXISTS idx_reports_route ON CollisionReports(RouteId)");
ExecuteNonQuery("CREATE INDEX IF NOT EXISTS idx_pathpoints_route ON PathPoints(RouteId)");
ExecuteNonQuery("CREATE INDEX IF NOT EXISTS idx_pathedges_route ON PathEdges(RouteId)");
}
/// <summary>
@ -138,8 +168,8 @@ namespace NavisworksTransport
// 保存路径基本信息
var sql = @"
INSERT OR REPLACE INTO PathRoutes
(Id, Name, TotalLength, EstimatedTime, MaxVehicleLength, MaxVehicleWidth, MaxVehicleHeight, SafetyMargin, GridSize, CreatedTime, LastModified)
VALUES (@id, @name, @length, @time, @maxLength, @maxWidth, @maxHeight, @safetyMargin, @gridSize, @created, @modified)
(Id, Name, TotalLength, EstimatedTime, TurnRadius, IsCurved, MaxVehicleLength, MaxVehicleWidth, MaxVehicleHeight, SafetyMargin, GridSize, CreatedTime, LastModified)
VALUES (@id, @name, @length, @time, @turnRadius, @isCurved, @maxLength, @maxWidth, @maxHeight, @safetyMargin, @gridSize, @created, @modified)
";
using (var cmd = new SQLiteCommand(sql, _connection))
@ -148,6 +178,8 @@ namespace NavisworksTransport
cmd.Parameters.AddWithValue("@name", route.Name);
cmd.Parameters.AddWithValue("@length", route.TotalLength);
cmd.Parameters.AddWithValue("@time", route.EstimatedTime);
cmd.Parameters.AddWithValue("@turnRadius", route.TurnRadius);
cmd.Parameters.AddWithValue("@isCurved", route.IsCurved ? 1 : 0);
cmd.Parameters.AddWithValue("@maxLength", route.MaxVehicleLength);
cmd.Parameters.AddWithValue("@maxWidth", route.MaxVehicleWidth);
cmd.Parameters.AddWithValue("@maxHeight", route.MaxVehicleHeight);
@ -158,16 +190,17 @@ namespace NavisworksTransport
cmd.ExecuteNonQuery();
}
// 删除旧的路径点
ExecuteNonQuery("DELETE FROM PathPoints WHERE RouteId=@id", new { id = route.Id });
// 删除旧的路径点和边
DeletePathPoints(route.Id);
DeletePathEdges(route.Id);
// 保存新的路径点
if (route.Points != null && route.Points.Count > 0)
{
var pointSql = @"
INSERT INTO PathPoints
(Id, RouteId, SequenceNumber, Name, X, Y, Z, Type)
VALUES (@id, @routeId, @seq, @name, @x, @y, @z, @type)
(Id, RouteId, SequenceNumber, Name, X, Y, Z, Type, CustomTurnRadius)
VALUES (@id, @routeId, @seq, @name, @x, @y, @z, @type, @customRadius)
";
for (int i = 0; i < route.Points.Count; i++)
@ -183,6 +216,8 @@ namespace NavisworksTransport
cmd.Parameters.AddWithValue("@y", point.Position.Y);
cmd.Parameters.AddWithValue("@z", point.Position.Z);
cmd.Parameters.AddWithValue("@type", (int)point.Type);
cmd.Parameters.AddWithValue("@customRadius", point.CustomTurnRadius.HasValue ?
(object)point.CustomTurnRadius.Value : DBNull.Value);
cmd.ExecuteNonQuery();
}
}
@ -190,6 +225,17 @@ namespace NavisworksTransport
LogManager.Info($"保存路径: {route.Name}，包含 {route.Points.Count} 个路径点");
}
// 保存路径边
if (route.Edges != null && route.Edges.Count > 0)
{
foreach (var edge in route.Edges)
{
SavePathEdge(route.Id, edge);
}
LogManager.Info($"保存路径: {route.Name}，包含 {route.Edges.Count} 个路径边");
}
transaction.Commit();
}
catch (Exception ex)
@ -441,6 +487,8 @@ namespace NavisworksTransport
Name = reader["Name"].ToString(),
TotalLength = Convert.ToDouble(reader["TotalLength"]),
EstimatedTime = Convert.ToDouble(reader["EstimatedTime"]),
TurnRadius = reader.IsDBNull(reader.GetOrdinal("TurnRadius")) ? 0.0 : Convert.ToDouble(reader["TurnRadius"]), // 0.0 表示未设置，实际使用时从配置获取
IsCurved = reader.IsDBNull(reader.GetOrdinal("IsCurved")) ? false : Convert.ToInt32(reader["IsCurved"]) == 1,
MaxVehicleLength = reader.IsDBNull(reader.GetOrdinal("MaxVehicleLength")) ? 1.0 : Convert.ToDouble(reader["MaxVehicleLength"]),
MaxVehicleWidth = reader.IsDBNull(reader.GetOrdinal("MaxVehicleWidth")) ? 1.0 : Convert.ToDouble(reader["MaxVehicleWidth"]),
MaxVehicleHeight = reader.IsDBNull(reader.GetOrdinal("MaxVehicleHeight")) ? 2.0 : Convert.ToDouble(reader["MaxVehicleHeight"]),
@ -463,10 +511,11 @@ namespace NavisworksTransport
}
}
// 为每个路径加载路径点
// 为每个路径加载路径点和路径边
foreach (var route in routes)
{
LoadPathPoints(route);
LoadPathEdges(route);
}
LogManager.Info($"从数据库加载了 {routes.Count} 条路径记录");
@ -501,12 +550,161 @@ namespace NavisworksTransport
Type = (PathPointType)Convert.ToInt32(reader["Type"])
};
// 加载自定义转向半径
if (!reader.IsDBNull(reader.GetOrdinal("CustomTurnRadius")))
{
point.CustomTurnRadius = Convert.ToDouble(reader["CustomTurnRadius"]);
}
route.Points.Add(point);
}
}
}
}
/// <summary>
/// 保存路径边
/// </summary>
private void SavePathEdge(string routeId, PathEdge edge)
{
var sql = @"
INSERT INTO PathEdges
(Id, RouteId, StartPointId, EndPointId, SegmentType, PhysicalLength,
Ts_X, Ts_Y, Ts_Z, Te_X, Te_Y, Te_Z,
ArcCenter_X, ArcCenter_Y, ArcCenter_Z,
RequestedRadius, ActualRadius, DeflectionAngle, ArcLength)
VALUES (@id, @routeId, @startId, @endId, @segType, @length,
@tsx, @tsy, @tsz, @tex, @tey, @tez,
@acx, @acy, @acz,
@reqR, @actR, @angle, @arcLen)
";
using (var cmd = new SQLiteCommand(sql, _connection))
{
cmd.Parameters.AddWithValue("@id", edge.Id);
cmd.Parameters.AddWithValue("@routeId", routeId);
cmd.Parameters.AddWithValue("@startId", edge.StartPointId);
cmd.Parameters.AddWithValue("@endId", edge.EndPointId);
cmd.Parameters.AddWithValue("@segType", (int)edge.SegmentType);
cmd.Parameters.AddWithValue("@length", edge.PhysicalLength);
// 圆弧轨迹数据
if (edge.SegmentType == PathSegmentType.Arc && edge.Trajectory != null)
{
cmd.Parameters.AddWithValue("@tsx", edge.Trajectory.Ts.X);
cmd.Parameters.AddWithValue("@tsy", edge.Trajectory.Ts.Y);
cmd.Parameters.AddWithValue("@tsz", edge.Trajectory.Ts.Z);
cmd.Parameters.AddWithValue("@tex", edge.Trajectory.Te.X);
cmd.Parameters.AddWithValue("@tey", edge.Trajectory.Te.Y);
cmd.Parameters.AddWithValue("@tez", edge.Trajectory.Te.Z);
cmd.Parameters.AddWithValue("@acx", edge.Trajectory.ArcCenter.X);
cmd.Parameters.AddWithValue("@acy", edge.Trajectory.ArcCenter.Y);
cmd.Parameters.AddWithValue("@acz", edge.Trajectory.ArcCenter.Z);
cmd.Parameters.AddWithValue("@reqR", edge.Trajectory.RequestedRadius);
cmd.Parameters.AddWithValue("@actR", edge.Trajectory.ActualRadius);
cmd.Parameters.AddWithValue("@angle", edge.Trajectory.DeflectionAngle);
cmd.Parameters.AddWithValue("@arcLen", edge.Trajectory.ArcLength);
}
else
{
// 直线段，圆弧参数为NULL
cmd.Parameters.AddWithValue("@tsx", DBNull.Value);
cmd.Parameters.AddWithValue("@tsy", DBNull.Value);
cmd.Parameters.AddWithValue("@tsz", DBNull.Value);
cmd.Parameters.AddWithValue("@tex", DBNull.Value);
cmd.Parameters.AddWithValue("@tey", DBNull.Value);
cmd.Parameters.AddWithValue("@tez", DBNull.Value);
cmd.Parameters.AddWithValue("@acx", DBNull.Value);
cmd.Parameters.AddWithValue("@acy", DBNull.Value);
cmd.Parameters.AddWithValue("@acz", DBNull.Value);
cmd.Parameters.AddWithValue("@reqR", DBNull.Value);
cmd.Parameters.AddWithValue("@actR", DBNull.Value);
cmd.Parameters.AddWithValue("@angle", DBNull.Value);
cmd.Parameters.AddWithValue("@arcLen", DBNull.Value);
}
cmd.ExecuteNonQuery();
}
}
/// <summary>
/// 删除路径点
/// </summary>
private void DeletePathPoints(string routeId)
{
using (var cmd = new SQLiteCommand("DELETE FROM PathPoints WHERE RouteId = @routeId", _connection))
{
cmd.Parameters.AddWithValue("@routeId", routeId);
cmd.ExecuteNonQuery();
}
}
/// <summary>
/// 删除路径边
/// </summary>
private void DeletePathEdges(string routeId)
{
using (var cmd = new SQLiteCommand("DELETE FROM PathEdges WHERE RouteId = @routeId", _connection))
{
cmd.Parameters.AddWithValue("@routeId", routeId);
cmd.ExecuteNonQuery();
}
}
/// <summary>
/// 加载路径边
/// </summary>
private void LoadPathEdges(PathRoute route)
{
var sql = "SELECT * FROM PathEdges WHERE RouteId = @routeId ORDER BY Id";
using (var cmd = new SQLiteCommand(sql, _connection))
{
cmd.Parameters.AddWithValue("@routeId", route.Id);
using (var reader = cmd.ExecuteReader())
{
while (reader.Read())
{
var edge = new PathEdge
{
Id = reader["Id"].ToString(),
StartPointId = reader["StartPointId"].ToString(),
EndPointId = reader["EndPointId"].ToString(),
SegmentType = (PathSegmentType)Convert.ToInt32(reader["SegmentType"]),
PhysicalLength = Convert.ToDouble(reader["PhysicalLength"])
};
// 加载圆弧轨迹数据
if (edge.SegmentType == PathSegmentType.Arc)
{
edge.Trajectory = new ArcTrajectory
{
Ts = new Point3D(
Convert.ToDouble(reader["Ts_X"]),
Convert.ToDouble(reader["Ts_Y"]),
Convert.ToDouble(reader["Ts_Z"])),
Te = new Point3D(
Convert.ToDouble(reader["Te_X"]),
Convert.ToDouble(reader["Te_Y"]),
Convert.ToDouble(reader["Te_Z"])),
ArcCenter = new Point3D(
Convert.ToDouble(reader["ArcCenter_X"]),
Convert.ToDouble(reader["ArcCenter_Y"]),
Convert.ToDouble(reader["ArcCenter_Z"])),
RequestedRadius = Convert.ToDouble(reader["RequestedRadius"]),
ActualRadius = Convert.ToDouble(reader["ActualRadius"]),
DeflectionAngle = Convert.ToDouble(reader["DeflectionAngle"]),
ArcLength = Convert.ToDouble(reader["ArcLength"])
};
}
route.Edges.Add(edge);
}
}
}
}
/// <summary>
/// 执行非查询SQL语句
/// </summary>

12
src/Core/PathPlanningManager.cs
View File

@ -1466,6 +1466,18 @@ namespace NavisworksTransport
}
}
// === 应用曲线化 ===
if (CurrentRoute != null)
{
double samplingStep = ConfigManager.Instance.Current.PathEditing.ArcSamplingStep;
// 使用配置文件中的默认转弯半径
CurrentRoute.TurnRadius = ConfigManager.Instance.Current.PathEditing.DefaultPathTurnRadius;
PathCurveEngine.ApplyCurvatureToRoute(CurrentRoute, samplingStep);
LogManager.Info($"路径曲线化完成: {CurrentRoute.Name}, 转弯半径: {CurrentRoute.TurnRadius:F2}m, 边数: {CurrentRoute.Edges.Count}");
}
// 如果是创建模式，将当前路径添加到路径集合
if (_pathEditState == PathEditState.Creating && CurrentRoute != null)
{

142
src/Core/PathPlanningModels.cs
View File

@ -269,6 +269,12 @@ namespace NavisworksTransport
/// </summary>
public double SpeedLimit { get; set; }
/// <summary>
/// 自定义转向半径（米），用于局部急转弯场景
/// 为 null 时使用全局默认值
/// </summary>
public double? CustomTurnRadius { get; set; }
/// <summary>
/// 构造函数
/// </summary>
@ -282,6 +288,7 @@ namespace NavisworksTransport
Index = 0;
Notes = string.Empty;
SpeedLimit = 0;
CustomTurnRadius = null;
}
/// <summary>
@ -300,6 +307,7 @@ namespace NavisworksTransport
Index = 0;
Notes = string.Empty;
SpeedLimit = 0;
CustomTurnRadius = null;
}
/// <summary>
@ -312,6 +320,114 @@ namespace NavisworksTransport
}
}
/// <summary>
/// 路径段类型
/// </summary>
public enum PathSegmentType
{
/// <summary>
/// 直线段
/// </summary>
Straight,
/// <summary>
/// 圆弧段
/// </summary>
Arc
}
/// <summary>
/// 圆弧轨迹数据
/// </summary>
[Serializable]
public class ArcTrajectory
{
/// <summary>
/// 进入切点
/// </summary>
public Point3D Ts { get; set; }
/// <summary>
/// 退出切点
/// </summary>
public Point3D Te { get; set; }
/// <summary>
/// 圆心位置
/// </summary>
public Point3D ArcCenter { get; set; }
/// <summary>
/// 请求半径（配置的转向半径）
/// </summary>
public double RequestedRadius { get; set; }
/// <summary>
/// 实际半径（安全截断后）
/// </summary>
public double ActualRadius { get; set; }
/// <summary>
/// 偏转角（弧度）
/// </summary>
public double DeflectionAngle { get; set; }
/// <summary>
/// 圆弧长度（米）
/// </summary>
public double ArcLength { get; set; }
}
/// <summary>
/// 路径边 - 连接两个连续控制点的物理路径段
/// </summary>
[Serializable]
public class PathEdge
{
/// <summary>
/// 边唯一标识符
/// </summary>
public string Id { get; set; }
/// <summary>
/// 起始控制点ID
/// </summary>
public string StartPointId { get; set; }
/// <summary>
/// 结束控制点ID
/// </summary>
public string EndPointId { get; set; }
/// <summary>
/// 路径段类型
/// </summary>
public PathSegmentType SegmentType { get; set; }
/// <summary>
/// 圆弧轨迹数据（仅当 SegmentType == Arc 时有效）
/// </summary>
public ArcTrajectory Trajectory { get; set; }
/// <summary>
/// 边的物理长度（米）
/// 直线段：两点间距离；圆弧段：直线段长度 + 圆弧长度
/// </summary>
public double PhysicalLength { get; set; }
/// <summary>
/// 采样点序列（用于碰撞检测和动画）
/// </summary>
[XmlIgnore]
public List<Point3D> SampledPoints { get; set; }
public PathEdge()
{
Id = Guid.NewGuid().ToString();
SampledPoints = new List<Point3D>();
}
}
/// <summary>
/// 路径路线数据模型
/// </summary>
@ -319,10 +435,15 @@ namespace NavisworksTransport
public class PathRoute
{
/// <summary>
/// 路径点集合
/// 路径点集合（控制点）
/// </summary>
public List<PathPoint> Points { get; set; }
/// <summary>
/// 路径边集合 - 存储物理路径段
/// </summary>
public List<PathEdge> Edges { get; set; }
/// <summary>
/// 路径名称
/// </summary>
@ -414,6 +535,17 @@ namespace NavisworksTransport
/// </summary>
public double SafetyMargin { get; set; } = 0.5;
/// <summary>
/// 路径转弯半径（米）- 路径允许的最大转弯半径
/// 车辆的最小转弯半径必须小于等于此值
/// </summary>
public double TurnRadius { get; set; } = 0.0;
/// <summary>
/// 是否已曲线化
/// </summary>
public bool IsCurved { get; set; } = false;
// 数据库分析相关属性
/// <summary>
/// 碰撞数量（从数据库加载）
@ -441,6 +573,7 @@ namespace NavisworksTransport
public PathRoute()
{
Points = new List<PathPoint>();
Edges = new List<PathEdge>();
Name = string.Empty;
Id = Guid.NewGuid().ToString();
EstimatedTime = 0.0;
@ -449,6 +582,8 @@ namespace NavisworksTransport
CreatedTime = DateTime.Now;
LastModified = DateTime.Now;
Description = string.Empty;
TurnRadius = 0.0; // 0.0 表示未设置，实际使用时从配置获取默认值
IsCurved = false;
}
/// <summary>
@ -458,12 +593,15 @@ namespace NavisworksTransport
public PathRoute(string name)
{
Points = new List<PathPoint>();
Edges = new List<PathEdge>();
Name = name;
Id = Guid.NewGuid().ToString();
EstimatedTime = 0.0;
AssociatedChannelIds = new List<string>();
TotalLength = 0.0;
CreatedTime = DateTime.Now;
TurnRadius = 0.0; // 0.0 表示未设置，实际使用时从配置获取默认值
IsCurved = false;
LastModified = DateTime.Now;
Description = string.Empty;
}