291 lines
10 KiB
C#
291 lines
10 KiB
C#
using System;
|
|
using System.Numerics;
|
|
|
|
namespace NavisworksTransport.Utils.CoordinateSystem
|
|
{
|
|
/// <summary>
|
|
/// 真实物体在地面/吊装平面路径下的起点姿态求解器。
|
|
///
|
|
/// 语义：
|
|
/// - 输入和输出都保持在宿主坐标系语义下
|
|
/// - 真实物体没有资产坐标系
|
|
/// - 从原始参考位姿的 6 个候选轴里，选择与路径前进方向最接近的那个
|
|
/// - 先求“原始前进轴 -> 路径前进方向”的旋转差，再作用到原始参考位姿上
|
|
/// </summary>
|
|
public static class RealObjectPlanarPoseSolver
|
|
{
|
|
private const float AxisEpsilon = 1e-6f;
|
|
|
|
public static bool TryCreatePlanarPoseFromReferencePose(
|
|
Quaternion referenceRotation,
|
|
Vector3 desiredForward,
|
|
Vector3 desiredUp,
|
|
out RealObjectPlanarPoseSolution solution)
|
|
{
|
|
return TryCreatePlanarPoseFromReferencePose(
|
|
referenceRotation,
|
|
desiredForward,
|
|
desiredUp,
|
|
null,
|
|
out solution);
|
|
}
|
|
|
|
public static bool TryCreatePlanarPoseFromReferencePose(
|
|
Quaternion referenceRotation,
|
|
Vector3 desiredForward,
|
|
Vector3 desiredUp,
|
|
LocalAxisDirection? fixedReferenceAxis,
|
|
out RealObjectPlanarPoseSolution solution)
|
|
{
|
|
solution = null;
|
|
|
|
if (!TryNormalize(referenceRotation, out Quaternion normalizedReferenceRotation))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!TryNormalize(desiredUp, out Vector3 normalizedUp))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!TryNormalize(desiredForward, out Vector3 normalizedForward))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
CandidateAxis bestCandidate = fixedReferenceAxis.HasValue
|
|
? CreateCandidateAxisFromFixedDirection(fixedReferenceAxis.Value, normalizedReferenceRotation)
|
|
: SelectBestCandidateAxis(
|
|
normalizedReferenceRotation,
|
|
normalizedForward);
|
|
|
|
if (bestCandidate.IsInvalid)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
Quaternion rotationDelta = CreateShortestArcQuaternion(
|
|
bestCandidate.WorldAxis,
|
|
normalizedForward,
|
|
normalizedUp);
|
|
|
|
Quaternion baselineRotation = Quaternion.Normalize(rotationDelta * normalizedReferenceRotation);
|
|
solution = new RealObjectPlanarPoseSolution(
|
|
normalizedForward,
|
|
normalizedUp,
|
|
bestCandidate.LocalAxisDirection,
|
|
bestCandidate.LocalAxis,
|
|
bestCandidate.WorldAxis,
|
|
rotationDelta,
|
|
baselineRotation);
|
|
return true;
|
|
}
|
|
|
|
private static CandidateAxis SelectBestCandidateAxis(
|
|
Quaternion referenceRotation,
|
|
Vector3 desiredForward)
|
|
{
|
|
CandidateAxis bestCandidate = CandidateAxis.Invalid;
|
|
|
|
foreach (var candidate in EnumerateCandidateAxes(referenceRotation))
|
|
{
|
|
float score = Vector3.Dot(candidate.WorldAxis, desiredForward);
|
|
if (bestCandidate.IsInvalid || score > bestCandidate.Score)
|
|
{
|
|
bestCandidate = new CandidateAxis(
|
|
candidate.LocalAxisDirection,
|
|
candidate.LocalAxis,
|
|
candidate.WorldAxis,
|
|
score);
|
|
}
|
|
}
|
|
|
|
return bestCandidate;
|
|
}
|
|
|
|
private static CandidateAxis CreateCandidateAxisFromFixedDirection(
|
|
LocalAxisDirection direction,
|
|
Quaternion referenceRotation)
|
|
{
|
|
Vector3 localAxis = GetAxisVector(direction);
|
|
Vector3 worldAxis = Vector3.Normalize(Vector3.Transform(localAxis, referenceRotation));
|
|
return new CandidateAxis(direction, localAxis, worldAxis, 0f);
|
|
}
|
|
|
|
private static CandidateAxis[] EnumerateCandidateAxes(Quaternion referenceRotation)
|
|
{
|
|
return new[]
|
|
{
|
|
CreateCandidateAxis(LocalAxisDirection.PositiveX, Vector3.UnitX, referenceRotation),
|
|
CreateCandidateAxis(LocalAxisDirection.NegativeX, -Vector3.UnitX, referenceRotation),
|
|
CreateCandidateAxis(LocalAxisDirection.PositiveY, Vector3.UnitY, referenceRotation),
|
|
CreateCandidateAxis(LocalAxisDirection.NegativeY, -Vector3.UnitY, referenceRotation),
|
|
CreateCandidateAxis(LocalAxisDirection.PositiveZ, Vector3.UnitZ, referenceRotation),
|
|
CreateCandidateAxis(LocalAxisDirection.NegativeZ, -Vector3.UnitZ, referenceRotation)
|
|
};
|
|
}
|
|
|
|
private static CandidateAxis CreateCandidateAxis(
|
|
LocalAxisDirection direction,
|
|
Vector3 localAxis,
|
|
Quaternion referenceRotation)
|
|
{
|
|
Vector3 worldAxis = Vector3.Normalize(Vector3.Transform(localAxis, referenceRotation));
|
|
return new CandidateAxis(direction, localAxis, worldAxis, float.MinValue);
|
|
}
|
|
|
|
private static Vector3 GetAxisVector(LocalAxisDirection direction)
|
|
{
|
|
switch (direction)
|
|
{
|
|
case LocalAxisDirection.PositiveX:
|
|
return Vector3.UnitX;
|
|
case LocalAxisDirection.NegativeX:
|
|
return -Vector3.UnitX;
|
|
case LocalAxisDirection.PositiveY:
|
|
return Vector3.UnitY;
|
|
case LocalAxisDirection.NegativeY:
|
|
return -Vector3.UnitY;
|
|
case LocalAxisDirection.PositiveZ:
|
|
return Vector3.UnitZ;
|
|
case LocalAxisDirection.NegativeZ:
|
|
return -Vector3.UnitZ;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(direction), direction, null);
|
|
}
|
|
}
|
|
|
|
private static Quaternion CreateShortestArcQuaternion(
|
|
Vector3 from,
|
|
Vector3 to,
|
|
Vector3 desiredUp)
|
|
{
|
|
Vector3 normalizedFrom = Vector3.Normalize(from);
|
|
Vector3 normalizedTo = Vector3.Normalize(to);
|
|
|
|
float dot = Vector3.Dot(normalizedFrom, normalizedTo);
|
|
dot = Math.Max(-1.0f, Math.Min(1.0f, dot));
|
|
|
|
if (dot > 1.0f - 1e-6f)
|
|
{
|
|
return Quaternion.Identity;
|
|
}
|
|
|
|
if (dot < -1.0f + 1e-6f)
|
|
{
|
|
Vector3 axis = Vector3.Cross(normalizedFrom, desiredUp);
|
|
if (axis.LengthSquared() < AxisEpsilon)
|
|
{
|
|
axis = Vector3.Cross(normalizedFrom, GetAnyOrthogonalVector(normalizedFrom));
|
|
}
|
|
|
|
axis = Vector3.Normalize(axis);
|
|
return Quaternion.Normalize(Quaternion.CreateFromAxisAngle(axis, (float)Math.PI));
|
|
}
|
|
|
|
Vector3 cross = Vector3.Cross(normalizedFrom, normalizedTo);
|
|
if (cross.LengthSquared() < AxisEpsilon)
|
|
{
|
|
return Quaternion.Identity;
|
|
}
|
|
|
|
cross = Vector3.Normalize(cross);
|
|
double angle = Math.Acos(dot);
|
|
return Quaternion.Normalize(Quaternion.CreateFromAxisAngle(cross, (float)angle));
|
|
}
|
|
|
|
private static Vector3 GetAnyOrthogonalVector(Vector3 source)
|
|
{
|
|
if (Math.Abs(source.X) < 0.9f)
|
|
{
|
|
return Vector3.UnitX;
|
|
}
|
|
|
|
if (Math.Abs(source.Y) < 0.9f)
|
|
{
|
|
return Vector3.UnitY;
|
|
}
|
|
|
|
return Vector3.UnitZ;
|
|
}
|
|
|
|
private static bool TryNormalize(Quaternion value, out Quaternion normalized)
|
|
{
|
|
float lengthSquared = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
|
|
if (lengthSquared < AxisEpsilon)
|
|
{
|
|
normalized = Quaternion.Identity;
|
|
return false;
|
|
}
|
|
|
|
normalized = Quaternion.Normalize(value);
|
|
return true;
|
|
}
|
|
|
|
private static bool TryNormalize(Vector3 value, out Vector3 normalized)
|
|
{
|
|
if (value.LengthSquared() < AxisEpsilon)
|
|
{
|
|
normalized = Vector3.Zero;
|
|
return false;
|
|
}
|
|
|
|
normalized = Vector3.Normalize(value);
|
|
return true;
|
|
}
|
|
|
|
private struct CandidateAxis
|
|
{
|
|
public static CandidateAxis Invalid => new CandidateAxis(LocalAxisDirection.PositiveX, Vector3.Zero, Vector3.Zero, float.MinValue);
|
|
|
|
public LocalAxisDirection LocalAxisDirection { get; }
|
|
public Vector3 LocalAxis { get; }
|
|
public Vector3 WorldAxis { get; }
|
|
public float Score { get; }
|
|
|
|
public bool IsInvalid => Score == float.MinValue;
|
|
|
|
public CandidateAxis(LocalAxisDirection localAxisDirection, Vector3 localAxis, Vector3 worldAxis, float score)
|
|
{
|
|
LocalAxisDirection = localAxisDirection;
|
|
LocalAxis = localAxis;
|
|
WorldAxis = worldAxis;
|
|
Score = score;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 真实物体平面起点姿态求解结果。
|
|
/// </summary>
|
|
public sealed class RealObjectPlanarPoseSolution
|
|
{
|
|
public Vector3 ProjectedForward { get; }
|
|
public Vector3 DesiredUp { get; }
|
|
public LocalAxisDirection SelectedReferenceAxisDirection { get; }
|
|
public Vector3 SelectedReferenceAxisLocal { get; }
|
|
public Vector3 SelectedReferenceAxisWorld { get; }
|
|
public Quaternion RotationDelta { get; }
|
|
public Quaternion BaselineRotation { get; }
|
|
|
|
public RealObjectPlanarPoseSolution(
|
|
Vector3 projectedForward,
|
|
Vector3 desiredUp,
|
|
LocalAxisDirection selectedReferenceAxisDirection,
|
|
Vector3 selectedReferenceAxisLocal,
|
|
Vector3 selectedReferenceAxisWorld,
|
|
Quaternion rotationDelta,
|
|
Quaternion baselineRotation)
|
|
{
|
|
ProjectedForward = projectedForward;
|
|
DesiredUp = desiredUp;
|
|
SelectedReferenceAxisDirection = selectedReferenceAxisDirection;
|
|
SelectedReferenceAxisLocal = selectedReferenceAxisLocal;
|
|
SelectedReferenceAxisWorld = selectedReferenceAxisWorld;
|
|
RotationDelta = rotationDelta;
|
|
BaselineRotation = baselineRotation;
|
|
}
|
|
}
|
|
}
|