tian
f75dcb1f50
- New method mirrors proven ApplyRotationCorrectionInPlace pattern - ViewModel only calls existing project APIs, no raw OverridePermanentTransform
6717 lines
291 KiB
C#
6717 lines
291 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Numerics;
|
||
using System.Runtime.InteropServices;
|
||
using System.Text;
|
||
using System.Windows;
|
||
using System.Windows.Threading;
|
||
using Autodesk.Navisworks.Api;
|
||
using Autodesk.Navisworks.Api.Clash;
|
||
using ComApiBridge = Autodesk.Navisworks.Api.ComApi.ComApiBridge;
|
||
using NavisworksTransport.Core.Config;
|
||
using NavisworksTransport.Core.Spatial;
|
||
using NavisworksTransport.Utils;
|
||
using NavisworksTransport.Utils.CoordinateSystem;
|
||
using NavisApplication = Autodesk.Navisworks.Api.Application;
|
||
|
||
namespace NavisworksTransport.Core.Animation
|
||
{
|
||
/// <summary>
|
||
/// 定义动画播放的状态
|
||
/// </summary>
|
||
public enum AnimationState
|
||
{
|
||
Idle, // 空闲，未生成动画
|
||
Ready, // 已就绪，动画已生成但未播放
|
||
Playing, // 播放中
|
||
Paused, // 暂停
|
||
Stopped, // 已停止
|
||
Finished // 已完成
|
||
}
|
||
|
||
/// <summary>
|
||
/// 碰撞对象对，用于去重
|
||
/// </summary>
|
||
public class CollisionPair : IEquatable<CollisionPair>
|
||
{
|
||
public ModelItem AnimatedObject { get; set; }
|
||
public ModelItem CollisionObject { get; set; }
|
||
|
||
public bool Equals(CollisionPair other)
|
||
{
|
||
if (other == null) return false;
|
||
return ReferenceEquals(AnimatedObject, other.AnimatedObject) &&
|
||
ReferenceEquals(CollisionObject, other.CollisionObject);
|
||
}
|
||
|
||
public override bool Equals(object obj)
|
||
{
|
||
return Equals(obj as CollisionPair);
|
||
}
|
||
|
||
public override int GetHashCode()
|
||
{
|
||
unchecked
|
||
{
|
||
int hash = 17;
|
||
hash = hash * 31 + (AnimatedObject?.GetHashCode() ?? 0);
|
||
hash = hash * 31 + (CollisionObject?.GetHashCode() ?? 0);
|
||
return hash;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 路径动画管理器 - 基于TimeLiner和动态变换实现沿路径的动画效果
|
||
/// 注意：由于Navisworks API限制，无法直接使用Animator API，因此使用OverridePermanentTransform实现动画
|
||
/// 已集成 TimeLiner 功能，支持在 TimeLiner 中显示和管理动画任务
|
||
/// </summary>
|
||
/// <summary>
|
||
/// 动画帧数据（包含位置、朝向和碰撞信息）
|
||
/// </summary>
|
||
public class AnimationFrame
|
||
{
|
||
public int Index { get; set; } // 帧索引
|
||
public double Progress { get; set; } // 进度(0-1)
|
||
public Point3D Position { get; set; } // 该帧的位置
|
||
public double YawRadians { get; set; } // 绕Z轴的偏航角（弧度）
|
||
public double? PlanarHostYawRadians { get; set; } // 平面真实物体逐帧宿主平面角
|
||
public Rotation3D Rotation { get; set; } // 可选的完整三维姿态
|
||
public bool HasCustomRotation { get; set; } // 是否使用完整三维姿态
|
||
public List<CollisionResult> Collisions { get; set; } // 该帧的碰撞结果
|
||
public bool HasCollision => Collisions?.Count > 0;
|
||
|
||
public AnimationFrame()
|
||
{
|
||
Collisions = new List<CollisionResult>();
|
||
YawRadians = 0.0;
|
||
PlanarHostYawRadians = null;
|
||
Rotation = Rotation3D.Identity;
|
||
HasCustomRotation = false;
|
||
}
|
||
}
|
||
|
||
internal enum AnimatedObjectMode
|
||
{
|
||
None,
|
||
RealObject,
|
||
VirtualObject
|
||
}
|
||
|
||
public class PathAnimationManager
|
||
{
|
||
private static PathAnimationManager _instance;
|
||
private static readonly Dictionary<string, int> _animationHashToRecordId = new Dictionary<string, int>(); // 动画配置哈希到检测记录ID的映射
|
||
private static readonly HashSet<string> _realObjectFragmentUpMismatchHintsShown = new HashSet<string>(StringComparer.OrdinalIgnoreCase);
|
||
private ModelItem _animatedObject;
|
||
private AnimatedObjectMode _animatedObjectMode = AnimatedObjectMode.None;
|
||
private double _virtualObjectLength = 0; // 虚拟物体长度（模型单位）
|
||
private double _virtualObjectWidth = 0; // 虚拟物体宽度（模型单位）
|
||
private double _virtualObjectHeight = 0; // 虚拟物体高度（模型单位）
|
||
private double _realObjectLength = 0; // 真实物体长度（模型单位，固定物理尺寸）
|
||
private double _realObjectWidth = 0; // 真实物体宽度（模型单位，固定物理尺寸）
|
||
private double _realObjectHeight = 0; // 真实物体高度（模型单位，固定物理尺寸）
|
||
private Quaternion _realObjectReferenceRotation = Quaternion.Identity;
|
||
private Vector3 _realObjectReferenceAxisX = Vector3.UnitX;
|
||
private Vector3 _realObjectReferenceAxisY = Vector3.UnitY;
|
||
private Vector3 _realObjectReferenceAxisZ = Vector3.UnitZ;
|
||
private LocalAxisDirection _realObjectReferenceHostWorldXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||
private LocalAxisDirection _realObjectReferenceHostWorldYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||
private LocalAxisDirection _realObjectReferenceHostWorldZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||
private LocalAxisDirection _realObjectReferenceHostSemanticXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||
private LocalAxisDirection _realObjectReferenceHostSemanticYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||
private LocalAxisDirection _realObjectReferenceHostSemanticZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||
private LocalAxisDirection _realObjectReferenceHostUpLocalAxis = LocalAxisDirection.PositiveY;
|
||
private bool _hasRealObjectReferenceRotation = false;
|
||
private LocalAxisDirection _realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX;
|
||
private bool _hasRealObjectPlanarSelectedForwardAxis = false;
|
||
private List<Point3D> _pathPoints;
|
||
private List<ModelItem> _manualCollisionTargets = new List<ModelItem>();
|
||
private bool _manualCollisionOverrideEnabled = false;
|
||
|
||
// === 角度修正 ===
|
||
private LocalEulerRotationCorrection _objectRotationCorrection = LocalEulerRotationCorrection.Zero; // 物体绕宿主 X/Y/Z 轴的角度修正
|
||
private double _groundPathObjectLiftHeight; // 地面路径物体上下偏移（模型单位，负值向下）
|
||
|
||
// === 碰撞排除列表缓存管理（从LogisticsAnimationManager迁移）===
|
||
private ModelItem _currentCachedAnimationObject;
|
||
private List<ModelItem> _cachedExclusionList;
|
||
private DateTime _cacheCreatedTime;
|
||
private readonly TimeSpan _cacheValidDuration = TimeSpan.FromMinutes(30); // 缓存有效期30分钟
|
||
|
||
// === 预计算动画系统 ===
|
||
private List<AnimationFrame> _animationFrames; // 所有帧数据
|
||
private int _currentFrameIndex = 0; // 当前帧索引
|
||
|
||
private List<CollisionResult> _allCollisionResults; // 所有碰撞结果（不去重）
|
||
|
||
// === Human-in-the-Loop 排除列表 ===
|
||
// 注意：排除列表是实时管理的，不与动画缓存绑定。用户选择排除哪些物体后，
|
||
// 这些物体会一直生效直到被清除或手动移除，不受动画缓存影响。
|
||
private HashSet<ModelItem> _excludedObjects = new HashSet<ModelItem>(); // 用户排除的物体列表
|
||
private bool _lastHighlightState = false; // 上一帧的高亮状态
|
||
private HashSet<ModelItem> _lastCollisionObjects = new HashSet<ModelItem>(); // 上一帧碰撞对象的集合
|
||
|
||
// === 路径相关 ===
|
||
private PathRoute _route = null; // 路径引用
|
||
|
||
// === 碰撞测试优化 ===
|
||
private string _currentAnimationHash; // 当前动画配置的哈希值
|
||
private int? _currentDetectionRecordId; // 当前检测记录ID（关联CollisionDetectionRecords表）
|
||
private bool _isUsingHistoryRecord; // 是否使用历史检测记录（动画完成后跳过ClashDetective）
|
||
|
||
// === 动画播放机制 ===
|
||
private double _frameInterval; // 帧间隔（毫秒）
|
||
private DateTime _lastFrameTime = DateTime.MinValue; // 上一帧时间
|
||
private DispatcherTimer _animationTimer; // 备用DispatcherTimer定时器
|
||
|
||
// === 动画参数 ===
|
||
private double _animationDuration; // 动画总时长（秒）
|
||
private DateTime _animationStartTime;
|
||
private int _animationFrameRate; // 动画帧率（默认30FPS）
|
||
private double _collisionDetectionAccuracy; // 检测精度（内部存储：模型单位/帧）
|
||
private double _movementSpeed; // 运动速度（仅用于显示，单位：米/秒）
|
||
private double _detectionTolerance; // 检测容差（米，从UI设置）
|
||
private double _safetyMargin; // 安全间隙（模型单位，从CreateAnimation传入）
|
||
private string _pathName; // 路径名称
|
||
private string _currentRouteId; // 当前路由ID
|
||
|
||
// === 双向播放和步进控制 ===
|
||
private int _playbackDirection = 1; // 播放方向：1=正向，-1=反向
|
||
private double _playbackSpeed = 1.0; // 播放速度倍率
|
||
|
||
// === 性能监控 ===
|
||
private int _fpsFrameCount = 0;
|
||
private DateTime _fpsCounterStart = DateTime.Now;
|
||
private double _actualFPS = 0;
|
||
|
||
private Transform3D _originalTransform;
|
||
private Point3D _originalCenter; // 存储部件的原始中心位置
|
||
private Point3D _trackedPosition; // 动画管理器内部业务跟踪点，不等于宿主即时读回的实时包围盒中心
|
||
private AnimationState _currentState = AnimationState.Idle;
|
||
private double _pausedProgress = 0.0; // 暂停时的进度（0-1之间）
|
||
private double _currentYaw = 0.0; // 当前偏航角（弧度）
|
||
private Rotation3D _trackedRotation = Rotation3D.Identity; // 动画管理器内部跟踪的完整姿态
|
||
private bool _hasTrackedRotation = false; // 当前是否使用完整姿态
|
||
private Rotation3D _groundRealObjectBaseRotation = Rotation3D.Identity;
|
||
private double _groundRealObjectBaseYaw = 0.0;
|
||
private bool _hasGroundRealObjectBasePose = false;
|
||
private Vector3D _railPreservedPoseTrackedCenterOffset = new Vector3D(0, 0, 0);
|
||
private bool _hasRailPreservedPoseTrackedCenterOffset = false;
|
||
|
||
// TimeLiner 集成
|
||
private TimeLinerIntegrationManager _timeLinerManager;
|
||
private string _currentTaskId;
|
||
|
||
// === 调试辅助 ===
|
||
private static HashSet<int> _arcInfoPrinted = new HashSet<int>(); // 记录已打印信息的圆弧段索引
|
||
|
||
// === 物体状态保存和恢复 ===
|
||
private Point3D _savedObjectPosition;
|
||
private double _savedObjectYaw;
|
||
private Rotation3D _savedObjectRotation = Rotation3D.Identity;
|
||
private bool _savedObjectHasCustomRotation = false;
|
||
private bool _hasSavedObjectState = false;
|
||
private ObjectStartPlacementMode _objectStartPlacementMode = ObjectStartPlacementMode.AlignToPathPose;
|
||
private Rotation3D _pathPreservedPoseRotation = Rotation3D.Identity;
|
||
private bool _hasPathPreservedPoseRotation = false;
|
||
|
||
private bool IsVirtualObjectMode => _animatedObjectMode == AnimatedObjectMode.VirtualObject;
|
||
private bool IsRealObjectMode => _animatedObjectMode == AnimatedObjectMode.RealObject;
|
||
|
||
private ModelItem CurrentControlledObject =>
|
||
IsVirtualObjectMode ? VirtualObjectManager.Instance.CurrentVirtualObject : _animatedObject;
|
||
|
||
/// <summary>
|
||
/// 当前正在进行动画的对象
|
||
/// </summary>
|
||
public ModelItem AnimatedObject => _animatedObject;
|
||
|
||
/// <summary>
|
||
/// 当前检测记录ID（关联CollisionDetectionRecords表）
|
||
/// </summary>
|
||
public int? CurrentDetectionRecordId
|
||
{
|
||
get => _currentDetectionRecordId;
|
||
set => _currentDetectionRecordId = value;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 是否使用历史检测记录（动画完成后跳过ClashDetective测试）
|
||
/// </summary>
|
||
public bool IsUsingHistoryRecord
|
||
{
|
||
get => _isUsingHistoryRecord;
|
||
set => _isUsingHistoryRecord = value;
|
||
}
|
||
|
||
// --- 新增事件 ---
|
||
/// <summary>
|
||
/// 当动画状态发生改变时触发
|
||
/// </summary>
|
||
public event EventHandler<AnimationState> StateChanged;
|
||
|
||
/// <summary>
|
||
/// 当动画进度更新时触发 (0.0-100.0)
|
||
/// </summary>
|
||
public event EventHandler<double> ProgressChanged;
|
||
|
||
public PathAnimationManager()
|
||
{
|
||
_pathPoints = new List<Point3D>();
|
||
_animationFrames = new List<AnimationFrame>();
|
||
_allCollisionResults = new List<CollisionResult>();
|
||
|
||
// 🔥 从配置初始化动画参数
|
||
var config = ConfigManager.Instance.Current;
|
||
_animationFrameRate = config.Animation.FrameRate;
|
||
_animationDuration = config.Animation.DurationSeconds;
|
||
|
||
// 初始化动画模式
|
||
_frameInterval = 1000.0 / _animationFrameRate; // 计算帧间隔
|
||
_lastFrameTime = DateTime.MinValue;
|
||
_fpsCounterStart = DateTime.Now;
|
||
|
||
// 初始化备用DispatcherTimer
|
||
_animationTimer = new DispatcherTimer(DispatcherPriority.Render)
|
||
{
|
||
Interval = TimeSpan.FromMilliseconds(_frameInterval)
|
||
};
|
||
_animationTimer.Tick += OnTimerTick;
|
||
|
||
// 初始化 TimeLiner 集成
|
||
try
|
||
{
|
||
_timeLinerManager = new TimeLinerIntegrationManager();
|
||
LogManager.Info($"PathAnimationManager 初始化完成 - DispatcherTimer模式, 目标FPS: {_animationFrameRate}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"TimeLiner 集成初始化失败，将使用基础动画功能: {ex.Message}");
|
||
_timeLinerManager = null;
|
||
}
|
||
|
||
// 订阅文档状态事件
|
||
DocumentStateManager.Instance.DocumentInvalidated += OnDocumentInvalidated;
|
||
|
||
// 订阅配置变更事件
|
||
SubscribeToConfigChanges();
|
||
}
|
||
|
||
#region 配置变更处理
|
||
|
||
/// <summary>
|
||
/// 订阅配置变更事件
|
||
/// </summary>
|
||
private void SubscribeToConfigChanges()
|
||
{
|
||
try
|
||
{
|
||
ConfigManager.Instance.CategoryConfigurationChanged += OnCategoryConfigurationChanged;
|
||
LogManager.Info("PathAnimationManager 已订阅配置变更事件");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"订阅配置变更事件失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 取消订阅配置变更事件
|
||
/// </summary>
|
||
private void UnsubscribeFromConfigChanges()
|
||
{
|
||
try
|
||
{
|
||
ConfigManager.Instance.CategoryConfigurationChanged -= OnCategoryConfigurationChanged;
|
||
LogManager.Info("PathAnimationManager 已取消订阅配置变更事件");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"取消订阅配置变更事件时发生异常: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 配置变更事件处理
|
||
/// </summary>
|
||
private void OnCategoryConfigurationChanged(object sender, Tuple<ConfigCategory, ConfigurationChangedEventArgs> args)
|
||
{
|
||
try
|
||
{
|
||
var category = args.Item1;
|
||
var eventArgs = args.Item2;
|
||
|
||
// 处理 Animation 类别变更
|
||
if (category == ConfigCategory.Animation || category == ConfigCategory.All)
|
||
{
|
||
LogManager.Info("收到 Animation 配置变更通知，正在更新动画参数...");
|
||
|
||
var config = ConfigManager.Instance.Current;
|
||
bool needReset = false;
|
||
|
||
// 检查帧率是否变更
|
||
if (_animationFrameRate != config.Animation.FrameRate)
|
||
{
|
||
_animationFrameRate = config.Animation.FrameRate;
|
||
_frameInterval = 1000.0 / _animationFrameRate;
|
||
|
||
// 更新定时器间隔
|
||
if (_animationTimer != null)
|
||
{
|
||
_animationTimer.Interval = TimeSpan.FromMilliseconds(_frameInterval);
|
||
}
|
||
|
||
needReset = true;
|
||
LogManager.Info($"动画帧率已更新: {_animationFrameRate} fps, 帧间隔: {_frameInterval:F1} ms");
|
||
}
|
||
|
||
// 检查持续时间是否变更
|
||
if (_animationDuration != config.Animation.DurationSeconds)
|
||
{
|
||
_animationDuration = config.Animation.DurationSeconds;
|
||
needReset = true;
|
||
LogManager.Info($"动画持续时间已更新: {_animationDuration} 秒");
|
||
}
|
||
|
||
// 如果动画正在运行，需要重置
|
||
if (needReset && (_currentState == AnimationState.Playing || _currentState == AnimationState.Paused))
|
||
{
|
||
// 停止当前动画，让用户重新生成
|
||
ShutdownAnimation();
|
||
LogManager.Info("动画参数已变更，当前动画已停止，请重新生成动画");
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"处理配置变更事件失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
/// <summary>
|
||
/// 获取单例实例
|
||
/// </summary>
|
||
public static PathAnimationManager GetInstance()
|
||
{
|
||
if (_instance == null)
|
||
{
|
||
_instance = new PathAnimationManager();
|
||
}
|
||
return _instance;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清理当前所有的动画生成结果（帧列表、碰撞结果、状态）
|
||
/// </summary>
|
||
public void ClearAnimationResults()
|
||
{
|
||
_animationFrames = null;
|
||
_allCollisionResults = null;
|
||
_arcInfoPrinted.Clear(); // 清除圆弧段信息打印记录
|
||
SetState(AnimationState.Idle);
|
||
LogManager.Info("[PathAnimationManager] 动画数据和状态已重置为 Idle");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 仅仅将物理模型同步到路径起点（不清理数据，不重置状态）
|
||
/// 用于加载碰撞检测结果时
|
||
/// </summary>
|
||
public void SyncToPathStart(ModelItem item, List<Point3D> points)
|
||
{
|
||
MoveObjectToPathStart(item, points);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在 CAD 原位仅施加宿主 X/Y/Z 三轴旋转修正，不移动物体。
|
||
/// <summary>
|
||
/// 用于自动调整评测——每次评测在固定 CAD 位置，以 X 轴为统一参考前向。
|
||
/// 调用前需确保物体处于 CAD 原位（已 ResetPermanentTransform）。
|
||
/// </summary>
|
||
public void ApplyRotationCorrectionInPlace(LocalEulerRotationCorrection correction)
|
||
{
|
||
var item = CurrentControlledObject;
|
||
if (item == null) return;
|
||
|
||
var modelItems = new ModelItemCollection { item };
|
||
var doc = NavisApplication.ActiveDocument;
|
||
doc.Models.ResetPermanentTransform(modelItems);
|
||
|
||
var center = GetLiveBoundingBoxCenter(item);
|
||
var tp = new Vector3((float)center.X, (float)center.Y, (float)center.Z);
|
||
|
||
// 与 TryApplyPlanarRealObjectStartIncrementalTransform 保持一致的分解顺序：
|
||
// qup * qnonUp * qx（即先X、再nonUp、最后up）
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
var totalQ = adapter.CreateHostRotationCorrection(correction);
|
||
|
||
var rotatedTp = Vector3.Transform(tp, totalQ);
|
||
|
||
var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
|
||
var comp = identity.Factor();
|
||
comp.Rotation = new Rotation3D(totalQ.X, totalQ.Y, totalQ.Z, totalQ.W);
|
||
comp.Translation = new Vector3D(
|
||
center.X - rotatedTp.X,
|
||
center.Y - rotatedTp.Y,
|
||
center.Z - rotatedTp.Z);
|
||
doc.Models.OverridePermanentTransform(modelItems, comp.Combine(), false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 直接用四元数绕物体包围盒中心旋转（增量模式）。
|
||
/// 用于贴地面等不需要走 Euler 角度链的场景。
|
||
/// </summary>
|
||
public void ApplyQuaternionRotationInPlace(Quaternion rotation)
|
||
{
|
||
var item = CurrentControlledObject;
|
||
if (item == null) return;
|
||
|
||
var modelItems = new ModelItemCollection { item };
|
||
var doc = NavisApplication.ActiveDocument;
|
||
|
||
var center = GetLiveBoundingBoxCenter(item);
|
||
var tp = new Vector3((float)center.X, (float)center.Y, (float)center.Z);
|
||
var rotatedTp = Vector3.Transform(tp, rotation);
|
||
|
||
var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
|
||
var comp = identity.Factor();
|
||
comp.Rotation = new Rotation3D(rotation.X, rotation.Y, rotation.Z, rotation.W);
|
||
comp.Translation = new Vector3D(
|
||
center.X - rotatedTp.X,
|
||
center.Y - rotatedTp.Y,
|
||
center.Z - rotatedTp.Z);
|
||
doc.Models.OverridePermanentTransform(modelItems, comp.Combine(), false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将当前显示姿态同步为跟踪状态（yaw=0 对应对齐 X 轴）。
|
||
/// 用于自动调整确认前，替代 RestoreObjectToCADPosition 的复位→设置链路。
|
||
/// </summary>
|
||
public void SyncTrackedStateToCurrentDisplayPose()
|
||
{
|
||
var item = CurrentControlledObject;
|
||
if (item == null) return;
|
||
_trackedPosition = GetLiveBoundingBoxCenter(item);
|
||
_currentYaw = 0.0;
|
||
_trackedRotation = item.Transform.Factor().Rotation;
|
||
_hasTrackedRotation = true;
|
||
_hasGroundRealObjectBasePose = false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将动画对象彻底恢复到 CAD 原始位置（清除所有覆盖变换）
|
||
/// 常用于模式切换或彻底清除动画干扰
|
||
/// </summary>
|
||
public void RestoreObjectToCADPosition()
|
||
{
|
||
try
|
||
{
|
||
var doc = NavisApplication.ActiveDocument;
|
||
if (doc == null) return;
|
||
|
||
// 🔥 支持虚拟物体的恢复
|
||
ModelItem objectToRestore = null;
|
||
bool isVirtual = IsVirtualObjectMode;
|
||
|
||
if (isVirtual)
|
||
{
|
||
objectToRestore = VirtualObjectManager.Instance.CurrentVirtualObject;
|
||
if (objectToRestore == null)
|
||
{
|
||
LogManager.Warning("[归位] 虚拟物体未激活，跳过恢复");
|
||
return;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
objectToRestore = _animatedObject;
|
||
if (objectToRestore == null)
|
||
{
|
||
LogManager.Warning("[归位] 没有动画对象，跳过恢复");
|
||
return;
|
||
}
|
||
}
|
||
|
||
var modelItems = new ModelItemCollection { objectToRestore };
|
||
|
||
// 1. 彻底清除 Navisworks 中的所有覆盖变换，强制恢复到设计文件定义的原始状态
|
||
doc.Models.ResetPermanentTransform(modelItems);
|
||
|
||
// 2. 重置内部跟踪变量（同步到原始状态）
|
||
// 注意：ResetPermanentTransform 后物体的 Transform 属性会自动变回原始值
|
||
if (isVirtual)
|
||
{
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(objectToRestore.Transform);
|
||
_trackedRotation = objectToRestore.Transform.Factor().Rotation;
|
||
_hasTrackedRotation = true;
|
||
}
|
||
else
|
||
{
|
||
SyncTrackedRotationToObjectReference(objectToRestore, isVirtualObject: false);
|
||
}
|
||
|
||
var originalBoundingBox = objectToRestore.BoundingBox();
|
||
_trackedPosition = ResolveInitialBusinessTrackedPosition(objectToRestore);
|
||
|
||
string objectName = isVirtual ? "虚拟物体" : objectToRestore.DisplayName;
|
||
_railPreservedPoseTrackedCenterOffset = new Vector3D(0, 0, 0);
|
||
_hasRailPreservedPoseTrackedCenterOffset = false;
|
||
LogManager.Debug($"[归位] {objectName} 已彻底恢复到原始位置, yaw={_currentYaw:F3}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[归位] 恢复物体到原始位置失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 生成动画帧
|
||
/// </summary>
|
||
/// <param name="animatedObject">要动画化的模型对象</param>
|
||
/// <param name="pathPoints">路径点列表</param>
|
||
/// <param name="durationSeconds">动画持续时间（秒）</param>
|
||
public void SetRoute(PathRoute route)
|
||
{
|
||
_route = route;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置动画对象（批处理专用）
|
||
/// </summary>
|
||
public void SetAnimatedObject(ModelItem animatedObject)
|
||
{
|
||
_animatedObject = animatedObject;
|
||
_animatedObjectMode = animatedObject == null
|
||
? AnimatedObjectMode.None
|
||
: AnimatedObjectMode.RealObject;
|
||
ResetRealObjectReferenceRotation();
|
||
|
||
if (animatedObject != null)
|
||
{
|
||
_originalTransform = animatedObject.Transform;
|
||
_originalCenter = animatedObject.BoundingBox().Center;
|
||
_trackedPosition = ResolveInitialBusinessTrackedPosition(animatedObject);
|
||
SyncTrackedRotationToObjectReference(animatedObject, isVirtualObject: false);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置虚拟物体参数（批处理专用）
|
||
/// 参数使用模型单位（与Navisworks API一致）
|
||
/// </summary>
|
||
public void SetVirtualObjectParameters(bool isVirtualObject, double length, double width, double height)
|
||
{
|
||
_animatedObjectMode = isVirtualObject
|
||
? AnimatedObjectMode.VirtualObject
|
||
: (_animatedObject != null ? AnimatedObjectMode.RealObject : AnimatedObjectMode.None);
|
||
if (isVirtualObject)
|
||
{
|
||
ResetRealObjectReferenceRotation();
|
||
}
|
||
_virtualObjectLength = length; // 模型单位
|
||
_virtualObjectWidth = width; // 模型单位
|
||
_virtualObjectHeight = height; // 模型单位
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置真实物体的固定物理尺寸（模型单位）。
|
||
/// 一旦动画开始生成，后续 Rail 帧和起点贴合应使用同一份尺寸语义，
|
||
/// 不能再从已经旋转后的当前 AABB 重新推导高度。
|
||
/// </summary>
|
||
public void SetRealObjectDimensions(double length, double width, double height)
|
||
{
|
||
_realObjectLength = length;
|
||
_realObjectWidth = width;
|
||
_realObjectHeight = height;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置动画参数（批处理专用）
|
||
/// </summary>
|
||
public void SetAnimationParameters(int frameRate, double duration, double detectionTolerance)
|
||
{
|
||
_animationFrameRate = frameRate;
|
||
_animationDuration = duration;
|
||
// 注意：预计算使用安全间隙，不使用检测容差
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检测容差（模型单位）- 用于ClashDetective精确检测
|
||
/// </summary>
|
||
public double DetectionTolerance
|
||
{
|
||
get
|
||
{
|
||
var config = ConfigManager.Instance.Current;
|
||
return config.Animation.DetectionTolerance;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置手工碰撞目标（批处理专用）
|
||
/// </summary>
|
||
public void SetManualCollisionTargets(List<ModelItem> targets)
|
||
{
|
||
if (targets != null && targets.Count > 0)
|
||
{
|
||
_manualCollisionTargets = targets;
|
||
_manualCollisionOverrideEnabled = true;
|
||
}
|
||
else
|
||
{
|
||
_manualCollisionTargets = new List<ModelItem>();
|
||
_manualCollisionOverrideEnabled = false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 预计算动画帧（批处理专用内部方法）
|
||
/// </summary>
|
||
public List<AnimationFrame> PrecomputeAnimationFramesInternal()
|
||
{
|
||
PrecomputeAnimationFrames();
|
||
return _animationFrames;
|
||
}
|
||
|
||
public void SetupAnimation(
|
||
ModelItem animatedObject,
|
||
double durationSeconds,
|
||
PathRoute route)
|
||
{
|
||
try
|
||
{
|
||
if (route == null)
|
||
throw new ArgumentNullException(nameof(route), "必须提供路径");
|
||
|
||
_animatedObject = animatedObject;
|
||
_animationDuration = durationSeconds;
|
||
_route = route;
|
||
|
||
LogManager.Info($"使用路径,边数:{route.Edges.Count},总长度:{route.TotalLength:F2}米,动画时长:{durationSeconds:F1}秒");
|
||
|
||
// 计算动画配置哈希（基于路径边，可以区分不同的曲线化路径）
|
||
_currentAnimationHash = ComputeAnimationConfigHashFromRoute(animatedObject, route);
|
||
LogManager.Info($"动画配置哈希: {_currentAnimationHash}");
|
||
|
||
// 重新计算动画帧和碰撞结果（不使用缓存，确保排除列表变化后结果正确）
|
||
LogManager.Info("开始计算动画帧和碰撞结果...");
|
||
PrecomputeAnimationFrames();
|
||
LogManager.Info($"动画帧计算完成，帧数: {_animationFrames?.Count ?? 0}, 碰撞数: {_allCollisionResults?.Count ?? 0}");
|
||
|
||
// 验证预计算结果
|
||
if (_animationFrames == null || _animationFrames.Count == 0)
|
||
{
|
||
throw new InvalidOperationException("动画帧预计算失败，无法创建动画");
|
||
}
|
||
|
||
LogManager.Info($"动画设置完成：对象={_animatedObject.DisplayName}, 时长={_animationDuration}秒");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"设置动画失败: {ex.Message}");
|
||
throw;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将物体移动到路径起点
|
||
/// </summary>
|
||
/// <param name="animatedObject">动画对象（可选，如果不提供则使用当前的_animatedObject）</param>
|
||
/// <param name="pathPoints">路径点（可选，如果不提供则使用当前的_pathPoints）</param>
|
||
/// <returns>是否成功移动</returns>
|
||
public bool MoveObjectToPathStart(ModelItem animatedObject = null, List<Point3D> pathPoints = null, bool skipCadRestore = false)
|
||
{
|
||
_objectStartPlacementMode = ObjectStartPlacementMode.AlignToPathPose;
|
||
|
||
// 前置检查：必须有路径
|
||
if (_route == null)
|
||
{
|
||
LogManager.Error("[MoveObjectToPathStart] 路径为空，无法移动物体到路径起点");
|
||
return false;
|
||
}
|
||
|
||
try
|
||
{
|
||
if (!skipCadRestore)
|
||
{
|
||
if (CurrentControlledObject != null || IsVirtualObjectMode)
|
||
{
|
||
RestoreObjectToCADPosition();
|
||
}
|
||
|
||
// 如果提供了参数，更新内部状态
|
||
if (animatedObject != null)
|
||
{
|
||
_animatedObject = animatedObject;
|
||
bool isVirtualObject =
|
||
VirtualObjectManager.Instance.IsVirtualObjectActive &&
|
||
ReferenceEquals(animatedObject, VirtualObjectManager.Instance.CurrentVirtualObject);
|
||
_animatedObjectMode = isVirtualObject
|
||
? AnimatedObjectMode.VirtualObject
|
||
: AnimatedObjectMode.RealObject;
|
||
ResetRealObjectReferenceRotation();
|
||
_originalTransform = animatedObject.Transform;
|
||
_originalCenter = animatedObject.BoundingBox().Center;
|
||
_trackedPosition = ResolveInitialBusinessTrackedPosition(animatedObject);
|
||
if (isVirtualObject)
|
||
{
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform);
|
||
_trackedRotation = _originalTransform.Factor().Rotation;
|
||
_hasTrackedRotation = true;
|
||
}
|
||
else
|
||
{
|
||
SyncTrackedRotationToObjectReference(animatedObject, isVirtualObject: false);
|
||
}
|
||
|
||
LogManager.Info(
|
||
$"[移动到起点] 更新动画对象内部状态: 对象={animatedObject.DisplayName}, 虚拟物体={IsVirtualObjectMode}");
|
||
}
|
||
|
||
if (pathPoints != null)
|
||
{
|
||
_pathPoints = pathPoints;
|
||
}
|
||
}
|
||
// skipCadRestore: 调用方已设置好 _trackedPosition、_currentYaw、_objectRotationCorrection
|
||
|
||
// 检查路径点
|
||
if (_pathPoints == null || _pathPoints.Count < 2)
|
||
{
|
||
LogManager.Warning("[移动到起点] 没有可用的路径点");
|
||
return false;
|
||
}
|
||
|
||
bool shouldUsePlanarPureIncrementStart =
|
||
IsRealObjectMode &&
|
||
(_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting);
|
||
|
||
// 根据路径类型调整起点位置
|
||
Point3D pathStartPoint = _pathPoints[0];
|
||
Point3D startPosition = pathStartPoint;
|
||
if (_route.PathType == PathType.Hoisting)
|
||
{
|
||
// 吊装路径：第一个路径点（起吊点）是地面位置，动画跟踪点统一使用几何中心
|
||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
|
||
LogManager.Debug($"[移动到起点] 吊装路径：起吊点已转换为物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
||
}
|
||
else if (_route.PathType == PathType.Rail)
|
||
{
|
||
Point3D previousPoint = _pathPoints[0];
|
||
Point3D nextPoint = _pathPoints.Count > 1 ? _pathPoints[1] : _pathPoints[0];
|
||
double objectHeight = GetAnimatedObjectRailNormalExtent(previousPoint, _pathPoints[0], nextPoint);
|
||
if (TryResolveRailPreservedPoseTrackedCenter(startPosition, previousPoint, nextPoint, objectHeight, out Point3D preservedTrackedCenter))
|
||
{
|
||
startPosition = preservedTrackedCenter;
|
||
LogManager.Debug(
|
||
$"[移动到起点] Rail保持姿态补偿: 参考点=({_pathPoints[0].X:F2},{_pathPoints[0].Y:F2},{_pathPoints[0].Z:F2}), " +
|
||
$"补偿后中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
||
}
|
||
else
|
||
{
|
||
startPosition = RailPathPoseHelper.ResolveObjectSpaceCenterPosition(_route, startPosition, previousPoint, nextPoint, objectHeight);
|
||
LogManager.Debug($"[移动到起点] Rail路径调整: 参考点=({_pathPoints[0].X:F2},{_pathPoints[0].Y:F2},{_pathPoints[0].Z:F2}), 物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2}), 物体高度={objectHeight:F2}, 安装={_route.RailMountMode}, 参考面={(PathRoute.IsTopReferenceFaceForMountMode(_route.RailMountMode) ? "顶面参考点" : "底面参考点")}");
|
||
}
|
||
|
||
if (TryCreateRailPathRotation(
|
||
previousPoint,
|
||
_pathPoints[0],
|
||
nextPoint,
|
||
out var railRotation))
|
||
{
|
||
var railLinearTransform = new Transform3D(railRotation).Linear;
|
||
LogManager.Info(
|
||
$"[移动到起点] Rail旋转姿态: " +
|
||
$"X=({railLinearTransform.Get(0, 0):F4},{railLinearTransform.Get(1, 0):F4},{railLinearTransform.Get(2, 0):F4}), " +
|
||
$"Y=({railLinearTransform.Get(0, 1):F4},{railLinearTransform.Get(1, 1):F4},{railLinearTransform.Get(2, 1):F4}), " +
|
||
$"Z=({railLinearTransform.Get(0, 2):F4},{railLinearTransform.Get(1, 2):F4},{railLinearTransform.Get(2, 2):F4})");
|
||
|
||
ApplyFullPoseUpdate(startPosition, railRotation);
|
||
LogManager.Info("[移动到起点] Rail路径已应用完整三维旋转姿态");
|
||
return true;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 地面路径：路径点在接触面上，动画跟踪点统一使用几何中心
|
||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
|
||
startPosition = ApplyGroundPathObjectLiftOffset(startPosition);
|
||
LogManager.Debug($"[移动到起点] 地面路径中心点=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
||
}
|
||
|
||
LogManager.Debug(
|
||
$"[路径起点诊断] 路径point0=({pathStartPoint.X:F3},{pathStartPoint.Y:F3},{pathStartPoint.Z:F3}), " +
|
||
$"起点目标trackedPoint=({startPosition.X:F3},{startPosition.Y:F3},{startPosition.Z:F3}), 路径类型={_route.PathType.GetDisplayName()}");
|
||
|
||
LogManager.Debug(
|
||
$"[移动到起点-诊断] _trackedPosition=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
|
||
$"startPosition=({startPosition.X:F3},{startPosition.Y:F3},{startPosition.Z:F3})");
|
||
|
||
// skipCadRestore 时，校正已原地施加，yaw 增量不应再包含校正的 up 轴分量
|
||
var savedCorrection = _objectRotationCorrection;
|
||
if (skipCadRestore)
|
||
{
|
||
_objectRotationCorrection = LocalEulerRotationCorrection.Zero;
|
||
}
|
||
|
||
if (shouldUsePlanarPureIncrementStart &&
|
||
TryApplyPlanarRealObjectStartIncrementalTransform(_route.PathType, startPosition, out double planarTargetYawRadians))
|
||
{
|
||
LogManager.Debug($"[移动到起点] {_route.PathType.GetDisplayName()} 真实物体已应用纯增量旋转+平移");
|
||
|
||
var startAppliedPoint = GetLiveBoundingBoxCenter(CurrentControlledObject ?? _animatedObject);
|
||
LogManager.Debug(
|
||
$"[路径起点诊断] 起点应用后实际包围盒中心=({startAppliedPoint.X:F3},{startAppliedPoint.Y:F3},{startAppliedPoint.Z:F3}), " +
|
||
$"相对目标偏差=({startAppliedPoint.X - startPosition.X:F3},{startAppliedPoint.Y - startPosition.Y:F3},{startAppliedPoint.Z - startPosition.Z:F3})");
|
||
|
||
var startOffset = new Vector3D(
|
||
startPosition.X - startAppliedPoint.X,
|
||
startPosition.Y - startAppliedPoint.Y,
|
||
startPosition.Z - startAppliedPoint.Z);
|
||
double startOffsetLength = Math.Sqrt(
|
||
startOffset.X * startOffset.X +
|
||
startOffset.Y * startOffset.Y +
|
||
startOffset.Z * startOffset.Z);
|
||
|
||
if (_route.PathType == PathType.Ground)
|
||
{
|
||
_groundRealObjectBaseRotation = _trackedRotation;
|
||
_groundRealObjectBaseYaw = planarTargetYawRadians;
|
||
_hasGroundRealObjectBasePose = true;
|
||
|
||
LogManager.Debug(
|
||
$"[Ground真实物体基姿态] {(CurrentControlledObject ?? _animatedObject)?.DisplayName ?? "(未知)"} BaseYaw={_groundRealObjectBaseYaw * 180.0 / Math.PI:F2}°, " +
|
||
$"已记录基姿态={_hasGroundRealObjectBasePose} (纯增量链)");
|
||
}
|
||
else
|
||
{
|
||
_hasGroundRealObjectBasePose = false;
|
||
}
|
||
}
|
||
else if (shouldUsePlanarPureIncrementStart)
|
||
{
|
||
LogManager.Error($"[移动到起点] {_route.PathType.GetDisplayName()} 真实物体纯增量起点失败，已禁止退回旧姿态链。");
|
||
return false;
|
||
}
|
||
else if (_route.PathType != PathType.Rail &&
|
||
TryCreatePlanarPathRotationAtStart(out var planarRotation))
|
||
{
|
||
ApplyFullPoseUpdate(startPosition, planarRotation);
|
||
LogManager.Debug("[移动到起点] 地面/吊装路径已应用完整三维姿态");
|
||
|
||
var startAppliedPoint = GetLiveBoundingBoxCenter(CurrentControlledObject ?? _animatedObject);
|
||
LogManager.Debug(
|
||
$"[路径起点诊断] 起点应用后实际包围盒中心=({startAppliedPoint.X:F3},{startAppliedPoint.Y:F3},{startAppliedPoint.Z:F3}), " +
|
||
$"相对目标偏差=({startAppliedPoint.X - startPosition.X:F3},{startAppliedPoint.Y - startPosition.Y:F3},{startAppliedPoint.Z - startPosition.Z:F3})");
|
||
|
||
// Ground 起点不再进行二次补偿；业务跟踪点语义由 _trackedPosition 主链负责
|
||
if (IsRealObjectMode && _route.PathType == PathType.Ground)
|
||
{
|
||
var startOffset = new Vector3D(
|
||
startPosition.X - startAppliedPoint.X,
|
||
startPosition.Y - startAppliedPoint.Y,
|
||
startPosition.Z - startAppliedPoint.Z);
|
||
double startOffsetLength = Math.Sqrt(
|
||
startOffset.X * startOffset.X +
|
||
startOffset.Y * startOffset.Y +
|
||
startOffset.Z * startOffset.Z);
|
||
|
||
}
|
||
|
||
if (IsRealObjectMode && _route.PathType == PathType.Ground)
|
||
{
|
||
_groundRealObjectBaseRotation = planarRotation;
|
||
_hasGroundRealObjectBasePose = TryResolveGroundRealObjectBaseYaw(out _groundRealObjectBaseYaw);
|
||
LogManager.Debug(
|
||
$"[Ground真实物体基姿态] {(CurrentControlledObject ?? _animatedObject)?.DisplayName ?? "(未知)"} BaseYaw={_groundRealObjectBaseYaw * 180.0 / Math.PI:F2}°, " +
|
||
$"已记录基姿态={_hasGroundRealObjectBasePose} (旋转偏移由逐帧方法内部处理)");
|
||
}
|
||
else if (IsRealObjectMode && _route.PathType == PathType.Hoisting)
|
||
{
|
||
}
|
||
else
|
||
{
|
||
_hasGroundRealObjectBasePose = false;
|
||
}
|
||
}
|
||
else if (_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting)
|
||
{
|
||
LogManager.Error("[移动到起点] 平面路径未能生成完整姿态，已禁止退回旧 yaw 链路");
|
||
return false;
|
||
}
|
||
else if (_route.PathType == PathType.Rail)
|
||
{
|
||
LogManager.Error("[移动到起点] Rail路径未能生成完整三维姿态，已禁止退回旧 yaw 链路");
|
||
return false;
|
||
}
|
||
|
||
if (skipCadRestore)
|
||
{
|
||
_objectRotationCorrection = savedCorrection;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"初始化物体到路径起点失败: {ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
public bool MoveObjectToPathStartPreservingInitialPose(ModelItem animatedObject = null, List<Point3D> pathPoints = null)
|
||
{
|
||
_objectStartPlacementMode = ObjectStartPlacementMode.PreserveInitialPose;
|
||
|
||
if (_route == null)
|
||
{
|
||
LogManager.Error("[平移到起点] 路径为空，无法移动物体到路径起点");
|
||
return false;
|
||
}
|
||
|
||
try
|
||
{
|
||
if (CurrentControlledObject != null || IsVirtualObjectMode)
|
||
{
|
||
RestoreObjectToCADPosition();
|
||
LogManager.Info($"[平移到起点] 已恢复物体到原始状态, _currentYaw={_currentYaw * 180 / Math.PI:F2}°");
|
||
}
|
||
|
||
if (animatedObject != null)
|
||
{
|
||
_animatedObject = animatedObject;
|
||
bool isVirtualObject =
|
||
VirtualObjectManager.Instance.IsVirtualObjectActive &&
|
||
ReferenceEquals(animatedObject, VirtualObjectManager.Instance.CurrentVirtualObject);
|
||
_animatedObjectMode = isVirtualObject
|
||
? AnimatedObjectMode.VirtualObject
|
||
: AnimatedObjectMode.RealObject;
|
||
ResetRealObjectReferenceRotation();
|
||
_originalTransform = animatedObject.Transform;
|
||
_originalCenter = animatedObject.BoundingBox().Center;
|
||
_trackedPosition = ResolveInitialBusinessTrackedPosition(animatedObject);
|
||
if (isVirtualObject)
|
||
{
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform);
|
||
_trackedRotation = _originalTransform.Factor().Rotation;
|
||
_hasTrackedRotation = true;
|
||
}
|
||
else
|
||
{
|
||
SyncTrackedRotationToObjectReference(animatedObject, isVirtualObject: false);
|
||
}
|
||
}
|
||
|
||
if (pathPoints != null)
|
||
{
|
||
_pathPoints = pathPoints;
|
||
}
|
||
|
||
if (_pathPoints == null || _pathPoints.Count < 2)
|
||
{
|
||
LogManager.Warning("[平移到起点] 没有可用的路径点");
|
||
return false;
|
||
}
|
||
|
||
Point3D pathStartPoint = _pathPoints[0];
|
||
Point3D startPosition = pathStartPoint;
|
||
if (_route.PathType == PathType.Hoisting)
|
||
{
|
||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
|
||
}
|
||
else if (_route.PathType == PathType.Rail)
|
||
{
|
||
Point3D previousPoint = _pathPoints[0];
|
||
Point3D nextPoint = _pathPoints.Count > 1 ? _pathPoints[1] : _pathPoints[0];
|
||
double objectHeight = GetAnimatedObjectRailNormalExtent(previousPoint, _pathPoints[0], nextPoint);
|
||
|
||
// Rail 平移模式不是重新求一套“轨上/轨下安装姿态”，
|
||
// 而是以终点处真实箱体当前位姿为真值，
|
||
// 记录其相对终点路径参考点的固定残差，再整体搬运到起点。
|
||
Point3D terminalReferencePoint = _pathPoints[_pathPoints.Count - 1];
|
||
Point3D terminalPreviousPoint = _pathPoints.Count > 1 ? _pathPoints[_pathPoints.Count - 2] : terminalReferencePoint;
|
||
Point3D terminalNextPoint = terminalReferencePoint;
|
||
CaptureRailPreservedPoseTrackedCenterOffset(
|
||
terminalPreviousPoint,
|
||
terminalReferencePoint,
|
||
terminalNextPoint,
|
||
objectHeight);
|
||
|
||
if (!TryResolveRailPreservedPoseTrackedCenter(startPosition, previousPoint, nextPoint, objectHeight, out Point3D preservedTrackedCenter))
|
||
{
|
||
preservedTrackedCenter = RailPathPoseHelper.ResolveObjectSpaceCenterPosition(_route, startPosition, previousPoint, nextPoint, objectHeight);
|
||
}
|
||
|
||
startPosition = preservedTrackedCenter;
|
||
}
|
||
else
|
||
{
|
||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
|
||
startPosition = ApplyGroundPathObjectLiftOffset(startPosition);
|
||
}
|
||
|
||
ApplyTrackedTranslationOnly(startPosition);
|
||
SyncTrackedRotationToDisplayedPose(CurrentControlledObject ?? _animatedObject);
|
||
_hasGroundRealObjectBasePose = false;
|
||
|
||
var startAppliedPoint = GetLiveBoundingBoxCenter(CurrentControlledObject ?? _animatedObject);
|
||
LogManager.Info(
|
||
$"[平移到起点] 已按终点原始位姿残差搬运到起点: 路径point0=({pathStartPoint.X:F3},{pathStartPoint.Y:F3},{pathStartPoint.Z:F3}), " +
|
||
$"目标trackedPoint=({startPosition.X:F3},{startPosition.Y:F3},{startPosition.Z:F3}), " +
|
||
$"实际包围盒中心=({startAppliedPoint.X:F3},{startAppliedPoint.Y:F3},{startAppliedPoint.Z:F3}), 路径类型={_route.PathType.GetDisplayName()}");
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"保持初始位姿平移到路径起点失败: {ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 预计算所有动画帧和碰撞信息
|
||
/// </summary>
|
||
private void PrecomputeAnimationFrames()
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("=== 使用路径预计算动画帧 ===");
|
||
|
||
// 🔥 重要：预计算前先将物体移动到起点位置
|
||
if (_animatedObject != null && _route != null && _route.Points != null && _route.Points.Count > 0)
|
||
{
|
||
var pathPoints = _route.Points.Select(p => p.Position).ToList();
|
||
MoveObjectToPathStartUsingCurrentPlacementMode(_animatedObject, pathPoints);
|
||
if (ShouldPreservePathRotationForFrames(
|
||
_route.PathType,
|
||
_objectStartPlacementMode,
|
||
_hasTrackedRotation))
|
||
{
|
||
_pathPreservedPoseRotation = _trackedRotation;
|
||
_hasPathPreservedPoseRotation = true;
|
||
LogManager.Info($"[预计算] {_route.PathType.GetDisplayName()}平移模式已锁定起点姿态为整段动画旋转基线");
|
||
}
|
||
else
|
||
{
|
||
_pathPreservedPoseRotation = Rotation3D.Identity;
|
||
_hasPathPreservedPoseRotation = false;
|
||
}
|
||
LogManager.Info("[预计算] 物体已移动到路径起点");
|
||
}
|
||
|
||
int totalFrames = (int)(_animationDuration * _animationFrameRate);
|
||
|
||
// 🔥 判断路径类型：空中路径使用Points，地面路径使用Edges
|
||
bool isAerialPath = _route.PathType == PathType.Rail || _route.PathType == PathType.Hoisting;
|
||
|
||
List<Point3D> allSampledPoints = new List<Point3D>();
|
||
List<double> segmentLengths = new List<double>();
|
||
|
||
if (isAerialPath)
|
||
{
|
||
// === 空中路径：直接使用路径点进行线性插值 ===
|
||
string aerialSubTypeName = _route.PathType == PathType.Rail ? "空轨" : "吊装";
|
||
LogManager.Info($"[{aerialSubTypeName}路径] 使用路径点进行线性插值，路径点数: {_route.Points.Count}");
|
||
|
||
// 将路径点转换为Point3D列表
|
||
foreach (var point in _route.Points)
|
||
{
|
||
allSampledPoints.Add(point.Position);
|
||
}
|
||
|
||
// 计算每段长度（内部计算统一使用模型单位）
|
||
for (int i = 0; i < allSampledPoints.Count - 1; i++)
|
||
{
|
||
double lengthInModelUnits = GeometryHelper.CalculatePointDistance(allSampledPoints[i], allSampledPoints[i + 1]);
|
||
segmentLengths.Add(lengthInModelUnits);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// === 地面路径：使用Edges进行曲线化插值 ===
|
||
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);
|
||
segmentLengths.Add(edge.PhysicalLength);
|
||
}
|
||
}
|
||
|
||
// 🔥 内部计算统一使用模型单位
|
||
double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor();
|
||
double totalLengthInModelUnits = _route.TotalLength * metersToModelUnits;
|
||
|
||
string pathTypeName = _route.PathType.GetDisplayName();
|
||
LogManager.Info($"路径总长度: {totalLengthInModelUnits / metersToModelUnits:F2}米, 采样点数: {allSampledPoints.Count}, 路径类型: {pathTypeName}");
|
||
|
||
// 2. 按帧数采样生成动画帧
|
||
_animationFrames = new List<AnimationFrame>();
|
||
_allCollisionResults = new List<CollisionResult>();
|
||
_currentFrameIndex = 0;
|
||
|
||
// 获取动画对象的包围盒信息
|
||
var originalBoundingBox = _animatedObject.BoundingBox();
|
||
var boundingBoxSize = new Vector3D(
|
||
originalBoundingBox.Max.X - originalBoundingBox.Min.X,
|
||
originalBoundingBox.Max.Y - originalBoundingBox.Min.Y,
|
||
originalBoundingBox.Max.Z - originalBoundingBox.Min.Z
|
||
);
|
||
|
||
// 空间索引设置
|
||
SpatialIndexManager spatialIndexManager = null;
|
||
var manualTargetsSnapshot = new List<ModelItem>();
|
||
bool manualOverrideActive = _manualCollisionOverrideEnabled && _manualCollisionTargets != null && _manualCollisionTargets.Count > 0;
|
||
|
||
// 获取配置
|
||
var config = ConfigManager.Instance.Current;
|
||
|
||
if (manualOverrideActive)
|
||
{
|
||
manualTargetsSnapshot = _manualCollisionTargets
|
||
.Where(item => item != null && ModelItemAnalysisHelper.IsModelItemValid(item) && !ReferenceEquals(item, _animatedObject))
|
||
.Distinct()
|
||
.ToList();
|
||
|
||
if (manualTargetsSnapshot.Count == 0)
|
||
{
|
||
manualOverrideActive = false;
|
||
LogManager.Warning("[手工碰撞] 手工碰撞对象列表为空或无效，自动回退到空间索引模式");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info($"[手工碰撞] 启用手工碰撞对象模式，数量: {manualTargetsSnapshot.Count}，跳过空间索引构建");
|
||
}
|
||
}
|
||
|
||
if (!manualOverrideActive)
|
||
{
|
||
LogManager.Info("=== 构建全局空间索引 ===");
|
||
|
||
spatialIndexManager = SpatialIndexManager.Instance;
|
||
|
||
// 使用动画专用的空间索引格子大小配置 - 使用模型单位接口
|
||
double cellSizeInModelUnits = config.Animation.SpatialIndexCellSize;
|
||
|
||
LogManager.Info($"[空间索引] 格子大小: {cellSizeInModelUnits:F2}模型单位 (动画配置)");
|
||
|
||
// 检查空间索引是否需要重新构建（格子大小变化或未初始化）
|
||
bool needRebuild = !spatialIndexManager.IsInitialized ||
|
||
Math.Abs(spatialIndexManager.CellSize - cellSizeInModelUnits) > 0.0001;
|
||
|
||
if (needRebuild)
|
||
{
|
||
LogManager.Info("[空间索引] 空间索引未初始化或格子大小已改变，开始重新构建...");
|
||
spatialIndexManager.BuildGlobalIndex(cellSizeInModelUnits);
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info("[空间索引] 使用已缓存的空间索引");
|
||
}
|
||
|
||
LogManager.Info(spatialIndexManager.GetStatistics());
|
||
|
||
// 🔥 优化：使用AABB查询代替球形查询，查询范围更精确
|
||
// 不再预计算球形半径，改为每帧计算物体AABB + 安全间隙的搜索AABB
|
||
LogManager.Info($"空间查询使用AABB模式，安全间隙: {_safetyMargin:F4}模型单位");
|
||
}
|
||
|
||
// 3. 生成每一帧
|
||
double distancePerFrameInModelUnits = totalFrames > 1
|
||
? totalLengthInModelUnits / (totalFrames - 1)
|
||
: 0.0;
|
||
LogManager.Info($"总帧数: {totalFrames}, 总长度: {totalLengthInModelUnits / metersToModelUnits:F2}米, 每帧移动距离: {distancePerFrameInModelUnits / metersToModelUnits:F4}米");
|
||
|
||
for (int i = 0; i < totalFrames; i++)
|
||
{
|
||
double targetDistance = i * distancePerFrameInModelUnits; // 按距离采样（模型单位），最后一帧应精确到终点
|
||
if (targetDistance > totalLengthInModelUnits)
|
||
{
|
||
targetDistance = totalLengthInModelUnits;
|
||
}
|
||
|
||
Point3D framePosition;
|
||
Point3D previousFramePoint = Point3D.Origin;
|
||
Point3D nextFramePoint = Point3D.Origin;
|
||
double yawRadians;
|
||
|
||
if (isAerialPath)
|
||
{
|
||
// === 空中路径：在路径点之间进行线性插值 ===
|
||
int segmentIndex = FindSegmentForDistance(targetDistance, segmentLengths);
|
||
if (segmentIndex < 0 || segmentIndex >= segmentLengths.Count)
|
||
{
|
||
string subTypeName = _route.PathType.GetDisplayName();
|
||
LogManager.Warning($"[{subTypeName}路径] 无法找到线段,目标距离:{targetDistance / metersToModelUnits:F2}米");
|
||
continue;
|
||
}
|
||
double accumulatedLength = segmentLengths.Take(segmentIndex).Sum();
|
||
double segmentProgress = (targetDistance - accumulatedLength) / segmentLengths[segmentIndex];
|
||
|
||
// 线性插值
|
||
Point3D p1 = allSampledPoints[segmentIndex];
|
||
Point3D p2 = allSampledPoints[segmentIndex + 1];
|
||
previousFramePoint = p1;
|
||
nextFramePoint = p2;
|
||
framePosition = new Point3D(
|
||
p1.X + segmentProgress * (p2.X - p1.X),
|
||
p1.Y + segmentProgress * (p2.Y - p1.Y),
|
||
p1.Z + segmentProgress * (p2.Z - p1.Z)
|
||
);
|
||
|
||
// 计算朝向（使用当前线段的方向）
|
||
yawRadians = Math.Atan2(p2.Y - p1.Y, p2.X - p1.X);
|
||
|
||
// 吊装路径：所有线段都使用水平吊运方向（与MoveObjectToPathStart保持一致）
|
||
if (_route.PathType == PathType.Hoisting)
|
||
{
|
||
if (_pathPoints.Count >= 3)
|
||
{
|
||
yawRadians = Math.Atan2(_pathPoints[2].Y - _pathPoints[1].Y, _pathPoints[2].X - _pathPoints[1].X);
|
||
}
|
||
else
|
||
{
|
||
// 2点路径（纯垂直）：无水平方向，保持当前朝向
|
||
yawRadians = _currentYaw;
|
||
}
|
||
}
|
||
|
||
// 🔥 空中路径：根据路径类型和线段索引调整物体位置
|
||
double objectHeight = _route.PathType == PathType.Rail
|
||
? GetAnimatedObjectRailNormalExtent(p1, framePosition, p2)
|
||
: GetAnimatedObjectGroundContactHeight();
|
||
|
||
if (_route.PathType == PathType.Hoisting)
|
||
{
|
||
if (_route.IsTwoPointVertical)
|
||
{
|
||
// 2点纯垂直路径：起点加半高补偿（地面→中心），终点不加（终点即中心位置）
|
||
var startCenter = ResolveGroundTrackedCenter(p1, GetAnimatedObjectGroundContactHeight());
|
||
var endCenter = p2;
|
||
framePosition = new Point3D(
|
||
startCenter.X + segmentProgress * (endCenter.X - startCenter.X),
|
||
startCenter.Y + segmentProgress * (endCenter.Y - startCenter.Y),
|
||
startCenter.Z + segmentProgress * (endCenter.Z - startCenter.Z));
|
||
LogManager.Debug($"[吊装路径-2点垂直] 进度={segmentProgress:F2}, 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2})");
|
||
}
|
||
else
|
||
{
|
||
// 吊装路径：支持动态数量的路径点
|
||
// 路径点结构：
|
||
// 点0（起吊点）：地面位置
|
||
// 点1（提升点）：悬挂点
|
||
// 点2...点N-2：中间悬挂点（用户添加的转向点）
|
||
// 点N-1（下降点）：悬挂点
|
||
// 点N（落地点）：地面位置
|
||
//
|
||
// 线段结构：
|
||
// 线段0：起吊段（地面→悬挂点）- 垂直
|
||
// 线段1...线段N-2：平移段（悬挂点→悬挂点）- 水平
|
||
// 线段N-1：下降段（悬挂点→地面）- 垂直
|
||
|
||
int totalSegments = _pathPoints.Count - 1;
|
||
int firstSegment = 0; // 起吊段
|
||
int lastSegment = totalSegments - 1; // 下降段
|
||
|
||
if (segmentIndex == firstSegment)
|
||
{
|
||
framePosition = ResolveHoistingTrackedCenter(framePosition, objectHeight, segmentProgress);
|
||
LogManager.Debug($"[吊装路径-起吊段] 进度={segmentProgress:F2}, 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2})");
|
||
}
|
||
else if (segmentIndex > firstSegment && segmentIndex < lastSegment)
|
||
{
|
||
// 平移段（中间的所有线段）：参考点位于顶部悬挂点
|
||
framePosition = ResolveHoistingTrackedCenter(framePosition, objectHeight, 1.0);
|
||
LogManager.Debug($"[吊装路径-平移段] 线段{segmentIndex}, 进度={segmentProgress:F2}, 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2})");
|
||
}
|
||
else if (segmentIndex == lastSegment)
|
||
{
|
||
framePosition = ResolveHoistingTrackedCenter(framePosition, objectHeight, 1.0 - segmentProgress);
|
||
LogManager.Debug($"[吊装路径-下降段] 进度={segmentProgress:F2}, 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2})");
|
||
}
|
||
}
|
||
}
|
||
else if (_route.PathType == PathType.Rail)
|
||
{
|
||
if (TryResolveRailPreservedPoseTrackedCenter(framePosition, p1, p2, objectHeight, out Point3D preservedTrackedCenter))
|
||
{
|
||
framePosition = preservedTrackedCenter;
|
||
LogManager.Debug(
|
||
$"[Rail路径] 保持姿态补偿: 参考点=({p1.X:F2},{p1.Y:F2},{p1.Z:F2})->({p2.X:F2},{p2.Y:F2},{p2.Z:F2}), " +
|
||
$"物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2})");
|
||
}
|
||
else
|
||
{
|
||
framePosition = RailPathPoseHelper.ResolveObjectSpaceCenterPosition(_route, framePosition, p1, p2, objectHeight);
|
||
LogManager.Debug($"[Rail路径] 调整物体位置: 参考点=({p1.X:F2},{p1.Y:F2},{p1.Z:F2})->({p2.X:F2},{p2.Y:F2},{p2.Z:F2}), 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2}), 安装={_route.RailMountMode}, 参考面={(PathRoute.IsTopReferenceFaceForMountMode(_route.RailMountMode) ? "顶面参考点" : "底面参考点")}");
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// === 地面路径：在边内插值 ===
|
||
int edgeIndex = FindEdgeForDistance(targetDistance, segmentLengths);
|
||
if (edgeIndex < 0 || edgeIndex >= _route.Edges.Count)
|
||
{
|
||
LogManager.Warning($"无法找到边,目标距离:{targetDistance / metersToModelUnits:F2}米");
|
||
continue;
|
||
}
|
||
var edge = _route.Edges[edgeIndex];
|
||
double accumulatedLength = segmentLengths.Take(edgeIndex).Sum();
|
||
double edgeProgress = (targetDistance - accumulatedLength) / edge.PhysicalLength;
|
||
|
||
Point3D sampledPreviousPoint;
|
||
Point3D sampledNextPoint;
|
||
|
||
// 在边内插值位置
|
||
if (edge.SegmentType == PathSegmentType.Straight)
|
||
{
|
||
int sampleCount = edge.SampledPoints.Count;
|
||
int pointIndex = (int)(edgeProgress * (sampleCount - 1));
|
||
pointIndex = Math.Max(0, Math.Min(pointIndex, sampleCount - 1));
|
||
|
||
framePosition = edge.SampledPoints[pointIndex];
|
||
sampledPreviousPoint = edge.SampledPoints[Math.Max(0, pointIndex - 1)];
|
||
sampledNextPoint = edge.SampledPoints[Math.Min(sampleCount - 1, pointIndex + 1)];
|
||
}
|
||
else // Arc
|
||
{
|
||
int sampleCount = edge.SampledPoints.Count;
|
||
int pointIndex = (int)(edgeProgress * (sampleCount - 1));
|
||
pointIndex = Math.Max(0, Math.Min(pointIndex, sampleCount - 1));
|
||
|
||
framePosition = edge.SampledPoints[pointIndex];
|
||
sampledPreviousPoint = edge.SampledPoints[Math.Max(0, pointIndex - 1)];
|
||
sampledNextPoint = edge.SampledPoints[Math.Min(sampleCount - 1, pointIndex + 1)];
|
||
}
|
||
|
||
framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectGroundContactHeight());
|
||
framePosition = ApplyGroundPathObjectLiftOffset(framePosition);
|
||
|
||
yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress);
|
||
previousFramePoint = ApplyGroundPathObjectLiftOffset(
|
||
ResolveGroundTrackedCenter(sampledPreviousPoint, GetAnimatedObjectGroundContactHeight()));
|
||
nextFramePoint = ApplyGroundPathObjectLiftOffset(
|
||
ResolveGroundTrackedCenter(sampledNextPoint, GetAnimatedObjectGroundContactHeight()));
|
||
|
||
if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) &&
|
||
i > 0 &&
|
||
(Math.Abs(previousFramePoint.X - nextFramePoint.X) > 1e-6 ||
|
||
Math.Abs(previousFramePoint.Y - nextFramePoint.Y) > 1e-6 ||
|
||
Math.Abs(previousFramePoint.Z - nextFramePoint.Z) > 1e-6))
|
||
{
|
||
LogManager.Debug(
|
||
$"[平面转弯诊断] 帧={i}, edge={edgeIndex}, " +
|
||
$"prev=({previousFramePoint.X:F3},{previousFramePoint.Y:F3},{previousFramePoint.Z:F3}), " +
|
||
$"cur=({framePosition.X:F3},{framePosition.Y:F3},{framePosition.Z:F3}), " +
|
||
$"next=({nextFramePoint.X:F3},{nextFramePoint.Y:F3},{nextFramePoint.Z:F3}), " +
|
||
$"yaw={yawRadians * 180.0 / Math.PI:F2}°");
|
||
}
|
||
}
|
||
|
||
// 创建帧并检测碰撞
|
||
var frame = new AnimationFrame
|
||
{
|
||
Index = i,
|
||
Progress = (double)i / (totalFrames - 1),
|
||
Position = framePosition,
|
||
YawRadians = yawRadians,
|
||
Collisions = new List<CollisionResult>()
|
||
};
|
||
|
||
if (ShouldUsePlanarRealObjectPureIncrementFrames(_route.PathType, IsRealObjectMode))
|
||
{
|
||
double planarHostYawRadians = yawRadians;
|
||
if (_route.PathType == PathType.Ground &&
|
||
!TryResolveGroundHostPlanarYawFromFramePoints(
|
||
previousFramePoint,
|
||
framePosition,
|
||
nextFramePoint,
|
||
CoordinateSystemManager.Instance.ResolvedType,
|
||
out planarHostYawRadians))
|
||
{
|
||
throw new InvalidOperationException(
|
||
$"{_route.PathType.GetDisplayName()}真实物体第 {i} 帧未能解析宿主平面目标角。");
|
||
}
|
||
|
||
if (!TryResolveRecordedPlanarRealObjectFrameRotation(
|
||
_route.PathType,
|
||
previousFramePoint,
|
||
framePosition,
|
||
nextFramePoint,
|
||
out var recordedRotation))
|
||
{
|
||
throw new InvalidOperationException(
|
||
$"{_route.PathType.GetDisplayName()}真实物体第 {i} 帧未能记录完整姿态。");
|
||
}
|
||
|
||
ApplyRecordedPlanarRealObjectFramePose(
|
||
frame,
|
||
planarHostYawRadians,
|
||
recordedRotation);
|
||
}
|
||
else if (ShouldPreservePathRotationForFrames(
|
||
_route.PathType,
|
||
_objectStartPlacementMode,
|
||
_hasPathPreservedPoseRotation))
|
||
{
|
||
frame.Rotation = _pathPreservedPoseRotation;
|
||
frame.HasCustomRotation = true;
|
||
}
|
||
else if (_route.PathType == PathType.Rail &&
|
||
TryCreateRailPathRotation(
|
||
previousFramePoint,
|
||
framePosition,
|
||
nextFramePoint,
|
||
out var railRotation))
|
||
{
|
||
frame.Rotation = railRotation;
|
||
frame.HasCustomRotation = true;
|
||
}
|
||
else if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) &&
|
||
!_route.IsTwoPointVertical &&
|
||
TryCreatePlanarPathRotationForFrame(previousFramePoint, framePosition, nextFramePoint, out var planarRotation))
|
||
{
|
||
if (_route.PathType == PathType.Ground &&
|
||
IsRealObjectMode &&
|
||
TryCreateGroundRealObjectConstrainedRotation(previousFramePoint, framePosition, nextFramePoint, out var constrainedRotation))
|
||
{
|
||
frame.Rotation = constrainedRotation;
|
||
frame.HasCustomRotation = true;
|
||
}
|
||
else
|
||
{
|
||
frame.Rotation = planarRotation;
|
||
frame.HasCustomRotation = true;
|
||
}
|
||
}
|
||
else if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) &&
|
||
!_route.IsTwoPointVertical)
|
||
{
|
||
throw new InvalidOperationException(
|
||
$"平面路径第 {i} 帧未能生成完整姿态，已禁止退回旧 yaw 链路。");
|
||
}
|
||
|
||
// 记录第一帧的角度（用于调试）
|
||
if (i == 0)
|
||
{
|
||
LogManager.Debug($"[预计算] 第0帧: pos=({framePosition.X:F2},{framePosition.Y:F2}), yaw={yawRadians * 180 / Math.PI:F2}度");
|
||
}
|
||
|
||
// 虚拟碰撞检测
|
||
|
||
// 计算物体包围盒相对于framePosition的偏移
|
||
var currentObjectBoundingBox = _animatedObject.BoundingBox();
|
||
var originalCenter = new Point3D(
|
||
(currentObjectBoundingBox.Min.X + currentObjectBoundingBox.Max.X) / 2,
|
||
(currentObjectBoundingBox.Min.Y + currentObjectBoundingBox.Max.Y) / 2,
|
||
(currentObjectBoundingBox.Min.Z + currentObjectBoundingBox.Max.Z) / 2
|
||
);
|
||
|
||
// 计算偏移量（当前包围盒中心到framePosition的偏移）
|
||
var offsetX = framePosition.X - originalCenter.X;
|
||
var offsetY = framePosition.Y - originalCenter.Y;
|
||
var offsetZ = framePosition.Z - originalCenter.Z;
|
||
|
||
// 创建新的包围盒（将当前包围盒中心移动到framePosition）
|
||
var virtualBoundingBox = new BoundingBox3D(
|
||
new Point3D(
|
||
currentObjectBoundingBox.Min.X + offsetX,
|
||
currentObjectBoundingBox.Min.Y + offsetY,
|
||
currentObjectBoundingBox.Min.Z + offsetZ
|
||
),
|
||
new Point3D(
|
||
currentObjectBoundingBox.Max.X + offsetX,
|
||
currentObjectBoundingBox.Max.Y + offsetY,
|
||
currentObjectBoundingBox.Max.Z + offsetZ
|
||
)
|
||
);
|
||
|
||
IEnumerable<ModelItem> nearbyObjects;
|
||
if (manualOverrideActive)
|
||
{
|
||
// 🔥 Human-in-the-Loop: 手工模式下也要应用排除列表
|
||
nearbyObjects = manualTargetsSnapshot;
|
||
if (_excludedObjects.Count > 0)
|
||
{
|
||
nearbyObjects = nearbyObjects.Where(obj => !_excludedObjects.Contains(obj));
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 🔥 优化：使用AABB查询代替球形查询，避免球体扩大的无效范围
|
||
// 查询AABB = 物体AABB + 安全间隙扩展
|
||
var searchBounds = new BoundingBox3D(
|
||
new Point3D(
|
||
virtualBoundingBox.Min.X - _safetyMargin,
|
||
virtualBoundingBox.Min.Y - _safetyMargin,
|
||
virtualBoundingBox.Min.Z - _safetyMargin
|
||
),
|
||
new Point3D(
|
||
virtualBoundingBox.Max.X + _safetyMargin,
|
||
virtualBoundingBox.Max.Y + _safetyMargin,
|
||
virtualBoundingBox.Max.Z + _safetyMargin
|
||
)
|
||
);
|
||
nearbyObjects = spatialIndexManager.FindInAABB(
|
||
searchBounds,
|
||
excludeObject: _animatedObject
|
||
);
|
||
|
||
// 🔥 Human-in-the-Loop: 应用用户排除列表过滤
|
||
if (_excludedObjects.Count > 0)
|
||
{
|
||
nearbyObjects = nearbyObjects.Where(obj => !_excludedObjects.Contains(obj));
|
||
}
|
||
}
|
||
|
||
foreach (var collider in nearbyObjects)
|
||
{
|
||
var colliderBox = collider.BoundingBox();
|
||
|
||
// 使用包围盒距离检测方法
|
||
double distance = BoundingBoxGeometryUtils.CalculateDistance(virtualBoundingBox, colliderBox);
|
||
// _safetyMargin 已经是模型单位（在CreateAnimation中已转换），直接使用
|
||
bool intersects = distance <= _safetyMargin;
|
||
|
||
if (intersects)
|
||
{
|
||
LogManager.Debug($"帧 {i} 检测到碰撞: {_animatedObject.DisplayName} <-> {collider.DisplayName}, 距离: {distance:F4}，阈值: {_safetyMargin:F4}");
|
||
LogManager.Debug($"移动物体位置: {framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2}");
|
||
LogManager.Debug($"被撞物体位置：{GetObjectPosition(collider).X:F2},{GetObjectPosition(collider).Y:F2},{GetObjectPosition(collider).Z:F2}");
|
||
|
||
double actualYawRadians = ResolveAnimationFramePlaybackYawRadians(frame);
|
||
|
||
var collisionResult = new CollisionResult
|
||
{
|
||
ClashGuid = Guid.NewGuid(),
|
||
DisplayName = $"预计算碰撞: {_animatedObject.DisplayName} <-> {collider.DisplayName}",
|
||
Status = ClashResultStatus.New,
|
||
Item1 = _animatedObject,
|
||
Item2 = collider,
|
||
CreatedTime = DateTime.Now,
|
||
Distance = BoundingBoxGeometryUtils.CalculateCenterDistance(virtualBoundingBox, colliderBox),
|
||
Center = new Point3D(
|
||
(virtualBoundingBox.Min.X + virtualBoundingBox.Max.X + colliderBox.Min.X + colliderBox.Max.X) / 4,
|
||
(virtualBoundingBox.Min.Y + virtualBoundingBox.Max.Y + colliderBox.Min.Y + colliderBox.Max.Y) / 4,
|
||
(virtualBoundingBox.Min.Z + virtualBoundingBox.Max.Z + colliderBox.Min.Z + colliderBox.Max.Z) / 4
|
||
),
|
||
// AnimatedObjectTrackedPosition 统一定义为动画主链路的跟踪点位置。
|
||
// 当前动画主链路已统一使用几何中心作为跟踪点，因此所有路径都直接记录 framePosition。
|
||
AnimatedObjectTrackedPosition = new Point3D(
|
||
framePosition.X,
|
||
framePosition.Y,
|
||
framePosition.Z),
|
||
Item2Position = GetObjectPosition(collider),
|
||
AnimatedObjectTrackedYawRadians = actualYawRadians, // 记录动画跟踪点对应的运动物体实际播放朝向
|
||
AnimatedObjectTrackedRotation = frame.HasCustomRotation ? frame.Rotation : Rotation3D.Identity,
|
||
AnimatedObjectHasTrackedRotation = frame.HasCustomRotation,
|
||
HasPositionInfo = true
|
||
};
|
||
|
||
LogManager.Debug(
|
||
$"[保存碰撞结果] 帧={i}, " +
|
||
$"位置语义=动画跟踪中心点, " +
|
||
$"原始Yaw={yawRadians * 180.0 / Math.PI:F2}°, " +
|
||
$"实际Yaw={actualYawRadians * 180.0 / Math.PI:F2}°, " +
|
||
$"customRotation={frame.HasCustomRotation}, " +
|
||
$"保存位置=({collisionResult.AnimatedObjectTrackedPosition.X:F3}, {collisionResult.AnimatedObjectTrackedPosition.Y:F3}, {collisionResult.AnimatedObjectTrackedPosition.Z:F3})");
|
||
|
||
frame.Collisions.Add(collisionResult);
|
||
_allCollisionResults.Add(collisionResult);
|
||
}
|
||
}
|
||
|
||
_animationFrames.Add(frame);
|
||
}
|
||
|
||
// 统计碰撞信息
|
||
var framesWithCollision = _animationFrames.Count(f => f.HasCollision);
|
||
var totalCollisions = _animationFrames.Sum(f => f.Collisions.Count);
|
||
// 检查碰撞结果是否包含位置信息
|
||
var collisionsWithPosition = _allCollisionResults.Count(c => c.HasPositionInfo && c.AnimatedObjectTrackedPosition != null);
|
||
|
||
if (_route.PathType == PathType.Rail && _animationFrames.Count > 0)
|
||
{
|
||
AnimationFrame firstRailFrame = _animationFrames[0];
|
||
AnimationFrame lastRailFrame = _animationFrames[_animationFrames.Count - 1];
|
||
LogManager.Info(
|
||
$"[Rail帧诊断] 首帧跟踪中心=({firstRailFrame.Position.X:F3},{firstRailFrame.Position.Y:F3},{firstRailFrame.Position.Z:F3}), " +
|
||
$"末帧跟踪中心=({lastRailFrame.Position.X:F3},{lastRailFrame.Position.Y:F3},{lastRailFrame.Position.Z:F3}), 总帧数={_animationFrames.Count}");
|
||
}
|
||
|
||
LogManager.Info($"=== 预计算完成 ===");
|
||
LogManager.Info($"总帧数: {_animationFrames.Count}");
|
||
LogManager.Info($"包含碰撞的帧: {framesWithCollision}");
|
||
LogManager.Info($"总碰撞次数: {totalCollisions}");
|
||
LogManager.Info($"记录的碰撞结果总数: {_allCollisionResults.Count} 个");
|
||
LogManager.Info($"包含位置信息的碰撞: {collisionsWithPosition}/{_allCollisionResults.Count}");
|
||
|
||
// 🔥 Human-in-the-Loop: 记录排除统计
|
||
if (_excludedObjects.Count > 0)
|
||
{
|
||
LogManager.Info($"[排除列表] 本次预计算排除了 {_excludedObjects.Count} 个物体");
|
||
}
|
||
|
||
// 🔥 清除移动物体集合
|
||
ClashDetectiveIntegration.ClearAnimatedObject();
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"预计算动画帧失败: {ex.Message}");
|
||
_animationFrames = new List<AnimationFrame>();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据路径计算指定帧的yaw角度
|
||
/// </summary>
|
||
/// <summary>
|
||
/// 计算路径上的朝向(改进版)
|
||
/// </summary>
|
||
private double ComputeYawOnPath(
|
||
int frameIndex,
|
||
List<Point3D> allSampledPoints,
|
||
int currentEdgeIndex,
|
||
double edgeProgress)
|
||
{
|
||
var edge = _route.Edges[currentEdgeIndex];
|
||
|
||
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;
|
||
|
||
// 计算切点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.Debug($"圆弧段[{currentEdgeIndex}]朝向信息:");
|
||
LogManager.Debug($" 前直线段朝向: {prevStraightYaw * 180.0 / Math.PI:F2}度");
|
||
LogManager.Debug($" 进入切点(Ts)朝向: {tsYaw * 180.0 / Math.PI:F2}度");
|
||
LogManager.Debug($" 圆弧偏转角度: {signedAngle * 180.0 / Math.PI:F2}度");
|
||
LogManager.Debug($" 退出切点(Te)朝向: {teYaw * 180.0 / Math.PI:F2}度");
|
||
LogManager.Debug($" 后直线段朝向: {nextStraightYaw * 180.0 / Math.PI:F2}度");
|
||
LogManager.Debug($" 前后直线段朝向差: {(nextStraightYaw - prevStraightYaw) * 180.0 / Math.PI:F2}度");
|
||
LogManager.Debug($" 圆弧段朝向变化: {yawChange * 180.0 / Math.PI:F2}度");
|
||
_arcInfoPrinted.Add(currentEdgeIndex);
|
||
}
|
||
|
||
// 每一帧都打印当前朝向
|
||
// LogManager.Debug($"帧[{frameIndex}] 圆弧段[{currentEdgeIndex}]: 进度={edgeProgress:F4}, 当前朝向={currentYaw * 180.0 / Math.PI:F2}度");
|
||
|
||
return currentYaw;
|
||
}
|
||
else
|
||
{
|
||
// 直线段: 使用直线方向向量，避免边界处的前后帧差分问题
|
||
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);
|
||
}
|
||
}
|
||
|
||
/// <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();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 开始播放动画
|
||
/// </summary>
|
||
public void StartAnimation()
|
||
{
|
||
try
|
||
{
|
||
if (_animatedObject == null || _pathPoints.Count < 2)
|
||
{
|
||
throw new InvalidOperationException("请先调用SetupAnimation设置动画参数");
|
||
}
|
||
|
||
// 🔥 关键：动画开始前确保物体在起点位置和朝向
|
||
// 动画结束时物体在终点，重新播放前必须重置
|
||
if (_animationFrames != null && _animationFrames.Count > 0)
|
||
{
|
||
var firstFrame = _animationFrames[0];
|
||
|
||
double distanceToStart;
|
||
bool needsReset;
|
||
string mismatchSummary;
|
||
|
||
if (firstFrame.HasCustomRotation)
|
||
{
|
||
var dx = _trackedPosition.X - firstFrame.Position.X;
|
||
var dy = _trackedPosition.Y - firstFrame.Position.Y;
|
||
var dz = _trackedPosition.Z - firstFrame.Position.Z;
|
||
distanceToStart = Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
|
||
var currentLinear = new Transform3D(_trackedRotation).Linear;
|
||
var targetLinear = new Transform3D(firstFrame.Rotation).Linear;
|
||
double rotationDiff =
|
||
Math.Abs(currentLinear.Get(0, 0) - targetLinear.Get(0, 0)) +
|
||
Math.Abs(currentLinear.Get(0, 1) - targetLinear.Get(0, 1)) +
|
||
Math.Abs(currentLinear.Get(0, 2) - targetLinear.Get(0, 2)) +
|
||
Math.Abs(currentLinear.Get(1, 0) - targetLinear.Get(1, 0)) +
|
||
Math.Abs(currentLinear.Get(1, 1) - targetLinear.Get(1, 1)) +
|
||
Math.Abs(currentLinear.Get(1, 2) - targetLinear.Get(1, 2)) +
|
||
Math.Abs(currentLinear.Get(2, 0) - targetLinear.Get(2, 0)) +
|
||
Math.Abs(currentLinear.Get(2, 1) - targetLinear.Get(2, 1)) +
|
||
Math.Abs(currentLinear.Get(2, 2) - targetLinear.Get(2, 2));
|
||
|
||
needsReset = distanceToStart > 0.1 || rotationDiff > 0.01;
|
||
mismatchSummary = $"距离={distanceToStart:F2}m, 三维姿态差={rotationDiff:F4}";
|
||
}
|
||
else
|
||
{
|
||
var currentPos = _animatedObject.BoundingBox().Center;
|
||
distanceToStart = Math.Sqrt(
|
||
Math.Pow(currentPos.X - firstFrame.Position.X, 2) +
|
||
Math.Pow(currentPos.Y - firstFrame.Position.Y, 2)
|
||
);
|
||
|
||
var yawDiff = Math.Abs(_currentYaw - ResolveAnimationFramePlaybackYawRadians(firstFrame));
|
||
if (yawDiff > Math.PI) yawDiff = 2 * Math.PI - yawDiff;
|
||
|
||
needsReset = distanceToStart > 0.1 || yawDiff > 0.01;
|
||
mismatchSummary = $"距离={distanceToStart:F2}m, 角度差={yawDiff * 180 / Math.PI:F2}°";
|
||
}
|
||
|
||
if (needsReset)
|
||
{
|
||
LogManager.Info($"[动画开始] 物体不在起点，重置到起点: {mismatchSummary}");
|
||
MoveObjectToPathStartUsingCurrentPlacementMode();
|
||
}
|
||
}
|
||
|
||
// 创建 TimeLiner 任务（如果可用且未创建过）
|
||
if (_timeLinerManager != null && _timeLinerManager.IsTimeLinerAvailable)
|
||
{
|
||
string objectName = "UnknownObject";
|
||
try
|
||
{
|
||
objectName = _animatedObject?.DisplayName ?? "UnknownObject";
|
||
}
|
||
catch (Exception)
|
||
{
|
||
objectName = "DisposedObject";
|
||
}
|
||
|
||
// 使用动画配置hash作为任务标识
|
||
var hashPrefix = _currentAnimationHash.Substring(0, Math.Min(8, _currentAnimationHash.Length));
|
||
var taskName = $"{objectName}_运输_{hashPrefix}";
|
||
|
||
// 检查是否已存在相同配置的任务
|
||
var existingTaskId = _timeLinerManager.FindTaskByName(taskName);
|
||
if (!string.IsNullOrEmpty(existingTaskId))
|
||
{
|
||
_currentTaskId = existingTaskId;
|
||
LogManager.Info($"复用现有 TimeLiner 任务: {taskName} (ID: {_currentTaskId})");
|
||
}
|
||
else
|
||
{
|
||
// 创建新任务
|
||
var duration = TimeSpan.FromSeconds(_animationDuration);
|
||
LogManager.Info($"创建 TimeLiner 任务: {taskName}");
|
||
|
||
_currentTaskId = _timeLinerManager.CreateTransportTask(
|
||
taskName,
|
||
_pathPoints,
|
||
duration,
|
||
_animatedObject);
|
||
|
||
if (!string.IsNullOrEmpty(_currentTaskId))
|
||
{
|
||
LogManager.Info($"✓ TimeLiner 任务创建成功: {taskName} (ID: {_currentTaskId})");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Warning("TimeLiner 任务创建失败，继续使用基础动画功能");
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info("TimeLiner 不可用，使用基础动画功能");
|
||
}
|
||
|
||
// 初始化动画状态
|
||
_animationStartTime = DateTime.Now;
|
||
_currentFrameIndex = 0; // 重置当前帧索引，确保从第一帧开始
|
||
_pausedProgress = 0.0; // 重置暂停进度
|
||
|
||
// 初始化动画状态时不再强行覆盖 _currentYaw 为第一帧的 yaw，
|
||
// 由记录姿态应用链自行解释第一帧姿态。
|
||
|
||
if (_animationFrames != null && _animationFrames.Count > 0)
|
||
{
|
||
var firstFrame = _animationFrames[0];
|
||
|
||
LogManager.Debug($"[动画开始] _currentYaw之前={_currentYaw * 180 / Math.PI:F2}度, 模式={_animatedObjectMode}");
|
||
|
||
// 🔥 确保物体在起点位置和朝向
|
||
// 注意：MoveObjectToPathStart 已在 StartAnimation 中调用，这里只是日志记录
|
||
LogManager.Debug(
|
||
$"[动画开始] 物体应在起点: pos=({firstFrame.Position.X:F2},{firstFrame.Position.Y:F2}), " +
|
||
$"yaw={ResolveAnimationFramePlaybackYawRadians(firstFrame):F3}rad");
|
||
}
|
||
else
|
||
{
|
||
_currentYaw = 0.0;
|
||
}
|
||
|
||
// 重置动画状态
|
||
_lastFrameTime = DateTime.MinValue;
|
||
_fpsFrameCount = 0;
|
||
_fpsCounterStart = DateTime.Now;
|
||
_frameInterval = 1000.0 / _animationFrameRate;
|
||
_lastHighlightState = false; // 重置高亮状态
|
||
_lastCollisionObjects.Clear(); // 重置碰撞对象集合
|
||
|
||
// 清除所有碰撞高亮
|
||
ModelHighlightHelper.ClearCollisionHighlights();
|
||
LogManager.Debug("[动画开始] 已清除所有碰撞高亮");
|
||
|
||
// 不再高亮物体对象，保持原始外观（客户要求）
|
||
// if (_animatedObject != null)
|
||
// {
|
||
// var objectItems = new List<ModelItem> { _animatedObject };
|
||
// ModelHighlightHelper.HighlightItems(ModelHighlightHelper.AnimatedObjectCategory, objectItems);
|
||
// LogManager.Debug("[动画开始] 已高亮物体对象为绿色");
|
||
// }
|
||
|
||
// 启动动画播放
|
||
StartAnimationPlayback();
|
||
|
||
SetState(AnimationState.Playing);
|
||
LogManager.Info($"动画开始播放 - DispatcherTimer模式");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"启动动画失败: {ex.Message}");
|
||
throw;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 启动动画播放（DispatcherTimer）
|
||
/// </summary>
|
||
private void StartAnimationPlayback()
|
||
{
|
||
try
|
||
{
|
||
// 停止之前的播放
|
||
StopAnimationPlayback();
|
||
|
||
// 使用DispatcherTimer模式
|
||
_animationTimer.Interval = TimeSpan.FromMilliseconds(_frameInterval);
|
||
_animationTimer.Start();
|
||
LogManager.Debug("[播放模式] 启动DispatcherTimer模式");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"启动动画播放失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 停止动画播放（停止DispatcherTimer）
|
||
/// </summary>
|
||
private void StopAnimationPlayback()
|
||
{
|
||
try
|
||
{
|
||
// 停止DispatcherTimer
|
||
if (_animationTimer != null)
|
||
{
|
||
_animationTimer.Stop();
|
||
}
|
||
|
||
LogManager.Debug("[播放模式] 已停止DispatcherTimer");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"停止动画播放时出现警告: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// DispatcherTimer事件处理器 - 主要动画机制
|
||
/// </summary>
|
||
private void OnTimerTick(object sender, EventArgs e)
|
||
{
|
||
try
|
||
{
|
||
// 处理动画帧
|
||
ProcessAnimationFrame();
|
||
|
||
// 关键改进：显式请求重绘，解决卡顿
|
||
NavisApplication.ActiveDocument.ActiveView.RequestDelayedRedraw(ViewRedrawRequests.All);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[DispatcherTimer模式] 动画更新异常: {ex.Message}");
|
||
// 发生异常时停止动画
|
||
ShutdownAnimation();
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 停止动画
|
||
/// </summary>
|
||
public void CancelAnimation()
|
||
{
|
||
try
|
||
{
|
||
// 检查动画状态，避免重复停止
|
||
if (_currentState == AnimationState.Stopped || _currentState == AnimationState.Idle)
|
||
{
|
||
LogManager.Debug($"动画已处于{_currentState}状态，跳过重复停止操作");
|
||
return;
|
||
}
|
||
|
||
// 停止播放机制
|
||
StopAnimationPlayback();
|
||
|
||
// 收集整个路径上的所有碰撞对象
|
||
var allCollisionResults = new List<CollisionResult>();
|
||
var uniqueCollidedItems = new HashSet<ModelItem>();
|
||
|
||
foreach (var frame in _animationFrames)
|
||
{
|
||
if (frame.HasCollision)
|
||
{
|
||
foreach (var collision in frame.Collisions)
|
||
{
|
||
// 收集唯一的碰撞对象
|
||
if (!uniqueCollidedItems.Contains(collision.Item2))
|
||
{
|
||
uniqueCollidedItems.Add(collision.Item2);
|
||
allCollisionResults.Add(collision);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
SetState(AnimationState.Stopped);
|
||
_pausedProgress = 0.0; // 重置暂停进度
|
||
_currentFrameIndex = 0; // 重置帧索引
|
||
|
||
// 清除所有高亮（包括物体和碰撞）
|
||
ModelHighlightHelper.ClearAllHighlights();
|
||
LogManager.Info("动画停止：已清除所有高亮");
|
||
|
||
// 动画停止时保持物体在当前位置（不移回起点）
|
||
LogManager.Info("动画已停止，物体保持在当前位置");
|
||
|
||
// 动画停止时不创建碰撞测试汇总，由动画完成事件统一处理
|
||
LogManager.Info("动画停止，等待动画完成事件统一处理碰撞测试...");
|
||
|
||
// 更新 TimeLiner 任务状态
|
||
if (_timeLinerManager != null && !string.IsNullOrEmpty(_currentTaskId))
|
||
{
|
||
_timeLinerManager.UpdateTaskProgress(_currentTaskId, 0.0, AnimationState.Stopped);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"停止动画失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 程序关闭时的动画清理（不执行UI操作）
|
||
/// </summary>
|
||
public void ShutdownAnimation()
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("[PathAnimationManager] 程序关闭，执行动画清理");
|
||
|
||
// 检查动画状态，避免重复停止
|
||
if (_currentState == AnimationState.Stopped || _currentState == AnimationState.Idle)
|
||
{
|
||
LogManager.Debug($"动画已处于{_currentState}状态，跳过关闭清理");
|
||
return;
|
||
}
|
||
|
||
// 停止播放机制（关闭时不记录详细日志）
|
||
try
|
||
{
|
||
if (_animationTimer != null) _animationTimer.Stop();
|
||
}
|
||
catch (Exception stopEx)
|
||
{
|
||
LogManager.Debug($"关闭清理时停止播放机制异常: {stopEx.Message}");
|
||
}
|
||
|
||
// 直接设置状态为停止，不执行任何UI操作
|
||
SetState(AnimationState.Stopped);
|
||
_pausedProgress = 0.0; // 重置暂停进度
|
||
_currentFrameIndex = 0; // 重置帧索引
|
||
|
||
LogManager.Info("[PathAnimationManager] 动画关闭清理完成");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[PathAnimationManager] 关闭动画清理失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 完成动画（当动画自然播放结束时调用）- 优化版本，使用预计算碰撞结果
|
||
/// </summary>
|
||
private void FinishAnimation()
|
||
{
|
||
try
|
||
{
|
||
// 停止播放机制
|
||
StopAnimationPlayback();
|
||
|
||
// 重置暂停进度
|
||
_pausedProgress = 0.0;
|
||
|
||
// 显式落到最后一帧，避免播放结束时视觉上停在倒数第二帧，
|
||
// 但后续碰撞链又按最终姿态重新摆正，造成“结束后又往前窜一点”。
|
||
if (_animationFrames != null && _animationFrames.Count > 0)
|
||
{
|
||
int lastFrameIndex = _animationFrames.Count - 1;
|
||
if (_currentFrameIndex != lastFrameIndex)
|
||
{
|
||
_currentFrameIndex = lastFrameIndex;
|
||
ApplyRecordedPose(_animationFrames[_currentFrameIndex], _currentFrameIndex);
|
||
LogManager.Info("[动画结束] 当前尚未到最后一帧，已补落最终姿态");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info("[动画结束] 当前已在最后一帧，保持真实结束位置，不再重复落位");
|
||
}
|
||
|
||
ProgressChanged?.Invoke(this, 100.0);
|
||
try
|
||
{
|
||
NavisApplication.ActiveDocument?.ActiveView?.RequestDelayedRedraw(ViewRedrawRequests.All);
|
||
}
|
||
catch (Exception redrawEx)
|
||
{
|
||
LogManager.Warning($"动画结束时请求最终重绘失败: {redrawEx.Message}");
|
||
}
|
||
}
|
||
|
||
// 使用预计算的所有碰撞结果进行高亮显示（需要去重以避免重复高亮）
|
||
var allCollisionResults = _allCollisionResults
|
||
.GroupBy(c => new { c.Item1, c.Item2 })
|
||
.Select(g => g.First())
|
||
.ToList();
|
||
|
||
// 动画自然结束时保持物体在最终位置（不移回起点）
|
||
LogManager.Info("动画播放自然结束，物体保持在最终位置");
|
||
LogEndAlignmentDiagnostics();
|
||
|
||
// 清除所有高亮（包括物体和碰撞）
|
||
ModelHighlightHelper.ClearAllHighlights();
|
||
LogManager.Info("动画完成：已清除所有高亮");
|
||
|
||
// 修复：显示正确的总帧数，而不是最后一帧的索引
|
||
var actualTotalFrames = _animationFrames?.Count ?? 0;
|
||
LogManager.Info($"动画播放完成，总帧数: {actualTotalFrames}, 平均FPS: {_actualFPS:F1}");
|
||
|
||
// 🔥 检查是否使用历史记录，如果是则跳过ClashDetective测试
|
||
if (_isUsingHistoryRecord)
|
||
{
|
||
LogManager.Info("[动画完成] 使用历史检测记录，跳过ClashDetective测试");
|
||
}
|
||
else
|
||
{
|
||
// 🔥 执行碰撞检测（重复检查已在保存检测记录时完成，这里直接执行）
|
||
LogManager.Info($"开始创建碰撞测试汇总（基于 {_allCollisionResults.Count} 个预计算碰撞记录）...");
|
||
|
||
// 设置等待光标，避免进度条关闭后的无响应
|
||
var oldCursor = System.Windows.Input.Mouse.OverrideCursor;
|
||
System.Windows.Input.Mouse.OverrideCursor = System.Windows.Input.Cursors.Wait;
|
||
|
||
bool testCompleted = false;
|
||
try
|
||
{
|
||
// 使用用户设置的检测容差（米）
|
||
testCompleted = ClashDetectiveIntegration.Instance.CreateAllAnimationCollisionTests(
|
||
_allCollisionResults,
|
||
_detectionTolerance,
|
||
_currentRouteId,
|
||
_animatedObject,
|
||
IsVirtualObjectMode,
|
||
_animationFrameRate,
|
||
_animationDuration,
|
||
_virtualObjectLength,
|
||
_virtualObjectWidth,
|
||
_virtualObjectHeight,
|
||
_pathPoints,
|
||
_currentDetectionRecordId
|
||
);
|
||
}
|
||
finally
|
||
{
|
||
// 恢复光标
|
||
System.Windows.Input.Mouse.OverrideCursor = oldCursor;
|
||
}
|
||
|
||
if (testCompleted)
|
||
{
|
||
LogManager.Info($"碰撞测试汇总已创建完成");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info("碰撞检测被取消");
|
||
}
|
||
}
|
||
|
||
// 后处理全部完成后再进入 Finished。
|
||
// 否则 UI 会在碰撞汇总仍在运行时启动报告生成，和当前物体恢复链路并发，导致结束后姿态被再次改写。
|
||
SetState(AnimationState.Finished);
|
||
|
||
// 更新 TimeLiner 任务状态
|
||
if (_timeLinerManager != null && !string.IsNullOrEmpty(_currentTaskId))
|
||
{
|
||
_timeLinerManager.UpdateTaskProgress(_currentTaskId, 1.0, AnimationState.Finished);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"完成动画失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 输出路径动画终点对齐诊断。
|
||
/// 只做日志，不改变业务行为，用于确认：
|
||
/// 1. 路径终点锚点
|
||
/// 2. 最后一帧动画跟踪点
|
||
/// 3. 动画结束时的实际跟踪点
|
||
/// 4. 沿路径前进方向的终点偏差
|
||
/// </summary>
|
||
private void LogEndAlignmentDiagnostics()
|
||
{
|
||
try
|
||
{
|
||
if (_route == null || _pathPoints == null || _pathPoints.Count < 2 || _animationFrames == null || _animationFrames.Count == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Point3D terminalAnchorPoint = _pathPoints[_pathPoints.Count - 1];
|
||
Point3D previousAnchorPoint = _pathPoints[_pathPoints.Count - 2];
|
||
var lastFrame = _animationFrames[_animationFrames.Count - 1];
|
||
Point3D finalTrackedPoint = _trackedPosition;
|
||
|
||
Point3D expectedTrackedEndPoint;
|
||
if (_route.PathType == PathType.Rail)
|
||
{
|
||
double objectHeight = GetAnimatedObjectRailNormalExtent(previousAnchorPoint, terminalAnchorPoint, terminalAnchorPoint);
|
||
if (!TryResolveRailPreservedPoseTrackedCenter(terminalAnchorPoint, previousAnchorPoint, terminalAnchorPoint, objectHeight, out expectedTrackedEndPoint))
|
||
{
|
||
expectedTrackedEndPoint = RailPathPoseHelper.ResolveObjectSpaceCenterPosition(
|
||
_route,
|
||
terminalAnchorPoint,
|
||
previousAnchorPoint,
|
||
terminalAnchorPoint,
|
||
objectHeight);
|
||
}
|
||
}
|
||
else if (_route.PathType == PathType.Hoisting)
|
||
{
|
||
expectedTrackedEndPoint = ResolveHoistingTrackedCenter(
|
||
terminalAnchorPoint,
|
||
GetAnimatedObjectGroundContactHeight(),
|
||
0.0);
|
||
}
|
||
else
|
||
{
|
||
expectedTrackedEndPoint = ResolveGroundTrackedCenter(
|
||
terminalAnchorPoint,
|
||
GetAnimatedObjectGroundContactHeight());
|
||
}
|
||
|
||
Vector3D forward = new Vector3D(
|
||
terminalAnchorPoint.X - previousAnchorPoint.X,
|
||
terminalAnchorPoint.Y - previousAnchorPoint.Y,
|
||
terminalAnchorPoint.Z - previousAnchorPoint.Z);
|
||
|
||
double forwardLength = Math.Sqrt(forward.X * forward.X + forward.Y * forward.Y + forward.Z * forward.Z);
|
||
if (forwardLength < 1e-9)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Vector3D normalizedForward = new Vector3D(
|
||
forward.X / forwardLength,
|
||
forward.Y / forwardLength,
|
||
forward.Z / forwardLength);
|
||
|
||
Vector3D frameDelta = new Vector3D(
|
||
lastFrame.Position.X - expectedTrackedEndPoint.X,
|
||
lastFrame.Position.Y - expectedTrackedEndPoint.Y,
|
||
lastFrame.Position.Z - expectedTrackedEndPoint.Z);
|
||
Vector3D finalDelta = new Vector3D(
|
||
finalTrackedPoint.X - expectedTrackedEndPoint.X,
|
||
finalTrackedPoint.Y - expectedTrackedEndPoint.Y,
|
||
finalTrackedPoint.Z - expectedTrackedEndPoint.Z);
|
||
|
||
double frameForwardDelta = Dot(frameDelta, normalizedForward);
|
||
double finalForwardDelta = Dot(finalDelta, normalizedForward);
|
||
|
||
string pathLabel = _route.PathType.GetDisplayName();
|
||
LogManager.Info(
|
||
$"[{pathLabel}终点诊断] 终点锚点=({terminalAnchorPoint.X:F3},{terminalAnchorPoint.Y:F3},{terminalAnchorPoint.Z:F3}), " +
|
||
$"期望跟踪点=({expectedTrackedEndPoint.X:F3},{expectedTrackedEndPoint.Y:F3},{expectedTrackedEndPoint.Z:F3}), " +
|
||
$"最后一帧跟踪点=({lastFrame.Position.X:F3},{lastFrame.Position.Y:F3},{lastFrame.Position.Z:F3}), " +
|
||
$"动画结束跟踪点=({finalTrackedPoint.X:F3},{finalTrackedPoint.Y:F3},{finalTrackedPoint.Z:F3})");
|
||
LogManager.Info(
|
||
$"[{pathLabel}终点诊断] 前进方向=({normalizedForward.X:F4},{normalizedForward.Y:F4},{normalizedForward.Z:F4}), " +
|
||
$"最后一帧偏差=({frameDelta.X:F3},{frameDelta.Y:F3},{frameDelta.Z:F3}), 沿前进轴={frameForwardDelta:F3}, " +
|
||
$"结束偏差=({finalDelta.X:F3},{finalDelta.Y:F3},{finalDelta.Z:F3}), 沿前进轴={finalForwardDelta:F3}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[终点诊断] 输出失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
private static double Dot(Vector3D a, Vector3D b)
|
||
{
|
||
return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 暂停动画
|
||
/// </summary>
|
||
public void PauseAnimation()
|
||
{
|
||
try
|
||
{
|
||
if (_currentState == AnimationState.Playing)
|
||
{
|
||
// 计算并保存当前进度
|
||
double elapsedSeconds = (DateTime.Now - _animationStartTime).TotalSeconds;
|
||
_pausedProgress = Math.Min(elapsedSeconds / _animationDuration, 1.0);
|
||
|
||
// 停止播放机制
|
||
StopAnimationPlayback();
|
||
|
||
SetState(AnimationState.Paused);
|
||
LogManager.Info($"动画已暂停，当前进度: {_pausedProgress:F3}");
|
||
|
||
// 更新 TimeLiner 任务状态
|
||
if (_timeLinerManager != null && !string.IsNullOrEmpty(_currentTaskId))
|
||
{
|
||
_timeLinerManager.UpdateTaskProgress(_currentTaskId, _pausedProgress, AnimationState.Paused);
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"暂停动画失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 恢复动画
|
||
/// </summary>
|
||
public void ResumeAnimation()
|
||
{
|
||
try
|
||
{
|
||
if (_currentState == AnimationState.Paused)
|
||
{
|
||
// 使用保存的暂停进度重新设置动画开始时间
|
||
// 设置开始时间，使得当前时间对应保存的暂停进度
|
||
_animationStartTime = DateTime.Now.AddSeconds(-(_pausedProgress * _animationDuration));
|
||
|
||
// 重新启动播放机制
|
||
_lastFrameTime = DateTime.MinValue; // 确保下一帧立即执行
|
||
StartAnimationPlayback();
|
||
|
||
SetState(AnimationState.Playing);
|
||
LogManager.Info($"动画已恢复，从进度 {_pausedProgress:F3} 继续播放");
|
||
|
||
// 更新 TimeLiner 任务状态
|
||
if (_timeLinerManager != null && !string.IsNullOrEmpty(_currentTaskId))
|
||
{
|
||
_timeLinerManager.UpdateTaskProgress(_currentTaskId, _pausedProgress, AnimationState.Playing);
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"恢复动画失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 重置动画对象到原始位置
|
||
/// </summary>
|
||
public void ResetAnimation()
|
||
{
|
||
try
|
||
{
|
||
// 只在需要时停止动画，避免重复调用
|
||
if (_currentState == AnimationState.Playing || _currentState == AnimationState.Paused)
|
||
{
|
||
CancelAnimation(); // 停止当前动画
|
||
}
|
||
|
||
// 恢复对象的原始变换（安全检查Navisworks对象可用性）
|
||
if (_animatedObject != null)
|
||
{
|
||
try
|
||
{
|
||
// 检查Navisworks应用程序和文档是否仍然可用
|
||
var activeDoc = NavisApplication.ActiveDocument;
|
||
if (activeDoc != null && activeDoc.Models != null)
|
||
{
|
||
// 第一步：仅仅彻底归位到 CAD 原始位置（不涉及路径起点），这是“清除”按钮的逻辑
|
||
RestoreObjectToCADPosition();
|
||
|
||
// 第二步：如果已经生成了动画路径，则重新对齐到起点（带旋转）
|
||
if (_animationFrames != null && _animationFrames.Count > 0)
|
||
{
|
||
MoveObjectToPathStart();
|
||
LogManager.Info("已重新对齐到路径起点");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info("Navisworks文档已不可用，跳回空闲状态");
|
||
}
|
||
}
|
||
catch (Exception resetEx)
|
||
{
|
||
LogManager.Warning($"重置对象位置时出现警告: {resetEx.Message}");
|
||
// 不再抛出异常，因为在资源清理阶段这是正常的
|
||
}
|
||
}
|
||
|
||
// 重置UI状态（这些操作不依赖Navisworks对象）
|
||
try
|
||
{
|
||
ProgressChanged?.Invoke(this, 0); // 重置进度条
|
||
SetState(AnimationState.Ready); // 重置后回到就绪状态
|
||
}
|
||
catch (Exception uiEx)
|
||
{
|
||
LogManager.Warning($"重置UI状态时出现警告: {uiEx.Message}");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"重置动画过程中发生异常: {ex.Message}");
|
||
// 在资源清理阶段不再抛出异常，避免影响程序正常关闭
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 安全获取ModelItem的DisplayName，避免访问已释放对象的警告
|
||
/// </summary>
|
||
/// <param name="item">ModelItem对象</param>
|
||
/// <returns>安全的显示名称</returns>
|
||
private string GetSafeObjectName(ModelItem item)
|
||
{
|
||
try
|
||
{
|
||
if (item == null)
|
||
{
|
||
return "NULL";
|
||
}
|
||
|
||
// 尝试访问DisplayName，如果对象已被释放会抛出异常
|
||
return item.DisplayName ?? "未命名对象";
|
||
}
|
||
catch (System.ObjectDisposedException)
|
||
{
|
||
return "已释放对象";
|
||
}
|
||
catch (System.Runtime.InteropServices.COMException)
|
||
{
|
||
return "COM对象已释放";
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
return $"访问失败({ex.GetType().Name})";
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 根据进度插值计算当前位置
|
||
/// </summary>
|
||
private Point3D InterpolatePosition(double progress)
|
||
{
|
||
if (_pathPoints.Count < 2)
|
||
return _pathPoints[0];
|
||
|
||
// 确保进度在0-1范围内
|
||
progress = Math.Max(0.0, Math.Min(1.0, progress));
|
||
|
||
// 如果进度达到100%，直接返回终点
|
||
if (progress >= 1.0)
|
||
{
|
||
return _pathPoints[_pathPoints.Count - 1];
|
||
}
|
||
|
||
// 计算总路径长度
|
||
var totalDistance = CalculateTotalPathDistance();
|
||
|
||
// 检查总距离是否有效
|
||
if (totalDistance <= 0.0 || double.IsNaN(totalDistance) || double.IsInfinity(totalDistance))
|
||
{
|
||
LogManager.Error($"路径总长度无效: {totalDistance}，返回起点坐标");
|
||
return _pathPoints[0];
|
||
}
|
||
|
||
var targetDistance = totalDistance * progress;
|
||
|
||
// 找到当前应该在哪两个点之间
|
||
var accumulatedDistance = 0.0;
|
||
for (int i = 0; i < _pathPoints.Count - 1; i++)
|
||
{
|
||
var segmentDistance = CalculateDistance(_pathPoints[i], _pathPoints[i + 1]);
|
||
|
||
// 检查段距离是否有效
|
||
if (double.IsNaN(segmentDistance) || double.IsInfinity(segmentDistance))
|
||
{
|
||
LogManager.Error($"路径段[{i}-{i + 1}]距离无效: {segmentDistance}，跳过此段");
|
||
continue;
|
||
}
|
||
|
||
if (accumulatedDistance + segmentDistance > targetDistance || i == _pathPoints.Count - 2)
|
||
{
|
||
// 在这个线段内，或者是最后一个线段
|
||
var segmentProgress = segmentDistance > 0 ? (targetDistance - accumulatedDistance) / segmentDistance : 0.0;
|
||
|
||
// 确保段内进度在0-1范围内
|
||
segmentProgress = Math.Max(0.0, Math.Min(1.0, segmentProgress));
|
||
|
||
var interpolatedPoint = InterpolatePoints(_pathPoints[i], _pathPoints[i + 1], segmentProgress);
|
||
|
||
// 验证插值结果
|
||
if (double.IsNaN(interpolatedPoint.X) || double.IsNaN(interpolatedPoint.Y) || double.IsNaN(interpolatedPoint.Z))
|
||
{
|
||
LogManager.Error($"插值计算结果无效: ({interpolatedPoint.X},{interpolatedPoint.Y},{interpolatedPoint.Z})，返回起点");
|
||
return _pathPoints[0];
|
||
}
|
||
|
||
return interpolatedPoint;
|
||
}
|
||
|
||
accumulatedDistance += segmentDistance;
|
||
}
|
||
|
||
// 如果到达这里，返回最后一个点
|
||
return _pathPoints[_pathPoints.Count - 1];
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在两点间插值
|
||
/// </summary>
|
||
private Point3D InterpolatePoints(Point3D point1, Point3D point2, double t)
|
||
{
|
||
return new Point3D(
|
||
point1.X + (point2.X - point1.X) * t,
|
||
point1.Y + (point2.Y - point1.Y) * t,
|
||
point1.Z + (point2.Z - point1.Z) * t
|
||
);
|
||
}
|
||
|
||
/// <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>
|
||
/// <param name="targetDistance">目标距离</param>
|
||
/// <param name="segmentLengths">线段长度列表</param>
|
||
/// <returns>线段索引</returns>
|
||
private int FindSegmentForDistance(double targetDistance, List<double> segmentLengths)
|
||
{
|
||
double accumulatedLength = 0.0;
|
||
|
||
for (int i = 0; i < segmentLengths.Count; i++)
|
||
{
|
||
if (accumulatedLength + segmentLengths[i] >= targetDistance)
|
||
{
|
||
return i;
|
||
}
|
||
accumulatedLength += segmentLengths[i];
|
||
}
|
||
|
||
return segmentLengths.Count - 1;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算路径总长度
|
||
/// </summary>
|
||
private double CalculateTotalPathDistance()
|
||
{
|
||
var totalDistance = 0.0;
|
||
for (int i = 0; i < _pathPoints.Count - 1; i++)
|
||
{
|
||
totalDistance += CalculateDistance(_pathPoints[i], _pathPoints[i + 1]);
|
||
}
|
||
return totalDistance;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取对象当前位置
|
||
/// </summary>
|
||
private Point3D GetObjectPosition(ModelItem item)
|
||
{
|
||
try
|
||
{
|
||
if (item == null) return new Point3D(0, 0, 0);
|
||
|
||
var bounds = item.BoundingBox();
|
||
if (bounds != null)
|
||
{
|
||
return new Point3D(
|
||
(bounds.Min.X + bounds.Max.X) / 2,
|
||
(bounds.Min.Y + bounds.Max.Y) / 2,
|
||
(bounds.Min.Z + bounds.Max.Z) / 2
|
||
);
|
||
}
|
||
return new Point3D(0, 0, 0);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"获取对象位置失败: {ex.Message}");
|
||
return new Point3D(0, 0, 0);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算两点间距离
|
||
/// </summary>
|
||
private double CalculateDistance(Point3D point1, Point3D point2)
|
||
{
|
||
var dx = point2.X - point1.X;
|
||
var dy = point2.Y - point1.Y;
|
||
var dz = point2.Z - point1.Z;
|
||
return Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 仅按业务跟踪点执行平移，不改变当前姿态语义。
|
||
/// PreserveInitialPose 起点搬运等链路应显式使用此入口，而不是借用旧 yaw 链。
|
||
/// </summary>
|
||
private void ApplyTrackedTranslationOnly(Point3D newPosition)
|
||
{
|
||
try
|
||
{
|
||
var doc = NavisApplication.ActiveDocument;
|
||
var modelItems = new ModelItemCollection { _animatedObject };
|
||
|
||
// 计算平移和旋转的增量
|
||
var deltaPos = new Vector3D(
|
||
newPosition.X - _trackedPosition.X,
|
||
newPosition.Y - _trackedPosition.Y,
|
||
newPosition.Z - _trackedPosition.Z
|
||
);
|
||
|
||
Transform3D incrementalTransform = Transform3D.CreateTranslation(deltaPos);
|
||
LogManager.Debug($"[TrackedTranslation] 纯平移: ({deltaPos.X:F2},{deltaPos.Y:F2},{deltaPos.Z:F2})");
|
||
|
||
// 应用增量变换（false = 增量模式）
|
||
doc.Models.OverridePermanentTransform(modelItems, incrementalTransform, false);
|
||
|
||
// 更新跟踪位置
|
||
_trackedPosition = newPosition;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"更新部件位置失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用完整三维姿态更新对象位置。
|
||
/// 当前先用于 Rail 路径，避免影响现有地面/吊装路径的 yaw 流程。
|
||
/// </summary>
|
||
private void ApplyFullPoseUpdate(Point3D newPosition, Rotation3D newRotation)
|
||
{
|
||
try
|
||
{
|
||
ModelItem controlledObject = CurrentControlledObject;
|
||
bool isRailRealObject = _route?.PathType == PathType.Rail && IsRealObjectMode && controlledObject != null;
|
||
if (controlledObject == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (IsVirtualObjectMode)
|
||
{
|
||
VirtualObjectManager.Instance.MoveVirtualObject(newPosition, newRotation);
|
||
_trackedPosition = newPosition;
|
||
_trackedRotation = newRotation;
|
||
_hasTrackedRotation = true;
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromRotation(newRotation);
|
||
|
||
if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting)
|
||
{
|
||
LogHostActualPoseAxes(CurrentControlledObject, "[平面姿态应用后宿主姿态]", false);
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
Point3D currentPositionForTransform = _trackedPosition;
|
||
Rotation3D currentRotation;
|
||
Rotation3D appliedTargetRotation = newRotation;
|
||
Point3D appliedTargetPosition = newPosition;
|
||
if (_hasTrackedRotation)
|
||
{
|
||
currentRotation = _trackedRotation;
|
||
}
|
||
else if (IsRealObjectMode && _hasRealObjectReferenceRotation)
|
||
{
|
||
currentRotation = CoordinateSystemManager.Instance
|
||
.CreateHostAdapter()
|
||
.FromHostQuaternionDirect(_realObjectReferenceRotation);
|
||
}
|
||
else
|
||
{
|
||
currentRotation = controlledObject.Transform.Factor().Rotation;
|
||
}
|
||
|
||
try
|
||
{
|
||
var actualHostPosition = GetLiveBoundingBoxCenter(controlledObject);
|
||
if (!(IsRealObjectMode && _route?.PathType == PathType.Ground))
|
||
{
|
||
currentPositionForTransform = actualHostPosition;
|
||
}
|
||
LogManager.Info(
|
||
$"[动画姿态入口] {controlledObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})");
|
||
|
||
if (IsRealObjectMode &&
|
||
(_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) &&
|
||
ModelItemTransformHelper.TryGetCurrentGeometryRotation(controlledObject, out var actualGeometryRotation))
|
||
{
|
||
var trackedLinear = new Transform3D(currentRotation).Linear;
|
||
var actualLinear = new Transform3D(actualGeometryRotation).Linear;
|
||
LogManager.Info(
|
||
$"[平面姿态基线诊断] {controlledObject.DisplayName} 跟踪旋转: " +
|
||
$"X=({trackedLinear.Get(0, 0):F4},{trackedLinear.Get(1, 0):F4},{trackedLinear.Get(2, 0):F4}), " +
|
||
$"Y=({trackedLinear.Get(0, 1):F4},{trackedLinear.Get(1, 1):F4},{trackedLinear.Get(2, 1):F4}), " +
|
||
$"Z=({trackedLinear.Get(0, 2):F4},{trackedLinear.Get(1, 2):F4},{trackedLinear.Get(2, 2):F4})");
|
||
LogManager.Info(
|
||
$"[平面姿态基线诊断] {controlledObject.DisplayName} 实际几何旋转: " +
|
||
$"X=({actualLinear.Get(0, 0):F4},{actualLinear.Get(1, 0):F4},{actualLinear.Get(2, 0):F4}), " +
|
||
$"Y=({actualLinear.Get(0, 1):F4},{actualLinear.Get(1, 1):F4},{actualLinear.Get(2, 1):F4}), " +
|
||
$"Z=({actualLinear.Get(0, 2):F4},{actualLinear.Get(1, 2):F4},{actualLinear.Get(2, 2):F4})");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[动画姿态入口] 读取宿主实际状态失败: {ex.Message}");
|
||
}
|
||
|
||
var currentLinear = new Transform3D(currentRotation).Linear;
|
||
var targetLinear = new Transform3D(appliedTargetRotation).Linear;
|
||
LogManager.Info(
|
||
$"[动画姿态入口] {controlledObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
|
||
$"目标点=({appliedTargetPosition.X:F3},{appliedTargetPosition.Y:F3},{appliedTargetPosition.Z:F3})");
|
||
LogManager.Info(
|
||
$"[动画姿态入口] {controlledObject.DisplayName} 跟踪姿态: " +
|
||
$"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " +
|
||
$"Y=({currentLinear.Get(0, 1):F4},{currentLinear.Get(1, 1):F4},{currentLinear.Get(2, 1):F4}), " +
|
||
$"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})");
|
||
LogManager.Info(
|
||
$"[动画姿态入口] {controlledObject.DisplayName} 目标姿态: " +
|
||
$"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " +
|
||
$"Y=({targetLinear.Get(0, 1):F4},{targetLinear.Get(1, 1):F4},{targetLinear.Get(2, 1):F4}), " +
|
||
$"Z=({targetLinear.Get(0, 2):F4},{targetLinear.Get(1, 2):F4},{targetLinear.Get(2, 2):F4})");
|
||
|
||
ModelItemTransformHelper.MoveItemIncrementallyToPositionAndRotation(
|
||
controlledObject,
|
||
currentPositionForTransform,
|
||
currentRotation,
|
||
appliedTargetPosition,
|
||
appliedTargetRotation);
|
||
|
||
_trackedPosition = appliedTargetPosition;
|
||
_trackedRotation = appliedTargetRotation;
|
||
_hasTrackedRotation = true;
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromRotation(appliedTargetRotation);
|
||
|
||
if (isRailRealObject)
|
||
{
|
||
Point3D hostActualAfter = GetLiveBoundingBoxCenter(controlledObject);
|
||
LogManager.Debug(
|
||
$"[Rail姿态应用] 宿主最终跟踪中心=({hostActualAfter.X:F3},{hostActualAfter.Y:F3},{hostActualAfter.Z:F3}), " +
|
||
$"目标跟踪中心=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3}), " +
|
||
$"偏差=({hostActualAfter.X - newPosition.X:F3},{hostActualAfter.Y - newPosition.Y:F3},{hostActualAfter.Z - newPosition.Z:F3})");
|
||
}
|
||
else if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting)
|
||
{
|
||
LogHostActualPoseAxes(CurrentControlledObject, "[平面姿态应用后宿主姿态]", false);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"使用完整姿态更新对象位置失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取文档单位信息（用于日志记录）
|
||
/// </summary>
|
||
private string GetDocumentUnitsInfo()
|
||
{
|
||
try
|
||
{
|
||
var doc = NavisApplication.ActiveDocument;
|
||
var units = doc.Units;
|
||
LogManager.Debug($"[单位检测] 文档单位: {units}");
|
||
return units.ToString();
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"获取文档单位信息失败: {ex.Message}");
|
||
return "Unknown";
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取模型当前变换
|
||
/// </summary>
|
||
private Transform3D GetCurrentTransform(ModelItem item)
|
||
{
|
||
// 获取包围盒中心作为参考点
|
||
var boundingBox = item.BoundingBox();
|
||
var center = boundingBox.Center;
|
||
|
||
// 创建基于中心点的单位变换
|
||
return Transform3D.CreateTranslation(new Vector3D(center.X, center.Y, center.Z));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置动画持续时间
|
||
/// </summary>
|
||
public void SetAnimationDuration(double durationSeconds)
|
||
{
|
||
_animationDuration = Math.Max(1.0, durationSeconds);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取动画状态
|
||
/// </summary>
|
||
public bool IsAnimating => _currentState == AnimationState.Playing;
|
||
|
||
/// <summary>
|
||
/// 获取 TimeLiner 是否可用
|
||
/// </summary>
|
||
public bool IsTimeLinerAvailable => _timeLinerManager?.IsTimeLinerAvailable ?? false;
|
||
|
||
/// <summary>
|
||
/// 获取当前动画状态
|
||
/// </summary>
|
||
public AnimationState CurrentState => _currentState;
|
||
|
||
/// <summary>
|
||
/// 获取当前 TimeLiner 任务ID
|
||
/// </summary>
|
||
public string CurrentTaskId => _currentTaskId;
|
||
|
||
/// <summary>
|
||
/// 获取 TimeLiner 集成管理器
|
||
/// </summary>
|
||
public TimeLinerIntegrationManager TimeLinerManager => _timeLinerManager;
|
||
|
||
#region 帧控制功能
|
||
|
||
/// <summary>
|
||
/// 获取当前帧索引
|
||
/// </summary>
|
||
public int CurrentFrame => _currentFrameIndex;
|
||
|
||
/// <summary>
|
||
/// 获取总帧数
|
||
/// </summary>
|
||
public int TotalFrames => _animationFrames?.Count ?? 0;
|
||
|
||
/// <summary>
|
||
/// 获取物体当前位置和朝向
|
||
/// 🔥 关键：返回实际当前朝向（_currentYaw），而不是 Transform 的 CAD 原始朝向
|
||
/// </summary>
|
||
public (Point3D Position, double Yaw) GetObjectCurrentPosition(ModelItem obj)
|
||
{
|
||
if (obj == null)
|
||
{
|
||
LogManager.Warning("GetObjectCurrentPosition: 对象为空");
|
||
return (null, 0);
|
||
}
|
||
|
||
var position = _trackedPosition;
|
||
// 🔥 关键：使用 _currentYaw（实际当前朝向），而不是 Transform 的 CAD 原始朝向
|
||
// Transform 返回的是 CAD 设计时的原始朝向，不是动画后的实际朝向
|
||
return (position, _currentYaw);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存物体当前位置和朝向
|
||
/// </summary>
|
||
public void SaveObjectState(ModelItem obj)
|
||
{
|
||
if (obj == null)
|
||
{
|
||
LogManager.Warning("SaveObjectState: 对象为空");
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
var (position, yaw) = GetObjectCurrentPosition(obj);
|
||
_savedObjectPosition = position;
|
||
_savedObjectYaw = yaw;
|
||
_savedObjectRotation = _trackedRotation;
|
||
_savedObjectHasCustomRotation = _hasTrackedRotation;
|
||
_hasSavedObjectState = true;
|
||
LogManager.Info($"已保存物体状态: pos=({position.X:F2},{position.Y:F2},{position.Z:F2}), yaw={yaw * 180 / Math.PI:F2}度, customRotation={_savedObjectHasCustomRotation}");
|
||
if (_savedObjectHasCustomRotation)
|
||
{
|
||
var linear = new Transform3D(_savedObjectRotation).Linear;
|
||
LogManager.Info(
|
||
$"[PAM保存姿态] {obj.DisplayName} 保存目标姿态: " +
|
||
$"位置=({_savedObjectPosition.X:F3},{_savedObjectPosition.Y:F3},{_savedObjectPosition.Z:F3}), " +
|
||
$"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " +
|
||
$"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " +
|
||
$"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"SaveObjectState: 保存物体状态失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 恢复动画物体到保存的位置和朝向
|
||
/// 此方法会更新 PathAnimationManager 的内部状态（_currentYaw等）
|
||
/// </summary>
|
||
public void RestoreAnimatedObjectState(ModelItem obj)
|
||
{
|
||
if (obj == null)
|
||
{
|
||
LogManager.Warning("RestoreAnimatedObjectState: 对象为空");
|
||
return;
|
||
}
|
||
|
||
if (!_hasSavedObjectState)
|
||
{
|
||
LogManager.Warning("RestoreAnimatedObjectState: 没有保存的物体状态");
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
var originalAnimatedObject = _animatedObject;
|
||
_animatedObject = obj;
|
||
|
||
LogHostActualPoseAxes(obj, "[PAM恢复前宿主姿态]", false);
|
||
|
||
if (_savedObjectHasCustomRotation)
|
||
{
|
||
var linear = new Transform3D(_savedObjectRotation).Linear;
|
||
LogManager.Debug(
|
||
$"[PAM恢复姿态] {obj.DisplayName} 恢复目标姿态: " +
|
||
$"位置=({_savedObjectPosition.X:F3},{_savedObjectPosition.Y:F3},{_savedObjectPosition.Z:F3}), " +
|
||
$"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " +
|
||
$"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " +
|
||
$"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})");
|
||
}
|
||
|
||
ApplyResolvedObjectPose(
|
||
_savedObjectPosition,
|
||
_savedObjectRotation,
|
||
_savedObjectHasCustomRotation,
|
||
-1,
|
||
planarHostYawRadians: _savedObjectYaw);
|
||
|
||
LogHostActualPoseAxes(obj, "[PAM恢复后宿主姿态]", false);
|
||
|
||
_animatedObject = originalAnimatedObject;
|
||
LogManager.Debug($"已恢复物体状态: pos=({_savedObjectPosition.X:F2},{_savedObjectPosition.Y:F2},{_savedObjectPosition.Z:F2}), yaw={_savedObjectYaw * 180 / Math.PI:F2}度, customRotation={_savedObjectHasCustomRotation}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"RestoreAnimatedObjectState: 恢复动画物体状态失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断给定对象是否由当前动画管理器控制。
|
||
/// 对虚拟物体，判断当前激活的虚拟物体；对真实模型，判断当前动画对象引用。
|
||
/// </summary>
|
||
public bool ControlsAnimatedObject(ModelItem obj)
|
||
{
|
||
if (obj == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (IsVirtualObjectMode)
|
||
{
|
||
return VirtualObjectManager.Instance.IsVirtualObjectActive &&
|
||
ReferenceEquals(VirtualObjectManager.Instance.CurrentVirtualObject, obj);
|
||
}
|
||
|
||
return ReferenceEquals(_animatedObject, obj);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用当前动画链路将动画对象移动到指定姿态。
|
||
/// 适用于碰撞点定位、报告截图等场景，确保与动画播放使用同一套位置/姿态应用语义。
|
||
/// </summary>
|
||
public void MoveAnimatedObjectToPose(
|
||
ModelItem obj,
|
||
Point3D targetPosition,
|
||
double targetYawRadians,
|
||
Rotation3D targetRotation,
|
||
bool hasCustomRotation)
|
||
{
|
||
if (obj == null)
|
||
{
|
||
LogManager.Warning("MoveAnimatedObjectToPose: 对象为空");
|
||
return;
|
||
}
|
||
|
||
if (!ControlsAnimatedObject(obj))
|
||
{
|
||
LogManager.Warning($"MoveAnimatedObjectToPose: 对象不受当前动画管理器控制: {obj.DisplayName}");
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
var originalAnimatedObject = _animatedObject;
|
||
if (!IsVirtualObjectMode)
|
||
{
|
||
_animatedObject = obj;
|
||
}
|
||
|
||
LogHostActualPoseAxes(obj, "[动画姿态复用前宿主姿态]", false);
|
||
|
||
ApplyResolvedObjectPose(
|
||
targetPosition,
|
||
targetRotation,
|
||
hasCustomRotation,
|
||
-1,
|
||
planarHostYawRadians: targetYawRadians);
|
||
|
||
LogHostActualPoseAxes(obj, "[动画姿态复用后宿主姿态]", false);
|
||
|
||
_animatedObject = originalAnimatedObject;
|
||
|
||
LogManager.Debug(
|
||
$"[动画姿态复用] {obj.DisplayName} 已移动到目标姿态: " +
|
||
$"位置=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), customRotation={hasCustomRotation}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"MoveAnimatedObjectToPose: 应用目标姿态失败: {ex.Message}", ex);
|
||
throw;
|
||
}
|
||
}
|
||
|
||
private void LogHostActualPoseAxes(ModelItem obj, string prefix, bool important)
|
||
{
|
||
if (obj == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
Transform3D transform = obj.Transform;
|
||
Matrix3 linear = transform.Linear;
|
||
string message =
|
||
$"{prefix} {obj.DisplayName}: " +
|
||
$"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " +
|
||
$"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " +
|
||
$"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})";
|
||
if (important)
|
||
{
|
||
LogManager.Info(message);
|
||
}
|
||
else
|
||
{
|
||
LogManager.Debug(message);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"{prefix} 读取失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取播放方向（1=正向，-1=反向）
|
||
/// </summary>
|
||
public int PlaybackDirection => _playbackDirection;
|
||
|
||
/// <summary>
|
||
/// 获取播放速度倍率
|
||
/// </summary>
|
||
public double PlaybackSpeed => _playbackSpeed;
|
||
|
||
/// <summary>
|
||
/// 获取是否正在反向播放
|
||
/// </summary>
|
||
public bool IsPlayingReverse => _playbackDirection == -1;
|
||
|
||
/// <summary>
|
||
/// 设置播放方向
|
||
/// </summary>
|
||
/// <param name="reverse">true=反向播放，false=正向播放</param>
|
||
public void SetPlaybackDirection(bool reverse)
|
||
{
|
||
_playbackDirection = reverse ? -1 : 1;
|
||
LogManager.Info($"播放方向设置为: {(reverse ? "反向" : "正向")}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置播放速度
|
||
/// </summary>
|
||
/// <param name="speed">播放速度倍率，负值表示反向</param>
|
||
public void SetPlaybackSpeed(double speed)
|
||
{
|
||
_playbackSpeed = Math.Abs(speed);
|
||
_playbackDirection = speed >= 0 ? 1 : -1;
|
||
|
||
// 更新帧间隔
|
||
_frameInterval = 1000.0 / (_animationFrameRate * _playbackSpeed);
|
||
LogManager.Info($"播放速度设置为: {speed}x，方向: {(speed >= 0 ? "正向" : "反向")}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 跳转到指定帧
|
||
/// </summary>
|
||
/// <param name="frameIndex">目标帧索引</param>
|
||
public void SeekToFrame(int frameIndex)
|
||
{
|
||
try
|
||
{
|
||
if (_animationFrames == null || _animationFrames.Count == 0)
|
||
{
|
||
LogManager.Warning("无动画帧数据，无法跳转");
|
||
return;
|
||
}
|
||
|
||
// 限制帧索引范围
|
||
frameIndex = Math.Max(0, Math.Min(frameIndex, _animationFrames.Count - 1));
|
||
|
||
if (frameIndex == _currentFrameIndex)
|
||
{
|
||
LogManager.Debug($"已处于目标帧 {frameIndex}，无需跳转");
|
||
return;
|
||
}
|
||
|
||
_currentFrameIndex = frameIndex;
|
||
|
||
// 更新对象位置和朝向，复用动画主播放与恢复使用的同一入口。
|
||
var frameData = _animationFrames[_currentFrameIndex];
|
||
ApplyRecordedPose(frameData, _currentFrameIndex);
|
||
|
||
// 更新碰撞高亮
|
||
UpdateCollisionHighlightFromFrame();
|
||
|
||
// 计算并触发进度事件
|
||
double progress = TotalFrames > 1 ? (double)_currentFrameIndex / (TotalFrames - 1) * 100 : 0;
|
||
ProgressChanged?.Invoke(this, progress);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"跳转到帧失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 跳转到指定进度
|
||
/// </summary>
|
||
/// <param name="progress">进度值（0.0-1.0）</param>
|
||
public void SeekToProgress(double progress)
|
||
{
|
||
try
|
||
{
|
||
progress = Math.Max(0.0, Math.Min(1.0, progress));
|
||
int targetFrame = TotalFrames > 1 ? (int)(progress * (TotalFrames - 1)) : 0;
|
||
SeekToFrame(targetFrame);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"跳转到进度失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 单帧前进
|
||
/// </summary>
|
||
public void StepForward()
|
||
{
|
||
StepForward(1);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 前进指定帧数
|
||
/// </summary>
|
||
/// <param name="frames">帧数</param>
|
||
public void StepForward(int frames)
|
||
{
|
||
try
|
||
{
|
||
if (frames <= 0) return;
|
||
|
||
int targetFrame = _currentFrameIndex + frames;
|
||
targetFrame = Math.Min(targetFrame, TotalFrames - 1);
|
||
|
||
SeekToFrame(targetFrame);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"前进步进失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 单帧后退
|
||
/// </summary>
|
||
public void StepBackward()
|
||
{
|
||
StepBackward(1);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 后退指定帧数
|
||
/// </summary>
|
||
/// <param name="frames">帧数</param>
|
||
public void StepBackward(int frames)
|
||
{
|
||
try
|
||
{
|
||
if (frames <= 0) return;
|
||
|
||
int targetFrame = _currentFrameIndex - frames;
|
||
targetFrame = Math.Max(targetFrame, 0);
|
||
|
||
SeekToFrame(targetFrame);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"后退步进失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 快进10帧
|
||
/// </summary>
|
||
public void FastForward()
|
||
{
|
||
StepForward(10);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 快退10帧
|
||
/// </summary>
|
||
public void FastBackward()
|
||
{
|
||
StepBackward(10);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 开始正向播放
|
||
/// </summary>
|
||
public void PlayForward()
|
||
{
|
||
try
|
||
{
|
||
SetPlaybackDirection(false);
|
||
if (_currentState == AnimationState.Paused)
|
||
{
|
||
ResumeAnimation(); // 从暂停位置继续
|
||
}
|
||
else if (_currentState != AnimationState.Playing)
|
||
{
|
||
StartAnimation(); // 只有非播放和非暂停状态才从头开始
|
||
}
|
||
LogManager.Info("开始正向播放");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"正向播放失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 开始反向播放
|
||
/// </summary>
|
||
public void PlayReverse()
|
||
{
|
||
try
|
||
{
|
||
SetPlaybackDirection(true);
|
||
if (_currentState == AnimationState.Paused)
|
||
{
|
||
ResumeAnimation(); // 从暂停位置继续
|
||
}
|
||
else if (_currentState != AnimationState.Playing)
|
||
{
|
||
StartAnimation(); // 只有非播放和非暂停状态才从头开始
|
||
}
|
||
LogManager.Info("开始反向播放");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"反向播放失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
/// <summary>
|
||
/// 清理对象引用
|
||
/// </summary>
|
||
public void ClearReferences()
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("[PathAnimationManager] 清理对象引用");
|
||
|
||
// 停止动画
|
||
if (_currentState == AnimationState.Playing || _currentState == AnimationState.Paused)
|
||
{
|
||
ShutdownAnimation();
|
||
}
|
||
|
||
// 清空对象引用
|
||
_animatedObject = null;
|
||
_pathPoints?.Clear();
|
||
_trackedPosition = new Point3D(0, 0, 0);
|
||
_originalCenter = new Point3D(0, 0, 0);
|
||
|
||
LogManager.Info("[PathAnimationManager] 对象引用已清理");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[PathAnimationManager] 清理对象引用失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 文档失效事件处理
|
||
/// </summary>
|
||
private void OnDocumentInvalidated(object sender, EventArgs e)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("[PathAnimationManager] 文档失效，停止动画并清理");
|
||
ClearReferences();
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[PathAnimationManager] 处理文档失效事件失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 资源清理
|
||
/// </summary>
|
||
public void Dispose()
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("开始清理PathAnimationManager资源");
|
||
|
||
// 0. 取消配置变更事件订阅
|
||
UnsubscribeFromConfigChanges();
|
||
|
||
// 1. 优先停止动画（最小化的操作，不触发复杂的重置逻辑）
|
||
// 只有在动画运行时才停止，避免重复调用
|
||
try
|
||
{
|
||
if (_currentState == AnimationState.Playing || _currentState == AnimationState.Paused)
|
||
{
|
||
ShutdownAnimation();
|
||
}
|
||
}
|
||
catch (Exception stopEx)
|
||
{
|
||
LogManager.Warning($"停止动画时出现警告: {stopEx.Message}，继续清理其他资源");
|
||
}
|
||
|
||
// 2. 在Dispose中不进行重置操作，避免访问已释放的对象
|
||
// 程序关闭时，对象位置重置是不必要的
|
||
LogManager.Info("跳过动画重置操作（程序关闭时不需要恢复对象位置）");
|
||
|
||
// 3. 清理 TimeLiner 资源
|
||
if (_timeLinerManager != null)
|
||
{
|
||
try
|
||
{
|
||
_timeLinerManager.Dispose();
|
||
_timeLinerManager = null;
|
||
LogManager.Info("TimeLiner 集成资源已清理");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"清理 TimeLiner 资源时出现警告: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
// 4. 清理DispatcherTimer资源
|
||
if (_animationTimer != null)
|
||
{
|
||
try
|
||
{
|
||
_animationTimer.Stop();
|
||
// DispatcherTimer不需要Dispose
|
||
_animationTimer = null;
|
||
LogManager.Info("DispatcherTimer资源已清理");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"清理DispatcherTimer资源时出现警告: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
// 5. 取消订阅文档事件
|
||
DocumentStateManager.Instance.DocumentInvalidated -= OnDocumentInvalidated;
|
||
|
||
LogManager.Info("PathAnimationManager资源清理完成");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"PathAnimationManager资源清理过程中发生异常: {ex.Message}");
|
||
// 不抛出异常，避免影响应用程序正常关闭
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置动画帧率
|
||
/// </summary>
|
||
public void SetAnimationFrameRate(int frameRate)
|
||
{
|
||
_animationFrameRate = Math.Max(10, Math.Min(60, frameRate)); // 限制在10-60FPS之间
|
||
_frameInterval = 1000.0 / _animationFrameRate; // 重新计算帧间隔
|
||
|
||
// 动态更新DispatcherTimer间隔
|
||
if (_animationTimer != null)
|
||
{
|
||
_animationTimer.Interval = TimeSpan.FromMilliseconds(_frameInterval);
|
||
LogManager.Info($"DispatcherTimer间隔已更新: {_frameInterval:F1}ms");
|
||
}
|
||
|
||
LogManager.Info($"动画帧率设置为: {_animationFrameRate}FPS (间隔: {_frameInterval:F1}ms)");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置碰撞检测精度（参数单位：米）
|
||
/// </summary>
|
||
public void SetCollisionDetectionAccuracy(double accuracyInMeters)
|
||
{
|
||
// 转换为模型单位存储
|
||
var accuracyInModelUnits = UnitsConverter.ConvertFromMeters(Math.Max(0.01, accuracyInMeters));
|
||
_collisionDetectionAccuracy = accuracyInModelUnits;
|
||
LogManager.Info($"碰撞检测精度设置为: {accuracyInMeters:F3}米");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置运动速度
|
||
/// </summary>
|
||
public void SetMovementSpeed(double speed)
|
||
{
|
||
_movementSpeed = Math.Max(0.1, speed); // 最小0.1米/秒
|
||
LogManager.Info($"运动速度设置为: {_movementSpeed}m/s");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置检测容差（参数单位：米）
|
||
/// </summary>
|
||
public void SetDetectionTolerance(double toleranceInMeters)
|
||
{
|
||
_detectionTolerance = Math.Max(0.0, toleranceInMeters);
|
||
LogManager.Info($"检测容差设置为: {_detectionTolerance:F2}米（仅用于ClashDetective）");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置手工碰撞对象覆写列表
|
||
/// </summary>
|
||
public void SetManualCollisionTargets(IEnumerable<ModelItem> targets, bool enabled)
|
||
{
|
||
if (!enabled)
|
||
{
|
||
_manualCollisionOverrideEnabled = false;
|
||
_manualCollisionTargets = new List<ModelItem>();
|
||
LogManager.Info("[PathAnimationManager] 手工碰撞对象覆写已关闭");
|
||
return;
|
||
}
|
||
|
||
if (targets == null)
|
||
{
|
||
_manualCollisionOverrideEnabled = false;
|
||
_manualCollisionTargets = new List<ModelItem>();
|
||
LogManager.Warning("[PathAnimationManager] 手工碰撞对象覆写启用但目标为空，已回退到默认模式");
|
||
return;
|
||
}
|
||
|
||
var validTargets = targets
|
||
.Where(item => item != null && ModelItemAnalysisHelper.IsModelItemValid(item))
|
||
.Where(item => !ReferenceEquals(item, _animatedObject))
|
||
.Distinct()
|
||
.ToList();
|
||
|
||
if (validTargets.Count == 0)
|
||
{
|
||
_manualCollisionOverrideEnabled = false;
|
||
_manualCollisionTargets = new List<ModelItem>();
|
||
LogManager.Warning("[PathAnimationManager] 手工碰撞对象覆写启用但均无效，已回退到默认模式");
|
||
return;
|
||
}
|
||
|
||
_manualCollisionOverrideEnabled = true;
|
||
_manualCollisionTargets = validTargets;
|
||
LogManager.Info($"[PathAnimationManager] 启用手工碰撞对象覆写，共 {validTargets.Count} 个对象");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取当前动画帧率
|
||
/// </summary>
|
||
public int AnimationFrameRate => _animationFrameRate;
|
||
|
||
/// <summary>
|
||
/// 获取所有碰撞结果（不去重）
|
||
/// </summary>
|
||
public List<CollisionResult> AllCollisionResults => _allCollisionResults;
|
||
|
||
/// <summary>
|
||
/// 获取当前物体的朝向（弧度）
|
||
/// 用于保存物体当前状态（因为Transform返回的是CAD原始值）
|
||
/// </summary>
|
||
public double CurrentYaw => _currentYaw;
|
||
public Rotation3D TrackedRotation => _trackedRotation;
|
||
public bool HasTrackedRotation => _hasTrackedRotation;
|
||
|
||
/// <summary>
|
||
/// 获取当前动画物体高度（模型单位）
|
||
/// </summary>
|
||
private double GetAnimatedObjectHeight()
|
||
{
|
||
if (IsVirtualObjectMode)
|
||
{
|
||
return _virtualObjectHeight;
|
||
}
|
||
|
||
if (_realObjectHeight > 0.0)
|
||
{
|
||
return _realObjectHeight;
|
||
}
|
||
|
||
if (_animatedObject == null)
|
||
{
|
||
throw new InvalidOperationException("动画对象为空，无法获取物体高度");
|
||
}
|
||
|
||
var boundingBox = _animatedObject.BoundingBox();
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
return ModelItemTransformHelper.GetHostHeight(boundingBox, adapter);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取当前姿态下，地面/吊装路径用于“接触点 -> 几何中心”换算的有效法线尺寸（模型单位）。
|
||
/// 与通行空间使用的旋转后法线尺寸保持一致，避免物体旋转后仍按原始高度抬升中心。
|
||
/// </summary>
|
||
private double GetAnimatedObjectGroundContactHeight()
|
||
{
|
||
double rawHeight = GetAnimatedObjectHeight();
|
||
if (_objectRotationCorrection.IsZero)
|
||
{
|
||
return rawHeight;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
ModelAxisConvention axisConvention;
|
||
double forwardSize;
|
||
double sideSize;
|
||
double upSize;
|
||
|
||
if (IsVirtualObjectMode)
|
||
{
|
||
axisConvention = ModelAxisConvention.CreateVirtualObjectAssetConvention();
|
||
forwardSize = _virtualObjectLength;
|
||
sideSize = _virtualObjectWidth;
|
||
upSize = _virtualObjectHeight;
|
||
}
|
||
else if (IsRealObjectMode && _realObjectLength > 0.0 && _realObjectWidth > 0.0 && _realObjectHeight > 0.0)
|
||
{
|
||
axisConvention = ModelAxisConvention.CreateDefaultForHost(adapter.HostType);
|
||
forwardSize = _realObjectLength;
|
||
sideSize = _realObjectWidth;
|
||
upSize = _realObjectHeight;
|
||
}
|
||
else
|
||
{
|
||
return rawHeight;
|
||
}
|
||
|
||
Quaternion correctionQuaternion = IsVirtualObjectMode
|
||
? adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection)
|
||
: adapter.CreateHostRotationCorrection(_objectRotationCorrection);
|
||
var projectedExtents = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||
axisConvention,
|
||
forwardSize,
|
||
sideSize,
|
||
upSize,
|
||
correctionQuaternion);
|
||
|
||
return projectedExtents.upExtent;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取当前碰撞检测精度
|
||
/// </summary>
|
||
public double CollisionDetectionAccuracy => _collisionDetectionAccuracy;
|
||
|
||
/// <summary>
|
||
/// 获取当前运动速度
|
||
/// </summary>
|
||
public double MovementSpeed => _movementSpeed;
|
||
|
||
/// <summary>
|
||
/// 获取当前检测间隙（单位：米）
|
||
/// </summary>
|
||
public double DetectionGap
|
||
{
|
||
get
|
||
{
|
||
return _detectionTolerance;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取路径名称
|
||
/// </summary>
|
||
public string PathName => _pathName;
|
||
|
||
/// <summary>
|
||
/// 获取动画时长
|
||
/// </summary>
|
||
public double AnimationDuration => _animationDuration;
|
||
|
||
/// <summary>
|
||
/// 设置并触发状态变更事件
|
||
/// </summary>
|
||
/// <param name="newState">新的动画状态</param>
|
||
private void SetState(AnimationState newState)
|
||
{
|
||
if (_currentState != newState)
|
||
{
|
||
_currentState = newState;
|
||
StateChanged?.Invoke(this, _currentState);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建动画
|
||
/// </summary>
|
||
/// <param name="animatedObject">要动画化的模型对象</param>
|
||
/// <param name="pathPoints">路径点列表</param>
|
||
/// <param name="durationSeconds">动画持续时间（秒）</param>
|
||
/// <param name="pathName">路径名称</param>
|
||
/// <param name="routeId">路由ID</param>
|
||
public void CreateAnimation(ModelItem animatedObject, List<Point3D> pathPoints, double durationSeconds = 10.0, string pathName = "未知路径", string routeId = null, bool isVirtualObject = false,
|
||
double virtualObjectLength = 0, double virtualObjectWidth = 0, double virtualObjectHeight = 0, double safetyMargin = 0)
|
||
{
|
||
// 🔥 清除之前的检测记录ID，确保每次生成新动画都创建新的检测记录
|
||
if (_currentDetectionRecordId.HasValue)
|
||
{
|
||
LogManager.Info($"[CreateAnimation] 清除之前的检测记录ID ({_currentDetectionRecordId})，将创建新记录");
|
||
_currentDetectionRecordId = null;
|
||
}
|
||
|
||
// 🔥 清除历史记录标志，确保动画生成时状态正确
|
||
if (_isUsingHistoryRecord)
|
||
{
|
||
LogManager.Info("[CreateAnimation] 清除历史记录标志，将创建新检测");
|
||
_isUsingHistoryRecord = false;
|
||
}
|
||
|
||
_pathName = pathName;
|
||
_currentRouteId = routeId;
|
||
_animatedObjectMode = isVirtualObject ? AnimatedObjectMode.VirtualObject : AnimatedObjectMode.RealObject;
|
||
_virtualObjectLength = virtualObjectLength; // 模型单位
|
||
_virtualObjectWidth = virtualObjectWidth; // 模型单位
|
||
_virtualObjectHeight = virtualObjectHeight; // 模型单位
|
||
_safetyMargin = safetyMargin; // 模型单位
|
||
_pathPoints = pathPoints;
|
||
_animatedObject = animatedObject;
|
||
|
||
// 初始化物体位置和朝向
|
||
if (animatedObject != null)
|
||
{
|
||
_originalTransform = animatedObject.Transform;
|
||
_originalCenter = animatedObject.BoundingBox().Center;
|
||
_trackedPosition = ResolveInitialBusinessTrackedPosition(animatedObject);
|
||
|
||
// 保持当前的 _currentYaw（因为物体可能已经被 MoveObjectToPathStart 旋转）
|
||
// 不要从 Transform 中提取，因为 Transform 返回的是原始值，不是当前值
|
||
LogManager.Debug($"[CreateAnimation] 保持_currentYaw={_currentYaw * 180 / Math.PI:F2}度不变, 模式={_animatedObjectMode}");
|
||
}
|
||
|
||
// 设置动画参数并预计算动画帧
|
||
// 注意：物体应该在调用CreateAnimation之前已经通过MoveObjectToPathStart旋转到起点
|
||
LogManager.Debug($"[CreateAnimation开始] _currentYaw之前={_currentYaw * 180 / Math.PI:F2}度, 模式={_animatedObjectMode}");
|
||
SetupAnimation(animatedObject, durationSeconds, _route);
|
||
LogManager.Debug($"[CreateAnimation结束] _currentYaw之后={_currentYaw * 180 / Math.PI:F2}度");
|
||
// 设置动画状态为Ready（动画已生成，可以播放）
|
||
SetState(AnimationState.Ready);
|
||
LogManager.Info($"[CreateAnimation] 动画已创建，状态设置为Ready");
|
||
}
|
||
|
||
private Point3D GetLiveBoundingBoxCenter(ModelItem item)
|
||
{
|
||
if (item == null)
|
||
{
|
||
return new Point3D(0, 0, 0);
|
||
}
|
||
|
||
var bounds = item.BoundingBox();
|
||
return bounds?.Center ?? new Point3D(0, 0, 0);
|
||
}
|
||
|
||
private Point3D ResolveInitialBusinessTrackedPosition(ModelItem item)
|
||
{
|
||
if (item == null)
|
||
{
|
||
return new Point3D(0, 0, 0);
|
||
}
|
||
|
||
if (ShouldUseOriginalBoundingBoxCenterForBusinessTrackedPosition(
|
||
_route?.PathType ?? PathType.Ground,
|
||
IsRealObjectMode) &&
|
||
_originalCenter != null)
|
||
{
|
||
return _originalCenter;
|
||
}
|
||
|
||
return GetLiveBoundingBoxCenter(item);
|
||
}
|
||
|
||
private Point3D ResolveGroundTrackedCenter(Point3D groundContactPoint, double objectHeight)
|
||
{
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
var canonicalReferencePoint = ToNumerics(adapter.ToCanonicalPoint(groundContactPoint));
|
||
var canonicalCenter = CanonicalTrackedPositionResolver.ResolveGroundTrackedCenter(
|
||
canonicalReferencePoint,
|
||
HostCoordinateAdapter.CanonicalUpVector3,
|
||
objectHeight);
|
||
return adapter.FromCanonicalPoint(new Point3D(canonicalCenter.X, canonicalCenter.Y, canonicalCenter.Z));
|
||
}
|
||
|
||
private Point3D ResolveHoistingTrackedCenter(Point3D referencePoint, double objectHeight, double referenceContactFactor)
|
||
{
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
var canonicalReferencePoint = ToNumerics(adapter.ToCanonicalPoint(referencePoint));
|
||
var canonicalCenter = CanonicalTrackedPositionResolver.ResolveCenterFromContactReference(
|
||
canonicalReferencePoint,
|
||
HostCoordinateAdapter.CanonicalUpVector3,
|
||
objectHeight,
|
||
referenceContactFactor);
|
||
return adapter.FromCanonicalPoint(new Point3D(canonicalCenter.X, canonicalCenter.Y, canonicalCenter.Z));
|
||
}
|
||
|
||
private bool TryResolveRailPreservedPoseTrackedCenter(
|
||
Point3D referencePoint,
|
||
Point3D previousPoint,
|
||
Point3D nextPoint,
|
||
double objectHeight,
|
||
out Point3D trackedCenter)
|
||
{
|
||
trackedCenter = referencePoint;
|
||
|
||
if (!IsRealObjectMode ||
|
||
_route?.PathType != PathType.Rail ||
|
||
_objectStartPlacementMode != ObjectStartPlacementMode.PreserveInitialPose ||
|
||
!_hasRailPreservedPoseTrackedCenterOffset)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
trackedCenter = RailPathPoseHelper.ResolvePreservedTrackedCenterPosition(
|
||
_route,
|
||
referencePoint,
|
||
previousPoint,
|
||
nextPoint,
|
||
objectHeight,
|
||
_railPreservedPoseTrackedCenterOffset);
|
||
return true;
|
||
}
|
||
|
||
private void CaptureRailPreservedPoseTrackedCenterOffset(
|
||
Point3D previousPoint,
|
||
Point3D referencePoint,
|
||
Point3D nextPoint,
|
||
double objectHeight)
|
||
{
|
||
_railPreservedPoseTrackedCenterOffset = new Vector3D(0, 0, 0);
|
||
_hasRailPreservedPoseTrackedCenterOffset = false;
|
||
|
||
if (!IsRealObjectMode ||
|
||
_route?.PathType != PathType.Rail ||
|
||
_objectStartPlacementMode != ObjectStartPlacementMode.PreserveInitialPose ||
|
||
CurrentControlledObject == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Point3D currentTrackedCenter = GetLiveBoundingBoxCenter(CurrentControlledObject ?? _animatedObject);
|
||
Point3D semanticTrackedCenter = RailPathPoseHelper.ResolveObjectSpaceCenterPosition(
|
||
_route,
|
||
referencePoint,
|
||
previousPoint,
|
||
nextPoint,
|
||
objectHeight);
|
||
_railPreservedPoseTrackedCenterOffset = RailPathPoseHelper.CalculatePreservedTrackedCenterOffset(
|
||
_route,
|
||
currentTrackedCenter,
|
||
referencePoint,
|
||
previousPoint,
|
||
nextPoint,
|
||
objectHeight);
|
||
_hasRailPreservedPoseTrackedCenterOffset = true;
|
||
|
||
LogManager.Info(
|
||
$"[Rail终点原位残差] 当前跟踪中心=({currentTrackedCenter.X:F3},{currentTrackedCenter.Y:F3},{currentTrackedCenter.Z:F3}), " +
|
||
$"终点路径参考对应中心=({semanticTrackedCenter.X:F3},{semanticTrackedCenter.Y:F3},{semanticTrackedCenter.Z:F3}), " +
|
||
$"固定偏移=({_railPreservedPoseTrackedCenterOffset.X:F3},{_railPreservedPoseTrackedCenterOffset.Y:F3},{_railPreservedPoseTrackedCenterOffset.Z:F3})");
|
||
}
|
||
|
||
internal static Rotation3D ResolvePreservedPoseRotation(
|
||
bool preferActualGeometryRotation,
|
||
bool hasActualGeometryRotation,
|
||
Rotation3D actualGeometryRotation,
|
||
Rotation3D fallbackRotation)
|
||
{
|
||
return preferActualGeometryRotation && hasActualGeometryRotation
|
||
? actualGeometryRotation
|
||
: fallbackRotation;
|
||
}
|
||
|
||
internal static bool ShouldPreservePathRotationForFrames(
|
||
PathType pathType,
|
||
ObjectStartPlacementMode placementMode,
|
||
bool hasPreservedRotation)
|
||
{
|
||
if (!hasPreservedRotation || placementMode != ObjectStartPlacementMode.PreserveInitialPose)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return pathType == PathType.Rail || pathType == PathType.Hoisting;
|
||
}
|
||
|
||
internal static bool ShouldAllowFragmentPlanarFallback(PathType pathType)
|
||
{
|
||
return pathType != PathType.Hoisting && pathType != PathType.Ground;
|
||
}
|
||
|
||
internal static bool ShouldUseReferenceBasedRealObjectPlanarPose(PathType pathType)
|
||
{
|
||
return pathType != PathType.Ground && pathType != PathType.Hoisting;
|
||
}
|
||
|
||
internal static bool ShouldUsePlanarRealObjectPureIncrementFrames(PathType pathType, bool isRealObjectMode)
|
||
{
|
||
return isRealObjectMode &&
|
||
(pathType == PathType.Ground || pathType == PathType.Hoisting);
|
||
}
|
||
|
||
internal static void ApplyRecordedPlanarRealObjectFramePose(
|
||
AnimationFrame frame,
|
||
double planarHostYawRadians,
|
||
Rotation3D recordedRotation)
|
||
{
|
||
if (frame == null)
|
||
{
|
||
throw new ArgumentNullException(nameof(frame));
|
||
}
|
||
|
||
if (recordedRotation == null)
|
||
{
|
||
throw new ArgumentNullException(nameof(recordedRotation));
|
||
}
|
||
|
||
frame.PlanarHostYawRadians = planarHostYawRadians;
|
||
frame.Rotation = recordedRotation;
|
||
frame.HasCustomRotation = true;
|
||
}
|
||
|
||
internal static bool TryResolveDisplayedPlanarYawFromRotation(
|
||
Rotation3D rotation,
|
||
CoordinateSystemType hostType,
|
||
out double yawRadians)
|
||
{
|
||
yawRadians = 0.0;
|
||
if (rotation == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
Matrix3 linear = new Transform3D(rotation).Linear;
|
||
Vector3 hostForward = new Vector3(
|
||
(float)linear.Get(0, 0),
|
||
(float)linear.Get(1, 0),
|
||
(float)linear.Get(2, 0));
|
||
|
||
return PathTargetFrameResolver.TryResolvePlanarHostYaw(hostForward, hostType, out yawRadians);
|
||
}
|
||
|
||
internal static double ResolvePlanarHostUpCorrectionRadians(
|
||
LocalEulerRotationCorrection correction,
|
||
CoordinateSystemType hostType)
|
||
{
|
||
double degrees = hostType == CoordinateSystemType.YUp
|
||
? correction.YDegrees
|
||
: correction.ZDegrees;
|
||
return degrees * Math.PI / 180.0;
|
||
}
|
||
|
||
internal static bool TryResolveGroundHostPlanarYawFromFramePoints(
|
||
Point3D previousPoint,
|
||
Point3D currentPoint,
|
||
Point3D nextPoint,
|
||
CoordinateSystemType hostType,
|
||
out double yawRadians)
|
||
{
|
||
yawRadians = 0.0;
|
||
|
||
Vector3 hostForward = new Vector3(
|
||
(float)(nextPoint.X - previousPoint.X),
|
||
(float)(nextPoint.Y - previousPoint.Y),
|
||
(float)(nextPoint.Z - previousPoint.Z));
|
||
if (hostForward.LengthSquared() < 1e-6f)
|
||
{
|
||
hostForward = new Vector3(
|
||
(float)(nextPoint.X - currentPoint.X),
|
||
(float)(nextPoint.Y - currentPoint.Y),
|
||
(float)(nextPoint.Z - currentPoint.Z));
|
||
}
|
||
|
||
if (hostForward.LengthSquared() < 1e-6f)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return PathTargetFrameResolver.TryResolvePlanarHostYaw(hostForward, hostType, out yawRadians);
|
||
}
|
||
|
||
internal static Rotation3D ResolveCurrentRotationBaseline(
|
||
PathType pathType,
|
||
bool isRealObjectMode,
|
||
bool hasTrackedRotation,
|
||
Rotation3D trackedRotation,
|
||
bool hasReferenceRotation,
|
||
Rotation3D referenceRotation,
|
||
Rotation3D transformRotation,
|
||
bool hasActualGeometryRotation,
|
||
Rotation3D actualGeometryRotation)
|
||
{
|
||
if (isRealObjectMode &&
|
||
pathType == PathType.Hoisting &&
|
||
hasActualGeometryRotation)
|
||
{
|
||
return actualGeometryRotation;
|
||
}
|
||
|
||
if (hasTrackedRotation)
|
||
{
|
||
return trackedRotation;
|
||
}
|
||
|
||
if (isRealObjectMode && hasReferenceRotation)
|
||
{
|
||
return referenceRotation;
|
||
}
|
||
|
||
return transformRotation;
|
||
}
|
||
|
||
internal static bool ShouldUseRealObjectOverrideRotation(bool isRealObjectMode)
|
||
{
|
||
return isRealObjectMode;
|
||
}
|
||
|
||
internal static bool ShouldUseOriginalBoundingBoxCenterForBusinessTrackedPosition(
|
||
PathType pathType,
|
||
bool isRealObjectMode)
|
||
{
|
||
return isRealObjectMode && pathType == PathType.Ground;
|
||
}
|
||
|
||
private void SyncTrackedRotationToDisplayedPose(ModelItem sourceObject)
|
||
{
|
||
if (sourceObject == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Rotation3D actualGeometryRotation = Rotation3D.Identity;
|
||
bool hasActualGeometryRotation =
|
||
IsRealObjectMode &&
|
||
ModelItemTransformHelper.TryGetCurrentGeometryRotation(sourceObject, out actualGeometryRotation);
|
||
|
||
Rotation3D resolvedRotation = ResolvePreservedPoseRotation(
|
||
preferActualGeometryRotation: IsRealObjectMode,
|
||
hasActualGeometryRotation: hasActualGeometryRotation,
|
||
actualGeometryRotation: actualGeometryRotation,
|
||
fallbackRotation: _trackedRotation);
|
||
|
||
_trackedRotation = resolvedRotation;
|
||
_hasTrackedRotation = true;
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromRotation(resolvedRotation);
|
||
|
||
var linear = new Transform3D(resolvedRotation).Linear;
|
||
LogManager.Info(
|
||
$"[平移保持终点原始姿态] {sourceObject.DisplayName} 锁定姿态: " +
|
||
$"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " +
|
||
$"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " +
|
||
$"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4}), " +
|
||
$"来源={(hasActualGeometryRotation ? "实际几何姿态" : "跟踪姿态")}");
|
||
}
|
||
|
||
private bool TryResolvePlanarRealObjectBaseYaw(PathType pathType, out double yawRadians)
|
||
{
|
||
yawRadians = 0.0;
|
||
if (_route?.PathType != pathType ||
|
||
!IsRealObjectMode ||
|
||
_pathPoints == null ||
|
||
_pathPoints.Count < 2)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var hostType = CoordinateSystemManager.Instance.ResolvedType;
|
||
var hostPoints = new List<Vector3>(_pathPoints.Count);
|
||
for (int i = 0; i < _pathPoints.Count; i++)
|
||
{
|
||
hostPoints.Add(new Vector3((float)_pathPoints[i].X, (float)_pathPoints[i].Y, (float)_pathPoints[i].Z));
|
||
}
|
||
|
||
return PathTargetFrameResolver.TryResolvePlanarStartHostYaw(pathType, hostPoints, hostType, out yawRadians);
|
||
}
|
||
|
||
private bool TryCreateGroundRealObjectConstrainedRotation(
|
||
Point3D previousFramePoint,
|
||
Point3D currentFramePoint,
|
||
Point3D nextFramePoint,
|
||
out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
if (!IsRealObjectMode ||
|
||
_route?.PathType != PathType.Ground ||
|
||
!_hasGroundRealObjectBasePose)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(
|
||
new Vector3(
|
||
(float)(nextFramePoint.X - previousFramePoint.X),
|
||
(float)(nextFramePoint.Y - previousFramePoint.Y),
|
||
(float)(nextFramePoint.Z - previousFramePoint.Z)),
|
||
CoordinateSystemManager.Instance.ResolvedType,
|
||
out var currentFrame))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!PathTargetFrameResolver.TryResolvePlanarHostYaw(
|
||
currentFrame.Forward,
|
||
CoordinateSystemManager.Instance.ResolvedType,
|
||
out double targetYawRadians))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
Vector3 hostUp = Vector3.Normalize(adapter.HostUpVector3);
|
||
double deltaYaw = NormalizeRadians(targetYawRadians - _groundRealObjectBaseYaw);
|
||
Quaternion deltaQuaternion = Quaternion.CreateFromAxisAngle(hostUp, (float)deltaYaw);
|
||
Quaternion baseQuaternion = Rotation3DToHostQuaternion(_groundRealObjectBaseRotation);
|
||
Quaternion targetQuaternion = Quaternion.Normalize(deltaQuaternion * baseQuaternion);
|
||
rotation = adapter.FromHostQuaternionDirect(targetQuaternion);
|
||
return true;
|
||
}
|
||
|
||
private bool TryCreateGroundRealObjectConstrainedRotationFromHostForward(
|
||
Vector3 hostForward,
|
||
out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
if (!IsRealObjectMode ||
|
||
_route?.PathType != PathType.Ground ||
|
||
!_hasGroundRealObjectBasePose)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(
|
||
hostForward,
|
||
CoordinateSystemManager.Instance.ResolvedType,
|
||
out var currentFrame))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!PathTargetFrameResolver.TryResolvePlanarHostYaw(
|
||
currentFrame.Forward,
|
||
CoordinateSystemManager.Instance.ResolvedType,
|
||
out double targetYawRadians))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
Vector3 hostUp = Vector3.Normalize(adapter.HostUpVector3);
|
||
double deltaYaw = NormalizeRadians(targetYawRadians - _groundRealObjectBaseYaw);
|
||
Quaternion deltaQuaternion = Quaternion.CreateFromAxisAngle(hostUp, (float)deltaYaw);
|
||
Quaternion baseQuaternion = Rotation3DToHostQuaternion(_groundRealObjectBaseRotation);
|
||
Quaternion targetQuaternion = Quaternion.Normalize(deltaQuaternion * baseQuaternion);
|
||
rotation = adapter.FromHostQuaternionDirect(targetQuaternion);
|
||
return true;
|
||
}
|
||
|
||
private bool TryResolveRecordedPlanarRealObjectFrameRotation(
|
||
PathType pathType,
|
||
Point3D previousFramePoint,
|
||
Point3D currentFramePoint,
|
||
Point3D nextFramePoint,
|
||
out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
|
||
if (!IsRealObjectMode)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (pathType == PathType.Ground)
|
||
{
|
||
return TryCreateGroundRealObjectConstrainedRotation(
|
||
previousFramePoint,
|
||
currentFramePoint,
|
||
nextFramePoint,
|
||
out rotation);
|
||
}
|
||
|
||
if (pathType == PathType.Hoisting)
|
||
{
|
||
Vector3 hostForward = new Vector3(
|
||
(float)(nextFramePoint.X - previousFramePoint.X),
|
||
(float)(nextFramePoint.Y - previousFramePoint.Y),
|
||
(float)(nextFramePoint.Z - previousFramePoint.Z));
|
||
|
||
if (hostForward.LengthSquared() <= 1e-6f)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return TryResolveHoistingActualPose(
|
||
hostForward,
|
||
out rotation,
|
||
out _,
|
||
out _,
|
||
out _,
|
||
out _);
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private bool TryResolveHoistingActualPose(
|
||
Vector3 hostForward,
|
||
out Rotation3D rotation,
|
||
out Quaternion baselineQuaternion,
|
||
out LocalAxisDirection selectedAxisDirection,
|
||
out Vector3 selectedAxisLocal,
|
||
out Vector3 projectedForward)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
baselineQuaternion = Quaternion.Identity;
|
||
selectedAxisDirection = LocalAxisDirection.PositiveX;
|
||
selectedAxisLocal = Vector3.UnitX;
|
||
projectedForward = Vector3.Zero;
|
||
if (!TryResolveHoistingStartBaselinePose(
|
||
hostForward,
|
||
out baselineQuaternion,
|
||
out selectedAxisDirection,
|
||
out selectedAxisLocal,
|
||
out projectedForward))
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
return false;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
Quaternion finalHostQuaternion = adapter.ComposeHostQuaternion(
|
||
baselineQuaternion,
|
||
_objectRotationCorrection);
|
||
rotation = adapter.FromHostQuaternionDirect(finalHostQuaternion);
|
||
|
||
Matrix4x4 hostBaselineLocalAxesLinear = Matrix4x4.CreateFromQuaternion(baselineQuaternion);
|
||
Matrix4x4 hostComposedLocalAxesLinear = Matrix4x4.CreateFromQuaternion(finalHostQuaternion);
|
||
|
||
LogManager.Debug(
|
||
$"[真实物体平面前进轴] Hoisting 已改用实际几何姿态基线，选中对象轴={selectedAxisDirection}。");
|
||
LogManager.Debug(
|
||
$"[真实物体起点姿态] 选中参考轴=({selectedAxisLocal.X:F3},{selectedAxisLocal.Y:F3},{selectedAxisLocal.Z:F3}), " +
|
||
$"投影前进=({projectedForward.X:F3},{projectedForward.Y:F3},{projectedForward.Z:F3}), " +
|
||
$"宿主修正={_objectRotationCorrection}, 本地修正=X=0.0°,Y=0.0°,Z=0.0°, " +
|
||
$"hostBaselineLocalXAxis=({hostBaselineLocalAxesLinear.M11:F4},{hostBaselineLocalAxesLinear.M21:F4},{hostBaselineLocalAxesLinear.M31:F4}), " +
|
||
$"hostBaselineLocalYAxis=({hostBaselineLocalAxesLinear.M12:F4},{hostBaselineLocalAxesLinear.M22:F4},{hostBaselineLocalAxesLinear.M32:F4}), " +
|
||
$"hostBaselineLocalZAxis=({hostBaselineLocalAxesLinear.M13:F4},{hostBaselineLocalAxesLinear.M23:F4},{hostBaselineLocalAxesLinear.M33:F4}), " +
|
||
$"hostComposedLocalXAxis=({hostComposedLocalAxesLinear.M11:F4},{hostComposedLocalAxesLinear.M21:F4},{hostComposedLocalAxesLinear.M31:F4}), " +
|
||
$"hostComposedLocalYAxis=({hostComposedLocalAxesLinear.M12:F4},{hostComposedLocalAxesLinear.M22:F4},{hostComposedLocalAxesLinear.M32:F4}), " +
|
||
$"hostComposedLocalZAxis=({hostComposedLocalAxesLinear.M13:F4},{hostComposedLocalAxesLinear.M23:F4},{hostComposedLocalAxesLinear.M33:F4})");
|
||
return true;
|
||
}
|
||
|
||
private bool TryResolveHoistingStartBaselinePose(
|
||
Vector3 hostForward,
|
||
out Quaternion baselineQuaternion,
|
||
out LocalAxisDirection selectedAxisDirection,
|
||
out Vector3 selectedAxisLocal,
|
||
out Vector3 projectedForward)
|
||
{
|
||
baselineQuaternion = Quaternion.Identity;
|
||
selectedAxisDirection = _hasRealObjectPlanarSelectedForwardAxis
|
||
? _realObjectPlanarSelectedForwardAxis
|
||
: LocalAxisDirection.PositiveX;
|
||
selectedAxisLocal = Vector3.UnitX;
|
||
projectedForward = Vector3.Zero;
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
Vector3 hostUp = adapter.HostType == CoordinateSystemType.YUp ? Vector3.UnitY : Vector3.UnitZ;
|
||
Vector3 normalizedHostForward = hostForward.LengthSquared() > 1e-6f
|
||
? Vector3.Normalize(hostForward)
|
||
: Vector3.UnitX;
|
||
Vector3 planarProjectedForward = normalizedHostForward - Vector3.Dot(normalizedHostForward, hostUp) * hostUp;
|
||
projectedForward = planarProjectedForward.LengthSquared() > 1e-6f
|
||
? Vector3.Normalize(planarProjectedForward)
|
||
: Vector3.UnitX;
|
||
Rotation3D sourceRotation;
|
||
if (_animatedObject == null ||
|
||
!ModelItemTransformHelper.TryGetCurrentGeometryRotation(_animatedObject, out sourceRotation))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
Quaternion sourceQuaternion = Rotation3DToHostQuaternion(sourceRotation);
|
||
return HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
|
||
sourceQuaternion,
|
||
projectedForward,
|
||
hostUp,
|
||
out baselineQuaternion,
|
||
out selectedAxisDirection,
|
||
out selectedAxisLocal,
|
||
out projectedForward);
|
||
}
|
||
|
||
private static Quaternion Rotation3DToHostQuaternion(Rotation3D rotation)
|
||
{
|
||
var linear = new Transform3D(rotation).Linear;
|
||
var matrix = new Matrix4x4(
|
||
(float)linear.Get(0, 0), (float)linear.Get(0, 1), (float)linear.Get(0, 2), 0f,
|
||
(float)linear.Get(1, 0), (float)linear.Get(1, 1), (float)linear.Get(1, 2), 0f,
|
||
(float)linear.Get(2, 0), (float)linear.Get(2, 1), (float)linear.Get(2, 2), 0f,
|
||
0f, 0f, 0f, 1f);
|
||
return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(matrix));
|
||
}
|
||
|
||
private bool TryResolveGroundRealObjectBaseYaw(out double yawRadians)
|
||
{
|
||
yawRadians = 0.0;
|
||
if (_route?.PathType != PathType.Ground ||
|
||
!IsRealObjectMode ||
|
||
_pathPoints == null ||
|
||
_pathPoints.Count < 2)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var hostType = CoordinateSystemManager.Instance.ResolvedType;
|
||
var hostPoints = new List<Vector3>(_pathPoints.Count);
|
||
for (int i = 0; i < _pathPoints.Count; i++)
|
||
{
|
||
hostPoints.Add(new Vector3((float)_pathPoints[i].X, (float)_pathPoints[i].Y, (float)_pathPoints[i].Z));
|
||
}
|
||
|
||
return PathTargetFrameResolver.TryResolvePlanarStartHostYaw(_route.PathType, hostPoints, hostType, out yawRadians);
|
||
}
|
||
|
||
private bool TryApplyPlanarRealObjectStartIncrementalTransform(
|
||
PathType pathType,
|
||
Point3D targetTrackedPosition,
|
||
out double targetYawRadians)
|
||
{
|
||
targetYawRadians = 0.0;
|
||
|
||
if (!IsRealObjectMode ||
|
||
(pathType != PathType.Ground && pathType != PathType.Hoisting) ||
|
||
_route?.PathType != pathType ||
|
||
CurrentControlledObject == null ||
|
||
_pathPoints == null ||
|
||
_pathPoints.Count < 2)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!PathTargetFrameResolver.TryResolvePlanarStartHostForward(
|
||
pathType,
|
||
_pathPoints,
|
||
out var hostForward))
|
||
{
|
||
LogManager.Error($"[{pathType.GetDisplayName()}纯增量起点] 无法解析路径起始方向。");
|
||
return false;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
if (!PathTargetFrameResolver.TryResolvePlanarHostYaw(hostForward, adapter.HostType, out targetYawRadians))
|
||
{
|
||
// 纯垂直路径无水平方向，无法从路径计算 yaw，保持物体当前朝向
|
||
LogManager.Debug($"[{pathType.GetDisplayName()}纯增量起点] 路径方向为纯垂直，保持当前朝向。");
|
||
targetYawRadians = _currentYaw;
|
||
}
|
||
|
||
// 提取非 up 轴修正角度
|
||
double angleXRadians = _objectRotationCorrection.XDegrees * Math.PI / 180.0;
|
||
double angleNonUpRadians = (adapter.HostType == CoordinateSystemType.YUp
|
||
? _objectRotationCorrection.ZDegrees
|
||
: _objectRotationCorrection.YDegrees) * Math.PI / 180.0;
|
||
var nonUpAxis = adapter.HostType == CoordinateSystemType.YUp
|
||
? Vector3.UnitZ : Vector3.UnitY;
|
||
|
||
double upAxisCorrectionRadians = ResolvePlanarHostUpCorrectionRadians(
|
||
_objectRotationCorrection,
|
||
adapter.HostType);
|
||
targetYawRadians = NormalizeRadians(targetYawRadians + upAxisCorrectionRadians);
|
||
double currentYawRadians = _currentYaw;
|
||
double deltaYawRadians = NormalizeRadians(targetYawRadians - currentYawRadians);
|
||
|
||
bool hasRotation = Math.Abs(angleXRadians) > 1e-9 ||
|
||
Math.Abs(angleNonUpRadians) > 1e-9 ||
|
||
Math.Abs(deltaYawRadians) > 1e-9;
|
||
double posDiff = Math.Abs(_trackedPosition.X - targetTrackedPosition.X) +
|
||
Math.Abs(_trackedPosition.Y - targetTrackedPosition.Y) +
|
||
Math.Abs(_trackedPosition.Z - targetTrackedPosition.Z);
|
||
bool hasTranslation = posDiff > 1e-6;
|
||
|
||
if (hasRotation || hasTranslation)
|
||
{
|
||
LogManager.Debug(
|
||
$"[{pathType.GetDisplayName()}纯增量起点] 当前Yaw={currentYawRadians * 180.0 / Math.PI:F2}°, " +
|
||
$"目标Yaw={targetYawRadians * 180.0 / Math.PI:F2}°, 增量Yaw={deltaYawRadians * 180.0 / Math.PI:F2}°, " +
|
||
$"当前跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
|
||
$"目标跟踪点=({targetTrackedPosition.X:F3},{targetTrackedPosition.Y:F3},{targetTrackedPosition.Z:F3}), " +
|
||
$"旋转变化={hasRotation}, 位移变化={hasTranslation}");
|
||
|
||
ModelItemTransformHelper.MoveItemCombinedAxisRotationAndTranslation(
|
||
CurrentControlledObject,
|
||
_trackedPosition,
|
||
Vector3.UnitX, angleXRadians,
|
||
nonUpAxis, angleNonUpRadians,
|
||
adapter.HostUpVector3, deltaYawRadians,
|
||
targetTrackedPosition);
|
||
}
|
||
|
||
_trackedPosition = targetTrackedPosition;
|
||
|
||
if (ModelItemTransformHelper.TryGetCurrentGeometryRotation(CurrentControlledObject, out var appliedRotation))
|
||
{
|
||
_trackedRotation = appliedRotation;
|
||
_hasTrackedRotation = true;
|
||
}
|
||
|
||
_currentYaw = targetYawRadians;
|
||
LogHostActualPoseAxes(CurrentControlledObject, $"[{pathType.GetDisplayName()}纯增量起点应用后宿主姿态]", false);
|
||
return true;
|
||
}
|
||
|
||
private void ApplyPlanarNonUpAxisCorrectionsAtTrackedPosition(
|
||
PathType pathType,
|
||
Point3D trackedPosition,
|
||
CoordinateSystemType hostType)
|
||
{
|
||
if (CurrentControlledObject == null || _objectRotationCorrection.IsZero)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var correctionSteps = new List<(string AxisName, Vector3 HostAxis, double AngleRadians)>();
|
||
correctionSteps.Add(("X", Vector3.UnitX, _objectRotationCorrection.XDegrees * Math.PI / 180.0));
|
||
|
||
if (hostType == CoordinateSystemType.YUp)
|
||
{
|
||
correctionSteps.Add(("Z", Vector3.UnitZ, _objectRotationCorrection.ZDegrees * Math.PI / 180.0));
|
||
}
|
||
else
|
||
{
|
||
correctionSteps.Add(("Y", Vector3.UnitY, _objectRotationCorrection.YDegrees * Math.PI / 180.0));
|
||
}
|
||
|
||
foreach (var step in correctionSteps)
|
||
{
|
||
if (Math.Abs(step.AngleRadians) < 1e-9)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
LogManager.Info(
|
||
$"[{pathType.GetDisplayName()}纯增量起点角度修正] 宿主{step.AxisName}轴增量={step.AngleRadians * 180.0 / Math.PI:F2}°, " +
|
||
$"跟踪点=({trackedPosition.X:F3},{trackedPosition.Y:F3},{trackedPosition.Z:F3})");
|
||
|
||
ModelItemTransformHelper.MoveItemIncrementallyByAxisRotationAndTranslation(
|
||
CurrentControlledObject,
|
||
trackedPosition,
|
||
step.HostAxis,
|
||
step.AngleRadians,
|
||
trackedPosition);
|
||
}
|
||
}
|
||
|
||
private static double NormalizeRadians(double angle)
|
||
{
|
||
while (angle > Math.PI)
|
||
{
|
||
angle -= 2.0 * Math.PI;
|
||
}
|
||
|
||
while (angle < -Math.PI)
|
||
{
|
||
angle += 2.0 * Math.PI;
|
||
}
|
||
|
||
return angle;
|
||
}
|
||
|
||
private static Vector3 ToNumerics(Point3D point)
|
||
{
|
||
return new Vector3((float)point.X, (float)point.Y, (float)point.Z);
|
||
}
|
||
|
||
private bool TryGetCurrentRailModelAxisConvention(
|
||
Point3D previousPoint,
|
||
Point3D currentPoint,
|
||
Point3D nextPoint,
|
||
out ModelAxisConvention convention)
|
||
{
|
||
convention = null;
|
||
|
||
if (IsVirtualObjectMode)
|
||
{
|
||
convention = ModelAxisConvention.CreateVirtualObjectAssetConvention();
|
||
return true;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
if (IsRealObjectMode && _hasRealObjectReferenceRotation)
|
||
{
|
||
if (PathTargetFrameResolver.TryCreateRailHostFrame(_route, previousPoint, currentPoint, nextPoint, out var targetFrame) &&
|
||
RealObjectRailAxisConventionResolver.TryResolve(
|
||
_realObjectReferenceAxisX,
|
||
_realObjectReferenceAxisY,
|
||
_realObjectReferenceAxisZ,
|
||
_realObjectReferenceHostUpLocalAxis,
|
||
targetFrame.Forward,
|
||
adapter.HostType,
|
||
out convention,
|
||
out var selectedForwardAxis,
|
||
out var selectedForwardWorldAxis,
|
||
out var selectedUpAxis,
|
||
out var selectedUpWorldAxis))
|
||
{
|
||
LogManager.Debug(
|
||
$"[Rail姿态修正] Host={adapter.HostType}, 虚拟物体={IsVirtualObjectMode}, " +
|
||
$"真实物体参考姿态生效, Forward={convention.ForwardAxis}, Up={convention.UpAxis}, " +
|
||
$"选中Forward轴={selectedForwardAxis}, 选中Forward世界轴=({selectedForwardWorldAxis.X:F4},{selectedForwardWorldAxis.Y:F4},{selectedForwardWorldAxis.Z:F4}), " +
|
||
$"选中Up轴={selectedUpAxis}, 选中Up世界轴=({selectedUpWorldAxis.X:F4},{selectedUpWorldAxis.Y:F4},{selectedUpWorldAxis.Z:F4}), " +
|
||
$"宿主Up对应本地轴={_realObjectReferenceHostUpLocalAxis}, " +
|
||
$"目标Forward=({targetFrame.Forward.X:F4},{targetFrame.Forward.Y:F4},{targetFrame.Forward.Z:F4}), " +
|
||
$"目标Up=({targetFrame.Up.X:F4},{targetFrame.Up.Y:F4},{targetFrame.Up.Z:F4})");
|
||
return true;
|
||
}
|
||
}
|
||
|
||
convention = ModelAxisConvention.CreateDefaultForHost(adapter.HostType);
|
||
LogManager.Debug(
|
||
$"[Rail姿态修正] Host={adapter.HostType}, 虚拟物体={IsVirtualObjectMode}, " +
|
||
$"Forward={convention.ForwardAxis}, Up={convention.UpAxis}, 来源=默认轴约定");
|
||
return true;
|
||
}
|
||
|
||
private static Vector3 ResolveReferenceWorldAxisForLocalDirection(
|
||
LocalAxisDirection axis,
|
||
Vector3 referenceAxisX,
|
||
Vector3 referenceAxisY,
|
||
Vector3 referenceAxisZ)
|
||
{
|
||
switch (axis)
|
||
{
|
||
case LocalAxisDirection.PositiveX: return Vector3.Normalize(referenceAxisX);
|
||
case LocalAxisDirection.NegativeX: return Vector3.Normalize(-referenceAxisX);
|
||
case LocalAxisDirection.PositiveY: return Vector3.Normalize(referenceAxisY);
|
||
case LocalAxisDirection.NegativeY: return Vector3.Normalize(-referenceAxisY);
|
||
case LocalAxisDirection.PositiveZ: return Vector3.Normalize(referenceAxisZ);
|
||
case LocalAxisDirection.NegativeZ: return Vector3.Normalize(-referenceAxisZ);
|
||
default:
|
||
throw new ArgumentOutOfRangeException(nameof(axis), axis, null);
|
||
}
|
||
}
|
||
|
||
private bool TryCalculateCurrentRealObjectRailProjectedExtents(
|
||
Point3D previousPoint,
|
||
Point3D currentPoint,
|
||
Point3D nextPoint,
|
||
out ModelAxisConvention convention,
|
||
out (double forwardExtent, double sideExtent, double upExtent) extents)
|
||
{
|
||
convention = null;
|
||
extents = (0.0, 0.0, 0.0);
|
||
|
||
if (!IsRealObjectMode ||
|
||
!_hasRealObjectReferenceRotation ||
|
||
_realObjectLength <= 0.0 ||
|
||
_realObjectWidth <= 0.0 ||
|
||
_realObjectHeight <= 0.0)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
if (!PathTargetFrameResolver.TryCreateRailHostFrame(_route, previousPoint, currentPoint, nextPoint, out var targetFrame))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!TryGetCurrentRailModelAxisConvention(previousPoint, currentPoint, nextPoint, out var railConvention))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!RailPathPoseHelper.TryCreateRailRotation(
|
||
_route,
|
||
previousPoint,
|
||
currentPoint,
|
||
nextPoint,
|
||
railConvention,
|
||
LocalEulerRotationCorrection.Zero,
|
||
out var baselineRotation))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
Quaternion baselineHostQuaternion = new Quaternion(
|
||
(float)baselineRotation.A,
|
||
(float)baselineRotation.B,
|
||
(float)baselineRotation.C,
|
||
(float)baselineRotation.D);
|
||
LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection();
|
||
Quaternion finalHostQuaternion = adapter.ComposeHostQuaternion(
|
||
baselineHostQuaternion,
|
||
localCorrection);
|
||
|
||
return RealObjectRailExtentResolver.TryResolveProjectedSemanticExtents(
|
||
_realObjectReferenceAxisX,
|
||
_realObjectReferenceAxisY,
|
||
_realObjectReferenceAxisZ,
|
||
_realObjectReferenceHostUpLocalAxis,
|
||
targetFrame.Forward,
|
||
adapter.HostType,
|
||
_realObjectLength,
|
||
_realObjectWidth,
|
||
_realObjectHeight,
|
||
baselineHostQuaternion,
|
||
finalHostQuaternion,
|
||
out convention,
|
||
out extents);
|
||
}
|
||
|
||
private bool TryCalculateCurrentRealObjectPlanarProjectedExtents(
|
||
Point3D previousPoint,
|
||
Point3D currentPoint,
|
||
Point3D nextPoint,
|
||
out ModelAxisConvention convention,
|
||
out (double forwardExtent, double sideExtent, double upExtent) extents)
|
||
{
|
||
convention = null;
|
||
extents = (0.0, 0.0, 0.0);
|
||
|
||
bool requiresReferenceRotation = _route?.PathType != PathType.Hoisting;
|
||
if (!IsRealObjectMode ||
|
||
(requiresReferenceRotation && !_hasRealObjectReferenceRotation) ||
|
||
_realObjectLength <= 0.0 ||
|
||
_realObjectWidth <= 0.0 ||
|
||
_realObjectHeight <= 0.0)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
Vector3 hostForward = ToNumerics(nextPoint) - ToNumerics(previousPoint);
|
||
if (hostForward.LengthSquared() < 1e-6f)
|
||
{
|
||
hostForward = ToNumerics(nextPoint) - ToNumerics(currentPoint);
|
||
}
|
||
|
||
return TryCalculateCurrentRealObjectPlanarProjectedExtents(
|
||
hostForward,
|
||
out convention,
|
||
out extents);
|
||
}
|
||
|
||
private bool TryCalculateCurrentRealObjectPlanarProjectedExtents(
|
||
Vector3 hostForward,
|
||
out ModelAxisConvention convention,
|
||
out (double forwardExtent, double sideExtent, double upExtent) extents)
|
||
{
|
||
convention = null;
|
||
extents = (0.0, 0.0, 0.0);
|
||
|
||
bool requiresReferenceRotation = ShouldUseReferenceBasedRealObjectPlanarPose(_route?.PathType ?? PathType.Ground);
|
||
if (!IsRealObjectMode ||
|
||
(requiresReferenceRotation && !_hasRealObjectReferenceRotation) ||
|
||
_realObjectLength <= 0.0 ||
|
||
_realObjectWidth <= 0.0 ||
|
||
_realObjectHeight <= 0.0)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(hostForward, adapter.HostType, out var targetFrame))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting)
|
||
{
|
||
convention = ModelAxisConvention.CreateDefaultForHost(adapter.HostType);
|
||
extents = RotatedObjectExtentHelper.CalculateGroundSemanticExtents(
|
||
adapter.HostType,
|
||
_realObjectLength,
|
||
_realObjectWidth,
|
||
_realObjectHeight,
|
||
_objectRotationCorrection);
|
||
LogManager.Debug(
|
||
$"[{_route.PathType.GetDisplayName()}通行空间尺寸] 使用宿主语义尺寸: " +
|
||
$"Forward={extents.forwardExtent:F3}, Side={extents.sideExtent:F3}, Up={extents.upExtent:F3}");
|
||
return true;
|
||
}
|
||
|
||
if (!TryCreateRealObjectPlanarPoseSolution(
|
||
targetFrame.Forward,
|
||
targetFrame.Up,
|
||
out var solution))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
LocalAxisDirection semanticUpAxis =
|
||
adapter.HostType == CoordinateSystemType.YUp
|
||
? LocalAxisDirection.PositiveY
|
||
: LocalAxisDirection.PositiveZ;
|
||
convention = new ModelAxisConvention(solution.SelectedReferenceAxisDirection, semanticUpAxis);
|
||
|
||
Quaternion finalHostQuaternion = adapter.ComposeHostQuaternion(
|
||
solution.BaselineRotation,
|
||
ResolveRealObjectLocalRotationCorrection());
|
||
extents = RealObjectProjectedExtentResolver.CalculateProjectedSemanticExtents(
|
||
convention,
|
||
_realObjectLength,
|
||
_realObjectWidth,
|
||
_realObjectHeight,
|
||
solution.BaselineRotation,
|
||
finalHostQuaternion,
|
||
targetFrame.Forward,
|
||
targetFrame.Up);
|
||
return true;
|
||
}
|
||
|
||
public bool TryGetCurrentRouteRealObjectPlanarProjectedExtents(
|
||
out double forwardExtent,
|
||
out double sideExtent,
|
||
out double upExtent)
|
||
{
|
||
forwardExtent = 0.0;
|
||
sideExtent = 0.0;
|
||
upExtent = 0.0;
|
||
|
||
if (_route == null ||
|
||
(_route.PathType != PathType.Ground && _route.PathType != PathType.Hoisting) ||
|
||
_pathPoints == null ||
|
||
_pathPoints.Count == 0)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!PathTargetFrameResolver.TryResolvePlanarStartHostForward(
|
||
_route.PathType,
|
||
_pathPoints,
|
||
out var hostForward))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!TryCalculateCurrentRealObjectPlanarProjectedExtents(
|
||
hostForward,
|
||
out _,
|
||
out var extents))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
forwardExtent = extents.forwardExtent;
|
||
sideExtent = extents.sideExtent;
|
||
upExtent = extents.upExtent;
|
||
return true;
|
||
}
|
||
|
||
public bool TryGetCurrentRouteRealObjectRailProjectedExtents(
|
||
out double forwardExtent,
|
||
out double sideExtent,
|
||
out double upExtent)
|
||
{
|
||
forwardExtent = 0.0;
|
||
sideExtent = 0.0;
|
||
upExtent = 0.0;
|
||
|
||
if (_route == null || _route.PathType != PathType.Rail || _pathPoints == null || _pathPoints.Count == 0)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
Point3D previousPoint = _pathPoints[0];
|
||
Point3D currentPoint = _pathPoints[0];
|
||
Point3D nextPoint = _pathPoints.Count > 1 ? _pathPoints[1] : _pathPoints[0];
|
||
|
||
if (!TryCalculateCurrentRealObjectRailProjectedExtents(
|
||
previousPoint,
|
||
currentPoint,
|
||
nextPoint,
|
||
out _,
|
||
out var extents))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
forwardExtent = extents.forwardExtent;
|
||
sideExtent = extents.sideExtent;
|
||
upExtent = extents.upExtent;
|
||
return true;
|
||
}
|
||
|
||
private bool TryCreateRailPathRotation(
|
||
Point3D previousPoint,
|
||
Point3D currentPoint,
|
||
Point3D nextPoint,
|
||
out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
|
||
if (!TryGetCurrentRailModelAxisConvention(previousPoint, currentPoint, nextPoint, out var railConvention))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (IsRealObjectMode)
|
||
{
|
||
if (!RailPathPoseHelper.TryCreateRailRotation(
|
||
_route,
|
||
previousPoint,
|
||
currentPoint,
|
||
nextPoint,
|
||
railConvention,
|
||
LocalEulerRotationCorrection.Zero,
|
||
out var baselineRotation))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
Quaternion baselineHostQuaternion = new Quaternion(
|
||
(float)baselineRotation.A,
|
||
(float)baselineRotation.B,
|
||
(float)baselineRotation.C,
|
||
(float)baselineRotation.D);
|
||
LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection();
|
||
Quaternion composedHostQuaternion = adapter.ComposeHostQuaternion(
|
||
baselineHostQuaternion,
|
||
localCorrection);
|
||
rotation = adapter.FromHostQuaternionDirect(composedHostQuaternion);
|
||
|
||
Matrix4x4 hostBaselineLocalAxesLinear = Matrix4x4.CreateFromQuaternion(baselineHostQuaternion);
|
||
Matrix4x4 hostComposedLocalAxesLinear = Matrix4x4.CreateFromQuaternion(composedHostQuaternion);
|
||
LogManager.Debug(
|
||
$"[Rail真实物体角度修正] hostBaselineLocalXAxis=({hostBaselineLocalAxesLinear.M11:F4},{hostBaselineLocalAxesLinear.M21:F4},{hostBaselineLocalAxesLinear.M31:F4}), " +
|
||
$"hostBaselineLocalYAxis=({hostBaselineLocalAxesLinear.M12:F4},{hostBaselineLocalAxesLinear.M22:F4},{hostBaselineLocalAxesLinear.M32:F4}), " +
|
||
$"hostBaselineLocalZAxis=({hostBaselineLocalAxesLinear.M13:F4},{hostBaselineLocalAxesLinear.M23:F4},{hostBaselineLocalAxesLinear.M33:F4}), " +
|
||
$"宿主修正={_objectRotationCorrection}, 本地修正={localCorrection}, " +
|
||
$"hostComposedLocalXAxis=({hostComposedLocalAxesLinear.M11:F4},{hostComposedLocalAxesLinear.M21:F4},{hostComposedLocalAxesLinear.M31:F4}), " +
|
||
$"hostComposedLocalYAxis=({hostComposedLocalAxesLinear.M12:F4},{hostComposedLocalAxesLinear.M22:F4},{hostComposedLocalAxesLinear.M32:F4}), " +
|
||
$"hostComposedLocalZAxis=({hostComposedLocalAxesLinear.M13:F4},{hostComposedLocalAxesLinear.M23:F4},{hostComposedLocalAxesLinear.M33:F4})");
|
||
return true;
|
||
}
|
||
|
||
return RailPathPoseHelper.TryCreateRailRotation(
|
||
_route,
|
||
previousPoint,
|
||
currentPoint,
|
||
nextPoint,
|
||
railConvention,
|
||
_objectRotationCorrection,
|
||
out rotation);
|
||
}
|
||
|
||
private ModelAxisConvention GetCurrentModelAxisConvention()
|
||
{
|
||
if (IsVirtualObjectMode)
|
||
{
|
||
return ModelAxisConvention.CreateVirtualObjectAssetConvention();
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
return ModelAxisConvention.CreateDefaultForHost(adapter.HostType);
|
||
}
|
||
|
||
private bool TryCreatePlanarPathRotationAtStart(out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
|
||
if (_pathPoints == null || _pathPoints.Count < 2)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (PathTargetFrameResolver.TryResolvePlanarStartHostForward(
|
||
_route?.PathType ?? PathType.Ground,
|
||
_pathPoints,
|
||
out var hostForward))
|
||
{
|
||
return TryCreatePlanarPathRotationFromHostForward(
|
||
new Vector3D(hostForward.X, hostForward.Y, hostForward.Z),
|
||
out rotation);
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private bool TryCreatePlanarPathRotationForFrame(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
|
||
if (_route?.PathType == PathType.Hoisting)
|
||
{
|
||
if (_pathPoints == null || _pathPoints.Count < 3)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return TryCreatePlanarPathRotationFromHostForward(
|
||
new Vector3D(
|
||
_pathPoints[2].X - _pathPoints[1].X,
|
||
_pathPoints[2].Y - _pathPoints[1].Y,
|
||
_pathPoints[2].Z - _pathPoints[1].Z),
|
||
out rotation);
|
||
}
|
||
|
||
return TryCreatePlanarPathRotationFromHostPoints(previousPoint, currentPoint, nextPoint, out rotation);
|
||
}
|
||
|
||
private bool TryCreatePlanarPathRotationFromHostPoints(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
if (IsRealObjectMode)
|
||
{
|
||
Vector3 hostForward = ToNumerics(nextPoint) - ToNumerics(previousPoint);
|
||
if (hostForward.LengthSquared() < 1e-6f)
|
||
{
|
||
hostForward = ToNumerics(nextPoint) - ToNumerics(currentPoint);
|
||
}
|
||
|
||
return TryCreateRealObjectPlanarRotationFromHostForward(hostForward, out rotation);
|
||
}
|
||
|
||
var convention = GetCurrentModelAxisConvention();
|
||
Vector3 canonicalPrevious = ToNumerics(adapter.ToCanonicalPoint(previousPoint));
|
||
Vector3 canonicalCurrent = ToNumerics(adapter.ToCanonicalPoint(currentPoint));
|
||
Vector3 canonicalNext = ToNumerics(adapter.ToCanonicalPoint(nextPoint));
|
||
|
||
Vector3 forward = canonicalNext - canonicalPrevious;
|
||
if (forward.LengthSquared() < 1e-6f)
|
||
{
|
||
forward = canonicalNext - canonicalCurrent;
|
||
}
|
||
|
||
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||
forward,
|
||
HostCoordinateAdapter.CanonicalUpVector3,
|
||
convention,
|
||
correctionQuaternion,
|
||
out var quaternion))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
rotation = adapter.FromCanonicalRotation(quaternion);
|
||
return true;
|
||
}
|
||
|
||
private double GetAnimatedObjectRailNormalExtent(
|
||
Point3D previousPoint,
|
||
Point3D currentPoint,
|
||
Point3D nextPoint)
|
||
{
|
||
if (TryCalculateCurrentRealObjectRailProjectedExtents(
|
||
previousPoint,
|
||
currentPoint,
|
||
nextPoint,
|
||
out var convention,
|
||
out var extents))
|
||
{
|
||
LogManager.Debug(
|
||
$"[Rail法向尺寸] 真实物体有效尺寸: Forward={extents.forwardExtent:F3}, Side={extents.sideExtent:F3}, Up={extents.upExtent:F3}, " +
|
||
$"ForwardAxis={convention.ForwardAxis}, UpAxis={convention.UpAxis}, 角度={_objectRotationCorrection}");
|
||
return extents.upExtent;
|
||
}
|
||
|
||
return GetAnimatedObjectHeight();
|
||
}
|
||
|
||
private bool TryCreatePlanarPathRotationFromHostForward(Vector3D hostForward, out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
if (IsRealObjectMode)
|
||
{
|
||
return TryCreateRealObjectPlanarRotationFromHostForward(
|
||
new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z),
|
||
out rotation);
|
||
}
|
||
|
||
var convention = GetCurrentModelAxisConvention();
|
||
Vector3 canonicalForward = adapter.ToCanonicalVector3(new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z));
|
||
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||
canonicalForward,
|
||
HostCoordinateAdapter.CanonicalUpVector3,
|
||
convention,
|
||
correctionQuaternion,
|
||
out var quaternion))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
rotation = adapter.FromCanonicalRotation(quaternion);
|
||
return true;
|
||
}
|
||
|
||
|
||
private bool TryCreateRealObjectPlanarRotationFromHostForward(Vector3 hostForward, out Rotation3D rotation)
|
||
{
|
||
rotation = Rotation3D.Identity;
|
||
|
||
if (_route?.PathType == PathType.Hoisting &&
|
||
TryCreateHoistingRealObjectRotationFromActualPose(hostForward, out rotation))
|
||
{
|
||
return true;
|
||
}
|
||
|
||
if (_route?.PathType == PathType.Hoisting)
|
||
{
|
||
LogManager.Error("[吊装真实物体姿态] 无法基于实际几何姿态生成起点姿态，已禁止回退到 fragment 参考姿态。");
|
||
return false;
|
||
}
|
||
|
||
if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(
|
||
hostForward,
|
||
CoordinateSystemManager.Instance.CreateHostAdapter().HostType,
|
||
out var targetFrame))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
if (_route?.PathType == PathType.Ground)
|
||
{
|
||
Rotation3D actualRotation = Rotation3D.Identity;
|
||
bool hasActualRotation =
|
||
_animatedObject != null &&
|
||
ModelItemTransformHelper.TryGetCurrentGeometryRotation(_animatedObject, out actualRotation);
|
||
|
||
if (!hasActualRotation && _hasTrackedRotation)
|
||
{
|
||
actualRotation = _trackedRotation;
|
||
hasActualRotation = true;
|
||
}
|
||
|
||
if (!hasActualRotation)
|
||
{
|
||
LogManager.Error("[真实物体起点姿态] Ground 无法读取当前实际几何姿态，禁止回退到 reference-based pose。");
|
||
return false;
|
||
}
|
||
|
||
if (!TryResolveDisplayedPlanarYawFromRotation(actualRotation, adapter.HostType, out double currentYawRadians))
|
||
{
|
||
LogManager.Error("[真实物体起点姿态] Ground 无法从当前显示姿态解析宿主平面角。");
|
||
return false;
|
||
}
|
||
|
||
if (!PathTargetFrameResolver.TryResolvePlanarHostYaw(hostForward, adapter.HostType, out double targetYawRadians))
|
||
{
|
||
LogManager.Error("[真实物体起点姿态] Ground 无法从路径方向解析宿主平面角。");
|
||
return false;
|
||
}
|
||
|
||
Quaternion targetQuaternion = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose(
|
||
Rotation3DToHostQuaternion(actualRotation),
|
||
currentYawRadians,
|
||
targetYawRadians,
|
||
adapter.HostUpVector3);
|
||
rotation = adapter.FromHostQuaternionDirect(targetQuaternion);
|
||
|
||
Matrix4x4 targetLinear = Matrix4x4.CreateFromQuaternion(targetQuaternion);
|
||
LogManager.Debug(
|
||
$"[Ground宿主平面旋转] 当前Yaw={currentYawRadians * 180.0 / Math.PI:F2}°, 目标Yaw={targetYawRadians * 180.0 / Math.PI:F2}°, " +
|
||
$"hostXAxis=({targetLinear.M11:F4},{targetLinear.M21:F4},{targetLinear.M31:F4}), " +
|
||
$"hostYAxis=({targetLinear.M12:F4},{targetLinear.M22:F4},{targetLinear.M32:F4}), " +
|
||
$"hostZAxis=({targetLinear.M13:F4},{targetLinear.M23:F4},{targetLinear.M33:F4})");
|
||
return true;
|
||
}
|
||
|
||
if (!TryCreateRealObjectPlanarPoseSolution(
|
||
targetFrame.Forward,
|
||
targetFrame.Up,
|
||
out var solution))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection();
|
||
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, localCorrection);
|
||
rotation = adapter.FromHostQuaternionDirect(hostComposedQuaternion);
|
||
|
||
Matrix4x4 hostBaselineLocalAxesLinear = Matrix4x4.CreateFromQuaternion(solution.BaselineRotation);
|
||
Matrix4x4 hostComposedLocalAxesLinear = Matrix4x4.CreateFromQuaternion(hostComposedQuaternion);
|
||
|
||
LogManager.Debug(
|
||
$"[真实物体起点姿态] 选中参考轴=({solution.SelectedReferenceAxisLocal.X:F3},{solution.SelectedReferenceAxisLocal.Y:F3},{solution.SelectedReferenceAxisLocal.Z:F3}), " +
|
||
$"投影前进=({solution.ProjectedForward.X:F3},{solution.ProjectedForward.Y:F3},{solution.ProjectedForward.Z:F3}), " +
|
||
$"宿主修正={_objectRotationCorrection}, 本地修正={localCorrection}, " +
|
||
$"hostBaselineLocalXAxis=({hostBaselineLocalAxesLinear.M11:F4},{hostBaselineLocalAxesLinear.M21:F4},{hostBaselineLocalAxesLinear.M31:F4}), " +
|
||
$"hostBaselineLocalYAxis=({hostBaselineLocalAxesLinear.M12:F4},{hostBaselineLocalAxesLinear.M22:F4},{hostBaselineLocalAxesLinear.M32:F4}), " +
|
||
$"hostBaselineLocalZAxis=({hostBaselineLocalAxesLinear.M13:F4},{hostBaselineLocalAxesLinear.M23:F4},{hostBaselineLocalAxesLinear.M33:F4}), " +
|
||
$"hostComposedLocalXAxis=({hostComposedLocalAxesLinear.M11:F4},{hostComposedLocalAxesLinear.M21:F4},{hostComposedLocalAxesLinear.M31:F4}), " +
|
||
$"hostComposedLocalYAxis=({hostComposedLocalAxesLinear.M12:F4},{hostComposedLocalAxesLinear.M22:F4},{hostComposedLocalAxesLinear.M32:F4}), " +
|
||
$"hostComposedLocalZAxis=({hostComposedLocalAxesLinear.M13:F4},{hostComposedLocalAxesLinear.M23:F4},{hostComposedLocalAxesLinear.M33:F4})");
|
||
return true;
|
||
}
|
||
|
||
private bool TryCreateHoistingRealObjectRotationFromActualPose(Vector3 hostForward, out Rotation3D rotation)
|
||
{
|
||
if (!TryResolveHoistingActualPose(
|
||
hostForward,
|
||
out rotation,
|
||
out _,
|
||
out var selectedAxisDirection,
|
||
out var selectedAxisLocal,
|
||
out var projectedForward))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
_realObjectPlanarSelectedForwardAxis = selectedAxisDirection;
|
||
_hasRealObjectPlanarSelectedForwardAxis = true;
|
||
return true;
|
||
}
|
||
|
||
private bool TryCreateRealObjectPlanarPoseSolution(
|
||
Vector3 targetForward,
|
||
Vector3 targetUp,
|
||
out RealObjectPlanarPoseSolution solution)
|
||
{
|
||
solution = null;
|
||
|
||
if (!ShouldUseReferenceBasedRealObjectPlanarPose(_route?.PathType ?? PathType.Ground))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!TryGetRealObjectReferenceRotation(out var referenceRotation))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
LocalAxisDirection? fixedForwardAxis = null;
|
||
if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting)
|
||
{
|
||
// Ground/Hoisting 的真实物体前进轴采用固定对象语义：
|
||
// 真实物体默认以本地 +X 表示前进方向，不能因为路径更偏向 Z 就切换成 +Z。
|
||
fixedForwardAxis = LocalAxisDirection.PositiveX;
|
||
}
|
||
|
||
if (!RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
|
||
referenceRotation,
|
||
targetForward,
|
||
targetUp,
|
||
fixedForwardAxis,
|
||
out solution))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if ((_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) &&
|
||
(!_hasRealObjectPlanarSelectedForwardAxis || _realObjectPlanarSelectedForwardAxis != LocalAxisDirection.PositiveX))
|
||
{
|
||
_realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX;
|
||
_hasRealObjectPlanarSelectedForwardAxis = true;
|
||
LogManager.Debug("[真实物体平面前进轴] Ground/Hoisting 已固定使用 PositiveX 作为对象前进轴语义。");
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
private void ShowRealObjectFragmentUpMismatchHintIfNeeded(Vector3 representativeYAxis, Vector3 representativeZAxis)
|
||
{
|
||
if (_route == null || (_route.PathType != PathType.Ground && _route.PathType != PathType.Hoisting))
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (!IsRealObjectMode)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var doc = NavisApplication.ActiveDocument;
|
||
string hostUpAxis = FragmentDefaultUpContext.GetCurrentHostUpAxis(doc);
|
||
string configuredFragmentUpAxis = FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis(doc);
|
||
Vector3 hostUpVector = string.Equals(hostUpAxis, "Y", StringComparison.OrdinalIgnoreCase)
|
||
? Vector3.UnitY
|
||
: Vector3.UnitZ;
|
||
|
||
float yAlignment = Math.Abs(Vector3.Dot(Vector3.Normalize(representativeYAxis), hostUpVector));
|
||
float zAlignment = Math.Abs(Vector3.Dot(Vector3.Normalize(representativeZAxis), hostUpVector));
|
||
string detectedFragmentUpAxis = zAlignment > yAlignment ? "Z" : "Y";
|
||
|
||
if (string.Equals(configuredFragmentUpAxis, detectedFragmentUpAxis, StringComparison.OrdinalIgnoreCase))
|
||
{
|
||
return;
|
||
}
|
||
|
||
string documentKey = FragmentDefaultUpContext.GetCurrentDocumentKey(doc);
|
||
if (string.IsNullOrWhiteSpace(documentKey))
|
||
{
|
||
return;
|
||
}
|
||
|
||
string hintKey = $"{documentKey}|{configuredFragmentUpAxis}|{detectedFragmentUpAxis}";
|
||
lock (_realObjectFragmentUpMismatchHintsShown)
|
||
{
|
||
if (_realObjectFragmentUpMismatchHintsShown.Contains(hintKey))
|
||
{
|
||
return;
|
||
}
|
||
|
||
_realObjectFragmentUpMismatchHintsShown.Add(hintKey);
|
||
}
|
||
|
||
string message =
|
||
$"请到系统管理的坐标系设置中，将 Fragment默认Up 调整为 {detectedFragmentUpAxis}。";
|
||
LogManager.Info($"[fragment默认Up提示] 当前值={configuredFragmentUpAxis}, 检测值={detectedFragmentUpAxis}, 模型Up={hostUpAxis}。{message}");
|
||
DialogHelper.ShowMessageBox(
|
||
message,
|
||
"Fragment默认Up提示",
|
||
MessageBoxButton.OK,
|
||
MessageBoxImage.Information);
|
||
}
|
||
|
||
private bool TryGetRealObjectReferenceRotation(out Quaternion referenceRotation)
|
||
{
|
||
referenceRotation = Quaternion.Identity;
|
||
|
||
if (_hasRealObjectReferenceRotation)
|
||
{
|
||
referenceRotation = _realObjectReferenceRotation;
|
||
return true;
|
||
}
|
||
|
||
if (_originalTransform == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!ShouldUseReferenceBasedRealObjectPlanarPose(_route?.PathType ?? PathType.Ground))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!TryCaptureRealObjectReferenceRotation(_animatedObject, out referenceRotation))
|
||
{
|
||
string objectName = _animatedObject?.DisplayName ?? "未知对象";
|
||
LogManager.Error($"[真实物体参考姿态] {objectName} 无法获取 fragment 参考姿态，已禁止回退默认姿态。");
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
private bool TryCaptureRealObjectReferenceRotation(ModelItem sourceObject, out Quaternion referenceRotation)
|
||
{
|
||
referenceRotation = Quaternion.Identity;
|
||
|
||
if (sourceObject == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
try
|
||
{
|
||
var doc = NavisApplication.ActiveDocument;
|
||
if (!RealObjectReferencePoseResolver.TryResolveFromModelItem(sourceObject, doc, out var referencePose))
|
||
{
|
||
LogManager.Warning($"[真实物体参考姿态] {sourceObject.DisplayName} fragment 姿态解析失败");
|
||
return false;
|
||
}
|
||
|
||
referenceRotation = referencePose.Rotation;
|
||
_realObjectReferenceRotation = referenceRotation;
|
||
_realObjectReferenceAxisX = referencePose.RawAxisX;
|
||
_realObjectReferenceAxisY = referencePose.RawAxisY;
|
||
_realObjectReferenceAxisZ = referencePose.RawAxisZ;
|
||
_realObjectReferenceHostWorldXAxisLocalAxis = referencePose.HostWorldXAxisLocalAxis;
|
||
_realObjectReferenceHostWorldYAxisLocalAxis = referencePose.HostWorldYAxisLocalAxis;
|
||
_realObjectReferenceHostWorldZAxisLocalAxis = referencePose.HostWorldZAxisLocalAxis;
|
||
_realObjectReferenceHostSemanticXAxisLocalAxis = referencePose.HostSemanticXAxisLocalAxis;
|
||
_realObjectReferenceHostSemanticYAxisLocalAxis = referencePose.HostSemanticYAxisLocalAxis;
|
||
_realObjectReferenceHostSemanticZAxisLocalAxis = referencePose.HostSemanticZAxisLocalAxis;
|
||
_realObjectReferenceHostUpLocalAxis = referencePose.HostUpLocalAxis;
|
||
_hasRealObjectReferenceRotation = true;
|
||
|
||
LogManager.Info(
|
||
$"[真实物体参考姿态] {sourceObject.DisplayName} 使用 fragment 代表姿态: " +
|
||
$"RawX=({_realObjectReferenceAxisX.X:F4},{_realObjectReferenceAxisX.Y:F4},{_realObjectReferenceAxisX.Z:F4}), " +
|
||
$"RawY=({_realObjectReferenceAxisY.X:F4},{_realObjectReferenceAxisY.Y:F4},{_realObjectReferenceAxisY.Z:F4}), " +
|
||
$"RawZ=({_realObjectReferenceAxisZ.X:F4},{_realObjectReferenceAxisZ.Y:F4},{_realObjectReferenceAxisZ.Z:F4}), " +
|
||
$"宿主世界X对应本地轴={referencePose.HostWorldXAxisLocalAxis}, 宿主世界Y对应本地轴={referencePose.HostWorldYAxisLocalAxis}, 宿主世界Z对应本地轴={referencePose.HostWorldZAxisLocalAxis}, " +
|
||
$"宿主语义X=({referencePose.AxisX.X:F4},{referencePose.AxisX.Y:F4},{referencePose.AxisX.Z:F4}), " +
|
||
$"宿主语义Y=({referencePose.AxisY.X:F4},{referencePose.AxisY.Y:F4},{referencePose.AxisY.Z:F4}), " +
|
||
$"宿主语义Z=({referencePose.AxisZ.X:F4},{referencePose.AxisZ.Y:F4},{referencePose.AxisZ.Z:F4}), " +
|
||
$"宿主语义X对应本地轴={referencePose.HostSemanticXAxisLocalAxis}, 宿主语义Y对应本地轴={referencePose.HostSemanticYAxisLocalAxis}, 宿主语义Z对应本地轴={referencePose.HostSemanticZAxisLocalAxis}, " +
|
||
$"宿主Up对应本地轴={referencePose.HostUpLocalAxis}, fragment数量={referencePose.FragmentCount}, Fragment默认Up={referencePose.FragmentDefaultUpAxis}, 模型Up={referencePose.HostUpAxis}");
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[真实物体参考姿态] 从 fragment 提取代表姿态失败: {ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
private void SyncTrackedRotationToObjectReference(ModelItem sourceObject, bool isVirtualObject)
|
||
{
|
||
if (sourceObject == null)
|
||
{
|
||
_trackedRotation = Rotation3D.Identity;
|
||
_hasTrackedRotation = false;
|
||
_currentYaw = 0.0;
|
||
return;
|
||
}
|
||
|
||
if (isVirtualObject)
|
||
{
|
||
_trackedRotation = CreateVirtualObjectReferenceRotation();
|
||
_hasTrackedRotation = true;
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromRotation(_trackedRotation);
|
||
|
||
var linear = new Transform3D(_trackedRotation).Linear;
|
||
LogManager.Info(
|
||
$"[虚拟物体参考姿态] {sourceObject.DisplayName} 使用资产约定基姿态: " +
|
||
$"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " +
|
||
$"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " +
|
||
$"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})");
|
||
return;
|
||
}
|
||
|
||
if (!isVirtualObject &&
|
||
!ShouldUseReferenceBasedRealObjectPlanarPose(_route?.PathType ?? PathType.Ground) &&
|
||
ModelItemTransformHelper.TryGetCurrentGeometryRotation(sourceObject, out var groundActualGeometryRotation))
|
||
{
|
||
_trackedRotation = groundActualGeometryRotation;
|
||
_hasTrackedRotation = true;
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromRotation(_trackedRotation);
|
||
|
||
var linear = new Transform3D(_trackedRotation).Linear;
|
||
LogManager.Debug(
|
||
$"[真实物体当前姿态] {sourceObject.DisplayName} Ground 路径已改用实际几何姿态同步，不再读取 fragment 参考姿态: " +
|
||
$"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " +
|
||
$"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " +
|
||
$"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})");
|
||
return;
|
||
}
|
||
|
||
if (!isVirtualObject && TryCaptureRealObjectReferenceRotation(sourceObject, out Quaternion referenceRotation))
|
||
{
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
_trackedRotation = adapter.FromHostQuaternionDirect(referenceRotation);
|
||
_hasTrackedRotation = true;
|
||
_currentYaw = ModelItemTransformHelper.GetYawFromRotation(_trackedRotation);
|
||
return;
|
||
}
|
||
|
||
if (!ShouldUseReferenceBasedRealObjectPlanarPose(_route?.PathType ?? PathType.Ground))
|
||
{
|
||
// Ground/Hoisting 路径：无需参考姿态，使用单位旋转
|
||
_trackedRotation = Rotation3D.Identity;
|
||
_hasTrackedRotation = true;
|
||
_currentYaw = 0.0;
|
||
return;
|
||
}
|
||
|
||
throw new InvalidOperationException(
|
||
$"[真实物体参考姿态] {sourceObject.DisplayName} 无法同步参考姿态：未能读取当前实际几何姿态，fragment 代表姿态也不可用，已禁止回退 Transform。");
|
||
}
|
||
|
||
private static Rotation3D CreateVirtualObjectReferenceRotation()
|
||
{
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
var convention = GetVirtualObjectAssetConvention();
|
||
return convention.CreateRotation(new Vector3D(1, 0, 0), adapter.HostUpVector);
|
||
}
|
||
|
||
private static ModelAxisConvention GetVirtualObjectAssetConvention()
|
||
{
|
||
return ModelAxisConvention.CreateVirtualObjectAssetConvention();
|
||
}
|
||
|
||
private void ResetRealObjectReferenceRotation()
|
||
{
|
||
_realObjectReferenceRotation = Quaternion.Identity;
|
||
_realObjectReferenceAxisX = Vector3.UnitX;
|
||
_realObjectReferenceAxisY = Vector3.UnitY;
|
||
_realObjectReferenceAxisZ = Vector3.UnitZ;
|
||
_realObjectReferenceHostWorldXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||
_realObjectReferenceHostWorldYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||
_realObjectReferenceHostWorldZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||
_realObjectReferenceHostSemanticXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||
_realObjectReferenceHostSemanticYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||
_realObjectReferenceHostSemanticZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||
_realObjectReferenceHostUpLocalAxis = LocalAxisDirection.PositiveY;
|
||
_hasRealObjectReferenceRotation = false;
|
||
_realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX;
|
||
_hasRealObjectPlanarSelectedForwardAxis = false;
|
||
}
|
||
|
||
private void ResetPlanarRealObjectBasePoseCache()
|
||
{
|
||
_groundRealObjectBaseRotation = Rotation3D.Identity;
|
||
_groundRealObjectBaseYaw = 0.0;
|
||
_hasGroundRealObjectBasePose = false;
|
||
}
|
||
|
||
private LocalEulerRotationCorrection ResolveRealObjectLocalRotationCorrection()
|
||
{
|
||
return HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes(
|
||
_objectRotationCorrection,
|
||
_realObjectReferenceHostWorldXAxisLocalAxis,
|
||
_realObjectReferenceHostWorldYAxisLocalAxis,
|
||
_realObjectReferenceHostWorldZAxisLocalAxis);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置物体绕宿主 X/Y/Z 轴的角度修正
|
||
/// </summary>
|
||
public void SetObjectRotationCorrection(LocalEulerRotationCorrection rotationCorrection)
|
||
{
|
||
_objectStartPlacementMode = ObjectStartPlacementMode.AlignToPathPose;
|
||
_hasPathPreservedPoseRotation = false;
|
||
_objectRotationCorrection = rotationCorrection;
|
||
|
||
// 如果动画已创建，更新物体到起点的朝向
|
||
if (_animatedObject != null && _pathPoints != null && _pathPoints.Count > 0)
|
||
{
|
||
try
|
||
{
|
||
// 重新计算并应用朝向
|
||
MoveObjectToPathStart();
|
||
LogManager.Info($"[角度修正] 物体角度已更新: {_objectRotationCorrection}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[角度修正] 更新物体角度失败: {ex.Message}");
|
||
}
|
||
}
|
||
}
|
||
|
||
public void ApplyObjectStartPlacementRequest(ObjectStartPlacementRequest request, bool skipCadRestore = false)
|
||
{
|
||
_objectStartPlacementMode = request.PlacementMode;
|
||
if (_objectStartPlacementMode != ObjectStartPlacementMode.PreserveInitialPose)
|
||
{
|
||
_hasPathPreservedPoseRotation = false;
|
||
}
|
||
|
||
_objectRotationCorrection = request.RotationCorrection;
|
||
_groundPathObjectLiftHeight = UnitsConverter.ConvertFromMeters(request.VerticalLiftInMeters);
|
||
|
||
LogManager.Debug(
|
||
$"[起点摆放] 已应用物体调整请求: 模式={_objectStartPlacementMode}, 角度={_objectRotationCorrection}, " +
|
||
$"上下偏移={request.VerticalLiftInMeters:F3}m");
|
||
|
||
if (_animatedObject != null && _pathPoints != null && _pathPoints.Count > 0)
|
||
{
|
||
try
|
||
{
|
||
MoveObjectToPathStartUsingCurrentPlacementMode(skipCadRestore: skipCadRestore);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[起点摆放] 应用物体调整请求失败: {ex.Message}");
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 单轴兼容入口：按当前宿主 up 轴映射成三轴角度修正。
|
||
/// </summary>
|
||
public void SetObjectRotationCorrection(double rotationCorrection)
|
||
{
|
||
SetObjectRotationCorrection(CreateSingleAxisUpCorrection(rotationCorrection));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 直接设置物体绕宿主 X/Y/Z 轴的角度修正（不触发物体旋转）
|
||
/// </summary>
|
||
public void SetObjectRotationCorrectionDirect(LocalEulerRotationCorrection rotationCorrection)
|
||
{
|
||
_objectRotationCorrection = rotationCorrection;
|
||
ResetPlanarRealObjectBasePoseCache();
|
||
LogManager.Debug($"[角度修正] 直接设置角度修正值: {_objectRotationCorrection}（不触发旋转）");
|
||
}
|
||
|
||
public void SetObjectStartVerticalLiftDirect(double verticalLiftInMeters)
|
||
{
|
||
_groundPathObjectLiftHeight = UnitsConverter.ConvertFromMeters(verticalLiftInMeters);
|
||
LogManager.Debug($"[起点摆放] 直接设置地面路径上下偏移: {verticalLiftInMeters:F3}m");
|
||
}
|
||
|
||
public void SetObjectStartPlacementMode(ObjectStartPlacementMode placementMode)
|
||
{
|
||
_objectStartPlacementMode = placementMode;
|
||
if (placementMode != ObjectStartPlacementMode.PreserveInitialPose)
|
||
{
|
||
_hasPathPreservedPoseRotation = false;
|
||
}
|
||
LogManager.Debug($"[起点摆放] 当前模式已设置为: {_objectStartPlacementMode}");
|
||
}
|
||
|
||
private bool MoveObjectToPathStartUsingCurrentPlacementMode(ModelItem animatedObject = null, List<Point3D> pathPoints = null, bool skipCadRestore = false)
|
||
{
|
||
return _objectStartPlacementMode == ObjectStartPlacementMode.PreserveInitialPose
|
||
? MoveObjectToPathStartPreservingInitialPose(animatedObject, pathPoints)
|
||
: MoveObjectToPathStart(animatedObject, pathPoints, skipCadRestore);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 单轴兼容入口：按当前宿主 up 轴映射成三轴角度修正。
|
||
/// </summary>
|
||
public void SetObjectRotationCorrectionDirect(double rotationCorrection)
|
||
{
|
||
SetObjectRotationCorrectionDirect(CreateSingleAxisUpCorrection(rotationCorrection));
|
||
}
|
||
|
||
private LocalEulerRotationCorrection CreateSingleAxisUpCorrection(double rotationCorrection)
|
||
{
|
||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||
return adapter.HostType == CoordinateSystemType.YUp
|
||
? new LocalEulerRotationCorrection(0.0, rotationCorrection, 0.0)
|
||
: new LocalEulerRotationCorrection(0.0, 0.0, rotationCorrection);
|
||
}
|
||
|
||
internal static Vector3 ResolveGroundPathObjectLiftOffsetVector(
|
||
double verticalLiftModelUnits,
|
||
CoordinateSystemType hostType)
|
||
{
|
||
if (Math.Abs(verticalLiftModelUnits) < 1e-9)
|
||
{
|
||
return Vector3.Zero;
|
||
}
|
||
|
||
var adapter = new HostCoordinateAdapter(hostType);
|
||
return Vector3.Normalize(adapter.HostUpVector3) * (float)verticalLiftModelUnits;
|
||
}
|
||
|
||
private Point3D ApplyGroundPathObjectLiftOffset(Point3D trackedCenter)
|
||
{
|
||
if (_route == null || _route.PathType != PathType.Ground || Math.Abs(_groundPathObjectLiftHeight) < 1e-9)
|
||
{
|
||
return trackedCenter;
|
||
}
|
||
|
||
Vector3 offset = ResolveGroundPathObjectLiftOffsetVector(
|
||
_groundPathObjectLiftHeight,
|
||
CoordinateSystemManager.Instance.ResolvedType);
|
||
|
||
return new Point3D(
|
||
trackedCenter.X + offset.X,
|
||
trackedCenter.Y + offset.Y,
|
||
trackedCenter.Z + offset.Z);
|
||
}
|
||
|
||
#region 动画实现方法
|
||
|
||
/// <summary>
|
||
/// DispatcherTimer动画处理核心
|
||
/// </summary>
|
||
/// <summary>
|
||
/// 根据当前帧索引更新碰撞高亮（状态跟踪优化版本）
|
||
/// </summary>
|
||
private void UpdateCollisionHighlightFromFrame()
|
||
{
|
||
try
|
||
{
|
||
// 确保帧索引有效
|
||
if (_currentFrameIndex < 0 || _currentFrameIndex >= _animationFrames.Count)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// 获取当前帧数据
|
||
var currentFrame = _animationFrames[_currentFrameIndex];
|
||
bool currentHasCollision = currentFrame.HasCollision;
|
||
|
||
// 收集当前帧碰撞对象的集合
|
||
var currentCollisionObjects = new HashSet<ModelItem>();
|
||
if (currentHasCollision && currentFrame.Collisions != null)
|
||
{
|
||
foreach (var collision in currentFrame.Collisions)
|
||
{
|
||
if (collision.Item1 != null)
|
||
currentCollisionObjects.Add(collision.Item1);
|
||
if (collision.Item2 != null)
|
||
currentCollisionObjects.Add(collision.Item2);
|
||
}
|
||
}
|
||
|
||
// 检查是否需要更新高亮：
|
||
// 1. 碰撞状态改变（有碰撞 vs 无碰撞）
|
||
// 2. 碰撞对象集合改变（碰撞的物体变了）
|
||
bool stateChanged = currentHasCollision != _lastHighlightState;
|
||
bool objectsChanged = !currentCollisionObjects.SetEquals(_lastCollisionObjects);
|
||
|
||
if (stateChanged || objectsChanged)
|
||
{
|
||
if (currentHasCollision)
|
||
{
|
||
// 有碰撞：高亮当前帧的碰撞对象（使用CollisionResultsCategory的默认颜色）
|
||
ModelHighlightHelper.ManageCollisionHighlightsByCategory(ModelHighlightHelper.PrecomputeCollisionResultsCategory, currentFrame.Collisions, clearOtherCategories: true);
|
||
LogManager.Debug($"[高亮状态] 帧{_currentFrameIndex}: 高亮碰撞 ({currentFrame.Collisions.Count}个, 对象{currentCollisionObjects.Count}个)");
|
||
}
|
||
else
|
||
{
|
||
// 无碰撞：清除碰撞高亮（保留物体高亮）
|
||
ModelHighlightHelper.ClearCategory(ModelHighlightHelper.PrecomputeCollisionResultsCategory);
|
||
LogManager.Debug($"[高亮状态] 帧{_currentFrameIndex}: 清除碰撞高亮");
|
||
}
|
||
|
||
// 更新状态记录
|
||
_lastHighlightState = currentHasCollision;
|
||
_lastCollisionObjects = currentCollisionObjects;
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"更新碰撞高亮失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 统一的动画帧处理逻辑（DispatcherTimer专用）
|
||
/// </summary>
|
||
private void ProcessAnimationFrame()
|
||
{
|
||
// 检查动画状态
|
||
if (_currentState != AnimationState.Playing)
|
||
{
|
||
return; // 非播放状态，跳过更新
|
||
}
|
||
|
||
// DispatcherTimer已经控制了调用间隔，无需手动帧率控制
|
||
var now = DateTime.Now;
|
||
|
||
// 计算下一帧索引（考虑播放方向和速度）
|
||
int nextFrameIndex = _currentFrameIndex;
|
||
|
||
if (_playbackDirection == 1) // 正向播放
|
||
{
|
||
nextFrameIndex = _currentFrameIndex + 1;
|
||
if (nextFrameIndex >= _animationFrames.Count)
|
||
{
|
||
// 正向播放到达终点
|
||
nextFrameIndex = _animationFrames.Count - 1;
|
||
StopAnimationPlayback();
|
||
FinishAnimation();
|
||
return;
|
||
}
|
||
}
|
||
else // 反向播放
|
||
{
|
||
nextFrameIndex = _currentFrameIndex - 1;
|
||
if (nextFrameIndex < 0)
|
||
{
|
||
// 反向播放到达起点
|
||
nextFrameIndex = 0;
|
||
StopAnimationPlayback();
|
||
FinishAnimation();
|
||
return;
|
||
}
|
||
}
|
||
|
||
// 更新帧索引和位置
|
||
if (nextFrameIndex != _currentFrameIndex)
|
||
{
|
||
_currentFrameIndex = nextFrameIndex;
|
||
|
||
// 使用预计算的帧位置和朝向
|
||
if (_currentFrameIndex < _animationFrames.Count)
|
||
{
|
||
var frameData = _animationFrames[_currentFrameIndex];
|
||
ApplyRecordedPose(frameData, _currentFrameIndex);
|
||
|
||
// 更新碰撞高亮（基于预计算结果）
|
||
UpdateCollisionHighlightFromFrame();
|
||
|
||
// 计算当前进度（基于帧索引）
|
||
double progress = _animationFrames.Count > 1 ? (double)_currentFrameIndex / (_animationFrames.Count - 1) : 0;
|
||
|
||
// 触发进度事件
|
||
var progressPercent = progress * 100;
|
||
ProgressChanged?.Invoke(this, progressPercent);
|
||
|
||
// 同步进度到 TimeLiner（如果可用）
|
||
if (_timeLinerManager != null && !string.IsNullOrEmpty(_currentTaskId))
|
||
{
|
||
_timeLinerManager.UpdateTaskProgress(_currentTaskId, progress, _currentState);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 记录帧时间和统计
|
||
_lastFrameTime = now;
|
||
|
||
// 性能监控和自动切换
|
||
UpdateFPSCounterAndCheckSwitch();
|
||
}
|
||
|
||
private void ApplyRecordedPose(AnimationFrame frameData, int frameIndex)
|
||
{
|
||
if (frameData == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
ApplyResolvedObjectPose(
|
||
frameData.Position,
|
||
frameData.Rotation,
|
||
frameData.HasCustomRotation,
|
||
frameIndex,
|
||
frameData.PlanarHostYawRadians.HasValue
|
||
? (double?)ResolveAnimationFramePlaybackYawRadians(frameData)
|
||
: null);
|
||
}
|
||
|
||
private void ApplyResolvedObjectPose(
|
||
Point3D targetPosition,
|
||
Rotation3D targetRotation,
|
||
bool hasCustomRotation,
|
||
int frameIndex,
|
||
double? planarHostYawRadians = null)
|
||
{
|
||
if (ShouldUsePlanarRealObjectPureIncrementFrames(_route?.PathType ?? PathType.Ground, IsRealObjectMode) &&
|
||
planarHostYawRadians.HasValue)
|
||
{
|
||
ApplyPlanarTrackedPose(targetPosition, planarHostYawRadians.Value);
|
||
return;
|
||
}
|
||
|
||
if (hasCustomRotation)
|
||
{
|
||
ApplyFullPoseUpdate(targetPosition, targetRotation);
|
||
return;
|
||
}
|
||
|
||
string pathTypeName = (_route?.PathType ?? PathType.Ground).GetDisplayName();
|
||
throw new InvalidOperationException(
|
||
$"{pathTypeName}记录姿态 {frameIndex} 缺少完整姿态，禁止退回旧 yaw 链路。");
|
||
}
|
||
|
||
private double ResolveAnimationFramePlaybackYawRadians(AnimationFrame frameData)
|
||
{
|
||
return ResolveAnimationFramePlaybackYawRadians(
|
||
frameData,
|
||
_objectRotationCorrection,
|
||
CoordinateSystemManager.Instance.ResolvedType);
|
||
}
|
||
|
||
internal static double ResolveAnimationFramePlaybackYawRadians(
|
||
AnimationFrame frameData,
|
||
LocalEulerRotationCorrection rotationCorrection,
|
||
CoordinateSystemType hostType)
|
||
{
|
||
if (frameData == null)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
if (frameData.PlanarHostYawRadians.HasValue)
|
||
{
|
||
return NormalizeRadians(
|
||
frameData.PlanarHostYawRadians.Value +
|
||
ResolvePlanarHostUpCorrectionRadians(
|
||
rotationCorrection,
|
||
hostType));
|
||
}
|
||
|
||
double resolvedYaw = frameData.YawRadians;
|
||
if (!frameData.HasCustomRotation && !rotationCorrection.IsZero)
|
||
{
|
||
resolvedYaw += rotationCorrection.ZDegrees * Math.PI / 180.0;
|
||
}
|
||
|
||
return NormalizeRadians(resolvedYaw);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 应用平面路径的已记录姿态。使用业务跟踪点 _trackedPosition 作为当前位置，不读取实时包围盒中心回写主链。
|
||
/// </summary>
|
||
private void ApplyPlanarTrackedPose(Point3D targetPosition, double targetYawRadians)
|
||
{
|
||
var controlledObject = CurrentControlledObject;
|
||
if (controlledObject == null) return;
|
||
|
||
Point3D currentPosition = _trackedPosition;
|
||
double currentYawRadians = _currentYaw;
|
||
double deltaYawRadians = NormalizeRadians(targetYawRadians - currentYawRadians);
|
||
Vector3 hostUp = Vector3.Normalize(CoordinateSystemManager.Instance.CreateHostAdapter().HostUpVector3);
|
||
string pathTypeName = (_route?.PathType ?? PathType.Ground).GetDisplayName();
|
||
|
||
LogManager.Debug(
|
||
$"[{pathTypeName}纯增量逐帧] 当前Yaw={currentYawRadians * 180.0 / Math.PI:F2}°, " +
|
||
$"目标Yaw={targetYawRadians * 180.0 / Math.PI:F2}°, 增量Yaw={deltaYawRadians * 180.0 / Math.PI:F2}°, " +
|
||
$"当前跟踪点=({currentPosition.X:F3},{currentPosition.Y:F3},{currentPosition.Z:F3}), " +
|
||
$"目标跟踪点=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3})");
|
||
|
||
ModelItemTransformHelper.MoveItemIncrementallyByAxisRotationAndTranslation(
|
||
controlledObject,
|
||
currentPosition,
|
||
hostUp,
|
||
deltaYawRadians,
|
||
targetPosition);
|
||
|
||
_trackedPosition = targetPosition;
|
||
if (ModelItemTransformHelper.TryGetCurrentGeometryRotation(controlledObject, out Rotation3D appliedRotation))
|
||
{
|
||
_trackedRotation = appliedRotation;
|
||
_hasTrackedRotation = true;
|
||
}
|
||
_currentYaw = targetYawRadians;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 更新FPS计数器（DispatcherTimer模式）
|
||
/// </summary>
|
||
private void UpdateFPSCounterAndCheckSwitch()
|
||
{
|
||
_fpsFrameCount++;
|
||
var elapsed = (DateTime.Now - _fpsCounterStart).TotalSeconds;
|
||
|
||
// 每秒更新一次FPS统计
|
||
if (elapsed >= 1.0)
|
||
{
|
||
_actualFPS = _fpsFrameCount / elapsed;
|
||
LogManager.Debug($"[性能监控] 实际FPS: {_actualFPS:F1} (目标: {_animationFrameRate}) - DispatcherTimer模式");
|
||
|
||
// 重置计数器
|
||
_fpsFrameCount = 0;
|
||
_fpsCounterStart = DateTime.Now;
|
||
}
|
||
}
|
||
|
||
#region 碰撞测试优化方法
|
||
|
||
/// <summary>
|
||
/// 计算动画配置的哈希值（基于路径点和对象）
|
||
/// </summary>
|
||
private string ComputeAnimationConfigHash(ModelItem animatedObject, List<Point3D> pathPoints)
|
||
{
|
||
try
|
||
{
|
||
var sb = new StringBuilder();
|
||
|
||
// 包含对象名称
|
||
sb.Append(animatedObject?.DisplayName ?? "null");
|
||
sb.Append("|");
|
||
|
||
// 包含路径点信息
|
||
if (pathPoints != null)
|
||
{
|
||
foreach (var point in pathPoints)
|
||
{
|
||
sb.Append($"{point.X:F2},{point.Y:F2},{point.Z:F2};");
|
||
}
|
||
}
|
||
|
||
// 计算哈希
|
||
using (var md5 = System.Security.Cryptography.MD5.Create())
|
||
{
|
||
var inputBytes = System.Text.Encoding.UTF8.GetBytes(sb.ToString());
|
||
var hashBytes = md5.ComputeHash(inputBytes);
|
||
return BitConverter.ToString(hashBytes).Replace("-", "");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"计算动画配置哈希失败: {ex.Message}");
|
||
return DateTime.Now.Ticks.ToString(); // 回退到时间戳
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算动画配置的哈希值（基于PathRoute的Edges，可以区分不同的曲线化路径）
|
||
/// </summary>
|
||
public string ComputeAnimationConfigHashFromRoute(ModelItem animatedObject, PathRoute route)
|
||
{
|
||
try
|
||
{
|
||
var sb = new StringBuilder();
|
||
|
||
// 包含对象的唯一标识（使用PathId而不是DisplayName）
|
||
if (animatedObject != null)
|
||
{
|
||
try
|
||
{
|
||
var pathId = Autodesk.Navisworks.Api.Application.ActiveDocument.Models.CreatePathId(animatedObject);
|
||
sb.Append($"{pathId.ModelIndex}:{pathId.PathId}");
|
||
}
|
||
catch
|
||
{
|
||
sb.Append(animatedObject.DisplayName ?? "null");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
sb.Append("null");
|
||
}
|
||
sb.Append("|");
|
||
|
||
// 包含路径ID（区分不同的路径）
|
||
sb.Append(route?.Id ?? "null");
|
||
sb.Append("|");
|
||
|
||
// 包含动画参数（确保参数改变时重新检测）
|
||
sb.Append($"Duration:{_animationDuration:F2}s");
|
||
sb.Append($"|FrameRate:{_animationFrameRate}");
|
||
sb.Append($"|DetectionTolerance:{_detectionTolerance:F4}");
|
||
sb.Append("|");
|
||
|
||
// 包含虚拟物体尺寸（确保物体尺寸改变时重新检测）
|
||
// 使用传入的物体尺寸参数，而不是从ConfigManager读取
|
||
var objectSizeString = $"Object:{_virtualObjectLength:F2}x{_virtualObjectWidth:F2}x{_virtualObjectHeight:F2}";
|
||
sb.Append(objectSizeString);
|
||
sb.Append("|");
|
||
|
||
// 包含角度修正（确保角度修正改变时重新检测）
|
||
sb.Append($"RotationCorrection:{_objectRotationCorrection}");
|
||
sb.Append("|");
|
||
|
||
// 包含手工检测对象列表（确保手工指定模式的目标变化时重新检测）
|
||
// 使用PathId作为唯一标识，而不是DisplayName
|
||
if (_manualCollisionOverrideEnabled && _manualCollisionTargets != null && _manualCollisionTargets.Count > 0)
|
||
{
|
||
sb.Append("ManualTargets:");
|
||
foreach (var target in _manualCollisionTargets.OrderBy(t => t.DisplayName))
|
||
{
|
||
try
|
||
{
|
||
var targetPathId = Autodesk.Navisworks.Api.Application.ActiveDocument.Models.CreatePathId(target);
|
||
sb.Append($"{targetPathId.ModelIndex}:{targetPathId.PathId}");
|
||
sb.Append(",");
|
||
}
|
||
catch (Exception)
|
||
{
|
||
// 忽略无效对象
|
||
}
|
||
}
|
||
sb.Append("|");
|
||
}
|
||
|
||
// 包含路径边的摘要信息（区分不同的曲线化结果）
|
||
if (route?.Edges != null && route.Edges.Count > 0)
|
||
{
|
||
foreach (var edge in route.Edges)
|
||
{
|
||
sb.Append(edge.SegmentType.ToString());
|
||
sb.Append($"({edge.PhysicalLength:F2})");
|
||
if (edge.SegmentType == PathSegmentType.Arc && edge.Trajectory != null)
|
||
{
|
||
sb.Append($"_R{edge.Trajectory.ActualRadius:F2}");
|
||
sb.Append($"_A{edge.Trajectory.DeflectionAngle:F2}");
|
||
}
|
||
sb.Append(";");
|
||
}
|
||
}
|
||
|
||
// 包含路径最后修改时间（确保路径修改后重新生成动画）
|
||
sb.Append($"LastModified:{route?.LastModified:yyyyMMddHHmmss}");
|
||
sb.Append("|");
|
||
|
||
// 🔥 包含排除列表（确保排除列表变化时重新检测）
|
||
if (_excludedObjects != null && _excludedObjects.Count > 0)
|
||
{
|
||
sb.Append("Excluded:");
|
||
foreach (var excluded in _excludedObjects.OrderBy(e => e.DisplayName))
|
||
{
|
||
try
|
||
{
|
||
var excludedPathId = Autodesk.Navisworks.Api.Application.ActiveDocument.Models.CreatePathId(excluded);
|
||
sb.Append($"{excludedPathId.ModelIndex}:{excludedPathId.PathId}");
|
||
sb.Append(",");
|
||
}
|
||
catch (Exception)
|
||
{
|
||
// 忽略无效对象
|
||
}
|
||
}
|
||
sb.Append("|");
|
||
}
|
||
|
||
// 计算哈希
|
||
using (var md5 = System.Security.Cryptography.MD5.Create())
|
||
{
|
||
var inputBytes = System.Text.Encoding.UTF8.GetBytes(sb.ToString());
|
||
var hashBytes = md5.ComputeHash(inputBytes);
|
||
return BitConverter.ToString(hashBytes).Replace("-", "");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"计算动画配置哈希失败: {ex.Message}");
|
||
return DateTime.Now.Ticks.ToString(); // 回退到时间戳
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除所有配置哈希映射
|
||
/// </summary>
|
||
public static void ClearAllCollisionTestRecords()
|
||
{
|
||
_animationHashToRecordId.Clear();
|
||
LogManager.Info("已清除所有配置哈希映射");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据动画配置哈希获取检测记录ID
|
||
/// </summary>
|
||
public int? GetDetectionRecordIdByHash(string animationHash)
|
||
{
|
||
if (string.IsNullOrEmpty(animationHash))
|
||
return null;
|
||
|
||
if (_animationHashToRecordId.TryGetValue(animationHash, out var recordId))
|
||
{
|
||
return recordId;
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 注册动画配置哈希与检测记录ID的映射
|
||
/// </summary>
|
||
public void RegisterAnimationHashToRecordId(string animationHash, int recordId)
|
||
{
|
||
if (!string.IsNullOrEmpty(animationHash))
|
||
{
|
||
_animationHashToRecordId[animationHash] = recordId;
|
||
LogManager.Info($"[AnimationHashMapping] 注册映射: Hash={animationHash.Substring(0, 8)}... -> RecordId={recordId}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 移除动画配置哈希映射
|
||
/// </summary>
|
||
public void RemoveDetectionRecordHash(string animationHash)
|
||
{
|
||
if (!string.IsNullOrEmpty(animationHash) && _animationHashToRecordId.ContainsKey(animationHash))
|
||
{
|
||
_animationHashToRecordId.Remove(animationHash);
|
||
LogManager.Info($"[AnimationHashMapping] 移除映射: Hash={animationHash.Substring(0, 8)}...");
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region 碰撞排除列表缓存管理（从LogisticsAnimationManager迁移）
|
||
|
||
/// <summary>
|
||
/// 为动画对象预计算并缓存排除列表
|
||
/// </summary>
|
||
/// <param name="animationObject">动画对象</param>
|
||
/// <returns>是否成功缓存</returns>
|
||
public bool PrecomputeCollisionExclusions(ModelItem animationObject)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info($"[缓存管理] 开始为动画对象预计算排除列表: {animationObject?.DisplayName ?? "NULL"}");
|
||
|
||
if (animationObject == null)
|
||
{
|
||
LogManager.Warning("[缓存管理] 动画对象为空，清除缓存");
|
||
ClearExclusionCache();
|
||
return false;
|
||
}
|
||
|
||
// 检查是否需要重新计算
|
||
if (IsCacheValid(animationObject))
|
||
{
|
||
LogManager.Info($"[缓存管理] 缓存仍然有效，跳过重新计算");
|
||
return true;
|
||
}
|
||
|
||
var startTime = DateTime.Now;
|
||
|
||
// 只构建特定于动画对象的逻辑排除列表
|
||
LogManager.Info("[缓存管理] 构建逻辑排除列表...");
|
||
var exclusionList = ModelItemAnalysisHelper.CollectRelatedNodes(animationObject);
|
||
|
||
// 更新缓存
|
||
_currentCachedAnimationObject = animationObject;
|
||
_cachedExclusionList = exclusionList;
|
||
_cacheCreatedTime = DateTime.Now;
|
||
|
||
var elapsedMs = (DateTime.Now - startTime).TotalMilliseconds;
|
||
LogManager.Info($"[缓存管理] 预计算完成，耗时 {elapsedMs:F1}ms，缓存 {exclusionList.Count} 个排除对象");
|
||
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[缓存管理] 预计算排除列表失败: {ex.Message}", ex);
|
||
ClearExclusionCache();
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取缓存的排除列表
|
||
/// </summary>
|
||
/// <param name="animationObject">动画对象</param>
|
||
/// <returns>排除列表，如果缓存无效则返回null</returns>
|
||
public List<ModelItem> GetCachedExclusionList(ModelItem animationObject)
|
||
{
|
||
try
|
||
{
|
||
if (!IsCacheValid(animationObject))
|
||
{
|
||
LogManager.Debug($"[缓存管理] 缓存无效，建议重新预计算");
|
||
return null;
|
||
}
|
||
|
||
LogManager.Debug($"[缓存管理] 返回缓存的排除列表: {_cachedExclusionList?.Count ?? 0} 个对象");
|
||
return _cachedExclusionList;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[缓存管理] 获取缓存失败: {ex.Message}");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查缓存是否有效
|
||
/// </summary>
|
||
/// <param name="animationObject">当前动画对象</param>
|
||
/// <returns>缓存是否有效</returns>
|
||
private bool IsCacheValid(ModelItem animationObject)
|
||
{
|
||
try
|
||
{
|
||
// 检查是否有缓存
|
||
if (_currentCachedAnimationObject == null || _cachedExclusionList == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
// 检查对象是否相同
|
||
if (!_currentCachedAnimationObject.Equals(animationObject))
|
||
{
|
||
string cachedObjectName = ModelItemAnalysisHelper.GetSafeDisplayName(_currentCachedAnimationObject);
|
||
string currentObjectName = ModelItemAnalysisHelper.GetSafeDisplayName(animationObject);
|
||
LogManager.Debug($"[缓存验证] 动画对象已变更: '{cachedObjectName}' -> '{currentObjectName}'");
|
||
return false;
|
||
}
|
||
|
||
// 检查缓存时间是否过期
|
||
if (DateTime.Now - _cacheCreatedTime > _cacheValidDuration)
|
||
{
|
||
LogManager.Debug($"[缓存验证] 缓存已过期: {(DateTime.Now - _cacheCreatedTime).TotalMinutes:F1} 分钟");
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[缓存验证] 验证过程异常: {ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除排除列表缓存
|
||
/// </summary>
|
||
public void ClearExclusionCache()
|
||
{
|
||
try
|
||
{
|
||
if (_currentCachedAnimationObject != null)
|
||
{
|
||
try
|
||
{
|
||
// 安全访问DisplayName，防止WeakRef已释放的警告
|
||
string objectName = ModelItemAnalysisHelper.GetSafeDisplayName(_currentCachedAnimationObject);
|
||
LogManager.Debug($"[缓存管理] 清除缓存: {objectName}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"[缓存管理] 清除缓存时访问对象名称失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
_currentCachedAnimationObject = null;
|
||
_cachedExclusionList = null;
|
||
_cacheCreatedTime = DateTime.MinValue;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[缓存管理] 清除缓存失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取缓存统计信息
|
||
/// </summary>
|
||
/// <returns>缓存统计信息</returns>
|
||
public string GetCacheStats()
|
||
{
|
||
try
|
||
{
|
||
if (_currentCachedAnimationObject == null)
|
||
{
|
||
return "缓存状态: 无缓存";
|
||
}
|
||
|
||
var age = DateTime.Now - _cacheCreatedTime;
|
||
string objectName = ModelItemAnalysisHelper.GetSafeDisplayName(_currentCachedAnimationObject);
|
||
var count = _cachedExclusionList?.Count ?? 0;
|
||
|
||
return $"缓存状态: 对象='{objectName}', 排除数={count}, 年龄={age.TotalSeconds:F1}秒";
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
return $"缓存状态: 获取失败 - {ex.Message}";
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Human-in-the-Loop 排除列表管理
|
||
|
||
/// <summary>
|
||
/// 添加排除物体
|
||
/// </summary>
|
||
/// <param name="obj">要排除的物体</param>
|
||
public void AddExcludedObject(ModelItem obj)
|
||
{
|
||
if (obj != null)
|
||
{
|
||
_excludedObjects.Add(obj);
|
||
LogManager.Debug($"[排除列表] 添加排除物体: {ModelItemAnalysisHelper.GetSafeDisplayName(obj)}, 当前共 {_excludedObjects.Count} 个");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 批量添加排除物体
|
||
/// </summary>
|
||
/// <param name="objects">要排除的物体列表</param>
|
||
public void AddExcludedObjects(IEnumerable<ModelItem> objects)
|
||
{
|
||
if (objects != null)
|
||
{
|
||
foreach (var obj in objects)
|
||
{
|
||
if (obj != null) _excludedObjects.Add(obj);
|
||
}
|
||
LogManager.Info($"[排除列表] 批量添加 {objects.Count()} 个排除物体, 当前共 {_excludedObjects.Count} 个");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 移除排除物体
|
||
/// </summary>
|
||
/// <param name="obj">要移除的物体</param>
|
||
public void RemoveExcludedObject(ModelItem obj)
|
||
{
|
||
if (obj != null && _excludedObjects.Remove(obj))
|
||
{
|
||
LogManager.Debug($"[排除列表] 移除排除物体: {ModelItemAnalysisHelper.GetSafeDisplayName(obj)}, 当前共 {_excludedObjects.Count} 个");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除所有排除物体
|
||
/// </summary>
|
||
public void ClearExcludedObjects()
|
||
{
|
||
var count = _excludedObjects.Count;
|
||
_excludedObjects.Clear();
|
||
LogManager.Info($"[排除列表] 清除所有 {count} 个排除物体");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取当前排除物体列表
|
||
/// </summary>
|
||
/// <returns>排除物体列表</returns>
|
||
public IReadOnlyCollection<ModelItem> GetExcludedObjects()
|
||
{
|
||
return _excludedObjects.ToList().AsReadOnly();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查物体是否在排除列表中
|
||
/// </summary>
|
||
/// <param name="obj">要检查的物体</param>
|
||
/// <returns>是否被排除</returns>
|
||
public bool IsObjectExcluded(ModelItem obj)
|
||
{
|
||
return obj != null && _excludedObjects.Contains(obj);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取排除物体数量
|
||
/// </summary>
|
||
public int ExcludedObjectCount => _excludedObjects.Count;
|
||
|
||
/// <summary>
|
||
/// 【已废弃】排除列表现在是实时管理的，不绑定到动画缓存
|
||
/// </summary>
|
||
[Obsolete("排除列表现在是实时管理的，此方法不再执行任何操作", false)]
|
||
public void SaveExclusionsToCache()
|
||
{
|
||
// 排除列表是实时管理的，不保存到动画缓存
|
||
LogManager.Debug($"[排除列表] 实时管理模式，当前共 {_excludedObjects.Count} 个排除物体");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 【已废弃】排除列表现在是实时管理的，不绑定到动画缓存
|
||
/// </summary>
|
||
[Obsolete("排除列表现在是实时管理的，此方法始终返回false", false)]
|
||
public bool LoadExclusionsFromCache()
|
||
{
|
||
// 排除列表是实时管理的，不从缓存加载
|
||
LogManager.Debug("[排除列表] 实时管理模式，不从缓存加载");
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 【已废弃】排除列表现在是实时管理的，不绑定到动画缓存
|
||
/// </summary>
|
||
[Obsolete("排除列表现在是实时管理的，此方法不再执行任何操作", false)]
|
||
public static void ClearAllExclusionCaches()
|
||
{
|
||
// 排除列表是实时管理的，没有缓存需要清除
|
||
LogManager.Debug("[排除列表] 实时管理模式，无缓存需要清除");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置排除物体并清除当前动画缓存（用于重新生成）
|
||
/// </summary>
|
||
/// <param name="objects">新的排除物体列表</param>
|
||
public void SetExcludedObjectsAndClearCache(IEnumerable<ModelItem> objects)
|
||
{
|
||
// 1. 更新排除列表
|
||
_excludedObjects.Clear();
|
||
if (objects != null)
|
||
{
|
||
foreach (var obj in objects)
|
||
{
|
||
if (obj != null) _excludedObjects.Add(obj);
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[排除列表] 已更新排除列表({_excludedObjects.Count}个)，下次生成动画时将重新计算碰撞结果");
|
||
|
||
// 🔥 清除当前检测记录ID，确保下次生成时创建新记录
|
||
if (_currentDetectionRecordId.HasValue)
|
||
{
|
||
LogManager.Info($"[排除列表] 清除当前检测记录ID ({_currentDetectionRecordId})，下次生成将创建新记录");
|
||
_currentDetectionRecordId = null;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从数据库加载排除对象
|
||
/// </summary>
|
||
public void LoadExcludedObjectsFromDatabase(PathDatabase database, string routeId = null)
|
||
{
|
||
try
|
||
{
|
||
if (database == null)
|
||
{
|
||
LogManager.Warning("[排除列表] 数据库为空，无法加载排除对象");
|
||
return;
|
||
}
|
||
|
||
// 从数据库获取排除对象记录
|
||
var records = database.GetExcludedObjects(routeId, includeGlobal: true);
|
||
if (records == null || records.Count == 0)
|
||
{
|
||
LogManager.Info($"[排除列表] 数据库中没有排除对象记录");
|
||
return;
|
||
}
|
||
|
||
// 将记录转换为 ModelItem
|
||
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||
if (doc == null || doc.IsClear)
|
||
{
|
||
LogManager.Warning("[排除列表] 没有活动文档，无法加载排除对象");
|
||
return;
|
||
}
|
||
|
||
var loadedObjects = new List<ModelItem>();
|
||
int notFoundCount = 0;
|
||
|
||
foreach (var record in records)
|
||
{
|
||
try
|
||
{
|
||
// 尝试通过 PathId 查找对象
|
||
var modelItem = FindModelItemByPathId(doc, record.PathId);
|
||
if (modelItem != null)
|
||
{
|
||
loadedObjects.Add(modelItem);
|
||
}
|
||
else
|
||
{
|
||
notFoundCount++;
|
||
LogManager.Debug($"[排除列表] 未找到排除对象: PathId={record.PathId}, Name={record.DisplayName}");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[排除列表] 加载排除对象失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
// 添加到排除列表
|
||
foreach (var obj in loadedObjects)
|
||
{
|
||
_excludedObjects.Add(obj);
|
||
}
|
||
|
||
LogManager.Info($"[排除列表] 从数据库加载 {loadedObjects.Count} 个排除对象，{notFoundCount} 个未找到");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[排除列表] 从数据库加载排除对象失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存排除对象到数据库
|
||
/// </summary>
|
||
public void SaveExcludedObjectsToDatabase(PathDatabase database, string routeId = null)
|
||
{
|
||
try
|
||
{
|
||
if (database == null)
|
||
{
|
||
LogManager.Warning("[排除列表] 数据库为空，无法保存排除对象");
|
||
return;
|
||
}
|
||
|
||
// 清除该路径的旧排除对象记录
|
||
database.ClearExcludedObjects(routeId);
|
||
|
||
// 保存当前排除对象
|
||
var records = new List<ExcludedObjectRecord>();
|
||
foreach (var obj in _excludedObjects)
|
||
{
|
||
try
|
||
{
|
||
var record = new ExcludedObjectRecord
|
||
{
|
||
RouteId = routeId,
|
||
ModelIndex = GetModelIndex(obj),
|
||
PathId = GetModelItemPathId(obj),
|
||
DisplayName = obj.DisplayName,
|
||
ObjectName = GetModelItemObjectName(obj),
|
||
ExcludedTime = DateTime.Now,
|
||
Reason = "用户排除",
|
||
IsGlobal = string.IsNullOrEmpty(routeId)
|
||
};
|
||
records.Add(record);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[排除列表] 转换排除对象记录失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
// 批量保存到数据库
|
||
database.SaveExcludedObjects(records);
|
||
LogManager.Info($"[排除列表] 已保存 {records.Count} 个排除对象到数据库");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[排除列表] 保存排除对象到数据库失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据 PathId 查找 ModelItem
|
||
/// </summary>
|
||
private ModelItem FindModelItemByPathId(Document doc, string pathId)
|
||
{
|
||
try
|
||
{
|
||
// 解析 PathId (格式: "模型索引:路径")
|
||
var parts = pathId.Split(':');
|
||
if (parts.Length < 2) return null;
|
||
|
||
if (!int.TryParse(parts[0], out int modelIndex)) return null;
|
||
|
||
// 获取模型
|
||
var model = doc.Models[modelIndex];
|
||
if (model == null) return null;
|
||
|
||
// 解析路径索引
|
||
var pathIndices = parts[1].Split(',')
|
||
.Select(p => int.TryParse(p, out int idx) ? idx : -1)
|
||
.Where(idx => idx >= 0)
|
||
.ToArray();
|
||
|
||
if (pathIndices.Length == 0) return null;
|
||
|
||
// 从根节点开始遍历
|
||
ModelItem current = model.RootItem;
|
||
foreach (var index in pathIndices)
|
||
{
|
||
if (current == null) return null;
|
||
|
||
// 将 Children 转换为列表以便索引访问
|
||
var childrenList = current.Children.ToList();
|
||
if (index >= childrenList.Count) return null;
|
||
current = childrenList[index];
|
||
}
|
||
|
||
return current;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[排除列表] 查找 ModelItem 失败: {ex.Message}");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取 ModelItem 的 PathId
|
||
/// </summary>
|
||
private string GetModelItemPathId(ModelItem item)
|
||
{
|
||
try
|
||
{
|
||
if (item == null) return string.Empty;
|
||
|
||
// 构建路径索引数组
|
||
var indices = new List<int>();
|
||
var current = item;
|
||
while (current != null && current.Parent != null)
|
||
{
|
||
var parent = current.Parent;
|
||
// 在父节点的子节点中查找当前节点的索引
|
||
var childrenList = parent.Children.ToList();
|
||
int index = childrenList.FindIndex(c => c == current);
|
||
if (index < 0) break;
|
||
indices.Insert(0, index);
|
||
current = parent;
|
||
}
|
||
|
||
// 获取模型索引
|
||
int modelIndex = GetModelIndex(item);
|
||
return $"{modelIndex}:{string.Join(",", indices)}";
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[排除列表] 获取 PathId 失败: {ex.Message}");
|
||
return string.Empty;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取 ModelItem 所在的模型索引
|
||
/// </summary>
|
||
private int GetModelIndex(ModelItem item)
|
||
{
|
||
try
|
||
{
|
||
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||
if (doc == null) return -1;
|
||
|
||
// 找到根节点
|
||
var root = item;
|
||
while (root.Parent != null) root = root.Parent;
|
||
|
||
// 在模型列表中查找
|
||
for (int i = 0; i < doc.Models.Count; i++)
|
||
{
|
||
if (doc.Models[i].RootItem == root)
|
||
return i;
|
||
}
|
||
return -1;
|
||
}
|
||
catch
|
||
{
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取 ModelItem 的对象名称
|
||
/// </summary>
|
||
private string GetModelItemObjectName(ModelItem item)
|
||
{
|
||
try
|
||
{
|
||
return item.ClassName ?? item.DisplayName ?? "Unknown";
|
||
}
|
||
catch
|
||
{
|
||
return "Unknown";
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#endregion
|
||
}
|
||
}
|
||
|
||
|