84 lines
3.1 KiB
C#
84 lines
3.1 KiB
C#
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
|
using NavisworksTransport.Utils.CoordinateSystem;
|
|
using System.Numerics;
|
|
|
|
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|
{
|
|
[TestClass]
|
|
public class CanonicalPlanarPoseBuilderTests
|
|
{
|
|
[TestMethod]
|
|
public void StraightForward_ZUpConvention_ShouldKeepLocalZAsWorldUp()
|
|
{
|
|
bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
|
new Vector3(5, 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 StraightForward_YUpConvention_ShouldMapLocalYToWorldUp()
|
|
{
|
|
bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
|
new Vector3(5, 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 ForwardWithVerticalComponent_ShouldProjectToHorizontalPlane()
|
|
{
|
|
bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
|
new Vector3(10, 0, 3),
|
|
Vector3.UnitZ,
|
|
ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
|
|
out Quaternion rotation);
|
|
|
|
Assert.IsTrue(ok);
|
|
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
|
|
Assert.AreEqual(0.0, linear.M31, 1e-6);
|
|
Assert.AreEqual(0.0, linear.M32, 1e-6);
|
|
Assert.AreEqual(1.0, linear.M33, 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;
|
|
}
|
|
}
|
|
}
|
|
}
|