using System; using System.Collections.Generic; using System.Reflection; using System.Runtime.Remoting.Messaging; using System.Runtime.Remoting.Proxies; using Sandbox.ModAPI.Ingame; using VRage.Game.ModAPI.Ingame; using VRageMath; using Xunit; using P = AutoMiningScript.Program; namespace AutoMiningScript.Tests { public class GyroControlTests { // Independent PB actuator semantics verified against the installed game's MyGyro setters // (Steam build 24675677) and the referenced Sandbox.Game.dll: all three setters negate // the input and ignore it while override is off. Terminal sliders have different signs. // This ideal rate actuator checks control direction; it does not emulate game physics. sealed class GyroApi : RealProxy { public readonly IMyGyro Value; public IMyCubeGrid Grid; public MatrixD World; public Vector3D PhysicalRate; public bool Override; public int IgnoredWrites; public GyroApi(IMyCubeGrid grid) : base(typeof(IMyGyro)) { Grid = grid; World = MatrixD.Identity; Value = (IMyGyro)GetTransparentProxy(); } public override IMessage Invoke(IMessage message) { var call = (IMethodCallMessage)message; object result = null; switch (call.MethodName) { case "get_CubeGrid": result = Grid; break; case "get_IsFunctional": case "get_Enabled": case "get_IsWorking": result = true; break; case "get_WorldMatrix": result = World; break; case "get_GyroOverride": result = Override; break; case "set_GyroOverride": Override = (bool)call.Args[0]; break; case "get_Pitch": result = (float)-PhysicalRate.X; break; case "get_Yaw": result = (float)-PhysicalRate.Y; break; case "get_Roll": result = (float)-PhysicalRate.Z; break; case "set_Pitch": case "set_Yaw": case "set_Roll": if (!Override) { IgnoredWrites++; break; } double rate = -(float)call.Args[0]; if (call.MethodName == "set_Pitch") PhysicalRate.X = rate; else if (call.MethodName == "set_Yaw") PhysicalRate.Y = rate; else PhysicalRate.Z = rate; break; } var method = (MethodInfo)call.MethodBase; if (result == null && method.ReturnType != typeof(void) && method.ReturnType.IsValueType) result = Activator.CreateInstance(method.ReturnType); return new ReturnMessage(result, call.Args, call.ArgCount, call.LogicalCallContext, call); } public Vector3D WorldRate { get { return World.Right * PhysicalRate.X + World.Up * PhysicalRate.Y + World.Backward * PhysicalRate.Z; } } } sealed class Rig { public readonly P Program = TestRig.Program(); public readonly P.ShipHardware Hardware; public readonly P.FlightController Flight; public readonly GyroApi Gyro; readonly FieldInfo basis = typeof(P.FlightController).GetField("basis", BindingFlags.Instance | BindingFlags.NonPublic); readonly MethodInfo attitude = typeof(P.FlightController).GetMethod("ApplyAttitude", BindingFlags.Instance | BindingFlags.NonPublic); public Rig() { Hardware = new P.ShipHardware(Program); Flight = new P.FlightController(Program, Hardware); Gyro = new GyroApi(Program.Me.CubeGrid); Hardware.Gyros.Add(Gyro.Value); } public Vector3D Command(MatrixD current, MatrixD desired, MatrixD mounting, Vector3D rate) { basis.SetValue(Flight, current); Gyro.World = mounting * current; Program.Now += 1.0 / 60; attitude.Invoke(Flight, new object[] { desired, rate }); return Gyro.WorldRate; } } static IEnumerable Mountings() { var axes = new[] { Vector3D.Right, Vector3D.Left, Vector3D.Up, Vector3D.Down, Vector3D.Forward, Vector3D.Backward }; foreach (var forward in axes) foreach (var up in axes) if (Math.Abs(Vector3D.Dot(forward, up)) < .1) yield return MatrixD.CreateWorld(Vector3D.Zero, forward, up); } static Vector3D Axis(int axis) { return axis == 0 ? Vector3D.Right : axis == 1 ? Vector3D.Up : Vector3D.Backward; } static Vector3D Rotate(Vector3D v, Vector3D axis, double angle) { // Rodrigues integration is independent of FlightController.RotationError and gyro mappings. return v * Math.Cos(angle) + Vector3D.Cross(axis, v) * Math.Sin(angle) + axis * Vector3D.Dot(axis, v) * (1 - Math.Cos(angle)); } static MatrixD Advance(MatrixD orientation, Vector3D rate, double dt) { double speed = rate.Length(); if (speed < 1e-12) return orientation; Vector3D axis = rate / speed; return MatrixD.CreateWorld(Vector3D.Zero, Rotate(orientation.Forward, axis, speed * dt), Rotate(orientation.Up, axis, speed * dt)); } static double PoseDistance(MatrixD a, MatrixD b) { return Vector3D.DistanceSquared(a.Forward, b.Forward) + Vector3D.DistanceSquared(a.Up, b.Up); } [Theory] [InlineData(0, -1)] [InlineData(0, 1)] [InlineData(1, -1)] [InlineData(1, 1)] [InlineData(2, -1)] [InlineData(2, 1)] public void EveryGyroMountingConvergesTowardRequestedWorldRotation(int axis, int sign) { int count = 0; foreach (MatrixD mounting in Mountings()) { count++; var r = new Rig(); MatrixD current = MatrixD.CreateFromYawPitchRoll(.31, -.47, .23); Vector3D requiredAxis = Axis(axis) * sign; MatrixD desired = Advance(current, requiredAxis, .32); Vector3D rate = r.Command(current, desired, mounting, Vector3D.Zero); Assert.True(Vector3D.Dot(rate, requiredAxis) > .5, "Gyro mounting " + count + " rotated away from target"); double previous = PoseDistance(current, desired); for (int step = 0; step < 360; step++) { current = Advance(current, rate, 1.0 / 60); double distance = PoseDistance(current, desired); Assert.True(distance <= previous + 1e-10, "Ideal actuator increased pose error at step " + step); previous = distance; rate = r.Command(current, desired, mounting, rate); } Assert.True(PoseDistance(current, desired) < 1e-7); Assert.Equal(0, r.Gyro.IgnoredWrites); } Assert.Equal(24, count); } [Theory] [InlineData(0)] [InlineData(1)] [InlineData(2)] public void ZeroPoseErrorDampsWorldAngularVelocityForEveryGyroMounting(int axis) { foreach (MatrixD mounting in Mountings()) { var r = new Rig(); MatrixD current = MatrixD.CreateFromYawPitchRoll(-.4, .5, .6); Vector3D velocity = Axis(axis) * .4; Vector3D command = r.Command(current, current, mounting, velocity); Assert.True(Vector3D.Dot(command, velocity) < 0); Assert.True(Vector3D.Distance(command, -velocity * .35) < 1e-7); } } [Fact] public void FirstEnableOverwritesDormantCommandsBeforeTheyCanBeUsed() { var r = new Rig(); r.Gyro.PhysicalRate = new Vector3D(.2, -.3, .4); Assert.False(r.Gyro.Override); Vector3D rate = r.Command(MatrixD.Identity, MatrixD.Identity, MatrixD.Identity, Vector3D.Zero); Assert.True(r.Gyro.Override); Assert.Equal(Vector3D.Zero, rate); Assert.Equal(0, r.Gyro.IgnoredWrites); } [Fact] public void ReleasingActiveControlZerosRatesBeforeDisablingOverride() { var r = new Rig(); r.Command(MatrixD.Identity, Advance(MatrixD.Identity, Vector3D.Up, .2), MatrixD.Identity, Vector3D.Zero); Assert.True(r.Gyro.PhysicalRate.Length() > .1); r.Flight.Release(); Assert.False(r.Gyro.Override); Assert.Equal(Vector3D.Zero, r.Gyro.PhysicalRate); Assert.Equal(0, r.Gyro.IgnoredWrites); } } }