Space Engineers - Ackermann Steering - Suspension Controller

WheelController wheelController;
static MyGridProgram GP;

const bool debug = true;

void Main(string arg)
{
    if(wheelController == null || arg == "reset")
    {
        if(debug)
            GP = this;
        List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
        GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(blocks);
        List<IMyTerminalBlock> wheels = new List<IMyTerminalBlock>();
        GridTerminalSystem.GetBlocksOfType<IMyMotorSuspension>(wheels);

        // Lets initialize an example wheel controller
        wheelController = new WheelController(
                blocks.Count == 0 ? Me : blocks[0],
                wheels,
                3f,
                1f,
                new Vector3D(0, 0, 0)
            );
    }
    wheelController.Update();
}

public class WheelController
{
    static readonly Dictionary<string, Vector3D> WheelOffsetLookup = new Dictionary<string, Vector3D>
        {
            { "Large", new Vector3D(0, 0, -3.1) },
            { "Small", new Vector3D(0, 0, -0.6) }
        };

    List<Wheel> Wheels = new List<Wheel>();
    IMyTerminalBlock Anchor;
    Vector3D AnchorOffset;
    float SteerSpeedFactor;
    float ReturnSpeedFactor;

    /*
        @anchor - The anchor block which gets used as reference for position and orientation.
        @wheels - The list of wheels with which we have to work.
        @steerSpeedFactor - This is a proportional factor for total steer speed over the max angle.
        @returnSpeedFactor - This is a proportional factor for total return speed over the max angle.
        @offset - The additional offset in forward and right direction.
    */
    public WheelController(IMyTerminalBlock anchor, List<IMyTerminalBlock> wheels, float steerSpeedFactor, float steerReturnFactor, Vector3D offset)
    {
        Wheels.Clear();
        Anchor = anchor;
        AnchorOffset = offset;
        Vector3D center = Anchor.GetPosition();
        MatrixD anchorMatrixInv = MatrixD.Invert(Anchor.WorldMatrix);

        SteerSpeedFactor = steerSpeedFactor;
        ReturnSpeedFactor = steerReturnFactor;

        double FocusLeft = double.MaxValue;
        double FocusRight = double.MinValue;

        bool hasDestoryedWheels = false;

        for(int i = 0; i < wheels.Count; i++)
        {
            // Check if the wheel actually is a suspension wheel.
            IMyMotorSuspension wheel = wheels[i] as IMyMotorSuspension;
            if(wheel == null)
                continue;

            // Get the transformed local matrix in respect to the new anchor with offset.
            MatrixD wheelMatrix = wheel.WorldMatrix * anchorMatrixInv;
            wheelMatrix.Translation += AnchorOffset;

            MatrixD wheelMatrixInv = wheelMatrix;
            wheelMatrixInv.Translation = new Vector3D(0,0,0);
            wheelMatrixInv = MatrixD.Invert(wheelMatrixInv);
            Vector3D wheelOffset = WheelOffsetLookup.ContainsKey(wheel.BlockDefinition.SubtypeId) ?
                WheelOffsetLookup[wheel.BlockDefinition.SubtypeId] : WheelOffsetLookup[wheel.CubeGrid.GridSizeEnum.ToString()];
            wheelOffset = Vector3D.Transform(wheelOffset, wheelMatrixInv);
            wheelMatrix.Translation += wheelOffset;

            float maxSteerAngle = wheel.GetMaximum<float>("MaxSteerAngle");
            if(wheel.CustomName.Contains("MaxSteerAngle="))
            {
                string[] split = wheel.CustomName.Split(' ');
                for(int j = 0; j < split.Length; j++)
                {
                    if(!split[j].StartsWith("MaxSteerAngle="))
                        continue;
                    string angleString = split[j].Replace("MaxSteerAngle=","");
                    float angle = 0;
                    bool isDegree = split[j].EndsWith("°");
                    if(isDegree)
                        split[j] = split[j].Replace("°","");
                    if(!float.TryParse(angleString, out angle))
                        continue;
                    if(isDegree)
                        angle = VRageMath.MathHelper.ToRadians(angle);
                    if(angle < 0)
                        angle *= -1;
                    if(angle < maxSteerAngle)
                        maxSteerAngle = angle;
                }
            }
            double Z = wheelMatrix.Translation.GetDim(2);
            double tan = Math.Tan(maxSteerAngle);
            double deltaX = Math.Abs(Z / tan);
            double X = wheelMatrix.Translation.GetDim(0);
            double CurrentFocusLeft = X - deltaX;
            double CurrentFocusRight = X + deltaX;
            //DEBUG
            if(false && debug)
                GP.Echo(string.Format("> {0}\nX:{1:0.00} - Z:{2:0.00}", wheel.CustomName, wheelMatrix.Translation.GetDim(0),wheelMatrix.Translation.GetDim(2)));
                //GP.Echo(string.Format("> {0}\nX:{3} - Z\nangle: {4:0.000} - tan: {5:0.000}\n{1}:{2}",wheel.CustomName, CurrentFocusLeft, CurrentFocusRight,X,Z,maxSteerAngle,tan));
            if(CurrentFocusLeft < FocusLeft)
                FocusLeft = CurrentFocusLeft;
            if(CurrentFocusRight > FocusRight)
                FocusRight = CurrentFocusRight;

            // Add the new wheel with parameters to the list of wheels.
            Wheels.Add(new Wheel(
                    wheel,
                    wheelMatrix,
                    maxSteerAngle
                ));
        }
        for(int i = 0; i < Wheels.Count; i++)
        {
            Wheels[i].SetFocus((float)FocusLeft, (float)FocusRight, SteerSpeedFactor, ReturnSpeedFactor);
        }
    }

    public void Update()
    {
        Wheels.RemoveAll(x => !x.IsFunctional());
        for(int i = 0; i < Wheels.Count; i++)
        {
            Wheels[i].Update();
        }
    }

    public class Wheel
    {
        IMyMotorSuspension Block;
        float AngleLeft;
        float AngleRight;
        float AngleMax;
        MatrixD MatrixLocal;
        MatrixD MatrixLocalInv;
        float SteerSpeedLeft, SteerSpeedRight;
        float ReturnSpeedLeft, ReturnSpeedRight;
        float LastAngle;
        float LastDirection;

        public Wheel(IMyMotorSuspension block, MatrixD matrix, float angleMax)
        {
            Block = block;
            MatrixLocal = matrix;
            AngleMax = angleMax;
        }

        public bool BlockExists()
        {
            return Block != null;
        }

        public bool IsFunctional()
        {
            return Block.IsFunctional;
        }

        public bool IsWorking()
        {
            return Block.IsWorking;
        }

        public bool Update()
        {
            if(!Block.IsWorking)
                return false;

            if(Block.SteerAngle < -0.001f)
            {
                if(Block.MaxSteerAngle != AngleLeft)
                    Block.SetValue<float>("MaxSteerAngle", AngleLeft);
                if(Block.SteerSpeed != SteerSpeedLeft)
                    Block.SetValue<float>("SteerSpeed", SteerSpeedLeft);
                if(Block.SteerReturnSpeed != ReturnSpeedLeft)
                    Block.SetValue<float>("SteerReturnSpeed", ReturnSpeedLeft);
            }
            else if(Block.SteerAngle > 0.001f)
            {
                if(Block.MaxSteerAngle != AngleRight)
                    Block.SetValue<float>("MaxSteerAngle", AngleRight);
                if(Block.SteerSpeed != SteerSpeedRight)
                    Block.SetValue<float>("SteerSpeed", SteerSpeedRight);
                if(Block.SteerReturnSpeed != ReturnSpeedRight)
                    Block.SetValue<float>("SteerReturnSpeed", ReturnSpeedRight);
            }
            float CurrentAngle = Block.SteerAngle;
            float CurrentDirection = CurrentAngle - LastAngle;

            // Final sets for the next run.
            LastAngle = CurrentAngle;
            LastDirection = CurrentDirection;
            return true;
        }

        public void SetFocus(float focusLeft, float focusRight, float steerSpeedFactor, float returnSpeedFactor)
        {
            float X = (float)MatrixLocal.Translation.GetDim(0);
            float Z = (float)MatrixLocal.Translation.GetDim(2);
            AngleLeft = (float)Math.Abs(Math.Atan(Z/((Z > 0 ? focusLeft : focusRight ) - X)));
            AngleRight = (float)Math.Abs(Math.Atan(Z/((Z > 0 ? focusRight : focusLeft ) - X)));
            SteerSpeedLeft = AngleLeft * steerSpeedFactor;
            SteerSpeedRight = AngleRight * steerSpeedFactor;
            ReturnSpeedLeft = AngleLeft * returnSpeedFactor;
            ReturnSpeedRight = AngleRight * returnSpeedFactor;
        }
    }
}