111 lines
4.2 KiB
C#
111 lines
4.2 KiB
C#
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
|
using NavisworksTransport.Utils.CoordinateSystem;
|
|
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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
}
|