Code for moving fuel to place center of mass over center of thrust

[afranius]

I coded up a little script to rebalance fuel tanks so that the center of mass is centered over the center of thrust, which is pretty useful for VTOL planes or asymmetric rockets. I didn't find anything else that has quite this functionality. It's kind of a cute problem, technically its a quadratic program, but I implemented a very simplified variant of projected Gauss Seidel that seems to more or less do the job. I just integrated it into Kerbal Engineering (since I didn't want to add yet another part to all my ships), but I thought I would post the code in case anyone found it useful. It's pretty simple, just a static function that takes a vessel as input. In my implementation, I just hooked it up to a UI button.

If anyone wants to take this and integrate it into a proper mod, please be my guest, I'm releasing this as public domain (anyone can do anything they want with it). It might make a nice addition to something like MechJeb or one of those fuel balancing mods.

There are a few shortcomings. For example, fuel is transferred instantly, it might be nice to have some delay for the sake of realism. Also, I can't seem to figure out a good way to figure out the direction an engine fires. I use the engine rotation as a proxy, but I don't think this is the best way to do it (it probably won't handle things like the B9 VTOL engines). I tried using thrustTransforms, but I think those are mainly for the VFX, as they are very inaccurate (I get thrust lines that point way off at a diagonal, resulting in very inaccurate balance).

Here is a screenshot of me flying some monstrosity with this thing. It doesn't really fly without automatic fuel balancing, for obvious reasons... not that I have any idea why anyone would want something like that to fly:

Here is the code:

using System;
using UnityEngine;

namespace Engineer
{
    public class FuelBalancer
    {
        // Set this to true to enable debugging printouts.
        private static bool debugging = false;

        public static void BalanceFuel(bool value, Vessel vessel)
        {
            // Only execute if button is pressed.
            if (!value) return;

            // Go through current engines and computer center of thrust.
            Vector3 thrustCenter = new Vector3(0,0,0);
            Vector3 thrustVector = new Vector3(0,0,0);
            bool bEngineFound = false;
            foreach(Part part in vessel.Parts)
            {
				foreach(PartModule module in part.Modules)
                {
					if(module is ModuleEngines)
                    {
                        ModuleEngines engine = (ModuleEngines)module;
                        if (engine.EngineIgnited)
                        {
                            // Found at least one engine.
                            bEngineFound = true;

                            // Get thrust position.
                            Vector3 eCenter = vessel.transform.InverseTransformPoint(engine.transform.position);

                            // Figure out the thrust direction... there has got to be a better way to do this.
                            Vector3 eVector = engine.transform.up;

                            // Undo ship rotation and apply maximum thrust.
                            eVector = vessel.transform.InverseTransformDirection(eVector)*engine.maxThrust;

                            // Compute new thrust center.
                            Vector3 numerator = Vector3.Cross(thrustCenter, thrustVector) + Vector3d.Cross(eCenter, eVector) - Vector3.Cross(eCenter, eVector + thrustVector);
                            Vector3 denominator = Vector3.Cross(thrustCenter, thrustVector + eVector) - Vector3.Cross(eCenter, thrustVector + eVector);
                            float u = 0;
                            if (Math.Abs(denominator.x) >= 1e-8)
                                u = numerator.x / denominator.x;
                            else if (Math.Abs(denominator.y) >= 1e-8)
                                u = numerator.y / denominator.y;
                            else if (Math.Abs(denominator.z) >= 1e-8)
                                u = numerator.z / denominator.z;
                            else
                                u = 0.5f;
                            thrustCenter = Vector3.Lerp(eCenter, thrustCenter, u);
                            // Add the values together to get the thrust vector.
                            thrustVector = thrustVector + eVector;
                        }
                    }
				}
			}

            if (bEngineFound)
            { // Only rebalance if we have at least one active engine.
                // Get center of mass.
                Vector3 centerOfMass = vessel.findLocalCenterOfMass();
                if (debugging) Debug.Log("Initial center of mass: " + (centerOfMass*100.0f).ToString());

                // Move the fuel.
                centerOfMass = MoveFuel(vessel, centerOfMass, thrustCenter, thrustVector, true);

                // Debug printout to specify the current center of thrust and thrust vector.
                if (debugging) Debug.Log("Center of thrust: " + (thrustCenter*100.0f).ToString() + " thrust vector: " + thrustVector.ToString() + " center of mass: " + (centerOfMass * 100.0f).ToString());
            }
        }

        // Transfer fuel to move the center of mass from current position to target.
        public static Vector3 MoveFuel(Vessel vessel, Vector3 centerOfMass, Vector3 targetPosition, Vector3 targetNormal, bool bProject)
        {
            float mass = vessel.GetTotalMass(); // Get total mass.
            int ITERATIONS = 2; // Number of PGS iterations.

            for (int i = 0; i < ITERATIONS; i++)
            {
                // Now step over all tanks and see if we need to transfer.
                foreach (Part part in vessel.parts)
                {
                    // Step over all resources in this tank.
                    foreach (PartResource resource in part.Resources)
                    {
                        // Only process nonempty tanks.
                        if (resource.info.density > 0)
                        { // Only move resources that have mass (don't move electricity!)
                            // Read position.
                            Vector3 sourceFullPos = vessel.transform.InverseTransformPoint(part.transform.position);
                            Vector3 sourcePos = sourceFullPos;
                            if (bProject) sourcePos = sourcePos - Vector3.Project(sourcePos, targetNormal);

                            // Step through all other parts for this resource.
                            foreach (Part part2 in vessel.parts)
                            {
                                if (part != part2)
                                {
                                    foreach (PartResource resource2 in part2.Resources)
                                    {
                                        if (resource2.resourceName == resource.resourceName)
                                        { // Only consider parts with the same type of resource.
                                            // Read position.
                                            Vector3 destFullPos = vessel.transform.InverseTransformPoint(part2.transform.position);
                                            Vector3 destPos = destFullPos;
                                            if (bProject) destPos = destPos - Vector3.Project(destPos, targetNormal);
                                            Vector3 normalizedDiff = destPos - sourcePos;
                                            normalizedDiff = normalizedDiff.normalized;

                                            // Formulate equation so that p2*moveAmount - p1*moveAmount + CoMprojection = CoM
                                            double p1 = Vector3.Dot(sourcePos, normalizedDiff);
                                            double p2 = Vector3.Dot(destPos, normalizedDiff);
                                            double CoMprojection = Vector3.Dot(centerOfMass, normalizedDiff);

                                            // Solve for c = CoT_projection - CoMprojection
                                            double CoTprojection = Vector3.Dot(targetPosition, normalizedDiff);
                                            double c = CoTprojection - CoMprojection;

                                            // Compute optimal resource quantity.
                                            double moveAmount = 0.0;
                                            double denominator = p2 - p1;
                                            if (denominator != 0)
                                                moveAmount = c / denominator;

                                            // Modify by resource density.
                                            moveAmount = moveAmount * mass / resource.info.density;

                                            // Clamp resource quantity by the amount available in the two tanks.
                                            moveAmount = Math.Min(moveAmount, resource.amount);
                                            moveAmount = Math.Max(moveAmount, -(resource.maxAmount - resource.amount));
                                            moveAmount = Math.Max(moveAmount, -resource2.amount);
                                            moveAmount = Math.Min(moveAmount, resource2.maxAmount - resource2.amount);

                                            // Move the resource.
                                            resource.amount -= moveAmount;
                                            resource2.amount += moveAmount;

                                            // Modify the center of mass.
                                            centerOfMass = (centerOfMass * mass - sourceFullPos * ((float)(moveAmount * resource.info.density)) + destFullPos * ((float)(moveAmount * resource.info.density))) * (1 / mass);

                                            // Print result.
                                            if (debugging) Debug.Log("Transferred " + moveAmount.ToString() + " units of " + resource.resourceName + " from " + part.ToString() + " to " + part2.ToString());
                                            if (debugging) Debug.Log("New projected error: " + Vector3.Exclude(targetNormal,centerOfMass-targetPosition).magnitude.ToString());
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
            // Return the center of mass.
            return centerOfMass;
        }
    }
}

[sjwt]

I can see *SO* many applications for this!

Well done!

[EndlessWaves]

I tried using thrustTransforms, but I think those are mainly for the VFX, as they are very inaccurate (I get thrust lines that point way off at a diagonal, resulting in very inaccurate balance).

Thust is applied to the position of that transform in the direction of it's -Z axis so that definitely determines it. You may have got the reference space wrong, being a transform it does have it's own co-ordinates so you need to use that transform's TransformDirection method rather than the top level part transform's.

[Mekan1k]

The MechJeb/Fuelballence Guys had better take notice of this!

Also, +Rep for cleverness!

[Lucid Hills]

Can you have this balance to center of lift?

[ZRM]

This looks like a very nice additional component for my project, KerbCom Avionics CRCS, which is aiming in the next release to provide VTOL support. I had forgotten the fuel distribution aspect of VTOL balancing. See my last post in the dev thread for details.

[afranius]

Thust is applied to the position of that transform in the direction of it's -Z axis so that definitely determines it. You may have got the reference space wrong, being a transform it does have it's own co-ordinates so you need to use that transform's TransformDirection method rather than the top level part transform's.

You mean thrustTransforms? Hmm... What I did was thrustTransforms.forward and transformed it by the inverse ship transform to place it in ship coordinates (like the rest of the quantities). I gathered from the Unity documentation that thrustTransforms.forward is in world space. It's actually very close to the part transform transform.up, but off by a few degrees, which actually makes a big difference. I wonder if thrust vectoring is part of the issue? I'm guessing thrustTransforms.forward gives the actual (vectored) thrust direction, and balancing the fuel to that has a nasty positive feedback effect -- the engine gimbals to stay balanced, fuel is rebalanced so that is the new set point, drains a little, and the engine has to gimbal even more, so the fuel is rebalanced to be even more uneven. In that case, any thoughts on how to get non-vectored thrust direction?

Can you have this balance to center of lift?

The MoveFuel function simply moves fuel so that the center of mass is aligned with the target position (center of thrust) on a plane defined by a specified normal (thrust direction). If you have a way to compute CoL, you could very easily use the same function. You can also turn off projection, in which case it will ignore the thrust direction and just align CoM and target in 3D. This might be useful if you want to move the CoM to match the initial CoM position for example (if you balanced your aircraft just right in the SPH and want to rebalance in flight to match).

[EndlessWaves]

It's actually very close to the part transform transform.up, but off by a few degrees, which actually makes a big difference. I wonder if thrust vectoring is part of the issue? I'm guessing thrustTransforms.forward gives the actual (vectored) thrust direction, and balancing the fuel to that has a nasty positive feedback effect -- the engine gimbals to stay balanced, fuel is rebalanced so that is the new set point, drains a little, and the engine has to gimbal even more, so the fuel is rebalanced to be even more uneven. In that case, any thoughts on how to get non-vectored thrust direction?

Yeah, ModuleGimbal rotates the thrustTransform object as it's the engine module that applies the thrust. Starting (neutral) rotations are stored in initRots of ModuleGimbal. Although if it's linear you may be able to simple work backwards; angle = current angle - maximum gimbal * steering (-1 to 1).