tian
66e68c6bd1
贴地后 overrideQ 是绝对旋转，但 _objectRotationCorrection 语义是 相对于 baseline(pathYaw) 的增量，且 PathAnimationManager 增量链从 CAD 姿态出发：finalQ = qup(pathYaw-currentYaw) * correctionHostQ * cadQ。 之前直接转 overrideQ 导致确认/动画时重复旋转。 核心改动： - ComposeHostCorrection 接受 cadQ，从中提取 currentYaw， correctionHostQ = qup(currentYaw - pathYaw) * overrideQ - HostQuaternionToCanonical 用相似变换 R_canon = M^-1 * R_host * M （非列向量映射，保证 Identity 不变性） - CanonicalQuaternionToHostEulerCorrection 加 hostType 参数重载， 去除全局 CoordinateSystemManager 依赖 - ComputeGroundLiftForCorrection：与自动调整同方法实测底边算 lift， finally 用新 correction+lift 完整重建（skipCadRestore=false）， 使物体停留在贴地等价姿态 - 新增 5 个增量链闭环单测（含 CAD 歪斜场景）
210 lines
10 KiB
C#
210 lines
10 KiB
C#
using System;
|
||
using System.Numerics;
|
||
|
||
namespace NavisworksTransport.Utils.CoordinateSystem
|
||
{
|
||
/// <summary>
|
||
/// 贴合地面后的最小截面 yaw 搜索器。
|
||
/// 在贴地旋转(把用户选定的面旋到 -hostUp)基础上，绕宿主 Up 轴搜索一个 yaw，
|
||
/// 使物体在"垂直路径方向 × 沿宿主 Up"截面上的投影面积最小。
|
||
/// 与 ObjectPassageProjectionOptimizer 同目标，但只在 yaw 单自由度上搜索——
|
||
/// 贴地旋转已固定俯仰/翻滚，只剩绕垂直轴的旋转自由度。
|
||
/// 适用于 CAD 姿态天然歪斜、无法靠"假设 local +X 是长轴"对齐路径的物体。
|
||
/// </summary>
|
||
public static class AlignToGroundMinCrossSectionYawSearcher
|
||
{
|
||
private const double CoarseStepDegrees = 5.0;
|
||
private const double FineStepDegrees = 1.0;
|
||
private const float HorizontalEpsilon = 1e-8f;
|
||
|
||
/// <summary>
|
||
/// 在贴地旋转基础上绕宿主 Up 搜索 yaw，使物体截面投影面积最小。
|
||
/// </summary>
|
||
/// <param name="faceDownRotation">贴地旋转(把面法线旋到 -hostUp)，宿主世界空间。</param>
|
||
/// <param name="cadRotation">CAD/复位姿态旋转，宿主世界空间。</param>
|
||
/// <param name="sizeX">物体 local +X 方向尺寸(模型单位)。</param>
|
||
/// <param name="sizeY">物体 local +Y 方向尺寸(模型单位)。</param>
|
||
/// <param name="sizeZ">物体 local +Z 方向尺寸(模型单位)。</param>
|
||
/// <param name="hostPathForward">路径起点方向(宿主世界)。</param>
|
||
/// <param name="adapter">宿主坐标系适配器。</param>
|
||
/// <returns>合成后的宿主世界四元数(已归一化)：yawQ * faceDownRotation。</returns>
|
||
public static Quaternion Search(
|
||
Quaternion faceDownRotation,
|
||
Quaternion cadRotation,
|
||
double sizeX,
|
||
double sizeY,
|
||
double sizeZ,
|
||
Vector3 hostPathForward,
|
||
HostCoordinateAdapter adapter)
|
||
{
|
||
if (adapter == null)
|
||
{
|
||
throw new ArgumentNullException(nameof(adapter));
|
||
}
|
||
|
||
Vector3 hostUp = adapter.HostUpVector3;
|
||
|
||
// 路径方向投影到水平面，构造 hostSide(垂直路径方向，水平)
|
||
Vector3 pathFwdHorizontal = hostPathForward - hostUp * Vector3.Dot(hostPathForward, hostUp);
|
||
if (pathFwdHorizontal.LengthSquared() < HorizontalEpsilon)
|
||
{
|
||
// 纯垂直路径：无水平方向，yaw 无意义，保持贴地姿态
|
||
return faceDownRotation;
|
||
}
|
||
pathFwdHorizontal = Vector3.Normalize(pathFwdHorizontal);
|
||
Vector3 hostSide = Vector3.Normalize(Vector3.Cross(pathFwdHorizontal, hostUp));
|
||
|
||
// 物体最终姿态 = yawQ * faceDown * cad
|
||
// local 轴在宿主世界的方向 = totalRotation * UnitX/Y/Z
|
||
// 在该姿态下，截面 = 沿 hostSide 的宽度 × 沿 hostUp 的高度
|
||
double bestYawRad = 0.0;
|
||
double bestArea = double.MaxValue;
|
||
|
||
// 粗搜 [0, 360°)，步长 5°
|
||
for (double deg = 0.0; deg < 360.0; deg += CoarseStepDegrees)
|
||
{
|
||
double yawRad = deg * Math.PI / 180.0;
|
||
double area = EvaluateCrossSectionArea(
|
||
faceDownRotation, cadRotation, yawRad, hostSide, hostUp, sizeX, sizeY, sizeZ);
|
||
if (area < bestArea)
|
||
{
|
||
bestArea = area;
|
||
bestYawRad = yawRad;
|
||
}
|
||
}
|
||
|
||
// 细搜 [best - step, best + step]，步长 1°
|
||
double loDeg = (bestYawRad * 180.0 / Math.PI) - CoarseStepDegrees;
|
||
double hiDeg = (bestYawRad * 180.0 / Math.PI) + CoarseStepDegrees;
|
||
for (double deg = loDeg; deg <= hiDeg; deg += FineStepDegrees)
|
||
{
|
||
double yawRad = deg * Math.PI / 180.0;
|
||
double area = EvaluateCrossSectionArea(
|
||
faceDownRotation, cadRotation, yawRad, hostSide, hostUp, sizeX, sizeY, sizeZ);
|
||
if (area < bestArea)
|
||
{
|
||
bestArea = area;
|
||
bestYawRad = yawRad;
|
||
}
|
||
}
|
||
|
||
return Quaternion.Normalize(
|
||
Quaternion.CreateFromAxisAngle(hostUp, (float)bestYawRad) * faceDownRotation);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算给定 deltaYaw 下物体截面投影面积。
|
||
/// 复用 ObjectPassageProjectionOptimizer.ProjectExtent 的 AABB 投影公式：
|
||
/// width = Σ |localAxis · hostSide| * sizeAxis (垂直路径方向宽度)
|
||
/// height = Σ |localAxis · hostUp | * sizeAxis (沿宿主 Up 高度)
|
||
/// area = width * height
|
||
/// </summary>
|
||
private static double EvaluateCrossSectionArea(
|
||
Quaternion faceDownRotation,
|
||
Quaternion cadRotation,
|
||
double deltaYawRadians,
|
||
Vector3 hostSide,
|
||
Vector3 hostUp,
|
||
double sizeX,
|
||
double sizeY,
|
||
double sizeZ)
|
||
{
|
||
Quaternion yawQ = Quaternion.CreateFromAxisAngle(hostUp, (float)deltaYawRadians);
|
||
Quaternion total = Quaternion.Normalize(yawQ * faceDownRotation * cadRotation);
|
||
|
||
Vector3 localX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, total));
|
||
Vector3 localY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, total));
|
||
Vector3 localZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, total));
|
||
|
||
double width =
|
||
Math.Abs(Vector3.Dot(localX, hostSide)) * sizeX +
|
||
Math.Abs(Vector3.Dot(localY, hostSide)) * sizeY +
|
||
Math.Abs(Vector3.Dot(localZ, hostSide)) * sizeZ;
|
||
double height =
|
||
Math.Abs(Vector3.Dot(localX, hostUp)) * sizeX +
|
||
Math.Abs(Vector3.Dot(localY, hostUp)) * sizeY +
|
||
Math.Abs(Vector3.Dot(localZ, hostUp)) * sizeZ;
|
||
|
||
return width * height;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 把贴地搜索后的绝对宿主姿态(overrideQ)转成"相对于路径 baseline 的 host 欧拉修正"，
|
||
/// 与自动调整返回的 correction 语义一致，供动画系统在 baseline 之上叠加。
|
||
///
|
||
/// 关键：PathAnimationManager 增量链从 CAD 姿态出发：
|
||
/// finalQ = qup(pathYaw + YDeg - currentYaw) * qnonUp(ZDeg) * qx(XDeg) * cadQ
|
||
/// 贴地时 finalQ = overrideQ * cadQ，所以：
|
||
/// qup(pathYaw + YDeg - currentYaw) * qnonUp(ZDeg) * qx(XDeg) = overrideQ
|
||
/// 而 CreateHostRotationCorrection(correction) = qup(YDeg) * qnonUp(ZDeg) * qx(XDeg)，所以：
|
||
/// qup(pathYaw - currentYaw) * CreateHostRotationCorrection(correction) = overrideQ
|
||
/// => correctionHostQ = qup(currentYaw - pathYaw) * overrideQ
|
||
///
|
||
/// currentYaw 是 CAD 姿态的 canonical yaw，从 cadQ 提取（与 GetYawFromRotation 一致）。
|
||
public static LocalEulerRotationCorrection ComposeHostCorrection(
|
||
Quaternion absoluteHostQ,
|
||
Quaternion cadRotation,
|
||
double pathYawRadians,
|
||
HostCoordinateAdapter adapter)
|
||
{
|
||
if (adapter == null)
|
||
{
|
||
throw new ArgumentNullException(nameof(adapter));
|
||
}
|
||
|
||
double currentYaw = ExtractCanonicalYaw(cadRotation, adapter);
|
||
Quaternion preRotation = Quaternion.CreateFromAxisAngle(
|
||
adapter.HostUpVector3, (float)(currentYaw - pathYawRadians));
|
||
Quaternion correctionHostQ = Quaternion.Normalize(preRotation * absoluteHostQ);
|
||
|
||
Quaternion correctionCanonQ = HostQuaternionToCanonical(correctionHostQ, adapter);
|
||
return ObjectPassageProjectionOptimizer.CanonicalQuaternionToHostEulerCorrection(
|
||
correctionCanonQ, adapter.HostType);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从宿主空间四元数提取 canonical yaw（Atan2(canonicalForward.Y, canonicalForward.X)）。
|
||
/// 与 ModelItemTransformHelper.GetYawFromRotation 一致：取 forward 轴(X列)，转 canonical，投影 XY 平面。
|
||
/// </summary>
|
||
private static double ExtractCanonicalYaw(Quaternion hostQ, HostCoordinateAdapter adapter)
|
||
{
|
||
Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostQ);
|
||
Vector3 hostForward = new Vector3(hostLinear.M11, hostLinear.M21, hostLinear.M31);
|
||
Vector3 canonFwd = adapter.ToCanonicalVector3(hostForward);
|
||
canonFwd.Z = 0f;
|
||
if (canonFwd.LengthSquared() < 1e-9f)
|
||
{
|
||
return 0.0;
|
||
}
|
||
canonFwd = Vector3.Normalize(canonFwd);
|
||
return Math.Atan2(canonFwd.Y, canonFwd.X);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 宿主空间四元数 → canonical 空间四元数。
|
||
/// 旋转的坐标系转换必须用相似变换 R_canon = M⁻¹ · R_host · M
|
||
/// （M = canonical→host 坐标变换矩阵，正交故 M⁻¹ = Mᵀ）。
|
||
/// 不能用列向量映射——那只在"基向量定义"场景成立，会破坏 Identity 不变性。
|
||
/// </summary>
|
||
public static Quaternion HostQuaternionToCanonical(Quaternion hostQ, HostCoordinateAdapter adapter)
|
||
{
|
||
Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostQ);
|
||
|
||
// M: canonical→host 坐标变换（列 = canonical 基在 host 中的表示）
|
||
Vector3 mColX = adapter.FromCanonicalVector3(Vector3.UnitX);
|
||
Vector3 mColY = adapter.FromCanonicalVector3(Vector3.UnitY);
|
||
Vector3 mColZ = adapter.FromCanonicalVector3(Vector3.UnitZ);
|
||
Matrix4x4 m = new Matrix4x4(
|
||
mColX.X, mColY.X, mColZ.X, 0f,
|
||
mColX.Y, mColY.Y, mColZ.Y, 0f,
|
||
mColX.Z, mColY.Z, mColZ.Z, 0f,
|
||
0f, 0f, 0f, 1f);
|
||
// M 正交，M⁻¹ = Mᵀ
|
||
Matrix4x4 mInv = Matrix4x4.Transpose(m);
|
||
// R_canon = M⁻¹ · R_host · M
|
||
Matrix4x4 canonLinear = Matrix4x4.Multiply(Matrix4x4.Multiply(mInv, hostLinear), m);
|
||
return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(canonLinear));
|
||
}
|
||
}
|
||
}
|