NavisworksTransport/UnitTests/CoordinateSystem/CanonicalTrackedPositionResolverTests.cs

using Microsoft.VisualStudio.TestTools.UnitTesting;
using NavisworksTransport.Utils.CoordinateSystem;
using System.Numerics;

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class CanonicalTrackedPositionResolverTests
    {
        [TestMethod]
        public void GroundReference_ShouldOffsetHalfHeightAlongUp()
        {
            Vector3 result = CanonicalTrackedPositionResolver.ResolveGroundTrackedCenter(
                new Vector3(10f, 20f, 30f),
                Vector3.UnitZ,
                4.0);

            AssertVector(result, 10.0, 20.0, 32.0);
        }

        [TestMethod]
        public void TopSuspensionReference_ShouldOffsetNegativeHalfHeightAlongUp()
        {
            Vector3 result = CanonicalTrackedPositionResolver.ResolveTopSuspensionTrackedCenter(
                new Vector3(10f, 20f, 30f),
                Vector3.UnitZ,
                4.0);

            AssertVector(result, 10.0, 20.0, 28.0);
        }

        [TestMethod]
        public void ArbitraryNormalReference_ShouldOffsetAlongProvidedNormal()
        {
            Vector3 normal = Vector3.Normalize(new Vector3(0f, 1f, 1f));
            Vector3 result = CanonicalTrackedPositionResolver.ResolveCenterFromContactReference(
                new Vector3(5f, 6f, 7f),
                normal,
                4.0,
                0.0);

            Vector3 expected = new Vector3(5f, 6f, 7f) + normal * 2f;
            AssertVector(result, expected.X, expected.Y, expected.Z);
        }

        [TestMethod]
        public void MidSurfaceFactor_ShouldProduceNoOffset()
        {
            Vector3 result = CanonicalTrackedPositionResolver.ResolveCenterFromContactReference(
                new Vector3(1f, 2f, 3f),
                Vector3.UnitZ,
                8.0,
                0.5);

            AssertVector(result, 1.0, 2.0, 3.0);
        }

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