504 lines
21 KiB
C#
504 lines
21 KiB
C#
using System;
|
|
using System.Numerics;
|
|
using Autodesk.Navisworks.Api;
|
|
using NavisworksTransport.Utils.CoordinateSystem;
|
|
|
|
namespace NavisworksTransport.Utils
|
|
{
|
|
/// <summary>
|
|
/// Rail 路径姿态辅助工具。
|
|
/// LegacySuspensionPoint 模式下保持世界 Z 偏移；
|
|
/// RailCenterLine 模式下按路径切线推导局部法向，支持斜轨的轨上/轨下偏移。
|
|
/// </summary>
|
|
public static class RailPathPoseHelper
|
|
{
|
|
private const double TangentEpsilon = 1e-9;
|
|
private static bool _rotationConstructorConventionLogged;
|
|
|
|
/// <summary>
|
|
/// 获取 Rail 参考点到构件对接点的有符号偏移（模型单位）。
|
|
/// LegacySuspensionPoint 模式下，路径点本身就是构件对接点，因此偏移为 0。
|
|
/// </summary>
|
|
public static double GetAnchorOffset(PathRoute route)
|
|
{
|
|
if (route == null || route.PathType != PathType.Rail)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
if (route.RailPathDefinitionMode == RailPathDefinitionMode.LegacySuspensionPoint)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
return route.RailMountMode == RailMountMode.OverRail
|
|
? route.RailReferenceToAnchorOffset
|
|
: -route.RailReferenceToAnchorOffset;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 计算构件底面中心相对于 Rail 参考点的 Z 偏移（模型单位）。
|
|
/// </summary>
|
|
public static double GetBottomZOffset(PathRoute route, double objectHeight)
|
|
{
|
|
double anchorOffset = GetAnchorOffset(route);
|
|
|
|
if (route == null || route.PathType != PathType.Rail)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
if (PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode))
|
|
{
|
|
return anchorOffset - objectHeight;
|
|
}
|
|
|
|
return anchorOffset;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 计算通行空间中心相对于 Rail 参考点的 Z 偏移（模型单位）。
|
|
/// 顶面对接时，通行空间中心位于对接点下方半高；
|
|
/// 底面对接时，通行空间中心位于对接点上方半高。
|
|
/// </summary>
|
|
public static double GetObjectSpaceCenterZOffset(PathRoute route, double objectSpaceHeight)
|
|
{
|
|
double anchorOffset = GetAnchorOffset(route);
|
|
|
|
if (route == null || route.PathType != PathType.Rail)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
if (PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode))
|
|
{
|
|
return anchorOffset - objectSpaceHeight / 2.0;
|
|
}
|
|
|
|
return anchorOffset + objectSpaceHeight / 2.0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 根据 Rail 路径参考点计算构件底面中心位置。
|
|
/// </summary>
|
|
public static Point3D ResolveBottomPosition(PathRoute route, Point3D referencePoint, double objectHeight)
|
|
{
|
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
|
var canonicalReferencePoint = adapter.ToCanonicalPoint(referencePoint);
|
|
var canonicalUp = HostCoordinateAdapter.CanonicalUp;
|
|
double bottomOffset = GetBottomOffsetMagnitude(route, objectHeight);
|
|
|
|
var canonicalBottomPoint = new Point3D(
|
|
canonicalReferencePoint.X + canonicalUp.X * bottomOffset,
|
|
canonicalReferencePoint.Y + canonicalUp.Y * bottomOffset,
|
|
canonicalReferencePoint.Z + canonicalUp.Z * bottomOffset);
|
|
|
|
return adapter.FromCanonicalPoint(canonicalBottomPoint);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 根据 Rail 路径参考点和相邻点计算构件底面中心位置。
|
|
/// RailCenterLine 模式下沿轨道局部法向偏移，适用于斜轨。
|
|
/// </summary>
|
|
public static Point3D ResolveBottomPosition(
|
|
PathRoute route,
|
|
Point3D referencePoint,
|
|
Point3D previousPoint,
|
|
Point3D nextPoint,
|
|
double objectHeight)
|
|
{
|
|
if (route == null || route.PathType != PathType.Rail)
|
|
{
|
|
return referencePoint;
|
|
}
|
|
|
|
if (route.RailPathDefinitionMode == RailPathDefinitionMode.LegacySuspensionPoint)
|
|
{
|
|
return ResolveBottomPosition(route, referencePoint, objectHeight);
|
|
}
|
|
|
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
|
var canonicalReferencePoint = adapter.ToCanonicalPoint(referencePoint);
|
|
var normal = ResolveRailNormal(previousPoint, referencePoint, nextPoint);
|
|
double bottomOffset = GetBottomOffsetMagnitude(route, objectHeight);
|
|
|
|
var canonicalBottomPoint = new Point3D(
|
|
canonicalReferencePoint.X + normal.X * bottomOffset,
|
|
canonicalReferencePoint.Y + normal.Y * bottomOffset,
|
|
canonicalReferencePoint.Z + normal.Z * bottomOffset);
|
|
|
|
return adapter.FromCanonicalPoint(canonicalBottomPoint);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 根据 Rail 路径参考点和相邻点计算通行空间中心位置。
|
|
/// </summary>
|
|
public static Point3D ResolveObjectSpaceCenterPosition(
|
|
PathRoute route,
|
|
Point3D referencePoint,
|
|
Point3D previousPoint,
|
|
Point3D nextPoint,
|
|
double objectSpaceHeight)
|
|
{
|
|
if (route == null || route.PathType != PathType.Rail)
|
|
{
|
|
return referencePoint;
|
|
}
|
|
|
|
double centerOffset = GetObjectSpaceCenterOffsetMagnitude(route, objectSpaceHeight);
|
|
|
|
if (route.RailPathDefinitionMode == RailPathDefinitionMode.LegacySuspensionPoint)
|
|
{
|
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
|
var canonicalReferencePoint = adapter.ToCanonicalPoint(referencePoint);
|
|
var canonicalCenterPoint = new Point3D(
|
|
canonicalReferencePoint.X,
|
|
canonicalReferencePoint.Y,
|
|
canonicalReferencePoint.Z + centerOffset);
|
|
return adapter.FromCanonicalPoint(canonicalCenterPoint);
|
|
}
|
|
|
|
var adapterForRail = CoordinateSystemManager.Instance.CreateHostAdapter();
|
|
var canonicalReferencePointForRail = adapterForRail.ToCanonicalPoint(referencePoint);
|
|
var normal = ResolveRailNormal(previousPoint, referencePoint, nextPoint);
|
|
var canonicalCenterPointForRail = new Point3D(
|
|
canonicalReferencePointForRail.X + normal.X * centerOffset,
|
|
canonicalReferencePointForRail.Y + normal.Y * centerOffset,
|
|
canonicalReferencePointForRail.Z + normal.Z * centerOffset);
|
|
return adapterForRail.FromCanonicalPoint(canonicalCenterPointForRail);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 根据轨道路径切向和法向创建 Rail 构件完整三维姿态。
|
|
/// 约定：构件本地 X 轴为前进方向，本地 Z 轴为上方向。
|
|
/// 返回值是可直接用于 Transform3D 的线性矩阵。
|
|
/// </summary>
|
|
public static bool TryCreateRailLinearTransform(
|
|
Point3D previousPoint,
|
|
Point3D currentPoint,
|
|
Point3D nextPoint,
|
|
out Matrix3 linearTransform)
|
|
{
|
|
return TryCreateRailLinearTransform(
|
|
previousPoint,
|
|
currentPoint,
|
|
nextPoint,
|
|
ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
|
|
out linearTransform);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 根据轨道路径切向和法向创建 Rail 构件完整三维姿态。
|
|
/// 通过模型局部轴约定显式指定本地哪个轴代表 forward/up。
|
|
/// </summary>
|
|
public static bool TryCreateRailLinearTransform(
|
|
Point3D previousPoint,
|
|
Point3D currentPoint,
|
|
Point3D nextPoint,
|
|
ModelAxisConvention axisConvention,
|
|
out Matrix3 linearTransform)
|
|
{
|
|
linearTransform = null;
|
|
|
|
if (axisConvention == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(axisConvention));
|
|
}
|
|
|
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
|
var canonicalPreviousPoint = ToNumerics(adapter.ToCanonicalPoint(previousPoint));
|
|
var canonicalCurrentPoint = ToNumerics(adapter.ToCanonicalPoint(currentPoint));
|
|
var canonicalNextPoint = ToNumerics(adapter.ToCanonicalPoint(nextPoint));
|
|
|
|
if (!CanonicalRailPoseBuilder.TryCreateBasis(
|
|
canonicalPreviousPoint,
|
|
canonicalCurrentPoint,
|
|
canonicalNextPoint,
|
|
HostCoordinateAdapter.CanonicalUpVector3,
|
|
out var canonicalForward,
|
|
out var canonicalLateral,
|
|
out var canonicalUp))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
Quaternion canonicalRotation = axisConvention.CreateQuaternion(canonicalForward, canonicalUp);
|
|
Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation);
|
|
|
|
var hostXAxis = Normalize(adapter.FromCanonicalVector(ToNavVector(new Vector3(canonicalLinear.M11, canonicalLinear.M21, canonicalLinear.M31))));
|
|
var hostYAxis = Normalize(adapter.FromCanonicalVector(ToNavVector(new Vector3(canonicalLinear.M12, canonicalLinear.M22, canonicalLinear.M32))));
|
|
var hostZAxis = Normalize(adapter.FromCanonicalVector(ToNavVector(new Vector3(canonicalLinear.M13, canonicalLinear.M23, canonicalLinear.M33))));
|
|
|
|
// Navisworks 在 Transform3D(Matrix3, ...) 中按列读取局部基向量：
|
|
// 第 1 列 = 本地 X 轴在世界中的方向
|
|
// 第 2 列 = 本地 Y 轴在世界中的方向
|
|
// 第 3 列 = 本地 Z 轴在世界中的方向
|
|
linearTransform = new Matrix3(
|
|
hostXAxis.X, hostYAxis.X, hostZAxis.X,
|
|
hostXAxis.Y, hostYAxis.Y, hostZAxis.Y,
|
|
hostXAxis.Z, hostYAxis.Z, hostZAxis.Z);
|
|
|
|
LogManager.Info(
|
|
$"[Rail姿态] Canonical切向=({canonicalForward.X:F4},{canonicalForward.Y:F4},{canonicalForward.Z:F4}), " +
|
|
$"Canonical侧向=({canonicalLateral.X:F4},{canonicalLateral.Y:F4},{canonicalLateral.Z:F4}), " +
|
|
$"Canonical法向=({canonicalUp.X:F4},{canonicalUp.Y:F4},{canonicalUp.Z:F4}), " +
|
|
$"Host法向=({hostZAxis.X:F4},{hostZAxis.Y:F4},{hostZAxis.Z:F4}), " +
|
|
$"模型Forward={axisConvention.ForwardAxis}, 模型Up={axisConvention.UpAxis}");
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 根据轨道路径切向和法向创建 Rail 构件完整三维旋转。
|
|
/// 约定：构件本地 X 轴为前进方向，本地 Z 轴为上方向。
|
|
/// </summary>
|
|
public static bool TryCreateRailRotation(
|
|
Point3D previousPoint,
|
|
Point3D currentPoint,
|
|
Point3D nextPoint,
|
|
out Rotation3D rotation)
|
|
{
|
|
return TryCreateRailRotation(
|
|
previousPoint,
|
|
currentPoint,
|
|
nextPoint,
|
|
ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
|
|
out rotation);
|
|
}
|
|
|
|
public static bool TryCreateRailRotation(
|
|
Point3D previousPoint,
|
|
Point3D currentPoint,
|
|
Point3D nextPoint,
|
|
ModelAxisConvention axisConvention,
|
|
out Rotation3D rotation)
|
|
{
|
|
rotation = Rotation3D.Identity;
|
|
LogRotationConstructorConventionOnce();
|
|
|
|
if (!TryCreateRailLinearTransform(previousPoint, currentPoint, nextPoint, axisConvention, out var linearTransform))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
rotation = CreateRotationFromLinearTransform(linearTransform);
|
|
var verifiedLinear = new Transform3D(rotation).Linear;
|
|
|
|
LogManager.Info(
|
|
$"[Rail旋转验证] X=({verifiedLinear.Get(0, 0):F4},{verifiedLinear.Get(1, 0):F4},{verifiedLinear.Get(2, 0):F4}), " +
|
|
$"Y=({verifiedLinear.Get(0, 1):F4},{verifiedLinear.Get(1, 1):F4},{verifiedLinear.Get(2, 1):F4}), " +
|
|
$"Z=({verifiedLinear.Get(0, 2):F4},{verifiedLinear.Get(1, 2):F4},{verifiedLinear.Get(2, 2):F4})");
|
|
return true;
|
|
}
|
|
|
|
private static Rotation3D CreateRotationFromLinearTransform(Matrix3 linearTransform)
|
|
{
|
|
double m00 = linearTransform.Get(0, 0);
|
|
double m01 = linearTransform.Get(0, 1);
|
|
double m02 = linearTransform.Get(0, 2);
|
|
double m10 = linearTransform.Get(1, 0);
|
|
double m11 = linearTransform.Get(1, 1);
|
|
double m12 = linearTransform.Get(1, 2);
|
|
double m20 = linearTransform.Get(2, 0);
|
|
double m21 = linearTransform.Get(2, 1);
|
|
double m22 = linearTransform.Get(2, 2);
|
|
|
|
double qx;
|
|
double qy;
|
|
double qz;
|
|
double qw;
|
|
|
|
double trace = m00 + m11 + m22;
|
|
if (trace > 0.0)
|
|
{
|
|
double s = Math.Sqrt(trace + 1.0) * 2.0;
|
|
qw = 0.25 * s;
|
|
qx = (m21 - m12) / s;
|
|
qy = (m02 - m20) / s;
|
|
qz = (m10 - m01) / s;
|
|
}
|
|
else if (m00 > m11 && m00 > m22)
|
|
{
|
|
double s = Math.Sqrt(1.0 + m00 - m11 - m22) * 2.0;
|
|
qw = (m21 - m12) / s;
|
|
qx = 0.25 * s;
|
|
qy = (m01 + m10) / s;
|
|
qz = (m02 + m20) / s;
|
|
}
|
|
else if (m11 > m22)
|
|
{
|
|
double s = Math.Sqrt(1.0 + m11 - m00 - m22) * 2.0;
|
|
qw = (m02 - m20) / s;
|
|
qx = (m01 + m10) / s;
|
|
qy = 0.25 * s;
|
|
qz = (m12 + m21) / s;
|
|
}
|
|
else
|
|
{
|
|
double s = Math.Sqrt(1.0 + m22 - m00 - m11) * 2.0;
|
|
qw = (m10 - m01) / s;
|
|
qx = (m02 + m20) / s;
|
|
qy = (m12 + m21) / s;
|
|
qz = 0.25 * s;
|
|
}
|
|
|
|
var rotation = new Rotation3D(qx, qy, qz, qw);
|
|
double error = CalculateLinearError(new Transform3D(rotation).Linear, linearTransform);
|
|
LogManager.Info(
|
|
$"[Rail旋转验证] quaternion候选=({qx:F4},{qy:F4},{qz:F4},{qw:F4}), 误差={error:F6}");
|
|
return rotation;
|
|
}
|
|
|
|
private static void LogRotationConstructorConventionOnce()
|
|
{
|
|
if (_rotationConstructorConventionLogged)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_rotationConstructorConventionLogged = true;
|
|
|
|
LogKnownAxisRotation("X90", new UnitVector3D(1, 0, 0), Math.PI / 2.0);
|
|
LogKnownAxisRotation("Y90", new UnitVector3D(0, 1, 0), Math.PI / 2.0);
|
|
LogKnownAxisRotation("Z90", new UnitVector3D(0, 0, 1), Math.PI / 2.0);
|
|
}
|
|
|
|
private static void LogKnownAxisRotation(string name, UnitVector3D axis, double angle)
|
|
{
|
|
Rotation3D axisAngleRotation = new Rotation3D(axis, angle);
|
|
Rotation3D reconstructed = new Rotation3D(
|
|
axisAngleRotation.A,
|
|
axisAngleRotation.B,
|
|
axisAngleRotation.C,
|
|
axisAngleRotation.D);
|
|
Matrix3 linear = new Transform3D(reconstructed).Linear;
|
|
|
|
LogManager.Info(
|
|
$"[Rotation构造测试] {name}: " +
|
|
$"A={axisAngleRotation.A:F6}, B={axisAngleRotation.B:F6}, C={axisAngleRotation.C:F6}, D={axisAngleRotation.D:F6}, " +
|
|
$"LinearX=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " +
|
|
$"LinearY=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " +
|
|
$"LinearZ=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})");
|
|
}
|
|
|
|
private static double CalculateLinearError(Matrix3 actual, Matrix3 expected)
|
|
{
|
|
double error = 0.0;
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
for (int column = 0; column < 3; column++)
|
|
{
|
|
double delta = actual.Get(row, column) - expected.Get(row, column);
|
|
error += delta * delta;
|
|
}
|
|
}
|
|
|
|
return Math.Sqrt(error);
|
|
}
|
|
|
|
private static double GetBottomOffsetMagnitude(PathRoute route, double objectHeight)
|
|
{
|
|
double anchorOffset = GetAnchorOffset(route);
|
|
return PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode)
|
|
? anchorOffset - objectHeight
|
|
: anchorOffset;
|
|
}
|
|
|
|
private static double GetObjectSpaceCenterOffsetMagnitude(PathRoute route, double objectSpaceHeight)
|
|
{
|
|
double anchorOffset = GetAnchorOffset(route);
|
|
return PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode)
|
|
? anchorOffset - objectSpaceHeight / 2.0
|
|
: anchorOffset + objectSpaceHeight / 2.0;
|
|
}
|
|
|
|
private static Vector3D ResolveRailNormal(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint)
|
|
{
|
|
var tangent = ResolveTangent(previousPoint, currentPoint, nextPoint);
|
|
double tangentLengthSquared = tangent.X * tangent.X + tangent.Y * tangent.Y + tangent.Z * tangent.Z;
|
|
|
|
if (tangentLengthSquared < TangentEpsilon)
|
|
{
|
|
return HostCoordinateAdapter.CanonicalUp;
|
|
}
|
|
|
|
tangent = new Vector3D(
|
|
tangent.X / Math.Sqrt(tangentLengthSquared),
|
|
tangent.Y / Math.Sqrt(tangentLengthSquared),
|
|
tangent.Z / Math.Sqrt(tangentLengthSquared));
|
|
|
|
var worldUp = HostCoordinateAdapter.CanonicalUp;
|
|
double projection = worldUp.X * tangent.X + worldUp.Y * tangent.Y + worldUp.Z * tangent.Z;
|
|
var normal = new Vector3D(
|
|
worldUp.X - projection * tangent.X,
|
|
worldUp.Y - projection * tangent.Y,
|
|
worldUp.Z - projection * tangent.Z);
|
|
|
|
double normalLengthSquared = normal.X * normal.X + normal.Y * normal.Y + normal.Z * normal.Z;
|
|
if (normalLengthSquared < TangentEpsilon)
|
|
{
|
|
return HostCoordinateAdapter.CanonicalUp;
|
|
}
|
|
|
|
double normalLength = Math.Sqrt(normalLengthSquared);
|
|
return new Vector3D(
|
|
normal.X / normalLength,
|
|
normal.Y / normalLength,
|
|
normal.Z / normalLength);
|
|
}
|
|
|
|
private static Vector3D ResolveTangent(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint)
|
|
{
|
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
|
var canonicalPreviousPoint = adapter.ToCanonicalPoint(previousPoint);
|
|
var canonicalCurrentPoint = adapter.ToCanonicalPoint(currentPoint);
|
|
var canonicalNextPoint = adapter.ToCanonicalPoint(nextPoint);
|
|
|
|
var tangent = new Vector3D(
|
|
canonicalNextPoint.X - canonicalPreviousPoint.X,
|
|
canonicalNextPoint.Y - canonicalPreviousPoint.Y,
|
|
canonicalNextPoint.Z - canonicalPreviousPoint.Z);
|
|
|
|
double tangentLengthSquared = tangent.X * tangent.X + tangent.Y * tangent.Y + tangent.Z * tangent.Z;
|
|
if (tangentLengthSquared < TangentEpsilon)
|
|
{
|
|
tangent = new Vector3D(
|
|
canonicalNextPoint.X - canonicalCurrentPoint.X,
|
|
canonicalNextPoint.Y - canonicalCurrentPoint.Y,
|
|
canonicalNextPoint.Z - canonicalCurrentPoint.Z);
|
|
}
|
|
|
|
return tangent;
|
|
}
|
|
|
|
private static Vector3D Normalize(Vector3D vector)
|
|
{
|
|
double lengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z;
|
|
if (lengthSquared < TangentEpsilon)
|
|
{
|
|
return new Vector3D(0, 0, 0);
|
|
}
|
|
|
|
double length = Math.Sqrt(lengthSquared);
|
|
return new Vector3D(vector.X / length, vector.Y / length, vector.Z / length);
|
|
}
|
|
|
|
private static Vector3D Cross(Vector3D a, Vector3D b)
|
|
{
|
|
return new Vector3D(
|
|
a.Y * b.Z - a.Z * b.Y,
|
|
a.Z * b.X - a.X * b.Z,
|
|
a.X * b.Y - a.Y * b.X);
|
|
}
|
|
|
|
private static Vector3 ToNumerics(Point3D point)
|
|
{
|
|
return new Vector3((float)point.X, (float)point.Y, (float)point.Z);
|
|
}
|
|
|
|
private static Vector3D ToNavVector(Vector3 vector)
|
|
{
|
|
return new Vector3D(vector.X, vector.Y, vector.Z);
|
|
}
|
|
}
|
|
}
|