105 lines
3.7 KiB
C#
105 lines
3.7 KiB
C#
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
|
using NavisworksTransport.Utils.CoordinateSystem;
|
|
using System.Collections.Generic;
|
|
using System.Numerics;
|
|
|
|
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|
{
|
|
[TestClass]
|
|
public class PathTargetFrameResolverTests
|
|
{
|
|
[TestMethod]
|
|
public void YUp_PlanarFrame_ShouldUseHostYAsUp()
|
|
{
|
|
bool ok = PathTargetFrameResolver.TryCreatePlanarHostFrame(
|
|
new Vector3(2.0f, 0.0f, 1.0f),
|
|
CoordinateSystemType.YUp,
|
|
out PathTargetFrame frame);
|
|
|
|
Assert.IsTrue(ok);
|
|
AssertVector(frame.Up, 0.0, 1.0, 0.0);
|
|
Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Up), 1e-6);
|
|
Assert.AreEqual(1.0, frame.Side.Length(), 1e-6);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void ZUp_PlanarFrame_ShouldUseHostZAsUp()
|
|
{
|
|
bool ok = PathTargetFrameResolver.TryCreatePlanarHostFrame(
|
|
new Vector3(2.0f, 1.0f, 0.0f),
|
|
CoordinateSystemType.ZUp,
|
|
out PathTargetFrame frame);
|
|
|
|
Assert.IsTrue(ok);
|
|
AssertVector(frame.Up, 0.0, 0.0, 1.0);
|
|
Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Up), 1e-6);
|
|
Assert.AreEqual(1.0, frame.Side.Length(), 1e-6);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void Hoisting_StartForward_ShouldUseHorizontalSegmentInsteadOfInitialLift()
|
|
{
|
|
var pathPoints = new List<Vector3>
|
|
{
|
|
new Vector3(-105.992f, -28.615f, 77.043f),
|
|
new Vector3(-105.992f, 16.230f, 77.043f),
|
|
new Vector3(-229.870f, 16.230f, -32.982f)
|
|
};
|
|
|
|
bool ok = PathTargetFrameResolver.TryResolvePlanarStartHostForward(
|
|
NavisworksTransport.PathType.Hoisting,
|
|
pathPoints,
|
|
out Vector3 hostForward);
|
|
|
|
Assert.IsTrue(ok);
|
|
AssertVector(Vector3.Normalize(hostForward), -0.7477, 0.0, -0.6640, 1e-4);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void Hoisting_StartFrame_ShouldUseHorizontalSegmentAndKeepHostUp()
|
|
{
|
|
var pathPoints = new List<Vector3>
|
|
{
|
|
new Vector3(-105.992f, -28.615f, 77.043f),
|
|
new Vector3(-105.992f, 16.230f, 77.043f),
|
|
new Vector3(-229.870f, 16.230f, -32.982f)
|
|
};
|
|
|
|
bool ok = PathTargetFrameResolver.TryCreatePlanarStartHostFrame(
|
|
NavisworksTransport.PathType.Hoisting,
|
|
pathPoints,
|
|
CoordinateSystemType.YUp,
|
|
out PathTargetFrame frame);
|
|
|
|
Assert.IsTrue(ok);
|
|
AssertVector(frame.Up, 0.0, 1.0, 0.0);
|
|
Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Up), 1e-6);
|
|
}
|
|
|
|
[TestMethod]
|
|
public void YUp_PlanarYaw_ShouldUseHostXZPlane_NotHostXY()
|
|
{
|
|
bool ok1 = PathTargetFrameResolver.TryResolvePlanarHostYaw(
|
|
new Vector3(1.0f, 0.0f, 0.0f),
|
|
CoordinateSystemType.YUp,
|
|
out double yaw1);
|
|
bool ok2 = PathTargetFrameResolver.TryResolvePlanarHostYaw(
|
|
new Vector3(0.0f, 0.0f, 1.0f),
|
|
CoordinateSystemType.YUp,
|
|
out double yaw2);
|
|
|
|
Assert.IsTrue(ok1);
|
|
Assert.IsTrue(ok2);
|
|
Assert.AreEqual(0.0, yaw1, 1e-6);
|
|
Assert.AreEqual(-System.Math.PI / 2.0, yaw2, 1e-6);
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|