修复斜线路径优化有局部锯齿的情况
This commit is contained in:
parent
4e43fb89b3
commit
f131d0f8b7
@ -2,6 +2,21 @@
|
||||
|
||||
基于真实官方示例的正确API用法总结
|
||||
|
||||
在线资源链接
|
||||
|
||||
1. Autodesk Platform Services (APS)
|
||||
- 主要开发者门户：<https://aps.autodesk.com/developer/overview/navisworks>
|
||||
- 提供Navisworks集成工具和SDK
|
||||
2. AEC DevBlog
|
||||
- 官方开发博客：<https://adndevblog.typepad.com/aec/navisworks/>
|
||||
- 包含2026版本新功能和技术文章
|
||||
3. Autodesk Developer Network
|
||||
- 开发者网络：<https://www.autodesk.com/developer-network/app-store/navisworks>
|
||||
- 提供开发资源和支持
|
||||
4. 非官方在线API文档
|
||||
- ApiDocs.co：<https://apidocs.co/apps/navisworks/>
|
||||
- （注：目前仅覆盖2017-2018版本）
|
||||
|
||||
## 参考示例来源
|
||||
|
||||
基于以下官方示例文件的真实API用法：
|
||||
|
||||
@ -751,10 +751,26 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 🔥 更激进的斜线检测：从P+2开始尝试，直到找到最远可连接点
|
||||
for (int testIndex = currentIndex + 2; testIndex < optimizedPath.Count; testIndex++)
|
||||
{
|
||||
if (IsDirectPathClear(optimizedPath[currentIndex], optimizedPath[testIndex], gridMap))
|
||||
var startPoint = optimizedPath[currentIndex];
|
||||
var endPoint = optimizedPath[testIndex];
|
||||
var distance = Math.Sqrt(Math.Pow(endPoint.X - startPoint.X, 2) +
|
||||
Math.Pow(endPoint.Y - startPoint.Y, 2));
|
||||
|
||||
if (IsDirectPathClear(startPoint, endPoint, gridMap))
|
||||
{
|
||||
// 可以直线连接，更新最远索引
|
||||
farthestIndex = testIndex;
|
||||
LogManager.Debug($"[斜线优化] ✓ 成功连接：点{currentIndex}→点{testIndex}，距离={distance:F2}m");
|
||||
}
|
||||
else
|
||||
{
|
||||
// 新增：记录失败详情
|
||||
var startGrid = gridMap.WorldToGrid(startPoint);
|
||||
var endGrid = gridMap.WorldToGrid(endPoint);
|
||||
LogManager.Debug($"[斜线优化] ✗ 连接失败：" +
|
||||
$"点{currentIndex}[网格({startGrid.X},{startGrid.Y})]→点{testIndex}[网格({endGrid.X},{endGrid.Y})]，" +
|
||||
$"世界坐标：({startPoint.X:F2},{startPoint.Y:F2})→({endPoint.X:F2},{endPoint.Y:F2})，" +
|
||||
$"距离={distance:F2}m，跨越{Math.Abs(endGrid.X-startGrid.X)+Math.Abs(endGrid.Y-startGrid.Y)}个网格");
|
||||
}
|
||||
// 🔧 关键改进：不像现有算法那样遇到失败就停止，而是继续尝试更远的点
|
||||
// 这样可以发现更多斜线连接机会
|
||||
@ -816,6 +832,9 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 最少采样5个点，最多10000个点（提高性能限制）
|
||||
samples = Math.Max(5, Math.Min(samples, 10000));
|
||||
|
||||
LogManager.Debug($"[斜线检查] 开始检查路径：起点({start.X:F2},{start.Y:F2})→终点({end.X:F2},{end.Y:F2})，" +
|
||||
$"距离={distance:F2}m，采样数={samples}");
|
||||
|
||||
for (int i = 0; i <= samples; i++)
|
||||
{
|
||||
// 线性插值计算采样点
|
||||
@ -830,12 +849,46 @@ namespace NavisworksTransport.PathPlanning
|
||||
var gridPos = gridMap.WorldToGrid(samplePoint);
|
||||
if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsWalkable(gridPos))
|
||||
{
|
||||
// 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
|
||||
var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize;
|
||||
var gridMinY = gridMap.Origin.Y + gridPos.Y * gridMap.CellSize;
|
||||
|
||||
// 计算网格中心点世界坐标（基于左下角）
|
||||
var gridCenterX = gridMinX + 0.5 * gridMap.CellSize;
|
||||
var gridCenterY = gridMinY + 0.5 * gridMap.CellSize;
|
||||
var gridCenterZ = samplePoint.Z; // Z保持不变
|
||||
|
||||
// 计算偏差
|
||||
var deltaX = samplePoint.X - gridCenterX;
|
||||
var deltaY = samplePoint.Y - gridCenterY;
|
||||
|
||||
LogManager.Debug($"[斜线检查] 采样点{i}/{samples}失败：" +
|
||||
$"采样点({samplePoint.X:F3},{samplePoint.Y:F3},{samplePoint.Z:F3})，" +
|
||||
$"网格({gridPos.X},{gridPos.Y})左下角({gridMinX:F3},{gridMinY:F3})，" +
|
||||
$"网格中心({gridCenterX:F3},{gridCenterY:F3},{gridCenterZ:F3})，" +
|
||||
$"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})，" +
|
||||
$"原因：{(!gridMap.IsValidGridPosition(gridPos) ? "网格无效" : "不可通行")}");
|
||||
return false;
|
||||
}
|
||||
|
||||
// 🔥 新增：邻居障碍位置检查
|
||||
if (!IsSamplePointSafeFromNeighborObstacles(samplePoint, gridPos, gridMap))
|
||||
{
|
||||
// 🔥 修复：使用Origin计算网格左下角
|
||||
var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize;
|
||||
var gridMinY = gridMap.Origin.Y + gridPos.Y * gridMap.CellSize;
|
||||
|
||||
// 计算网格中心点世界坐标
|
||||
var gridCenterX = gridMinX + 0.5 * gridMap.CellSize;
|
||||
var gridCenterY = gridMinY + 0.5 * gridMap.CellSize;
|
||||
var deltaX = samplePoint.X - gridCenterX;
|
||||
var deltaY = samplePoint.Y - gridCenterY;
|
||||
|
||||
LogManager.Debug($"[斜线检查] 采样点{i}/{samples}邻居障碍检查失败：" +
|
||||
$"采样点({samplePoint.X:F3},{samplePoint.Y:F3})，" +
|
||||
$"网格左下角({gridMinX:F3},{gridMinY:F3})，" +
|
||||
$"网格中心({gridCenterX:F3},{gridCenterY:F3})，" +
|
||||
$"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@ -860,9 +913,10 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
try
|
||||
{
|
||||
// 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
|
||||
// 计算采样点在所在网格内的相对位置 (0.0 到 1.0)
|
||||
var gridMinX = gridMap.Bounds.Min.X + gridPos.X * gridMap.CellSize;
|
||||
var gridMinY = gridMap.Bounds.Min.Y + gridPos.Y * gridMap.CellSize;
|
||||
var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize;
|
||||
var gridMinY = gridMap.Origin.Y + gridPos.Y * gridMap.CellSize;
|
||||
|
||||
var relativeX = (samplePoint.X - gridMinX) / gridMap.CellSize;
|
||||
var relativeY = (samplePoint.Y - gridMinY) / gridMap.CellSize;
|
||||
@ -898,6 +952,20 @@ namespace NavisworksTransport.PathPlanning
|
||||
|
||||
if (!constraintSatisfied)
|
||||
{
|
||||
// 🔥 修复：基于正确的网格左下角计算网格中心点
|
||||
var gridCenterX = gridMinX + 0.5 * gridMap.CellSize;
|
||||
var gridCenterY = gridMinY + 0.5 * gridMap.CellSize;
|
||||
var deltaX = samplePoint.X - gridCenterX;
|
||||
var deltaY = samplePoint.Y - gridCenterY;
|
||||
|
||||
LogManager.Debug($"[邻居障碍] 位置约束失败：" +
|
||||
$"{directionNames[i]}方向有障碍，" +
|
||||
$"网格({gridPos.X},{gridPos.Y})，" +
|
||||
$"采样点({samplePoint.X:F3},{samplePoint.Y:F3})，" +
|
||||
$"网格左下角({gridMinX:F3},{gridMinY:F3})，" +
|
||||
$"网格中心({gridCenterX:F3},{gridCenterY:F3})，" +
|
||||
$"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})，" +
|
||||
$"相对位置({relativeX:F3},{relativeY:F3})");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@ -921,43 +989,50 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// <returns>是否满足位置约束</returns>
|
||||
private bool CheckPositionConstraintForObstacleNeighbor(int neighborDirection, double relativeX, double relativeY)
|
||||
{
|
||||
// 🔧 定义容差值来处理浮点数精度问题
|
||||
// 容差值仅用于处理浮点数精度问题
|
||||
const double tolerance = 1e-3;
|
||||
|
||||
bool result;
|
||||
|
||||
switch (neighborDirection)
|
||||
{
|
||||
case 0: // 左上邻居是障碍 → 排除左上象限，允许其他3个象限
|
||||
result = (relativeX >= 0.5 - tolerance) || (relativeY >= 0.5 - tolerance);
|
||||
case 0: // 左上邻居是障碍
|
||||
// 采样点不能同时X<0且Y>1
|
||||
result = (relativeX >= -tolerance) || (relativeY <= 1.0 + tolerance);
|
||||
break;
|
||||
|
||||
case 1: // 上邻居是障碍 → 采样点必须在下半部分
|
||||
result = relativeY >= 0.5 - tolerance;
|
||||
case 1: // 上邻居是障碍
|
||||
// 采样点不能Y>1（允许Y=1）
|
||||
result = relativeY <= 1.0 + tolerance;
|
||||
break;
|
||||
|
||||
case 2: // 右上邻居是障碍 → 排除右上象限，允许其他3个象限
|
||||
result = (relativeX <= 0.5 + tolerance) || (relativeY >= 0.5 - tolerance);
|
||||
case 2: // 右上邻居是障碍
|
||||
// 采样点不能同时X>1且Y>1
|
||||
result = (relativeX <= 1.0 + tolerance) || (relativeY <= 1.0 + tolerance);
|
||||
break;
|
||||
|
||||
case 3: // 左邻居是障碍 → 采样点必须在右半部分
|
||||
result = relativeX >= 0.5 - tolerance;
|
||||
case 3: // 左邻居是障碍
|
||||
// 采样点不能X<0（允许X=0）
|
||||
result = relativeX >= -tolerance;
|
||||
break;
|
||||
|
||||
case 4: // 右邻居是障碍 → 采样点必须在左半部分
|
||||
result = relativeX <= 0.5 + tolerance;
|
||||
case 4: // 右邻居是障碍
|
||||
// 采样点不能X>1（允许X=1）
|
||||
result = relativeX <= 1.0 + tolerance;
|
||||
break;
|
||||
|
||||
case 5: // 左下邻居是障碍 → 排除左下象限，允许其他3个象限
|
||||
result = (relativeX >= 0.5 - tolerance) || (relativeY <= 0.5 + tolerance);
|
||||
case 5: // 左下邻居是障碍
|
||||
// 采样点不能同时X<0且Y<0
|
||||
result = (relativeX >= -tolerance) || (relativeY >= -tolerance);
|
||||
break;
|
||||
|
||||
case 6: // 下邻居是障碍 → 采样点必须在上半部分
|
||||
result = relativeY <= 0.5 + tolerance;
|
||||
case 6: // 下邻居是障碍
|
||||
// 采样点不能Y<0（允许Y=0）
|
||||
result = relativeY >= -tolerance;
|
||||
break;
|
||||
|
||||
case 7: // 右下邻居是障碍 → 排除右下象限，允许其他3个象限
|
||||
result = (relativeX <= 0.5 + tolerance) || (relativeY <= 0.5 + tolerance);
|
||||
case 7: // 右下邻居是障碍
|
||||
// 采样点不能同时X>1且Y<0
|
||||
result = (relativeX <= 1.0 + tolerance) || (relativeY >= -tolerance);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user