From 93135d3c2916b76ed783f3765d884c00c95b608f Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Tue, 30 Dec 2025 18:29:41 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E8=B7=AF=E5=BE=84?= =?UTF-8?q?=E6=9B=B2=E7=BA=BF=E5=8C=96=E7=9A=84=E6=96=B9=E6=A1=88=EF=BC=8C?= =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E4=BA=86=E5=9F=BA=E7=A1=80=E7=9A=84=E6=9B=B2?= =?UTF-8?q?=E7=BA=BF=E5=8C=96=E8=B7=AF=E5=BE=84=E6=95=B0=E6=8D=AE=E7=BB=93?= =?UTF-8?q?=E6=9E=84=E5=92=8C=E5=AD=98=E5=82=A8=EF=BC=8C=E4=BB=A5=E5=8F=8A?= =?UTF-8?q?=E6=9B=B2=E7=BA=BF=E5=8C=96=E6=A0=B8=E5=BF=83=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- NavisworksTransport.UnitTests.csproj | 53 +- NavisworksTransportPlugin.csproj | 1 + ...hreadSafeObservableCollectionBasicTests.cs | 2 + .../Core/PathCurveEngineStandaloneTests.cs | 565 ++++++++++++++++ UnitTests/Core/PathCurveEngineTests.cs | 518 +++++++++++++++ UnitTests/SimpleTest.cs | 36 + UnitTests/TestHelpers/TestViewModel.cs | 3 + config.toml.example | 8 + doc/working/路径曲线化实施方案_20251230.md | 614 ++++++++++++++++++ run-unit-tests.bat | 2 +- src/Core/Config/SystemConfig.cs | 14 + src/Core/PathCurveEngine.cs | 354 ++++++++++ src/Core/PathDataManager.cs | 63 ++ src/Core/PathDatabase.cs | 214 +++++- src/Core/PathPlanningManager.cs | 12 + src/Core/PathPlanningModels.cs | 142 +++- 16 files changed, 2559 insertions(+), 42 deletions(-) create mode 100644 UnitTests/Core/PathCurveEngineStandaloneTests.cs create mode 100644 UnitTests/Core/PathCurveEngineTests.cs create mode 100644 UnitTests/SimpleTest.cs create mode 100644 doc/working/路径曲线化实施方案_20251230.md create mode 100644 src/Core/PathCurveEngine.cs diff --git a/NavisworksTransport.UnitTests.csproj b/NavisworksTransport.UnitTests.csproj index 219f778..39d5249 100644 --- a/NavisworksTransport.UnitTests.csproj +++ b/NavisworksTransport.UnitTests.csproj @@ -16,27 +16,34 @@ true - + true full false - bin\Debug\ + bin\x64\Debug\ DEBUG;TRACE prompt 4 x64 - + pdbonly true - bin\Release\ + bin\x64\Release\ TRACE prompt 4 + x64 + + + ..\..\..\..\Program Files\Autodesk\Navisworks Manage 2026\Autodesk.Navisworks.Api.dll + False + + @@ -48,6 +55,7 @@ + @@ -67,45 +75,28 @@ - + bin\x64\Debug\NavisworksTransportPlugin.dll True - - - - packages\RoyT.AStar.3.0.2\lib\netstandard2.0\Roy-T.AStar.dll - True - - - - - packages\geometry4Sharp.1.0.0\lib\net462\geometry4Sharp.dll - True - - - - - - - - - - - - - - + + + + + - + + + + \ No newline at end of file diff --git a/NavisworksTransportPlugin.csproj b/NavisworksTransportPlugin.csproj index 4ef64b7..7bcd54f 100644 --- a/NavisworksTransportPlugin.csproj +++ b/NavisworksTransportPlugin.csproj @@ -112,6 +112,7 @@ + diff --git a/UnitTests/Collections/ThreadSafeObservableCollectionBasicTests.cs b/UnitTests/Collections/ThreadSafeObservableCollectionBasicTests.cs index 6f2343b..f580fdd 100644 --- a/UnitTests/Collections/ThreadSafeObservableCollectionBasicTests.cs +++ b/UnitTests/Collections/ThreadSafeObservableCollectionBasicTests.cs @@ -1,3 +1,5 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; + namespace NavisworksTransport.UnitTests { /// diff --git a/UnitTests/Core/PathCurveEngineStandaloneTests.cs b/UnitTests/Core/PathCurveEngineStandaloneTests.cs new file mode 100644 index 0000000..df0219a --- /dev/null +++ b/UnitTests/Core/PathCurveEngineStandaloneTests.cs @@ -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） + + /// + /// 简单的 3D 点结构体（用于测试） + /// + public struct TestPoint3D : IEquatable + { + 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); + } + } + + /// + /// 简单的 3D 向量结构体（用于测试） + /// + 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 + ); + } + } + + /// + /// 测试用圆弧轨迹数据 + /// + 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 + + /// + /// PathCurveEngine 独立测试版本（不依赖 Navisworks API） + /// 用于测试核心算法逻辑的正确性 + /// + 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; + } + + /// + /// 计算圆弧切点和轨迹参数 + /// + 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 + }; + } + + /// + /// 采样圆弧为离散点序列 + /// + public static List SampleArc(TestArcTrajectory trajectory, double samplingStep) + { + var points = new List(); + + 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; + } + + /// + /// 绕轴旋转点（罗德里格斯旋转公式） + /// + 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; + } + } + + /// + /// PathCurveEngine 核心算法独立测试 + /// 测试路径曲线化算法的正确性（不依赖 Navisworks API） + /// + [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 + } +} \ No newline at end of file diff --git a/UnitTests/Core/PathCurveEngineTests.cs b/UnitTests/Core/PathCurveEngineTests.cs new file mode 100644 index 0000000..f7e7544 --- /dev/null +++ b/UnitTests/Core/PathCurveEngineTests.cs @@ -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 +{ + /// + /// PathCurveEngine 核心算法测试 + /// 测试路径曲线化算法的正确性 + /// + [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 + { + 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 + { + 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 + { + 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 + { + 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 + { + new PathEdge + { + Id = Guid.NewGuid().ToString(), + SegmentType = PathSegmentType.Straight, + PhysicalLength = 10.0, + SampledPoints = new List() + }, + new PathEdge + { + Id = Guid.NewGuid().ToString(), + SegmentType = PathSegmentType.Straight, + PhysicalLength = 5.0, + SampledPoints = new List() + } + }; + + // 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 + { + new PathEdge + { + Id = Guid.NewGuid().ToString(), + SegmentType = PathSegmentType.Straight, + PhysicalLength = 10.0, + SampledPoints = new List() + }, + new PathEdge + { + Id = Guid.NewGuid().ToString(), + SegmentType = PathSegmentType.Arc, + PhysicalLength = Math.PI, // 半圆 + SampledPoints = new List() + }, + new PathEdge + { + Id = Guid.NewGuid().ToString(), + SegmentType = PathSegmentType.Straight, + PhysicalLength = 5.0, + SampledPoints = new List() + } + }; + + // 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() + }; + + // 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 + { + 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 + } +} \ No newline at end of file diff --git a/UnitTests/SimpleTest.cs b/UnitTests/SimpleTest.cs new file mode 100644 index 0000000..9b86f00 --- /dev/null +++ b/UnitTests/SimpleTest.cs @@ -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"); + } + } +} \ No newline at end of file diff --git a/UnitTests/TestHelpers/TestViewModel.cs b/UnitTests/TestHelpers/TestViewModel.cs index 2da2dc8..c2f6042 100644 --- a/UnitTests/TestHelpers/TestViewModel.cs +++ b/UnitTests/TestHelpers/TestViewModel.cs @@ -1,3 +1,6 @@ +using System; +using System.Threading.Tasks; + namespace NavisworksTransport.UnitTests { /// diff --git a/config.toml.example b/config.toml.example index 3538812..21e157e 100644 --- a/config.toml.example +++ b/config.toml.example @@ -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 diff --git a/doc/working/路径曲线化实施方案_20251230.md b/doc/working/路径曲线化实施方案_20251230.md new file mode 100644 index 0000000..e9beb26 --- /dev/null +++ b/doc/working/路径曲线化实施方案_20251230.md @@ -0,0 +1,614 @@ +无人物流车转弯路径曲线化功能 - 实施方案 + +1. 目标概述 +实现基于圆弧过渡（Arc Fillet）的路径曲线化功能，替代现有的直线连接方式，确保仿真系统能准确检测车辆转弯时的扫掠路径（Swept Path）碰撞。 + +核心原理 +控制点与物理点分离：用户操作的 +PathPoint + 作为控制点，实际物理路径由 PathEdge 表示 +圆弧过渡法：在相邻路径段转折处插入切圆弧，使用进入切点 Ts 和退出切点 Te 连接 +安全截断：当计算的切线长度超过线段长度45%时，自动缩减半径，防止圆弧过大 +2. 数据结构改动 +2.1 新增数据模型 +[NEW] PathSegmentType 枚举 +/// +/// 路径段类型 +/// +public enum PathSegmentType +{ + /// + /// 直线段 + /// + Straight, + + /// + /// 圆弧段 + /// + Arc +} +[NEW] ArcTrajectory 类 +/// +/// 圆弧轨迹数据 +/// +[Serializable] +public class ArcTrajectory +{ + /// + /// 进入切点 + /// + public Point3D Ts { get; set; } + + /// + /// 退出切点 + /// + public Point3D Te { get; set; } + + /// + /// 圆心位置 + /// + public Point3D ArcCenter { get; set; } + + /// + /// 请求半径（配置的转向半径） + /// + public double RequestedRadius { get; set; } + + /// + /// 实际半径（安全截断后） + /// + public double ActualRadius { get; set; } + + /// + /// 偏转角（弧度） + /// + public double DeflectionAngle { get; set; } + + /// + /// 圆弧长度（米） + /// + public double ArcLength { get; set; } +} +[NEW] PathEdge 类 +/// +/// 路径边 - 连接两个连续控制点的物理路径段 +/// +[Serializable] +public class PathEdge +{ + /// + /// 边唯一标识符 + /// + public string Id { get; set; } + + /// + /// 起始控制点ID + /// + public string StartPointId { get; set; } + + /// + /// 结束控制点ID + /// + public string EndPointId { get; set; } + + /// + /// 路径段类型 + /// + public PathSegmentType SegmentType { get; set; } + + /// + /// 圆弧轨迹数据（仅当 SegmentType == Arc 时有效） + /// + public ArcTrajectory Trajectory { get; set; } + + /// + /// 边的物理长度（米） + /// 直线段：两点间距离；圆弧段：直线段长度 + 圆弧长度 + /// + public double PhysicalLength { get; set; } + + /// + /// 采样点序列（用于碰撞检测和动画） + /// + [XmlIgnore] + public List SampledPoints { get; set; } +} +2.2 修改现有数据模型 +[MODIFY] +PathPoint.cs +新增属性： + +/// +/// 自定义转向半径（米），用于局部急转弯场景 +/// 为 null 时使用全局默认值 +/// +public double? CustomTurnRadius { get; set; } +[MODIFY] +PathRoute.cs +新增属性： + +/// +/// 路径边集合 - 存储物理路径段 +/// +public List Edges { get; set; } = new List(); +/// +/// 默认转向半径（米） +/// +public double TurnRadius { get; set; } = 1.5; +/// +/// 是否已曲线化 +/// +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 方法 +/// +/// 计算圆弧切点和轨迹参数 +/// +/// 前一个控制点 +/// 当前控制点 +/// 下一个控制点 +/// 转向半径（米） +/// 圆弧轨迹数据 +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 方法 +/// +/// 圆弧采样为离散点序列 +/// +/// 圆弧轨迹 +/// 采样步长（米） +/// 采样点列表 +public static List SampleArc( + ArcTrajectory trajectory, + double samplingStep) +{ + var points = new List(); + + // 根据采样步长计算采样点数量 + 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 方法 +/// +/// 对路径应用曲线化处理 +/// +/// 待处理的路径 +/// 采样步长（米） +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 + 类中新增： + +/// +/// 默认转向半径（米） +/// 推荐值：1.0-2.0 +/// +public double DefaultTurnRadius { get; set; } = 1.5; +/// +/// 圆弧采样步长（米） +/// 推荐值：0.02-0.1 +/// +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 edges) +LoadPathEdges(string routeId) : List +DeletePathEdges(string routeId) +修改方法： + +SavePathRoute(PathRoute route) +: 同时保存 Edges +GetAllPathRoutes() +: 同时加载 Edges +DeletePathRoute(string routeId) +: 级联删除 Edges +5.3 导出格式扩展 +[MODIFY] +PathDataManager.cs +DELMIA XML 格式： + + + + + + + + + 1.5 + + + + +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包含完整的 节点 +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: 测试与验证 + 功能测试 + 性能测试 + 边界场景测试 diff --git a/run-unit-tests.bat b/run-unit-tests.bat index c0fb828..04dc3f9 100644 --- a/run-unit-tests.bat +++ b/run-unit-tests.bat @@ -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! diff --git a/src/Core/Config/SystemConfig.cs b/src/Core/Config/SystemConfig.cs index f54a484..e607d9e 100644 --- a/src/Core/Config/SystemConfig.cs +++ b/src/Core/Config/SystemConfig.cs @@ -87,6 +87,20 @@ namespace NavisworksTransport.Core.Config /// 安全间隙（米） /// public double SafetyMarginMeters { get; set; } = 0.05; + + // 路径曲线化配置 + + /// + /// 路径默认转弯半径（米） + /// 表示路径允许的最大转弯半径，车辆的最小转弯半径必须小于等于此值 + /// + public double DefaultPathTurnRadius { get; set; } = 2.5; + + /// + /// 圆弧采样步长（米） + /// 推荐值：0.02-0.1 + /// + public double ArcSamplingStep { get; set; } = 0.05; } /// diff --git a/src/Core/PathCurveEngine.cs b/src/Core/PathCurveEngine.cs new file mode 100644 index 0000000..3eacc68 --- /dev/null +++ b/src/Core/PathCurveEngine.cs @@ -0,0 +1,354 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Autodesk.Navisworks.Api; + +namespace NavisworksTransport +{ + /// + /// 路径曲线化引擎 + /// 实现基于圆弧过渡（Arc Fillet）的路径曲线化功能 + /// + public static class PathCurveEngine + { + /// + /// 将值限制在指定范围内（.NET Framework 4.8 不支持 Math.Clamp） + /// + private static double Clamp(double value, double min, double max) + { + if (value < min) return min; + if (value > max) return max; + return value; + } + + /// + /// 计算圆弧切点和轨迹参数 + /// + /// 前一个控制点 + /// 当前控制点 + /// 下一个控制点 + /// 转向半径（米） + /// 圆弧轨迹数据 + 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 + }; + } + + /// + /// Rodrigues旋转公式 - 绕任意轴旋转点 + /// + /// 要旋转的点 + /// 旋转中心 + /// 旋转轴（单位向量） + /// 旋转角度（弧度） + /// 旋转后的点 + 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; + } + + /// + /// 圆弧采样为离散点序列 + /// + /// 圆弧轨迹 + /// 采样步长（米） + /// 采样点列表 + public static List SampleArc( + ArcTrajectory trajectory, + double samplingStep) + { + var points = new List(); + + // 如果圆弧长度很小，直接返回起止点 + 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; + } + + /// + /// 构建直线边 + /// + /// 起始点 + /// 结束点 + /// 采样步长（米） + /// 路径边 + 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(); + + 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; + } + + /// + /// 构建包含圆弧的边 + /// + /// 起始点 + /// 结束点 + /// 圆弧轨迹 + /// 采样步长（米） + /// 路径边 + 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; + } + + /// + /// 生成采样点序列（用于延迟加载） + /// + /// 路径边 + /// 采样步长（米） + /// 采样点列表 + public static List 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(); + + 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(); + } + + /// + /// 对路径应用曲线化处理 + /// + /// 待处理的路径 + /// 采样步长（米） + 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); + } + + /// + /// 重新计算路径总长度 + /// + /// 路径 + public static void RecalculateRouteLength(PathRoute route) + { + if (route == null || route.Edges == null) + { + route.TotalLength = 0; + return; + } + + route.TotalLength = route.Edges.Sum(e => e.PhysicalLength); + } + } +} \ No newline at end of file diff --git a/src/Core/PathDataManager.cs b/src/Core/PathDataManager.cs index 224463d..3d5f624 100644 --- a/src/Core/PathDataManager.cs +++ b/src/Core/PathDataManager.cs @@ -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; } diff --git a/src/Core/PathDatabase.cs b/src/Core/PathDatabase.cs index a311b0e..35bdfac 100644 --- a/src/Core/PathDatabase.cs +++ b/src/Core/PathDatabase.cs @@ -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)"); } /// @@ -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); } } } } + /// + /// 保存路径边 + /// + 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(); + } + } + + /// + /// 删除路径点 + /// + private void DeletePathPoints(string routeId) + { + using (var cmd = new SQLiteCommand("DELETE FROM PathPoints WHERE RouteId = @routeId", _connection)) + { + cmd.Parameters.AddWithValue("@routeId", routeId); + cmd.ExecuteNonQuery(); + } + } + + /// + /// 删除路径边 + /// + private void DeletePathEdges(string routeId) + { + using (var cmd = new SQLiteCommand("DELETE FROM PathEdges WHERE RouteId = @routeId", _connection)) + { + cmd.Parameters.AddWithValue("@routeId", routeId); + cmd.ExecuteNonQuery(); + } + } + + /// + /// 加载路径边 + /// + 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); + } + } + } + } + /// /// 执行非查询SQL语句 /// diff --git a/src/Core/PathPlanningManager.cs b/src/Core/PathPlanningManager.cs index b564fd0..db9ec7d 100644 --- a/src/Core/PathPlanningManager.cs +++ b/src/Core/PathPlanningManager.cs @@ -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) { diff --git a/src/Core/PathPlanningModels.cs b/src/Core/PathPlanningModels.cs index 37e2c9f..d2b7cf4 100644 --- a/src/Core/PathPlanningModels.cs +++ b/src/Core/PathPlanningModels.cs @@ -269,6 +269,12 @@ namespace NavisworksTransport /// public double SpeedLimit { get; set; } + /// + /// 自定义转向半径（米），用于局部急转弯场景 + /// 为 null 时使用全局默认值 + /// + public double? CustomTurnRadius { get; set; } + /// /// 构造函数 /// @@ -282,6 +288,7 @@ namespace NavisworksTransport Index = 0; Notes = string.Empty; SpeedLimit = 0; + CustomTurnRadius = null; } /// @@ -300,6 +307,7 @@ namespace NavisworksTransport Index = 0; Notes = string.Empty; SpeedLimit = 0; + CustomTurnRadius = null; } /// @@ -312,6 +320,114 @@ namespace NavisworksTransport } } + /// + /// 路径段类型 + /// + public enum PathSegmentType + { + /// + /// 直线段 + /// + Straight, + + /// + /// 圆弧段 + /// + Arc + } + + /// + /// 圆弧轨迹数据 + /// + [Serializable] + public class ArcTrajectory + { + /// + /// 进入切点 + /// + public Point3D Ts { get; set; } + + /// + /// 退出切点 + /// + public Point3D Te { get; set; } + + /// + /// 圆心位置 + /// + public Point3D ArcCenter { get; set; } + + /// + /// 请求半径（配置的转向半径） + /// + public double RequestedRadius { get; set; } + + /// + /// 实际半径（安全截断后） + /// + public double ActualRadius { get; set; } + + /// + /// 偏转角（弧度） + /// + public double DeflectionAngle { get; set; } + + /// + /// 圆弧长度（米） + /// + public double ArcLength { get; set; } + } + + /// + /// 路径边 - 连接两个连续控制点的物理路径段 + /// + [Serializable] + public class PathEdge + { + /// + /// 边唯一标识符 + /// + public string Id { get; set; } + + /// + /// 起始控制点ID + /// + public string StartPointId { get; set; } + + /// + /// 结束控制点ID + /// + public string EndPointId { get; set; } + + /// + /// 路径段类型 + /// + public PathSegmentType SegmentType { get; set; } + + /// + /// 圆弧轨迹数据（仅当 SegmentType == Arc 时有效） + /// + public ArcTrajectory Trajectory { get; set; } + + /// + /// 边的物理长度（米） + /// 直线段：两点间距离；圆弧段：直线段长度 + 圆弧长度 + /// + public double PhysicalLength { get; set; } + + /// + /// 采样点序列（用于碰撞检测和动画） + /// + [XmlIgnore] + public List SampledPoints { get; set; } + + public PathEdge() + { + Id = Guid.NewGuid().ToString(); + SampledPoints = new List(); + } + } + /// /// 路径路线数据模型 /// @@ -319,10 +435,15 @@ namespace NavisworksTransport public class PathRoute { /// - /// 路径点集合 + /// 路径点集合（控制点） /// public List Points { get; set; } - + + /// + /// 路径边集合 - 存储物理路径段 + /// + public List Edges { get; set; } + /// /// 路径名称 /// @@ -414,6 +535,17 @@ namespace NavisworksTransport /// public double SafetyMargin { get; set; } = 0.5; + /// + /// 路径转弯半径（米）- 路径允许的最大转弯半径 + /// 车辆的最小转弯半径必须小于等于此值 + /// + public double TurnRadius { get; set; } = 0.0; + + /// + /// 是否已曲线化 + /// + public bool IsCurved { get; set; } = false; + // 数据库分析相关属性 /// /// 碰撞数量（从数据库加载） @@ -441,6 +573,7 @@ namespace NavisworksTransport public PathRoute() { Points = new List(); + Edges = new List(); 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; } /// @@ -458,12 +593,15 @@ namespace NavisworksTransport public PathRoute(string name) { Points = new List(); + Edges = new List(); Name = name; Id = Guid.NewGuid().ToString(); EstimatedTime = 0.0; AssociatedChannelIds = new List(); TotalLength = 0.0; CreatedTime = DateTime.Now; + TurnRadius = 0.0; // 0.0 表示未设置，实际使用时从配置获取默认值 + IsCurved = false; LastModified = DateTime.Now; Description = string.Empty; }