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

把曲线化路径与动画集成

Browse Source
This commit is contained in:
tian 2026-01-05 12:48:57 +08:00
parent 174749e287
commit 6cc48c3500
7 changed files with 1470 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

531
IFLOW.md
View File

@ -2,104 +2,525 @@
## 项目简介
NavisworksTransport 是一个用于 Autodesk Navisworks Manage 2026 的插件，专为物流路径规划和动画仿真而设计。它能够帮助用户在复杂的建筑模型中定义物流通道、规划运输路径，并生成动画来模拟设备移动过程，同时进行动态碰撞检测。
NavisworksTransport 是一个用于 Autodesk Navisworks Manage 2026 的插件,专为物流路径规划和动画仿真而设计。它能够帮助用户在复杂的建筑模型中定义物流通道、规划运输路径,并生成动画来模拟设备移动过程,同时进行动态碰撞检测。
**当前版本**: 0.14.0
## 技术架构
- **编程语言**: C# 7.3
- **开发环境**: .NET Framework 4.8
- **目标平台**: x64
- **主要依赖**:
- Autodesk Navisworks API (2026版本)
- Roy-T.AStar (路径规划算法)
- System.Data.SQLite (数据存储)
- Tomlyn (TOML配置文件解析)
- geometry4Sharp (3D几何计算)
- Roy-T.AStar 3.0.2 (路径规划算法)
- System.Data.SQLite 1.0.118.0 (数据存储)
- Tomlyn 0.19.0 (TOML配置文件解析)
- geometry4Sharp 1.0.0 (3D几何计算)
- MSTest.TestFramework 3.0.4 (单元测试)
- NETStandard.Library 2.0.3 (.NET Standard支持)
## 核心功能模块
### 1. 物流属性管理
- 为模型元素分配物流类别属性（如门、电梯、楼梯、通道、障碍物）
- 为模型元素分配物流类别属性(如门、电梯、楼梯、通道、障碍物、无关项)
- 批量处理多个选中项目
- COM API 集成确保属性正确添加
- 物流属性列表与3D视图选择同步
- 支持单个模型可见性控制和属性回填
- 严格的数据验证机制,确保数据一致性
### 2. 路径规划
- 手动路径点编辑
- 自动路径规划 (A*算法)
- 2.5D网格地图生成
- 路径优化和分析
#### 手动路径编辑
- 路径点添加、编辑、删除
- 智能工具切换系统(十字光标导航)
- 空格键快速切换工具状态
- 路径点高亮和可视化
#### 自动路径规划
- **A*算法集成**: 使用Roy-T.AStar库实现高性能路径查找
- **2.5D网格地图生成**: 基于包围盒的高效网格生成算法
- **高度计算系统**:
- 通道地面高度精确检测
- 坡度分析和楼梯台阶高度处理
- 线性和非线性坡度支持
- **路径优化算法**:
- 最短路径策略(Shortest)
- 局部直线优先策略(Straightest) - 室内导航优化
- 基于中心距离的安全优先算法
- 斜线路径优化(台阶处保留2个路径点)
- **多层网格系统**: 支持楼梯、楼面等复杂场景
- **网格缓存机制**: 大幅提升路径规划速度
- **楼层过滤系统**: 基于通道楼层属性的智能过滤
#### 路径分析
- SQLite数据库存储路径数据
- 路径信息统计和分析
- 路径参数持久化(车辆尺寸、安全间隙、网格尺寸等)
### 3. 动画与仿真
- 基于路径的动画生成
- TimeLiner 集成
- 动态碰撞检测 (Clash Detective集成)
- **动画系统**:
- 基于路径的动画生成
- WPF Timer机制实现流畅动画播放
- 媒体控制功能(播放、暂停、步进、快放、反向播放)
- 动画性能优化(单步时间从5秒降至15毫秒)
- **TimeLiner集成**: 动画播放时自动创建运输任务
- **动态碰撞检测**:
- Clash Detective API集成
- 基于动画帧的碰撞检测
- 碰撞位置记录和恢复机制
- 碰撞报告生成和统计
### 4. 可视化
- 3D路径点和路线渲染
- 网格地图可视化
- 碰撞结果高亮显示
- **3D路径点和路线渲染**:
- 手动路径连线(黑色)
- 自动路径连线(橙色)
- 路径点高亮和选择反馈
- **网格地图可视化**:
- 空洞、障碍物、通行网格的可视化
- 网格点大小自适应
- 多种形状类型(圆形、正方形、立方体、点)
- Google Material Design配色标准
- **碰撞结果高亮显示**
- **导航地图生成**: 导出当前路径场景为图片
### 5. 数据管理
### 5. 分层管理
- SQLite数据库存储路径数据
- TOML配置文件管理系统设置
- **智能楼层分析**: 支持按楼层和自定义属性分层
- **分层预览功能**:
- 1级、2级、3级、全部四种遍历深度
- 实时预览结果(名称、对象数量、估算大小)
- 缓存机制优化性能
- **分层导出**:
- 批量导出功能
- 智能文件命名(项目名_分层名_时间戳)
- 支持NWD导出选项配置
- 线程安全导出机制
- **可视化控制**:
- 单独显示功能
- 保存开关控制
- 智能可见性保护算法
- **自定义分层属性**: 支持任意Navisworks模型属性作为分层依据
### 6. 数据管理
- **SQLite数据库**: 路径数据和碰撞报告存储
- **TOML配置文件**: 系统设置管理(default_config.toml)
- **JSON路径文件导入**: 支持灵活的数据交换格式
- **几何体缓存**: 提高模型加载和渲染性能
### 7. 系统管理
- **MSI安装包**: 自动化安装包生成,支持静默安装
- **日志系统**: 完整的日志记录和查看功能
- **配置编辑器**: 可视化配置文件编辑
- **帮助系统**: 内置帮助文档和关于对话框
## 项目结构
```graph
```
NavisworksTransport/
├── src/
│ ├── Commands/ # 命令模式实现
│ ├── Core/ # 核心业务逻辑
│ │ ├── Animation/ # 动画管理
│ │ ├── Collision/ # 碰撞检测
│ │ ├── Config/ # 配置管理
│ │ ├── Properties/ # 属性管理
│ │ ├── Spatial/ # 空间索引
│ │ └── UIUpdate/ # UI更新管理
│ ├── PathPlanning/ # 路径规划算法
│ ├── UI/ ├── WPF/ # WPF用户界面
│ └── Utils/ # 工具类
├── doc/ # 文档
├── packages/ # NuGet包
└── tests/ # 单元测试
│ ├── Commands/ # 命令模式实现
│ │ ├── IPathPlanningCommand.cs
│ │ ├── CommandBase.cs
│ │ ├── CommandManager.cs
│ │ ├── CommandExecutor.cs
│ │ ├── AutoPathPlanningCommand.cs
│ │ ├── DeletePathCommand.cs
│ │ ├── ExportPathCommand.cs
│ │ ├── ImportPathCommand.cs
│ │ ├── SetLogisticsAttributeCommand.cs
│ │ ├── StartAnimationCommand.cs
│ │ ├── GenerateCollisionReportCommand.cs
│ │ └── VoxelPathFindingTestCommand.cs
│ ├── Core/ # 核心业务逻辑
│ │ ├── Animation/ # 动画管理
│ │ ├── Collision/ # 碰撞检测
│ │ ├── Config/ # 配置管理
│ │ ├── Properties/ # 属性管理
│ │ ├── Spatial/ # 空间索引
│ │ ├── UIUpdate/ # UI更新管理
│ │ ├── MainPlugin.cs # 插件入口
│ │ ├── PathPlanningManager.cs
│ │ ├── PathDataManager.cs
│ │ ├── PathDatabase.cs
│ │ ├── PathAnalysisService.cs
│ │ ├── PathCurveEngine.cs
│ │ ├── UIStateManager.cs
│ │ ├── IdleEventManager.cs
│ │ ├── DocumentStateManager.cs
│ │ ├── VirtualVehicleManager.cs
│ │ ├── ModelSplitterManager.cs
│ │ ├── NavigationMapGenerator.cs
│ │ ├── PathPointRenderPlugin.cs
│ │ └── FloorAttributeManager.cs
│ ├── PathPlanning/ # 路径规划算法
│ │ ├── GridMap.cs
│ │ ├── GridMapGenerator.cs
│ │ ├── GridMapCache.cs
│ │ ├── AutoPathFinder.cs
│ │ ├── PathOptimizer.cs
│ │ ├── ChannelHeightDetector.cs
│ │ ├── SlopeAnalyzer.cs
│ │ ├── OptimizedHeightCalculator.cs
│ │ ├── ChannelBasedGridBuilder.cs
│ │ ├── TimeMarkerCalculationService.cs
│ │ ├── VoxelGrid.cs # 3D体素路径规划(实验性)
│ │ ├── VoxelPathFinder.cs
│ │ └── VoxelGridVisualizer.cs
│ ├── UI/ # WPF用户界面
│ │ └── WPF/
│ │ ├── Views/ # 视图
│ │ ├── ViewModels/ # 视图模型
│ │ ├── Models/ # 数据模型
│ │ ├── Commands/ # WPF命令
│ │ ├── Converters/ # 值转换器
│ │ ├── Collections/ # 线程安全集合
│ │ ├── Services/ # UI服务
│ │ ├── Interfaces/ # 接口定义
│ │ └── Resources/ # 资源文件
│ ├── Utils/ # 工具类
│ │ ├── LogManager.cs
│ │ ├── NavisworksApiHelper.cs
│ │ ├── GeometryHelper.cs
│ │ ├── BoundingBoxGeometryUtils.cs
│ │ ├── CoordinateConverter.cs
│ │ ├── UnitsConverter.cs
│ │ ├── FloorDetector.cs
│ │ ├── ModelItemAnalysisHelper.cs
│ │ ├── VisibilityHelper.cs
│ │ └── GeometryCacheManager.cs
│ └── Resources/ # 资源文件
├── UnitTests/ # 单元测试
│ ├── Collections/
│ ├── Commands/
│ ├── Core/
│ ├── Utils/
│ └── TestHelpers/
├── doc/ # 文档
│ ├── architecture/ # 架构文档
│ ├── design/ # 设计文档
│ ├── guide/ # 使用指南
│ ├── migration/ # 迁移文档
│ ├── navisworks_api/ # API文档
│ ├── requirement/ # 需求文档
│ └── working/ # 工作文档
├── packages/ # NuGet包
├── resources/ # 资源文件
├── bin/ # 编译输出
├── obj/ # 中间文件
├── TestResults/ # 测试结果
├── NavisworksTransport.sln # 解决方案文件
├── NavisworksTransportPlugin.csproj # 主项目文件
├── NavisworksTransport.UnitTests.csproj # 测试项目文件
├── packages.config # NuGet包配置
├── default_config.toml # 默认配置文件
├── compile.bat # 编译脚本
├── run-unit-tests.bat # 运行单元测试脚本
├── deploy-plugin.bat # 部署插件脚本
├── VERSION.md # 版本信息
├── CHANGELOG.md # 变更日志
└── README.md # 项目说明
```
## 构建与运行
1. **环境要求**:
- Windows 7 或更高版本
- Navisworks Manage 2026
- .NET Framework 4.8
### 环境要求
2. **构建步骤**:
- 使用 Visual Studio 打开 `NavisworksTransport.sln`
- 还原 NuGet 包
- 编译解决方案
- **操作系统**: Windows 10 或更高版本
- **Navisworks**: Navisworks Manage 2026
- **开发工具**: Visual Studio 2022 (Community或Professional)
- **运行时**: .NET Framework 4.8
- **测试工具**: MSTest Test Platform
3. **部署**:
- 编译生成 `NavisworksTransportPlugin.dll`
- 插件会自动安装到Navisworks插件目录：
`[Navisworks安装路径]\Plugins\NavisworksTransportPlugin\`
- 重启Navisworks即可使用
### 构建步骤
#### 使用Visual Studio
1. 使用 Visual Studio 2022 打开 `NavisworksTransport.sln`
2. 还原 NuGet 包(自动或手动)
3. 选择配置: Release / x64
4. 编译解决方案
#### 使用命令行
```batch
# 编译插件
compile.bat
# 运行单元测试
run-unit-tests.bat
```
### 部署
#### 自动部署
```batch
# 部署插件到Navisworks插件目录
deploy-plugin.bat
```
#### 手动部署
1. 编译生成 `NavisworksTransportPlugin.dll`
2. 将输出目录内容复制到Navisworks插件目录:
`[Navisworks安装路径]\Plugins\NavisworksTransportPlugin\`
3. 重启Navisworks即可使用
### 安装包生成
项目支持MSI安装包生成:
- 使用Visual Studio Installer Projects扩展
- 支持自动化构建和静默安装
- 安装包位置: `bin\x64\Release\NavisworksTransport.Setup.msi`
静默安装命令:
```batch
msiexec /i NavisworksTransport.Setup.msi /qn /l*v install.log
```
## 开发约定
- 使用MVVM模式进行UI开发
- 通过Command模式封装业务逻辑
- 利用PathPlanningManager作为核心业务逻辑协调器
- 配置文件使用TOML格式
- 日志记录使用内置的LogManager
- 异常处理通过GlobalExceptionHandler统一管理
### 架构模式
## 核心组件说明
- **MVVM模式**: WPF UI开发使用MVVM架构
- **Command模式**: 业务逻辑通过Command模式封装
- **依赖注入**: 使用服务接口实现松耦合
- **MainPlugin**: 插件入口点，负责初始化和生命周期管理
### 核心组件
- **MainPlugin**: 插件入口点,负责初始化和生命周期管理
- **PathPlanningManager**: 路径规划核心业务逻辑管理器
- **CategoryAttributeManager**: 物流属性管理
- **PathPointRenderPlugin**: 3D路径可视化渲染
- **ConfigManager**: 系统配置管理
- **UIStateManager**: UI状态和线程安全更新管理
- **IdleEventManager**: Idle事件管理,优化UI性能
- **DocumentStateManager**: 文档状态管理
- **VirtualVehicleManager**: 虚拟车辆管理
- **PathAnimationManager**: 动画管理
- **TimeLinerIntegrationManager**: TimeLiner集成
- **ClashDetectiveIntegration**: 碰撞检测集成
### 代码规范
- **配置文件**: 使用TOML格式
- **日志记录**: 使用内置的LogManager
- **异常处理**: 通过GlobalExceptionHandler统一管理,不掩盖问题
- **线程安全**: COM API调用必须在主UI线程(STA)执行
- **单位系统**: 统一使用米(m)为单位
- **向后兼容**: 不写回退和向后兼容代码,出错即抛异常
### 测试
- **单元测试**: 使用MSTest框架
- **测试覆盖**: 核心组件和关键业务逻辑
- **测试运行**: `run-unit-tests.bat`
- **测试结果**: 保存在TestResults目录
### 性能优化
- **空间索引**: 使用SpatialHashGrid优化碰撞检测
- **网格缓存**: GridMapCache提升路径规划速度
- **几何缓存**: GeometryCacheManager优化模型加载
- **线程安全**: ThreadSafeObservableCollection确保UI更新安全
- **Idle事件**: 使用Idle机制替代Timer,优化UI响应
## 快速开始
### 1. 插件安装
1. 运行MSI安装包或手动部署
2. 启动Navisworks Manage 2026
3. 在"附加模块"选项卡中找到"Transport Plugin"
### 2. 基本使用流程
#### 物流属性设置
1. 在Navisworks中选择要设置属性的模型项目
2. 点击"Transport Plugin" → "物流属性管理"
3. 在弹出窗口中选择物流类别(门、电梯、楼梯、通道、障碍物)
4. 点击"应用"为选中项目添加属性
#### 路径规划
1. 点击"路径编辑"页签
2. 选择路径策略(最短路径或直线优先)
3. 在3D视图中点击添加路径点
4. 点击"自动规划"生成路径
5. 调整路径参数(车辆尺寸、网格大小等)
#### 动画仿真
1. 点击"动画控制"页签
2. 选择要播放的路径
3. 设置动画参数(帧率、持续时间)
4. 点击"播放"开始动画
5. 查看碰撞检测结果
#### 分层管理
1. 点击"分层管理"页签
2. 选择分层属性(楼层、区域等)
3. 设置遍历深度
4. 点击"预览"查看分层结果
5. 选择要导出的分层,点击"导出"
### 3. 配置管理
编辑 `default_config.toml` 文件调整系统参数:
```toml
[path_editing]
cell_size_meters = 0.5
vehicle_length_meters = 1.0
vehicle_width_meters = 1.0
vehicle_height_meters = 2.0
safety_margin_meters = 0.05
[animation]
frame_rate = 30
duration_seconds = 10.0
detection_gap_meters = 0.05
```
## 版本历史
### v0.14.0 (2025-12-01)
- MSI安装包制作
- JSON路径文件导入
- 网格点可视化系统重构
- 空间索引优化
- 几何体缓存优化
### v0.13.0 (2025-10-11)
- 3D路径规划系统
- 多层网格系统
- 路径策略算法增强
- SQLite数据库集成
- 网格生成优化
### v0.12.0 (2025-09-16)
- 路径规划算法优化
- 可视化系统统一
- 动画系统重构
- 碰撞检测系统完善
- UI架构现代化
- 物流属性管理系统完善
- 分层管理系统优化
- 内存管理与性能优化
### v0.11.0 (2025-09-08)
- 局部直线优先路径算法
- 路径策略选择系统
- 网格可视化系统
- Roy-T.AStar集成优化
- UI架构现代化升级
- 内存管理优化
### v0.10.0 (2025-09-06)
- 智能工具切换系统
- 工具状态管理重构
- ClashDetective API重构
### v0.9.0 (2025-09-04)
- SearchAPI性能优化
- 代码架构重构
- UI原型开发
- 算法优化
### v0.8.0 (2025-08-27)
- 自定义分层管理系统
- VisibilityManager重构
- 智能文件命名系统
### v0.7.0 (2025-08-18)
- 分层管理功能完整实现
- WPF架构重构
- 线程安全解决方案
### v0.6.0 (2025-08-17)
- UI架构重构
- 线程安全与稳定性提升
### v0.5.0 (2025-08-15)
- Z坐标高度计算系统
- 楼层过滤系统
### v0.4.0 (2025-08-14)
- 单位转换系统重构
- 2.5D路径规划实现
### v0.3.0 (2025-08-13)
- Navisworks2026迁移
- WPF界面实现
### v0.2.0 (2025-07-21)
- 动画碰撞检测集成
- Clash Detective集成
### v0.1.12 (2025-07-18)
- TimeLiner集成功能
## 技术支持
### 常见问题
1. **插件未加载**: 确保Navisworks Manage 2026已正确安装,插件目录路径正确
2. **崩溃问题**: 查看日志文件,检查Navisworks API调用线程安全性
3. **路径规划失败**: 检查物流属性是否正确设置,通道是否连续
4. **碰撞检测不准确**: 调整检测间隙参数,检查车辆尺寸配置
### 日志位置
日志文件位于:
`[Navisworks安装路径]\Plugins\NavisworksTransportPlugin\logs\debug.log`
### 文档资源
- **使用说明**: `doc/working/使用说明.md`
- **开发文档**: `doc/working/`
- **API文档**: `doc/navisworks_api/`
- **迁移文档**: `doc/migration/`
- **设计文档**: `doc/design/`
- **需求文档**: `doc/requirement/`
## 许可与贡献
本项目为Navisworks插件开发项目,遵循Autodesk插件开发规范。
## 联系方式
- **项目仓库**: <http://10.0.0.99:4000/tian/NavisworksTransport.git>
- **问题反馈**: 通过Git仓库Issue系统提交
- **开发文档**: 参考项目doc目录下的相关文档
---
**最后更新**: 2026年1月5日

552
doc/working/动画检测曲线化集成方案_20260105.md Normal file
View File

@ -0,0 +1,552 @@
# 动画检测集成方案
## 一、现状分析
### 1.1 当前动画系统架构
#### 核心组件
- **PathAnimationManager**: 动画管理器,负责动画帧预计算和播放
- **AnimationControlViewModel**: 动画控制视图模型,处理UI交互
- **PathCurveEngine**: 路径曲线化引擎,已实现圆弧过渡功能
#### 当前路径插值机制
```csharp
// PathAnimationManager.cs:1253
private Point3D InterpolatePosition(double progress)
```
- 基于原始控制点(_pathPoints)进行线性插值
- 按路径段距离比例计算位置
- **问题**: 未使用PathRoute.Edges,动画沿直线段运动
#### 当前朝向计算机制
```csharp
// PathAnimationManager.cs:600
private double ComputeYawFromPath(int frameIndex, List<Point3D> framePositions)
```
- 基于前后帧位置计算方向向量
- 使用atan2计算偏航角
- **问题**: 在直线段上方向准确,但在圆弧处需要改进
### 1.2 路径数据结构
#### PathRoute模型
```csharp
// PathPlanningModels.cs
public class PathRoute
{
public List<PathPoint> Points { get; set; } // 控制点
public List<PathEdge> Edges { get; set; } // 物理路径段
public bool IsCurved { get; set; } // 是否已曲线化
public double TurnRadius { get; set; } // 转向半径
}
```
#### PathEdge类型
```csharp
public enum PathSegmentType
{
Straight, // 直线段
Arc // 圆弧段
}
public class PathEdge
{
public PathSegmentType SegmentType { get; set; }
public ArcTrajectory Trajectory { get; set; } // 圆弧轨迹数据
public List<Point3D> SampledPoints { get; set; } // 采样点序列
public double PhysicalLength { get; set; } // 物理长度
}
```
#### ArcTrajectory数据
```csharp
public class ArcTrajectory
{
public Point3D Ts { get; set; } // 进入切点
public Point3D Te { get; set; } // 退出切点
public Point3D ArcCenter { get; set; } // 圆心
public double ActualRadius { get; set; } // 实际半径
public double DeflectionAngle { get; set; } // 偏转角(弧度)
public double ArcLength { get; set; } // 圆弧长度
}
```
### 1.3 PathCurveEngine已实现功能
- **CalculateFillet**: 计算圆弧切点和轨迹参数
- **SampleArc**: 圆弧采样为离散点序列
- **ApplyCurvatureToRoute**: 对路径应用曲线化处理
- **GenerateSampledPoints**: 生成采样点序列
## 二、集成方案设计
### 2.1 核心目标
1. **动画帧预计算使用路径**: 替代原有的直线插值
2. **圆弧处方向计算准确**: 确保车辆在圆弧段正确朝向
3. **性能优化**: 避免重复计算采样点
### 2.2 数据流设计
```
PathRoute (控制点)
↓
PathCurveEngine.ApplyCurvatureToRoute()
↓
PathRoute.Edges (包含直线段和圆弧段)
↓
PathAnimationManager.PrecomputeAnimationFrames()
↓
使用PathEdge.SampledPoints生成动画帧
↓
动画播放
```
### 2.3 关键修改点
#### 修改点1: PathAnimationManager.SetupAnimation()
**当前实现**:
```csharp
// PathAnimationManager.cs:407
public void SetupAnimation(ModelItem animatedObject, List<Point3D> pathPoints, double durationSeconds = 10.0)
{
// 直接使用pathPoints作为路径
_pathPoints = new List<Point3D>(pathPoints);
PrecomputeAnimationFrames();
}
```
**修改方案**:
```csharp
public void SetupAnimation(
ModelItem animatedObject,
double durationSeconds,
PathRoute route)
{
if (route == null)
throw new ArgumentNullException(nameof(route), "必须提供路径");
_animatedObject = animatedObject;
_animationDuration = durationSeconds;
_route = route;
LogManager.Info($"使用路径,边数:{route.Edges.Count},总长度:{route.TotalLength:F2}米,动画时长:{durationSeconds:F1}秒");
PrecomputeAnimationFrames();
}
```
#### 修改点2: PathAnimationManager.PrecomputeAnimationFrames()
**当前实现**:
```csharp
// PathAnimationManager.cs:468
private void PrecomputeAnimationFrames()
{
// 第一遍:收集所有帧位置(使用直线插值)
var framePositions = new List<Point3D>();
for (int i = 0; i < totalFrames; i++)
{
double progress = (double)i / (totalFrames - 1);
var framePosition = InterpolatePosition(progress); // 直线插值
framePositions.Add(framePosition);
}
// 第二遍:计算朝向并预计算每一帧
for (int i = 0; i < totalFrames; i++)
{
double yawRadians = ComputeYawFromPath(i, framePositions);
// ...
}
}
```
**修改方案**:
```csharp
private void PrecomputeAnimationFrames()
{
LogManager.Info("=== 使用路径预计算动画帧 ===");
int totalFrames = (int)(_animationDuration * _animationFrameRate);
// 1. 构建完整的采样点序列(按边顺序拼接)
var allSampledPoints = new List<Point3D>();
var edgeLengths = new List<double>(); // 每条边的长度
var edgeStartIndices = new List<int>(); // 每条边在采样序列中的起始索引
foreach (var edge in _route.Edges)
{
if (edge.SampledPoints == null || edge.SampledPoints.Count == 0)
{
// 延迟加载采样点
var samplingStep = ConfigManager.Instance.Current.PathEditing.ArcSamplingStep;
edge.SampledPoints = PathCurveEngine.GenerateSampledPoints(edge, samplingStep);
}
edgeStartIndices.Add(allSampledPoints.Count);
allSampledPoints.AddRange(edge.SampledPoints);
edgeLengths.Add(edge.PhysicalLength);
}
double totalLength = _route.TotalLength;
LogManager.Info($"路径总长度: {totalLength:F2}米, 采样点数: {allSampledPoints.Count}");
// 2. 按帧数采样生成动画帧
_animationFrames = new List<AnimationFrame>();
for (int i = 0; i < totalFrames; i++)
{
double progress = (double)i / (totalFrames - 1);
double targetDistance = totalLength * progress;
// 3. 找到当前在哪条边上
int edgeIndex = FindEdgeForDistance(targetDistance, edgeLengths);
if (edgeIndex < 0 || edgeIndex >= _route.Edges.Count)
{
LogManager.Warning($"无法找到边,目标距离:{targetDistance:F2}米");
continue;
}
var edge = _route.Edges[edgeIndex];
double accumulatedLength = edgeLengths.Take(edgeIndex).Sum();
double edgeProgress = (targetDistance - accumulatedLength) / edge.PhysicalLength;
// 4. 在边内插值位置
Point3D framePosition;
if (edge.SegmentType == PathSegmentType.Straight)
{
framePosition = InterpolateOnStraightEdge(edge, edgeProgress);
}
else // Arc
{
framePosition = InterpolateOnArcEdge(edge, edgeProgress);
}
// 5. 计算朝向(关键改进)
double yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress);
// 6. 创建帧并检测碰撞
var frame = new AnimationFrame
{
Index = i,
Progress = progress,
Position = framePosition,
YawRadians = yawRadians,
Collisions = new List<CollisionResult>()
};
// ... 碰撞检测逻辑保持不变 ...
_animationFrames.Add(frame);
}
}
```
#### 修改点3: 曲线边内插值方法
```csharp
/// <summary>
/// 在直线边上插值位置
/// </summary>
private Point3D InterpolateOnStraightEdge(PathEdge edge, double progress)
{
if (edge.SampledPoints == null || edge.SampledPoints.Count < 2)
return edge.Trajectory?.Ts ?? new Point3D();
int pointIndex = (int)(progress * (edge.SampledPoints.Count - 1));
pointIndex = Math.Max(0, Math.Min(pointIndex, edge.SampledPoints.Count - 1));
return edge.SampledPoints[pointIndex];
}
/// <summary>
/// 在圆弧边上插值位置
/// </summary>
private Point3D InterpolateOnArcEdge(PathEdge edge, double progress)
{
if (edge.Trajectory == null || edge.SampledPoints == null || edge.SampledPoints.Count < 2)
return edge.Trajectory?.Ts ?? new Point3D();
// 使用采样点插值(更精确)
int pointIndex = (int)(progress * (edge.SampledPoints.Count - 1));
pointIndex = Math.Max(0, Math.Min(pointIndex, edge.SampledPoints.Count - 1));
return edge.SampledPoints[pointIndex];
}
/// <summary>
/// 查找指定距离对应的边索引
/// </summary>
private int FindEdgeForDistance(double targetDistance, List<double> edgeLengths)
{
double accumulatedLength = 0.0;
for (int i = 0; i < edgeLengths.Count; i++)
{
if (accumulatedLength + edgeLengths[i] >= targetDistance)
{
return i;
}
accumulatedLength += edgeLengths[i];
}
return edgeLengths.Count - 1; // 返回最后一条边
}
```
#### 修改点4: 路径朝向计算(核心)
```csharp
/// <summary>
/// 计算路径上的朝向(改进版)
/// </summary>
private double ComputeYawOnPath(
int frameIndex,
List<Point3D> allSampledPoints,
int currentEdgeIndex,
double edgeProgress)
{
Point3D currentPos = allSampledPoints.Count > 0 ? allSampledPoints[Math.Min(frameIndex, allSampledPoints.Count - 1)] : new Point3D();
Point3D nextPos;
var edge = _route.Edges[currentEdgeIndex];
if (edge.SegmentType == PathSegmentType.Arc && edge.Trajectory != null)
{
// 圆弧段: 计算切线方向
// 使用圆弧参数方程计算精确方向
// 1. 获取当前点在圆弧上的角度
Vector3D startVec = (edge.Trajectory.Ts - edge.Trajectory.ArcCenter).Normalize();
double signedAngle = edge.Trajectory.DeflectionAngle; // 总偏转角
double currentAngle = edgeProgress * signedAngle;
// 2. 旋转起始向量得到当前方向向量
Point3D rotatedPoint = RotatePointAroundAxis(
edge.Trajectory.Ts,
edge.Trajectory.ArcCenter,
GetArcRotationAxis(edge.Trajectory),
currentAngle
);
// 3. 计算切线方向(沿圆弧切线)
Vector3D radiusVec = (rotatedPoint - edge.Trajectory.ArcCenter).Normalize();
Vector3D tangentVec = Vector3D.CrossProduct(
GetArcRotationAxis(edge.Trajectory),
radiusVec
);
// 4. 使用atan2计算yaw
return Math.Atan2(tangentVec.Y, tangentVec.X);
}
else
{
// 直线段: 查找下一个采样点
int nextIndex = Math.Min(frameIndex + 1, allSampledPoints.Count - 1);
nextPos = allSampledPoints[nextIndex];
double dx = nextPos.X - currentPos.X;
double dy = nextPos.Y - currentPos.Y;
double length = Math.Sqrt(dx * dx + dy * dy);
if (length < 1e-6)
{
// 如果距离太小,尝试查找更远的点
for (int offset = 2; offset < 10 && frameIndex + offset < allSampledPoints.Count; offset++)
{
nextPos = allSampledPoints[frameIndex + offset];
dx = nextPos.X - currentPos.X;
dy = nextPos.Y - currentPos.Y;
length = Math.Sqrt(dx * dx + dy * dy);
if (length > 1e-6) break;
}
}
return length > 1e-6 ? Math.Atan2(dy, dx) : 0.0;
}
}
/// <summary>
/// 获取圆弧旋转轴
/// </summary>
private Vector3D GetArcRotationAxis(ArcTrajectory trajectory)
{
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)
);
return new Vector3D(cross.X, cross.Y, cross.Z).Normalize();
}
```
### 2.4 UI集成修改
#### 修改点2: AnimationControlViewModel.ExecuteGenerateAnimation()
**当前实现**:
```csharp
// AnimationControlViewModel.cs:815
private void ExecuteGenerateAnimation()
{
// 获取当前路径的控制点
var pathPoints = CurrentPathRoute.Points.Select(p => p.Position).ToList();
// 调用SetupAnimation
_pathAnimationManager.SetupAnimation(
_animatedObject,
pathPoints,
_animationDuration
);
}
```
**修改方案**:
```csharp
private void ExecuteGenerateAnimation()
{
if (CurrentPathRoute == null)
throw new InvalidOperationException("未选择路径");
LogManager.Info($"使用路径生成动画,边数:{CurrentPathRoute.Edges.Count},总长度:{CurrentPathRoute.TotalLength:F2}米");
// 调用SetupAnimation,传入路径和用户配置的动画时长
_pathAnimationManager.SetupAnimation(
_animatedObject,
_animationDuration,
CurrentPathRoute
);
SetState(AnimationState.Ready);
UpdateMainStatus($"动画已生成,时长:{_animationDuration}秒,帧率:{_animationFrameRate}fps");
}
```
### 2.4 新增字段
在PathAnimationManager中添加:
```csharp
// 路径相关
private PathRoute _route = null; // 路径引用
```
## 三、实现步骤
### 阶段1: 数据结构准备
1. ✅ PathRoute、PathEdge、ArcTrajectory已存在
2. ✅ PathCurveEngine已实现曲线化算法
3. ⏳ 在PathAnimationManager中添加_route字段
### 阶段2: 核心算法实现
1. ⏳ 修改SetupAnimation()方法,必需传入route参数
2. ⏳ 重写PrecomputeAnimationFrames()方法,使用路径
3. ⏳ 实现曲线边插值方法(InterpolateOnStraightEdge, InterpolateOnArcEdge)
4. ⏳ 实现FindEdgeForDistance()方法
5. ⏳ 实现ComputeYawOnPath()方法(核心)
6. ⏳ 实现GetArcRotationAxis()辅助方法
### 阶段3: UI集成
1. ⏳ 修改AnimationControlViewModel.ExecuteGenerateAnimation()
2. ⏳ 添加路径验证和边数据生成
3. ⏳ 更新日志输出
### 阶段4: 测试验证
1. ⏳ 单元测试:曲线边插值准确性
2. ⏳ 单元测试:圆弧朝向计算准确性
3. ⏳ 集成测试:完整动画播放流程
4. ⏳ 性能测试:预计算时间
## 四、关键技术点
### 4.1 圆弧朝向计算原理
#### 方法1: 采样点差分法(简单但精度较低)
```csharp
// 使用前后采样点计算方向
Point3D prevPoint = allSampledPoints[Math.Max(0, frameIndex - 1)];
Point3D nextPoint = allSampledPoints[Math.Min(frameIndex + 1, allSampledPoints.Count - 1)];
Vector3D direction = (nextPoint - prevPoint).Normalize();
double yaw = Math.Atan2(direction.Y, direction.X);
```
#### 方法2: 圆弧切线法(精确)
```csharp
// 计算圆弧切线方向
Vector3D radiusVec = (currentPoint - arcCenter).Normalize();
Vector3D rotationAxis = GetArcRotationAxis(trajectory);
Vector3D tangentVec = Vector3D.CrossProduct(rotationAxis, radiusVec);
double yaw = Math.Atan2(tangentVec.Y, tangentVec.X);
```
**推荐使用方法2**,因为:
1. 数学上精确,不依赖采样密度
2. 在圆弧段上方向连续平滑
3. 性能更好,无需查找相邻采样点
### 4.2 采样点密度控制
```toml
# default_config.toml
[path_editing]
# 圆弧采样步长(米) - 推荐值:0.02-0.1
arc_sampling_step = 0.05
```
- 步长越小:动画越平滑,但内存占用越大
- 步长越大:内存占用小,但可能不够平滑
- **推荐值**:0.05米(平衡精度和性能)
### 4.3 边界情况处理
1. **单边路径**:只有一条直线边
2. **全圆弧路径**:所有边都是圆弧
3. **混合路径**:直线段和圆弧段交替
4. **零长度边**:跳过或合并
5. **采样点不足**:自动补充或调整步长
### 4.4 性能优化
1. **采样点缓存**:PathEdge.SampledPoints只生成一次
2. **边长度缓存**:edgeLengths只计算一次
3. **二分查找优化**:FindEdgeForDistance可使用二分查找
4. **并行计算**:碰撞检测可并行化(考虑线程安全)
## 五、后续优化方向
1. **贝塞尔曲线支持**: 扩展PathCurveEngine支持三次贝塞尔曲线
2. **速度曲线**: 支持在路径上设置变速点
3. **碰撞精度自适应**: 根据曲率动态调整检测密度
4. **GPU加速**: 使用CUDA加速大量碰撞检测计算
---
**文档版本**: 1.0
**创建日期**: 2026-01-05
**作者**: iFlow CLI
**状态**: 待评审

429
src/Core/Animation/PathAnimationManager.cs
View File

@ -104,6 +104,9 @@ namespace NavisworksTransport.Core.Animation
private List<CollisionResult> _allCollisionResults; // 所有碰撞结果（不去重）
private bool _lastHighlightState = false; // 上一帧的高亮状态
// === 路径相关 ===
private PathRoute _route = null; // 路径引用
// === 碰撞测试优化 ===
private string _currentAnimationHash = ""; // 当前动画配置的哈希值
@ -144,6 +147,9 @@ namespace NavisworksTransport.Core.Animation
private TimeLinerIntegrationManager _timeLinerManager;
private string _currentTaskId;
// === 调试辅助 ===
private static HashSet<int> _arcInfoPrinted = new HashSet<int>(); // 记录已打印信息的圆弧段索引
/// <summary>
/// 当前正在进行动画的对象
/// </summary>
@ -218,6 +224,7 @@ namespace NavisworksTransport.Core.Animation
{
_animationFrames = null;
_allCollisionResults = null;
_arcInfoPrinted.Clear(); // 清除圆弧段信息打印记录
SetState(AnimationState.Idle);
LogManager.Info("[PathAnimationManager] 动画数据和状态已重置为 Idle");
}
@ -300,56 +307,27 @@ namespace NavisworksTransport.Core.Animation
/// <param name="animatedObject">要动画化的模型对象</param>
/// <param name="pathPoints">路径点列表</param>
/// <param name="durationSeconds">动画持续时间（秒）</param>
public void SetupAnimation(ModelItem animatedObject, List<Point3D> pathPoints, double durationSeconds = 10.0)
public void SetRoute(PathRoute route)
{
_route = route;
}
public void SetupAnimation(
ModelItem animatedObject,
double durationSeconds,
PathRoute route)
{
try
{
if (animatedObject == null)
throw new ArgumentNullException(nameof(animatedObject));
if (pathPoints == null || pathPoints.Count < 2)
throw new ArgumentException("路径点数量必须至少为2个", nameof(pathPoints));
// 添加路径点坐标有效性验证
LogManager.Info("=== 动画管理器坐标验证 ===");
bool hasInvalidCoordinates = false;
for (int i = 0; i < pathPoints.Count; i++)
{
var point = pathPoints[i];
bool isValid = !double.IsNaN(point.X) && !double.IsNaN(point.Y) && !double.IsNaN(point.Z) &&
!double.IsInfinity(point.X) && !double.IsInfinity(point.Y) && !double.IsInfinity(point.Z);
LogManager.Info($"路径点[{i}]坐标验证: ({point.X:F6},{point.Y:F6},{point.Z:F6}) - {(isValid ? "有效" : "无效")}");
if (!isValid)
{
hasInvalidCoordinates = true;
LogManager.Error($"检测到无效坐标: 路径点[{i}] = ({point.X},{point.Y},{point.Z})");
}
// 检查是否为零坐标（可能表示数据丢失）
if (isValid && point.X == 0.0 && point.Y == 0.0 && point.Z == 0.0)
{
LogManager.Warning($"路径点[{i}]坐标为零，可能存在数据丢失问题");
}
}
if (hasInvalidCoordinates)
{
throw new ArgumentException("路径点包含无效坐标（NaN或无穷大），无法创建动画");
}
LogManager.Info("=== 坐标验证完成 ===");
// 计算并保存当前动画配置的哈希值
_currentAnimationHash = ComputeAnimationConfigHash(animatedObject, pathPoints);
LogManager.Info($"动画配置哈希: {_currentAnimationHash}");
if (route == null)
throw new ArgumentNullException(nameof(route), "必须提供路径");
_animatedObject = animatedObject;
_pathPoints = new List<Point3D>(pathPoints);
_animationDuration = durationSeconds;
_route = route;
LogManager.Info($"使用路径,边数:{route.Edges.Count},总长度:{route.TotalLength:F2}米,动画时长:{durationSeconds:F1}秒");
// 预计算动画帧和碰撞
PrecomputeAnimationFrames();
// 验证预计算结果
@ -358,15 +336,7 @@ namespace NavisworksTransport.Core.Animation
throw new InvalidOperationException("动画帧预计算失败，无法创建动画");
}
// 添加详细的调试信息
LogManager.Info($"动画设置完成：对象={_animatedObject.DisplayName}, 路径点数={_pathPoints.Count}, 时长={_animationDuration}秒");
LogManager.Info($"路径起点: ({_pathPoints[0].X:F2},{_pathPoints[0].Y:F2},{_pathPoints[0].Z:F2})");
LogManager.Info($"路径终点: ({_pathPoints[_pathPoints.Count - 1].X:F2},{_pathPoints[_pathPoints.Count - 1].Y:F2},{_pathPoints[_pathPoints.Count - 1].Z:F2})");
var totalDistInModelUnits = CalculateTotalPathDistance();
var totalDistInMeters = UnitsConverter.ConvertToMeters(totalDistInModelUnits);
LogManager.Info($"路径总长度: {totalDistInMeters:F2}米");
LogManager.Info($"车辆已移动到路径起点，动画将从起点开始");
LogManager.Info($"动画设置完成：对象={_animatedObject.DisplayName}, 时长={_animationDuration}秒");
}
catch (Exception ex)
{
@ -405,24 +375,32 @@ namespace NavisworksTransport.Core.Animation
{
try
{
// 基于动画时长和帧率计算总帧数
LogManager.Info("=== 使用路径预计算动画帧 ===");
int totalFrames = (int)(_animationDuration * _animationFrameRate);
// 反向计算检测精度（确保每帧都检测）
var totalDistanceInModelUnits = CalculateTotalPathDistance();
_collisionDetectionAccuracy = totalDistanceInModelUnits / totalFrames;
// 1. 构建完整的采样点序列(按边顺序拼接)
var allSampledPoints = new List<Point3D>();
var edgeLengths = new List<double>();
var edgeStartIndices = new List<int>();
var totalDistanceInMeters = UnitsConverter.ConvertToMeters(totalDistanceInModelUnits);
var detectionAccuracyInMeters = UnitsConverter.ConvertToMeters(_collisionDetectionAccuracy);
foreach (var edge in _route.Edges)
{
if (edge.SampledPoints == null || edge.SampledPoints.Count == 0)
{
var samplingStep = ConfigManager.Instance.Current.PathEditing.ArcSamplingStep;
edge.SampledPoints = PathCurveEngine.GenerateSampledPoints(edge, samplingStep);
}
LogManager.Info($"=== 预计算动画帧 ===");
LogManager.Info($"动画时长: {_animationDuration}秒");
LogManager.Info($"动画帧率: {_animationFrameRate} FPS");
LogManager.Info($"总帧数: {totalFrames}");
LogManager.Info($"路径总长: {totalDistanceInMeters:F2}米");
LogManager.Info($"检测精度: {detectionAccuracyInMeters:F3}米/帧");
edgeStartIndices.Add(allSampledPoints.Count);
allSampledPoints.AddRange(edge.SampledPoints);
edgeLengths.Add(edge.PhysicalLength);
}
// 初始化帧列表
double totalLength = _route.TotalLength;
LogManager.Info($"路径总长度: {totalLength:F2}米, 采样点数: {allSampledPoints.Count}");
// 2. 按帧数采样生成动画帧
_animationFrames = new List<AnimationFrame>();
_allCollisionResults = new List<CollisionResult>();
_currentFrameIndex = 0;
@ -435,9 +413,9 @@ namespace NavisworksTransport.Core.Animation
originalBoundingBox.Max.Z - originalBoundingBox.Min.Z
);
// 空间索引设置
SpatialIndexManager spatialIndexManager = null;
double searchRadiusInModelUnits = 0;
var manualTargetsSnapshot = new List<ModelItem>();
bool manualOverrideActive = _manualCollisionOverrideEnabled && _manualCollisionTargets != null && _manualCollisionTargets.Count > 0;
@ -461,7 +439,6 @@ namespace NavisworksTransport.Core.Animation
if (!manualOverrideActive)
{
// 构建全局空间索引（单层架构）
LogManager.Info("=== 构建全局空间索引 ===");
spatialIndexManager = SpatialIndexManager.Instance;
@ -487,42 +464,61 @@ namespace NavisworksTransport.Core.Animation
boundingBoxSize.Y * boundingBoxSize.Y +
boundingBoxSize.Z * boundingBoxSize.Z
);
//空间查询半径，不能小于objectDiagonal，否则会漏掉周围的对象
searchRadiusInModelUnits = objectDiagonal + _detectionGap;
searchRadiusInModelUnits = objectDiagonal + _detectionGap;
LogManager.Info($"空间查询半径: {searchRadiusInModelUnits:F2} 模型单位");
}
// 第一遍：收集所有帧位置
var framePositions = new List<Point3D>();
// 3. 生成每一帧
double distancePerFrame = totalLength / totalFrames;
LogManager.Info($"总帧数: {totalFrames}, 总长度: {totalLength:F2}米, 每帧移动距离: {distancePerFrame:F4}米");
for (int i = 0; i < totalFrames; i++)
{
// 修正进度计算，确保最后一帧进度为 1.0
double progress = totalFrames > 1 ? (double)i / (totalFrames - 1) : 0.0;
var framePosition = InterpolatePosition(progress);
framePositions.Add(framePosition);
}
double targetDistance = i * distancePerFrame; // 按距离采样，不是按进度
// 第二遍：计算朝向并预计算每一帧
for (int i = 0; i < totalFrames; i++)
{
double progress = totalFrames > 1 ? (double)i / (totalFrames - 1) : 0.0;
var framePosition = framePositions[i];
// 找到当前在哪条边上
int edgeIndex = FindEdgeForDistance(targetDistance, edgeLengths);
if (edgeIndex < 0 || edgeIndex >= _route.Edges.Count)
{
LogManager.Warning($"无法找到边,目标距离:{targetDistance:F2}米");
continue;
}
// 计算朝向（基于前后帧）
double yawRadians = ComputeYawFromPath(i, framePositions);
var edge = _route.Edges[edgeIndex];
double accumulatedLength = edgeLengths.Take(edgeIndex).Sum();
double edgeProgress = (targetDistance - accumulatedLength) / edge.PhysicalLength;
// 创建帧数据
// 在边内插值位置
Point3D framePosition;
if (edge.SegmentType == PathSegmentType.Straight)
{
framePosition = InterpolateOnStraightEdge(edge, edgeProgress);
}
else // Arc
{
framePosition = InterpolateOnArcEdge(edge, edgeProgress);
}
double yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress);
// 添加朝向计算日志
if (edge.SegmentType == PathSegmentType.Arc)
{
LogManager.Info($"帧[{i}] 圆弧段: 边索引={edgeIndex}, 边进度={edgeProgress:F4}, 朝向={yawRadians * 180.0 / Math.PI:F2}度, 位置=({framePosition.X:F3},{framePosition.Y:F3},{framePosition.Z:F3})");
}
// 创建帧并检测碰撞
var frame = new AnimationFrame
{
Index = i,
Progress = progress,
Progress = (double)i / (totalFrames - 1),
Position = framePosition,
YawRadians = yawRadians,
Collisions = new List<CollisionResult>()
};
// 虚拟碰撞检测（不移动实际物体），考虑旋转
// 虚拟碰撞检测
var virtualBoundingBox = CreateVirtualBoundingBox(framePosition, boundingBoxSize, yawRadians);
IEnumerable<ModelItem> nearbyObjects;
@ -539,12 +535,10 @@ namespace NavisworksTransport.Core.Animation
);
}
// 检测与附近对象的碰撞
foreach (var collider in nearbyObjects)
{
var colliderBox = collider.BoundingBox();
// 使用包围盒检测
if (BoundingBoxGeometryUtils.BoundingBoxesIntersectWithTolerance(virtualBoundingBox, colliderBox, _detectionGap))
{
var collisionResult = new CollisionResult
@ -567,14 +561,11 @@ namespace NavisworksTransport.Core.Animation
};
frame.Collisions.Add(collisionResult);
// 添加到所有碰撞结果中（不去重，保留完整仿真数据）
_allCollisionResults.Add(collisionResult);
}
}
_animationFrames.Add(frame);
}
// 统计碰撞信息
@ -589,7 +580,6 @@ namespace NavisworksTransport.Core.Animation
catch (Exception ex)
{
LogManager.Error($"预计算动画帧失败: {ex.Message}");
// 如果预计算失败，创建空帧列表以避免崩溃
_animationFrames = new List<AnimationFrame>();
}
}
@ -597,46 +587,185 @@ namespace NavisworksTransport.Core.Animation
/// <summary>
/// 根据路径计算指定帧的yaw角度
/// </summary>
private double ComputeYawFromPath(int frameIndex, List<Point3D> framePositions)
/// <summary>
/// 计算路径上的朝向(改进版)
/// </summary>
private double ComputeYawOnPath(
int frameIndex,
List<Point3D> allSampledPoints,
int currentEdgeIndex,
double edgeProgress)
{
if (framePositions == null || framePositions.Count < 2)
return 0.0;
var edge = _route.Edges[currentEdgeIndex];
int totalFrames = framePositions.Count;
Point3D currentPos = framePositions[frameIndex];
Point3D nextPos;
if (edge.SegmentType == PathSegmentType.Arc && edge.Trajectory != null)
{
// 圆弧段: 使用切线法计算精确朝向
Vector3D startVec = (edge.Trajectory.Ts - edge.Trajectory.ArcCenter).Normalize();
double signedAngle = edge.Trajectory.DeflectionAngle;
double currentAngle = edgeProgress * signedAngle;
// 首帧或中间帧：看向下一帧
if (frameIndex < totalFrames - 1)
{
nextPos = framePositions[frameIndex + 1];
}
// 尾帧：保持上一帧的方向
else if (frameIndex > 0)
{
Point3D prevPos = framePositions[frameIndex - 1];
nextPos = new Point3D(
currentPos.X + (currentPos.X - prevPos.X),
currentPos.Y + (currentPos.Y - prevPos.Y),
currentPos.Z
// 计算切点Ts的朝向（进入切点的切线方向）
Vector3D tsRadiusVec = (edge.Trajectory.Ts - edge.Trajectory.ArcCenter).Normalize();
Vector3D rotationAxis = GetArcRotationAxis(edge.Trajectory);
Point3D tsTangent;
if (signedAngle >= 0)
{
tsTangent = GeometryHelper.CrossProduct(
new Point3D(rotationAxis.X, rotationAxis.Y, rotationAxis.Z),
new Point3D(tsRadiusVec.X, tsRadiusVec.Y, tsRadiusVec.Z)
);
}
else
{
tsTangent = GeometryHelper.CrossProduct(
new Point3D(tsRadiusVec.X, tsRadiusVec.Y, tsRadiusVec.Z),
new Point3D(rotationAxis.X, rotationAxis.Y, rotationAxis.Z)
);
}
double tsYaw = Math.Atan2(tsTangent.Y, tsTangent.X);
// 计算切点Te的朝向（退出切点的切线方向）
Vector3D teRadiusVec = (edge.Trajectory.Te - edge.Trajectory.ArcCenter).Normalize();
Point3D teTangent;
if (signedAngle >= 0)
{
teTangent = GeometryHelper.CrossProduct(
new Point3D(rotationAxis.X, rotationAxis.Y, rotationAxis.Z),
new Point3D(teRadiusVec.X, teRadiusVec.Y, teRadiusVec.Z)
);
}
else
{
teTangent = GeometryHelper.CrossProduct(
new Point3D(teRadiusVec.X, teRadiusVec.Y, teRadiusVec.Z),
new Point3D(rotationAxis.X, rotationAxis.Y, rotationAxis.Z)
);
}
double teYaw = Math.Atan2(teTangent.Y, teTangent.X);
// 计算当前帧的朝向
Point3D rotatedPoint = GeometryHelper.RotatePointAroundAxis(
edge.Trajectory.Ts,
edge.Trajectory.ArcCenter,
rotationAxis,
currentAngle
);
Vector3D radiusVec = (rotatedPoint - edge.Trajectory.ArcCenter).Normalize();
// 计算切线方向：根据旋转方向确定切线顺序
Point3D tangentPoint;
if (signedAngle >= 0)
{
tangentPoint = GeometryHelper.CrossProduct(
new Point3D(rotationAxis.X, rotationAxis.Y, rotationAxis.Z),
new Point3D(radiusVec.X, radiusVec.Y, radiusVec.Z)
);
}
else
{
tangentPoint = GeometryHelper.CrossProduct(
new Point3D(radiusVec.X, radiusVec.Y, radiusVec.Z),
new Point3D(rotationAxis.X, rotationAxis.Y, rotationAxis.Z)
);
}
double currentYaw = Math.Atan2(tangentPoint.Y, tangentPoint.X);
double yawChange = teYaw - tsYaw;
// 计算圆弧段前后直线段的朝向
double prevStraightYaw = 0;
double nextStraightYaw = 0;
// 圆弧段前的直线段朝向
if (currentEdgeIndex > 0 && _route.Edges[currentEdgeIndex - 1].SegmentType == PathSegmentType.Straight)
{
var prevEdge = _route.Edges[currentEdgeIndex - 1];
if (prevEdge.SampledPoints != null && prevEdge.SampledPoints.Count >= 2)
{
Point3D p1 = prevEdge.SampledPoints[0];
Point3D p2 = prevEdge.SampledPoints[prevEdge.SampledPoints.Count - 1];
prevStraightYaw = Math.Atan2(p2.Y - p1.Y, p2.X - p1.X);
}
}
// 圆弧段后的直线段朝向
if (currentEdgeIndex < _route.Edges.Count - 1 && _route.Edges[currentEdgeIndex + 1].SegmentType == PathSegmentType.Straight)
{
var nextEdge = _route.Edges[currentEdgeIndex + 1];
if (nextEdge.SampledPoints != null && nextEdge.SampledPoints.Count >= 2)
{
Point3D p1 = nextEdge.SampledPoints[0];
Point3D p2 = nextEdge.SampledPoints[nextEdge.SampledPoints.Count - 1];
nextStraightYaw = Math.Atan2(p2.Y - p1.Y, p2.X - p1.X);
}
}
// 在第一帧打印完整信息（使用静态变量确保每个圆弧段只打印一次）
if (!_arcInfoPrinted.Contains(currentEdgeIndex))
{
LogManager.Info($"圆弧段[{currentEdgeIndex}]朝向信息:");
LogManager.Info($" 前直线段朝向: {prevStraightYaw * 180.0 / Math.PI:F2}度");
LogManager.Info($" 进入切点(Ts)朝向: {tsYaw * 180.0 / Math.PI:F2}度");
LogManager.Info($" 圆弧偏转角度: {signedAngle * 180.0 / Math.PI:F2}度");
LogManager.Info($" 退出切点(Te)朝向: {teYaw * 180.0 / Math.PI:F2}度");
LogManager.Info($" 后直线段朝向: {nextStraightYaw * 180.0 / Math.PI:F2}度");
LogManager.Info($" 前后直线段朝向差: {(nextStraightYaw - prevStraightYaw) * 180.0 / Math.PI:F2}度");
LogManager.Info($" 圆弧段朝向变化: {yawChange * 180.0 / Math.PI:F2}度");
_arcInfoPrinted.Add(currentEdgeIndex);
}
// 每一帧都打印当前朝向
LogManager.Info($"帧[{frameIndex}] 圆弧段[{currentEdgeIndex}]: 进度={edgeProgress:F4}, 当前朝向={currentYaw * 180.0 / Math.PI:F2}度");
return currentYaw;
}
else
{
return 0.0; // 只有一帧
// 直线段: 使用直线方向向量，避免边界处的前后帧差分问题
Point3D startPos = edge.SampledPoints[0];
Point3D endPos = edge.SampledPoints[edge.SampledPoints.Count - 1];
double dx = endPos.X - startPos.X;
double dy = endPos.Y - startPos.Y;
double length = Math.Sqrt(dx * dx + dy * dy);
if (length < 1e-6)
{
// 直线段长度太短，尝试使用前后帧差分
Point3D currentPos = allSampledPoints.Count > 0 ? allSampledPoints[Math.Min(frameIndex, allSampledPoints.Count - 1)] : new Point3D();
int nextIndex = Math.Min(frameIndex + 1, allSampledPoints.Count - 1);
Point3D nextPos = allSampledPoints[nextIndex];
dx = nextPos.X - currentPos.X;
dy = nextPos.Y - currentPos.Y;
length = Math.Sqrt(dx * dx + dy * dy);
if (length < 1e-6)
{
return 0.0;
}
}
return Math.Atan2(dy, dx);
}
}
// 计算XY平面的方向向量
double dx = nextPos.X - currentPos.X;
double dy = nextPos.Y - currentPos.Y;
double length = Math.Sqrt(dx * dx + dy * dy);
/// <summary>
/// 获取圆弧旋转轴
/// </summary>
private Vector3D GetArcRotationAxis(ArcTrajectory trajectory)
{
Vector3D startVec = (trajectory.Ts - trajectory.ArcCenter).Normalize();
Vector3D endVec = (trajectory.Te - trajectory.ArcCenter).Normalize();
// 如果距离太小，保持当前yaw（或返回0）
if (length < 1e-6)
return (frameIndex > 0 && frameIndex < totalFrames) ? ComputeYawFromPath(frameIndex - 1, framePositions) : 0.0;
Point3D cross = GeometryHelper.CrossProduct(
new Point3D(startVec.X, startVec.Y, startVec.Z),
new Point3D(endVec.X, endVec.Y, endVec.Z)
);
// 计算yaw角度（atan2返回弧度）
return Math.Atan2(dy, dx);
return new Vector3D(cross.X, cross.Y, cross.Z).Normalize();
}
/// <summary>
@ -1328,6 +1457,53 @@ namespace NavisworksTransport.Core.Animation
);
}
/// <summary>
/// 在直线边上插值位置
/// </summary>
private Point3D InterpolateOnStraightEdge(PathEdge edge, double progress)
{
if (edge.SampledPoints == null || edge.SampledPoints.Count < 2)
return edge.Trajectory?.Ts ?? new Point3D();
int pointIndex = (int)(progress * (edge.SampledPoints.Count - 1));
pointIndex = Math.Max(0, Math.Min(pointIndex, edge.SampledPoints.Count - 1));
return edge.SampledPoints[pointIndex];
}
/// <summary>
/// 在圆弧边上插值位置
/// </summary>
private Point3D InterpolateOnArcEdge(PathEdge edge, double progress)
{
if (edge.Trajectory == null || edge.SampledPoints == null || edge.SampledPoints.Count < 2)
return edge.Trajectory?.Ts ?? new Point3D();
int pointIndex = (int)(progress * (edge.SampledPoints.Count - 1));
pointIndex = Math.Max(0, Math.Min(pointIndex, edge.SampledPoints.Count - 1));
return edge.SampledPoints[pointIndex];
}
/// <summary>
/// 查找指定距离对应的边索引
/// </summary>
private int FindEdgeForDistance(double targetDistance, List<double> edgeLengths)
{
double accumulatedLength = 0.0;
for (int i = 0; i < edgeLengths.Count; i++)
{
if (accumulatedLength + edgeLengths[i] >= targetDistance)
{
return i;
}
accumulatedLength += edgeLengths[i];
}
return edgeLengths.Count - 1;
}
/// <summary>
/// 计算路径总长度
/// </summary>
@ -2076,14 +2252,15 @@ namespace NavisworksTransport.Core.Animation
/// <param name="routeId">路由ID</param>
public void CreateAnimation(ModelItem animatedObject, List<Point3D> pathPoints, double durationSeconds = 10.0, string pathName = "未知路径", string routeId = null)
{
_pathName = pathName; // 存储路径名称
_currentRouteId = routeId; // 存储路由ID
_animatedObjectName = animatedObject?.DisplayName ?? "未知对象"; // 存储动画对象名称
_pathName = pathName;
_currentRouteId = routeId;
_animatedObjectName = animatedObject?.DisplayName ?? "未知对象";
// 添加调试日志
LogManager.Info($"[PathAnimationManager.CreateAnimation] 接收到参数: pathName='{pathName}', routeId='{routeId}', animatedObjectName='{_animatedObjectName}'");
LogManager.Info($"[PathAnimationManager.CreateAnimation] 存储的值: _pathName='{_pathName}', _currentRouteId='{_currentRouteId}'");
SetupAnimation(animatedObject, pathPoints, durationSeconds);
// 注意：pathPoints参数已废弃，现在使用PathRoute
SetupAnimation(animatedObject, durationSeconds, _route);
SetState(AnimationState.Ready);
}

110
src/Core/PathCurveEngine.cs
View File

@ -28,22 +28,39 @@ namespace NavisworksTransport
/// <param name="pCurr">当前控制点</param>
/// <param name="pNext">下一个控制点</param>
/// <param name="turnRadius">转向半径（米）</param>
/// <param name="warningMessage">警告信息（如果有）</param>
/// <returns>圆弧轨迹数据</returns>
public static ArcTrajectory CalculateFillet(
Point3D pPrev,
Point3D pCurr,
Point3D pNext,
double turnRadius)
double turnRadius,
out string warningMessage)
{
// 初始化警告信息
warningMessage = null;
// 1. 计算单位向量
Vector3D v1 = (pPrev - pCurr).Normalize();
Vector3D v2 = (pNext - pCurr).Normalize();
// 2. 计算夹角 α
// 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));
// 计算叉乘以确定转向方向
Point3D cross = GeometryHelper.CrossProduct(
new Point3D(v1.X, v1.Y, v1.Z),
new Point3D(v2.X, v2.Y, v2.Z));
double crossMag = new Vector3D(cross.X, cross.Y, cross.Z).Length;
// 使用Acos计算两个向量之间的夹角（范围 [0, π]）
// 限制cosAlpha在[-1, 1]范围内以避免数值误差
double clampedCosAlpha = Math.Max(-1.0, Math.Min(1.0, cosAlpha));
double angleRad = Math.Acos(clampedCosAlpha);
LogManager.Info($"[CalculateFillet] 角度计算: cosAlpha={cosAlpha:F4}, clampedCosAlpha={clampedCosAlpha:F4}, angleRad={angleRad:F4}rad ({angleRad * 180.0 / Math.PI:F2}度)");
// 如果几乎共线，返回无效轨迹
if (angleRad < 0.01 || angleRad > Math.PI - 0.01)
@ -69,23 +86,57 @@ namespace NavisworksTransport
double maxAllowedLt = Math.Min(seg1Length, seg2Length) * 0.45;
double actualRadius = turnRadius;
bool radiusTruncated = false;
if (Lt > maxAllowedLt)
{
// 记录截断前的参数
double originalLt = Lt;
double originalRadius = turnRadius;
Lt = maxAllowedLt;
actualRadius = Lt * Math.Tan(angleRad / 2.0);
radiusTruncated = true;
// 发出警告（日志）
LogManager.Warning($"圆弧半径被安全截断: 请求半径={originalRadius:F2}米, 实际半径={actualRadius:F2}米");
// 设置用户友好的警告信息
double radiusDiff = originalRadius - actualRadius;
double radiusDiffPercent = (radiusDiff / originalRadius) * 100.0;
warningMessage = $"警告：转弯半径被自动缩小了 {radiusDiffPercent:F0}%（从 {originalRadius:F1}米 减小到 {actualRadius:F1}米），因为路径段太短。建议：增加路径点之间的距离，或者减小转弯半径设置。";
}
// 5. 计算切点
Point3D ts = pCurr + v1 * Lt;
Point3D te = pCurr + v2 * Lt;
// 6. 计算圆心 (使用角平分线方向)
// 6. 计算圆心 (使用角平分线方向)
Vector3D bisector = (v1 + v2).Normalize();
double distToCenter = actualRadius / Math.Sin(angleRad / 2.0);
Point3D arcCenter = pCurr + bisector * distToCenter;
// 7. 计算圆弧长度
double arcLength = actualRadius * angleRad;
// 7. 计算圆弧长度和实际偏转角度
// 始终使用Ts和Te到圆心的向量之间的实际夹角，确保角度正确
Vector3D tsToCenter = (ts - arcCenter).Normalize();
Vector3D teToCenter = (te - arcCenter).Normalize();
double dotTsTe = GeometryHelper.DotProduct(
new Point3D(tsToCenter.X, tsToCenter.Y, tsToCenter.Z),
new Point3D(teToCenter.X, teToCenter.Y, teToCenter.Z));
// 限制在[-1, 1]范围内
dotTsTe = Math.Max(-1.0, Math.Min(1.0, dotTsTe));
double actualDeflectionAngle = Math.Acos(dotTsTe);
LogManager.Info($"[CalculateFillet] 计算实际偏转角度:");
LogManager.Info($" Ts=({ts.X:F3},{ts.Y:F3},{ts.Z:F3}), Te=({te.X:F3},{te.Y:F3},{te.Z:F3})");
LogManager.Info($" ArcCenter=({arcCenter.X:F3},{arcCenter.Y:F3},{arcCenter.Z:F3})");
LogManager.Info($" Ts到圆心向量: ({tsToCenter.X:F3},{tsToCenter.Y:F3},{tsToCenter.Z:F3})");
LogManager.Info($" Te到圆心向量: ({teToCenter.X:F3},{teToCenter.Y:F3},{teToCenter.Z:F3})");
LogManager.Info($" 点积: {dotTsTe:F4}");
LogManager.Info($" 实际偏转角度: {actualDeflectionAngle * 180.0 / Math.PI:F2}度");
LogManager.Info($" 半径截断: {radiusTruncated}, 请求半径: {turnRadius:F2}米, 实际半径: {actualRadius:F2}米");
double arcLength = actualRadius * actualDeflectionAngle;
return new ArcTrajectory
{
@ -94,7 +145,7 @@ namespace NavisworksTransport
ArcCenter = arcCenter,
RequestedRadius = turnRadius,
ActualRadius = actualRadius,
DeflectionAngle = angleRad,
DeflectionAngle = actualDeflectionAngle,
ArcLength = arcLength
};
}
@ -107,29 +158,7 @@ namespace NavisworksTransport
/// <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;
}
// RotatePointAroundAxis方法已移到GeometryHelper工具类中
/// <summary>
/// 圆弧采样为离散点序列
@ -179,7 +208,11 @@ namespace NavisworksTransport
// 使用 Atan2 计算有向角度，确保旋转方向正确
double signedAngle = Math.Atan2(crossMag, dot);
LogManager.Info($" 有向角度: {signedAngle:F4}rad ({signedAngle * 180.0 / Math.PI:F2}度)");
LogManager.Info($" 重新计算的有向角度: {signedAngle:F4}rad ({signedAngle * 180.0 / Math.PI:F2}度)");
LogManager.Info($" 使用DeflectionAngle: {trajectory.DeflectionAngle:F4}rad ({trajectory.DeflectionAngle * 180.0 / Math.PI:F2}度)");
// 使用trajectory.DeflectionAngle而不是重新计算的角度
signedAngle = trajectory.DeflectionAngle;
// 等角度插值
for (int i = 0; i <= sampleCount; i++)
@ -188,7 +221,7 @@ namespace NavisworksTransport
double theta = t * signedAngle;
// Rodrigues旋转公式
Point3D sampledPoint = RotatePointAroundAxis(
Point3D sampledPoint = GeometryHelper.RotatePointAroundAxis(
trajectory.Ts,
trajectory.ArcCenter,
rotationAxis,
@ -379,11 +412,13 @@ namespace NavisworksTransport
/// </summary>
/// <param name="route">待处理的路径</param>
/// <param name="samplingStep">采样步长（米）</param>
public static void ApplyCurvatureToRoute(PathRoute route, double samplingStep)
/// <param name="warningMessages">警告信息列表（如果有）</param>
public static void ApplyCurvatureToRoute(PathRoute route, double samplingStep, out List<string> warningMessages)
{
if (route == null)
throw new ArgumentNullException(nameof(route));
warningMessages = new List<string>();
route.Edges.Clear();
var sortedPoints = route.Points.OrderBy(p => p.Index).ToList();
@ -416,8 +451,15 @@ namespace NavisworksTransport
double radius = currPoint.CustomTurnRadius ?? route.TurnRadius;
// 计算圆弧轨迹
var arcTraj = CalculateFillet(prevPoint.Position, currPoint.Position, nextPoint.Position, radius);
string warning;
var arcTraj = CalculateFillet(prevPoint.Position, currPoint.Position, nextPoint.Position, radius, out warning);
arcTrajectories[i] = arcTraj;
// 收集警告信息
if (!string.IsNullOrEmpty(warning))
{
warningMessages.Add(warning);
}
}
// 第二步：构建边集合

18
src/Core/PathPlanningManager.cs
View File

@ -2911,9 +2911,25 @@ namespace NavisworksTransport
{
double samplingStep = ConfigManager.Instance.Current.PathEditing.ArcSamplingStep;
route.TurnRadius = ConfigManager.Instance.Current.PathEditing.DefaultPathTurnRadius;
PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);
List<string> warnings;
PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep, out warnings);
LogManager.Info($"路径曲线化完成（{context}）: {route.Name}, 转弯半径: {route.TurnRadius:F2}m, 边数: {route.Edges.Count}");
// 如果有警告，显示到状态栏
if (warnings != null && warnings.Count > 0)
{
// 只显示第一个警告（避免刷屏）
string firstWarning = warnings[0];
if (warnings.Count > 1)
{
firstWarning += $"（还有 {warnings.Count - 1} 个类似问题）";
}
// 通过StatusChanged事件发送警告
RaiseStatusChanged(firstWarning, PathPlanningStatusType.Warning);
LogManager.Warning($"路径曲线化警告: {firstWarning}");
}
// 曲线化计算完成后，立即保存数据库
SavePathToDatabase(route);
LogManager.Info($"路径曲线化后已保存到数据库: {route.Name}");

14
src/UI/WPF/ViewModels/AnimationControlViewModel.cs
View File

@ -1972,6 +1972,20 @@ namespace NavisworksTransport.UI.WPF.ViewModels
LogManager.Info($"[ExecuteGenerateAnimation] 准备创建动画: 路径名称='{CurrentPathRoute.Name}', ID='{CurrentPathRoute.Id}', " +
$"动画对象='{animatedObject.DisplayName}'");
// 从PathPlanningManager获取PathRoute对象
var pathPlanningManager = PathPlanningManager.Instance;
var pathRoute = pathPlanningManager.GetAllRoutes().FirstOrDefault(r => r.Id == CurrentPathRoute.Id);
if (pathRoute == null)
{
LogManager.Error($"无法找到路径对象，ID: {CurrentPathRoute.Id}");
UpdateMainStatus("生成失败：找不到路径数据");
return;
}
// 先设置路径到动画管理器
_pathAnimationManager.SetRoute(pathRoute);
_pathAnimationManager.CreateAnimation(animatedObject, pathPoints, AnimationDuration, CurrentPathRoute.Name, CurrentPathRoute.Id);
var totalElapsed = (DateTime.Now - cacheStartTime).TotalMilliseconds;

32
src/Utils/GeometryHelper.cs
View File

@ -983,6 +983,38 @@ namespace NavisworksTransport
);
}
/// <summary>
/// Rodrigues旋转公式 - 绕任意轴旋转点
/// </summary>
/// <param name="point">要旋转的点</param>
/// <param name="center">旋转中心</param>
/// <param name="axis">旋转轴（单位向量）</param>
/// <param name="angle">旋转角度（弧度）</param>
/// <returns>旋转后的点</returns>
public 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 = CrossProduct(
new Point3D(axis.X, axis.Y, axis.Z),
new Point3D(v.X, v.Y, v.Z));
double kdv = 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>