238 lines
9.3 KiB
C#
238 lines
9.3 KiB
C#
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
|
using NavisworksTransport.Utils.CoordinateSystem;
|
|
using System;
|
|
using System.Numerics;
|
|
|
|
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|
{
|
|
[TestClass]
|
|
public class CanonicalRailPoseBuilderTests
|
|
{
|
|
[TestMethod]
|
|
public void StraightCanonicalPath_ZUpConvention_ShouldProduceIdentityLikeBasis()
|
|
{
|
|
bool ok = CanonicalRailPoseBuilder.TryCreateQuaternion(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(1, 0, 0),
|
|
new Vector3(2, 0, 0),
|
|
Vector3.UnitZ,
|
|
ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
|
|
out Quaternion rotation);
|
|
|
|
Assert.IsTrue(ok);
|
|
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
|
|
AssertColumn(linear, 0, 1, 0, 0);
|
|
AssertColumn(linear, 1, 0, 1, 0);
|
|
AssertColumn(linear, 2, 0, 0, 1);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void StraightCanonicalPath_YUpConvention_ShouldMapLocalYToCanonicalUp()
|
|
{
|
|
bool ok = CanonicalRailPoseBuilder.TryCreateQuaternion(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(1, 0, 0),
|
|
new Vector3(2, 0, 0),
|
|
Vector3.UnitZ,
|
|
ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp),
|
|
out Quaternion rotation);
|
|
|
|
Assert.IsTrue(ok);
|
|
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
|
|
AssertColumn(linear, 0, 1, 0, 0);
|
|
AssertColumn(linear, 1, 0, 0, 1);
|
|
AssertColumn(linear, 2, 0, -1, 0);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void SlopedPath_ShouldKeepForwardAlignedWithPathAndUpOrthogonalized()
|
|
{
|
|
bool ok = CanonicalRailPoseBuilder.TryCreateBasis(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(1, 1, 1),
|
|
new Vector3(2, 2, 2),
|
|
Vector3.UnitZ,
|
|
out Vector3 forward,
|
|
out Vector3 lateral,
|
|
out Vector3 up);
|
|
|
|
Assert.IsTrue(ok);
|
|
Assert.AreEqual(1.0, forward.Length(), 1e-6);
|
|
Assert.AreEqual(1.0, lateral.Length(), 1e-6);
|
|
Assert.AreEqual(1.0, up.Length(), 1e-6);
|
|
Assert.AreEqual(0.0, Vector3.Dot(forward, up), 1e-6);
|
|
Assert.AreEqual(0.0, Vector3.Dot(forward, lateral), 1e-6);
|
|
Assert.AreEqual(0.0, Vector3.Dot(lateral, up), 1e-6);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void SlopedPath_ShouldCreateRailLocalFrameWithStableNormalCloseToCanonicalUp()
|
|
{
|
|
bool ok = CanonicalRailPoseBuilder.TryCreateLocalFrame(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(10, 0, 1),
|
|
new Vector3(20, 0, 2),
|
|
Vector3.UnitZ,
|
|
out RailLocalFrame frame);
|
|
|
|
Assert.IsTrue(ok);
|
|
Assert.IsTrue(Vector3.Dot(frame.Forward, Vector3.UnitX) > 0.99f);
|
|
Assert.IsTrue(Vector3.Dot(frame.Normal, Vector3.UnitZ) > 0.99f);
|
|
Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Normal), 1e-6);
|
|
Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Lateral), 1e-6);
|
|
Assert.AreEqual(0.0, Vector3.Dot(frame.Lateral, frame.Normal), 1e-6);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void ZeroLocalUpRotation_ShouldMatchCurrentRailBaseline()
|
|
{
|
|
var convention = ModelAxisConvention.CreateRailAssetConvention();
|
|
|
|
Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(1, 0, 0),
|
|
new Vector3(2, 0, 0),
|
|
Vector3.UnitZ,
|
|
convention,
|
|
out Quaternion baselineRotation));
|
|
|
|
Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(1, 0, 0),
|
|
new Vector3(2, 0, 0),
|
|
Vector3.UnitZ,
|
|
convention,
|
|
0.0,
|
|
out Quaternion correctedRotation));
|
|
|
|
AssertQuaternionEquivalent(baselineRotation, correctedRotation);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void LocalUpRotation_ShouldAlignCorrectedLocalForwardWithRailTangent()
|
|
{
|
|
var convention = ModelAxisConvention.CreateRailAssetConvention();
|
|
|
|
Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(1, 0, 0),
|
|
new Vector3(2, 0, 0),
|
|
Vector3.UnitZ,
|
|
convention,
|
|
90.0,
|
|
out Quaternion rotation));
|
|
|
|
Quaternion localPreRotation = Quaternion.CreateFromAxisAngle(
|
|
Vector3.Normalize(convention.UpUnitVector),
|
|
(float)(90.0 * Math.PI / 180.0));
|
|
|
|
Vector3 correctedLocalForward = Vector3.Normalize(Vector3.Transform(convention.ForwardUnitVector, localPreRotation));
|
|
Vector3 correctedLocalUp = Vector3.Normalize(Vector3.Transform(convention.UpUnitVector, localPreRotation));
|
|
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
|
|
|
|
Vector3 mappedForward = TransformLocalVector(linear, correctedLocalForward);
|
|
Vector3 mappedUp = TransformLocalVector(linear, correctedLocalUp);
|
|
|
|
AssertVector(mappedForward, 1, 0, 0);
|
|
AssertVector(mappedUp, 0, 0, 1);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void LocalEulerCorrection_ShouldAlignCorrectedLocalForwardWithRailTangent()
|
|
{
|
|
var convention = ModelAxisConvention.CreateRailAssetConvention();
|
|
var correction = new LocalEulerRotationCorrection(0.0, 90.0, 0.0);
|
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
|
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(correction);
|
|
|
|
Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(1, 0, 0),
|
|
new Vector3(2, 0, 0),
|
|
Vector3.UnitZ,
|
|
convention,
|
|
null,
|
|
correctionQuaternion,
|
|
out Quaternion rotation));
|
|
|
|
LocalAxisPoseBuilder.ApplyLocalPreRotation(convention, correctionQuaternion, out var correctedLocalForward, out var correctedLocalUp);
|
|
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
|
|
|
|
Vector3 mappedForward = TransformLocalVector(linear, correctedLocalForward);
|
|
Vector3 mappedUp = TransformLocalVector(linear, correctedLocalUp);
|
|
|
|
AssertVector(mappedForward, 1, 0, 0);
|
|
AssertVector(mappedUp, 0, 0, 1);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void PreferredNormal_ShouldOverrideCanonicalUpForRailFrame()
|
|
{
|
|
bool ok = CanonicalRailPoseBuilder.TryCreateLocalFrame(
|
|
new Vector3(0, 0, 0),
|
|
new Vector3(1, 0, 0),
|
|
new Vector3(2, 0, 0),
|
|
Vector3.UnitZ,
|
|
Vector3.UnitY,
|
|
out RailLocalFrame frame);
|
|
|
|
Assert.IsTrue(ok);
|
|
AssertVector(frame.Forward, 1, 0, 0);
|
|
AssertVector(frame.Normal, 0, 1, 0);
|
|
AssertVector(frame.Lateral, 0, 0, -1);
|
|
}
|
|
|
|
private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z)
|
|
{
|
|
switch (column)
|
|
{
|
|
case 0:
|
|
Assert.AreEqual(x, matrix.M11, 1e-6);
|
|
Assert.AreEqual(y, matrix.M21, 1e-6);
|
|
Assert.AreEqual(z, matrix.M31, 1e-6);
|
|
break;
|
|
case 1:
|
|
Assert.AreEqual(x, matrix.M12, 1e-6);
|
|
Assert.AreEqual(y, matrix.M22, 1e-6);
|
|
Assert.AreEqual(z, matrix.M32, 1e-6);
|
|
break;
|
|
case 2:
|
|
Assert.AreEqual(x, matrix.M13, 1e-6);
|
|
Assert.AreEqual(y, matrix.M23, 1e-6);
|
|
Assert.AreEqual(z, matrix.M33, 1e-6);
|
|
break;
|
|
default:
|
|
Assert.Fail("Only first 3 columns are valid.");
|
|
break;
|
|
}
|
|
}
|
|
|
|
private static void AssertVector(Vector3 actual, double x, double y, double z)
|
|
{
|
|
Assert.AreEqual(x, actual.X, 1e-6);
|
|
Assert.AreEqual(y, actual.Y, 1e-6);
|
|
Assert.AreEqual(z, actual.Z, 1e-6);
|
|
}
|
|
|
|
private static void AssertQuaternionEquivalent(Quaternion expected, Quaternion actual)
|
|
{
|
|
double dot = Math.Abs(
|
|
expected.X * actual.X +
|
|
expected.Y * actual.Y +
|
|
expected.Z * actual.Z +
|
|
expected.W * actual.W);
|
|
|
|
Assert.AreEqual(1.0, dot, 1e-6);
|
|
}
|
|
|
|
private static Vector3 TransformLocalVector(Matrix4x4 linear, Vector3 localVector)
|
|
{
|
|
Vector3 world = new Vector3(
|
|
linear.M11 * localVector.X + linear.M12 * localVector.Y + linear.M13 * localVector.Z,
|
|
linear.M21 * localVector.X + linear.M22 * localVector.Y + linear.M23 * localVector.Z,
|
|
linear.M31 * localVector.X + linear.M32 * localVector.Y + linear.M33 * localVector.Z);
|
|
return Vector3.Normalize(world);
|
|
}
|
|
}
|
|
}
|