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); } } }