Resurrecting Gimbal Auto Trim - Help Wanted

[Deltac]

I'm trying my best to resurrect the old Gimbal Auto Trim that Sarbian created long ago in a distant land.  Unfortunately my unfamiliarity with the KSP API has me in vector river without a paddle.

Here is the code so far:

using System;
using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace GimbalAutoTrim
{
    [KSPModule("Auto-Trim")]
    public class ModuleGimbalAutoTrim : ModuleGimbal
    {

        // Backup of the backup of the default engine rotation
        public List<Quaternion> initalRots;

        public struct ThrustInfo
        {
            public Vector3 com;          // Center of Mass
            public Vector3 cotAll;       // Center of Thrust for all Engines
            public Vector3 dotOther;     // Direction of Thrust for engines without AutoTrim Active
            public Vector3 dotAligned;   // Direction of Thrust for engines with AutoTrim Active
            public float thrustOther;    // Total Thrust or engines without AutoTrim Active
            public float thrustAligned;  // Total Thrust or engines with AutoTrim Active
        }

        private static float lastFixedTime = 0;
        private static Dictionary<Guid, ThrustInfo> vesselsThrustInfo = new Dictionary<Guid, ThrustInfo>();

        public override void OnStart(PartModule.StartState state)
        {
            base.OnStart(state);

            initalRots = new List<Quaternion>();
            foreach (Quaternion q in initRots)
                initalRots.Add(q);
        }

        [KSPField(isPersistant = true, guiActive = true, guiActiveEditor = true, guiName = "Auto-Trim"),
        UI_Toggle(scene = UI_Scene.All)]
        public bool gimbalAutoTrim = false;

        [KSPField(isPersistant = true, guiActive = false, guiActiveEditor = true, guiName = "Auto-Trim Limit"),
        UI_FloatRange(minValue = 0f, maxValue = 90f, scene = UI_Scene.Editor, stepIncrement = 5f)]
        public float trimLimit = 45f;

        [KSPField(isPersistant = false, guiActive = false, guiName = "Angle")]
        public string trimStatus = "";

        [KSPField(isPersistant = false, guiActive = true, guiName = "correction")]
        public string correction = "";

        [KSPField(isPersistant = false, guiActive = true, guiName = "trimAngle")]
        public string trimAngle = "";

        public override string GetInfo()
        {
            return base.GetInfo();
        }

        public ThrustInfo updateThrust()
        {
            ThrustInfo ti = new ThrustInfo();

            ti.cotAll = Vector3.zero;
            ti.dotOther = Vector3.zero;
            ti.dotAligned = Vector3.zero;
            ti.thrustOther = 0;
            ti.thrustAligned = 0;

            foreach (Part p in vessel.Parts)
            {
                Vector3 coT = Vector3.zero;
                Vector3 doT = Vector3.zero;
                float thrust = 0;

                Vector3 thurstForward = Vector3.zero;

                ModuleEngines engine = p.Modules.OfType<ModuleEngines>().FirstOrDefault();
                ModuleEnginesFX enginefx = p.Modules.OfType<ModuleEnginesFX>().Where(e => e.isEnabled).FirstOrDefault();

                if (enginefx != null || engine != null)
                {
                    List<Transform> thrustTransforms;

                    ModuleGimbal gimbal = p.Modules.OfType<ModuleGimbal>().FirstOrDefault();

                    if (engine != null)
                        thrustTransforms = engine.thrustTransforms;
                    else // ModuleEnginesFX
                        thrustTransforms = enginefx.thrustTransforms;

                    List<Vector3> forwards;
                    if (gimbal != null)
                    {
                        List<Quaternion> initRots = gimbal.initRots;
                        forwards = new List<Vector3>(initRots.Count);
                        for (int i = 0; i < initRots.Count; i++)
                        {
                            Transform tt = thrustTransforms[i];
                            Quaternion ori = tt.localRotation;
                            tt.localRotation = (gimbal is ModuleGimbalAutoTrim) ? (gimbal as ModuleGimbalAutoTrim).initalRots[i] : initRots[i];
                            forwards.Add(tt.forward);
                            tt.localRotation = ori;
                        }
                    }
                    else
                    {
                        forwards = new List<Vector3>(thrustTransforms.Count);
                        foreach (Transform thrustTransform in thrustTransforms)
                            forwards.Add(thrustTransform.forward);
                    }

                    for (int i = 0; i < thrustTransforms.Count; i++)
                        foreach (Transform thrustTransform in thrustTransforms)
                        {
                            coT = coT + (thrustTransforms[i].position - p.transform.position);
                            doT = doT + forwards[i];
                        }
                    coT = (coT / (float)thrustTransforms.Count) + p.transform.position;
                    doT = doT / (float)thrustTransforms.Count;

                    thrust = (engine != null) ? engine.finalThrust : enginefx.finalThrust;

                    if (gimbal != null && (gimbal is ModuleGimbalAutoTrim) && (gimbal as ModuleGimbalAutoTrim).gimbalAutoTrim)
                    {
                        ti.dotAligned += doT * thrust;
                        ti.thrustAligned += thrust;
                    }
                    else
                    {
                        ti.dotOther += doT * thrust;
                        ti.thrustOther += thrust;
                    }
                    ti.cotAll += coT * thrust;
                }

            }
            ti.cotAll = ti.cotAll / (ti.thrustOther + ti.thrustAligned);
            ti.com = this.vessel.localCoM;

            return ti;
        }

        public override void OnFixedUpdate()
        {
            if (HighLogic.LoadedSceneIsEditor || this.vessel == null)
                return;

            Fields["trimStatus"].guiActive = this.gimbalAutoTrim;

            if (this.gimbalAutoTrim)
            {
                ThrustInfo ti;
                // If we moved to a new phyisic frame then clear the cache
                if (lastFixedTime != Time.fixedTime)
                {
                    vesselsThrustInfo = new Dictionary<Guid, ThrustInfo>();
                    lastFixedTime = Time.fixedTime;
                }
                if (!vesselsThrustInfo.ContainsKey(vessel.id))
                {
                    ti = updateThrust();
                    vesselsThrustInfo.Add(vessel.id, ti);
                }
                else
                    ti = vesselsThrustInfo[vessel.id];

                if (ti.thrustAligned > 0f)
                {
                    Vector3 optimalDot = ti.cotAll - ti.com;
                    Vector3 currentDot = ti.dotOther + ti.dotAligned;

                    // CoT in front of CoM
                    if (Vector3.Dot(optimalDot, currentDot) < 0f)
                        optimalDot = -optimalDot;

                    // We work in a 2D plane whose axis are correction and optimalDot 
                    // correction is the perpendicular to optimalDot in the plane defined by optimalDot/currentDot 
                    //Vector3 correction = Vector3.ProjectOnPlane(optimalDot, currentDot); //.Exclude(optimalDot, currentDot);
                    Vector3 correction = Vector3.Exclude(optimalDot, currentDot);
                    this.correction = PrettyPrint(correction, "F2");

                    //float thrustAlignedX = Vector3.ProjectOnPlane(optimalDot, ti.dotAligned).magnitude; //Vector3.Exclude(optimalDot, ti.dotAligned).magnitude;
                    float thrustAlignedX = Vector3.Exclude(optimalDot, ti.dotAligned).magnitude;

                    float x = Mathf.Clamp(thrustAlignedX - correction.magnitude, -ti.thrustAligned, ti.thrustAligned);

                    float y = Mathf.Sqrt(ti.thrustAligned * ti.thrustAligned - x * x);

                    Vector3 trimedDotAligned = correction.normalized * x + optimalDot.normalized * y;

                    float trimAngle = Vector3.Angle(ti.dotAligned, trimedDotAligned);
                    this.trimAngle = trimAngle.ToString("F2");

                    Quaternion trimRotation = Quaternion.FromToRotation(ti.dotAligned, trimedDotAligned);

                    //print("optimalDir " + PrettyPrint(optimalDir) + " currentDir " + PrettyPrint(currentDir) + " correction " + PrettyPrint(correction) + " angle " + Vector3.Angle(currentDir, optimalDir).ToString("F2") + " cAngle " + trimAngle.ToString("F3"));

                    float trimRatio = Mathf.Min(trimLimit / trimAngle, 1f);

                    this.trimStatus = (trimAngle * trimRatio).ToString("F1") + " deg";

                    for (int i = 0; i < this.initRots.Count(); i++)
                    {
                        gimbalTransforms[i].localRotation = initalRots[i];
                        gimbalTransforms[i].forward = Quaternion.Lerp(Quaternion.identity, trimRotation, trimRatio) * gimbalTransforms[i].forward;
                        this.initRots[i] = gimbalTransforms[i].localRotation;
                    }
                }
                else
                {
                    for (int i = 0; i < this.initalRots.Count(); i++)
                    {
                        this.initRots[i] = initalRots[i];
                    }
                }
            }
            else
            {
                for (int i = 0; i < this.initalRots.Count(); i++)
                {
                    this.initRots[i] = initalRots[i];
                }
            }

            base.OnFixedUpdate();
        }


        //public override void OnUpdate()
        //{

        //    base.OnUpdate();
        //}

        public static string PrettyPrint(Vector3d vector, string format = "F3")
        {
            return "[" + (vector.x >= 0 ? " " : "") + vector.x.ToString(format) + ", " + (vector.y >= 0 ? " " : "") + vector.y.ToString(format) + ", " + (vector.z >= 0 ? " " : "") + vector.z.ToString(format) + "]";
        }

        public static new void print(object message)
        {
            MonoBehaviour.print("[ModuleGimbalAligned] " + message);
        }


    }
}

Now I have it "working" in KSP, however the engine doesn't gimbal into the center of mass, as evidenced by these pictures:

 

I'm hoping the community can help me understand this code so I can make the corrections necessary and give back to this community.

Edited March 26, 2019 by Deltac

[Deltac]

(oh no double post)

At the moment, after looking over Sarb's code, I'm thinking about rewriting the code to simplify it.  However, I have some questions:

1.  Does each engine in a vessel have their own Center of Thrust vector?  I know there's the total COT for the whole vessel, but I only want the COT of 1 engine so I can tell the engine to gimbal in a certain direction.  Or do I just use the location of the part as a whole to then do maths with the COM location to find the rotation needed?

2.  How would I get the engine's thrust's current rotation?  Or the gimbal's current rotation?  Or is it the thrust transform specifically?

 

Psuedo code ahead!

 

Quaternion Current_Rot = 0
Quaternion Rot_needed = 0

Vector3 COM = (0,0,0)
Vector3 COT = (0,0,0)


COM = Get Vessel COM //this.vessle.localCOM?
COT = This parts Center of Thrust, not the whole vessel's.
Current_Rot = current rotation of the engine's thrust transform?

Code and math to get the Rot_needed value

Math to find out the difference between Current_Rot and Rot_needed.  //If it's within a certain error, don't reset it?  We still want the engines to have                                                                        //some gimbal.  Is there a way to know when the user or mechjeb are acting on the                                                                          //control inputs?

Set the Engine's thrust's rotation to Rot_needed.  //Problem:  What happens if the user rotates the engine in the editor?  Will that throw things off?

 

I certainly hope someone can help me out with this.  I'll look at the API, but I'm not sure where to start to find out what I need to rotate.

[Deltac]

Here's the code I could come up with so far

 

using System;
using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Gimbal_Auto_Trim
{
    [KSPModule("Auto-Trim")]
    public class ModuleGimbalAutoTrim : ModuleGimbal
    {

        // Backup of the backup of the default engine rotation
        public List<Quaternion> initalRots;
        public List<Quaternion> ZeroGimbal;

        //probable don't need
        public struct ThrustInfo
        {
            public Vector3 com;          // Center of Mass
            public Vector3 cotAll;       // Center of Thrust for all Engines
            public Vector3 dotOther;     // Direction of Thrust for engines without AutoTrim Active
            public Vector3 dotAligned;   // Direction of Thrust for engines with AutoTrim Active
            public float thrustOther;    // Total Thrust or engines without AutoTrim Active
            public float thrustAligned;  // Total Thrust or engines with AutoTrim Active
        }

        private static float lastFixedTime = 0;
        private static Dictionary<Guid, ThrustInfo> vesselsThrustInfo = new Dictionary<Guid, ThrustInfo>();

        public override void OnStart(PartModule.StartState state)
        {
            base.OnStart(state);

            //get the initial rotation
            initalRots = new List<Quaternion>();
            foreach (Quaternion q in initRots)
                initalRots.Add(q);

            //I think this is the same as initRots.
            ZeroGimbal = new List<Quaternion>();
            for (int i = 0; i < gimbalTransforms.Count(); i++)
            {
                ZeroGimbal.Add(gimbalTransforms[i].localRotation);

            }
        }

        [KSPField(isPersistant = true, guiActive = true, guiActiveEditor = true, guiName = "Auto-Trim"),
        UI_Toggle(scene = UI_Scene.All)]
        public bool gimbalAutoTrim = false;

        //Was trying to see if I could refine the gimbal, but failed.
        [KSPField(isPersistant = true, guiActive = true, guiActiveEditor = true, guiName = "Auto-Trim Limit"),
        UI_FloatRange(minValue = 80f, maxValue = 90f, scene = UI_Scene.All, stepIncrement = .1f)]
        public float trimLimit = 90f;

        //leftover code
        [KSPField(isPersistant = false, guiActive = false, guiName = "Angle")]
        public string trimStatus = "";

        [KSPField(isPersistant = false, guiActive = true, guiName = "correction")]
        public string correction = "";

        [KSPField(isPersistant = false, guiActive = true, guiName = "trimAngle")]
        public string trimAngle = "";

        public override string GetInfo()
        {
            return base.GetInfo();
        }


        public override void OnFixedUpdate()
        {
            //If we're in the editor, move on, nothing to see here.
            //if (HighLogic.LoadedSceneIsEditor || this.vessel == null)
                //return;


            //Fields["trimStatus"].guiActive = this.gimbalAutoTrim;

            //If the user turned on auto trim
            if (this.gimbalAutoTrim)
            {
                
                // If we moved to a new phyisic frame then clear the cache
                if (lastFixedTime != Time.fixedTime)
                {
                    vesselsThrustInfo = new Dictionary<Guid, ThrustInfo>();
                    lastFixedTime = Time.fixedTime;
                }

                //this thrustTransform stuff we probably don't need.
                ModuleEngines engine = part.Modules.OfType<ModuleEngines>().FirstOrDefault();
                ModuleEnginesFX enginefx = part.Modules.OfType<ModuleEnginesFX>().Where(e => e.isEnabled).FirstOrDefault();

                List<Transform> thrustTransforms; //Some engines can have more than 1 thrusttransform

                //Get the list of thrust transforms from the current engine.
                if (engine != null)
                    thrustTransforms = engine.thrustTransforms;
                else // ModuleEnginesFX
                    thrustTransforms = enginefx.thrustTransforms;



                Vector3 com;

                com = this.vessel.localCoM;

                Quaternion target_rot;

                for (int i = 0; i < this.initRots.Count(); i++)
                {
                    //thrustTransforms[i].rotation = Quaternion.Euler(x, 0, 0);

                    //gimbalTransforms[i].localRotation = Quaternion.FromToRotation(gimbalTransforms[i].localPosition, com);

                    //Debug.Log("ZeroGimbal " + ZeroGimbal[i].eulerAngles);

                    Debug.Log("initRots " + this.initRots[i].eulerAngles);

                    Debug.Log("Transform " + gimbalTransforms[i].localRotation.eulerAngles);

                    //Debug.Log("Thrust Transform " + thrustTransforms[i].localRotation.eulerAngles);

                    //Get the rotation from the gimbalTransform to the Center of Mass
                    target_rot = Quaternion.FromToRotation(gimbalTransforms[i].localPosition, com);
                    //Then we do some fancy math to get close, but torque still exists >.<
                    this.initRots[i] = (initalRots[i] * (ZeroGimbal[i] * Quaternion.Inverse(target_rot))) * Quaternion.Euler(trimLimit, 0f, 0f);

                    //gimbalTransforms[i].LookAt(com);

                    //this.initRots[i] = initalRots[i] * (ZeroGimbal[i] * Quaternion.Inverse(target_rot));

                    Debug.Log("target rot " + target_rot.eulerAngles);

                    Debug.Log("Difference " + (initalRots[i] * (ZeroGimbal[i] * Quaternion.Inverse(target_rot))).eulerAngles);

                    //this.initRots[i] = ZeroGimbal[i] * Quaternion.Inverse(target_rot);


                }
            }
            else //Reset the engine gimbal
            {
                for (int i = 0; i < this.initalRots.Count(); i++)
                {
                    this.initRots[i] = initalRots[i];
                    Debug.Log("Gimbal Transform Rotation " + gimbalTransforms[i].eulerAngles);
                }
            }

            base.OnFixedUpdate();
        }


        //public override void OnUpdate()
        //{

        //    base.OnUpdate();
        //}

        public static string PrettyPrint(Vector3d vector, string format = "F3")
        {
            return "[" + (vector.x >= 0 ? " " : "") + vector.x.ToString(format) + ", " + (vector.y >= 0 ? " " : "") + vector.y.ToString(format) + ", " + (vector.z >= 0 ? " " : "") + vector.z.ToString(format) + "]";
        }

        public static new void print(object message)
        {
            MonoBehaviour.print("[ModuleGimbalAligned] " + message);
        }
    }
}

This only works for the MainSail engine and engines that were modeled like it.  For some reason the Gimbal Transform (I guess) has no standardization to how it's rotated.

 

Anyway, since there's no interest in this project, I'm calling it quits.  If someone wants to mess around with my code, feel free.  I'm going to take a break from KSP for a little while but I'll be back.  I'll always be back...