74 lines
3.0 KiB
C#
74 lines
3.0 KiB
C#
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
|
using NavisworksTransport.Utils.CoordinateSystem;
|
|
using System.Numerics;
|
|
|
|
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|
{
|
|
[TestClass]
|
|
public class HoistingRealObjectPoseHelperTests
|
|
{
|
|
[TestMethod]
|
|
public void ShouldKeepActualUpAxis_WhenAligningHoistingForward()
|
|
{
|
|
Quaternion actualRotation = Quaternion.Normalize(new Quaternion(0.70710677f, 0.0f, 0.0f, 0.70710677f));
|
|
Vector3 hostUp = Vector3.UnitY;
|
|
Vector3 targetForward = -Vector3.UnitX;
|
|
|
|
bool ok = HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
|
|
actualRotation,
|
|
targetForward,
|
|
hostUp,
|
|
out Quaternion resultRotation,
|
|
out LocalAxisDirection selectedDirection,
|
|
out Vector3 selectedAxisLocal,
|
|
out Vector3 projectedForward);
|
|
|
|
Assert.IsTrue(ok);
|
|
Assert.AreEqual(LocalAxisDirection.PositiveX, selectedDirection);
|
|
AssertVector(selectedAxisLocal, 1.0, 0.0, 0.0);
|
|
AssertVector(projectedForward, 1.0, 0.0, 0.0);
|
|
|
|
Vector3 transformedUp = Vector3.Normalize(Vector3.Transform(-Vector3.UnitZ, resultRotation));
|
|
AssertVector(transformedUp, 0.0, 1.0, 0.0, 1e-4);
|
|
|
|
Vector3 transformedForward = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, resultRotation));
|
|
AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void ShouldChooseHorizontalAxisFromActualPose_ForHoisting()
|
|
{
|
|
Quaternion actualRotation = Quaternion.Normalize(new Quaternion(0.70710677f, 0.0f, 0.0f, 0.70710677f));
|
|
Vector3 hostUp = Vector3.UnitY;
|
|
Vector3 targetForward = Vector3.UnitX;
|
|
|
|
bool ok = HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
|
|
actualRotation,
|
|
targetForward,
|
|
hostUp,
|
|
out Quaternion resultRotation,
|
|
out LocalAxisDirection selectedDirection,
|
|
out Vector3 selectedAxisLocal,
|
|
out Vector3 projectedForward);
|
|
|
|
Assert.IsTrue(ok);
|
|
Assert.AreEqual(LocalAxisDirection.PositiveX, selectedDirection);
|
|
AssertVector(selectedAxisLocal, 1.0, 0.0, 0.0);
|
|
AssertVector(projectedForward, 1.0, 0.0, 0.0);
|
|
|
|
Vector3 transformedUp = Vector3.Normalize(Vector3.Transform(-Vector3.UnitZ, resultRotation));
|
|
AssertVector(transformedUp, 0.0, 1.0, 0.0, 1e-4);
|
|
|
|
Vector3 transformedForward = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, resultRotation));
|
|
AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
|
|
}
|
|
|
|
private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6)
|
|
{
|
|
Assert.AreEqual(x, actual.X, tolerance);
|
|
Assert.AreEqual(y, actual.Y, tolerance);
|
|
Assert.AreEqual(z, actual.Z, tolerance);
|
|
}
|
|
}
|
|
}
|