Computes the optimal trajectory from A to B in a solar system using classical mechanics.

TrajectoryCalculator.java

/**
 * Authored 2012 by Christer Swahn
 */

import nu.chervil.util.math.SpatialTuple;
import nu.chervil.util.math.SpatialVector;

import org.apache.log4j.Logger;

/**
 * Computes the optimal trajectory from A to B in a solar system using classical
 * mechanics. Finds the trajectory legs (each expressed as a thrust vector and a
 * length of time) that as soon as possible will move the ship to the same
 * position and velocity as the destination.
 * <P>
 * The objective is to plan the space trajectory needed to rendezvous with a
 * given destination within the solar system. It is needed to implement the
 * game's autopilot which is used by both computer-controlled ships and players.
 * The trajectory must be computed in advance since simply going in the
 * direction of the destination would result in arriving with a speed very
 * different from that of the destination object. We need to know beforehand
 * when to 'gas' and when to 'break', and in what precise directions to do this.
 * <P>
 * The planned trajectory should be fairly efficient (primarily regarding travel
 * time, but also regarding fuel consumption) and preferably not cheat by using
 * different physics than the player experiences with the manual controls. It
 * must also be fairly fast to compute - it's an MMO game and there can be many
 * thousands of traveling ships.
 * <P>
 * The formal problem is: The traveling spaceship, at current position P in
 * space and with current velocity V, is to travel and rendezvous with a
 * destination object (a planet, another ship etc) which is at current position
 * Q and with current velocity W. Since it's a rendezvous (e.g. docking or
 * landing) the traveling spaceship must have the same velocity W as the
 * destination upon arrival.
 * <P>
 * <I>Input:</I> An instance of TrajectoryParameters that contains:<BR>
 * deltaPosition<BR>
 * deltaVelocity<BR>
 * maxForce (ship's max thrust)<BR>
 * mass (ship's mass)
 * <P>
 * <I>Output:</I> An array of TrajectoryLeg instances, each containing:<BR>
 * thrust vector (length of which is the thrust force; less than or equal to maxForce)<BR>
 * time (length of time to apply the thrust)
 * <P>
 * The problem solved here makes no mention of changes to the destination's
 * velocity over time. It's been simplified to disregard the destination's own
 * acceleration. In theory the change in velocity direction over time is
 * possible to compute for orbiting celestial bodies, although difficult within
 * the context of this problem. But other spaceships are impossible to predict
 * because someone else is controlling them! <I>Therefore the trajectory must be
 * recomputed regularly during the journey - and we might as well disregard the
 * complication of the destination's continuously changing acceleration in this
 * solution.</I>
 *
 * @author Christer Swahn
 */
public class TrajectoryCalculator {
    public static final Logger LOG = Logger.getLogger(TrajectoryCalculator.class);
    public static final StatCompiler STAT = StatCompiler.getStatCompiler(TrajectoryCalculator.class);

    /** the time tolerance */
    static final double TIME_TOLERANCE = 1e-6;
    /** defines what is regarded as equivalent position and speed */
    static final double GEOMETRIC_TOLERANCE = 1e-6;
    /** the force tolerance during calculation */
    static final double FORCE_TOLERANCE = 1e-6;
    /** the force difference tolerance of the result */
    static final double RESULT_FORCE_TOLERANCE = 1e-2;

    private static final int ITER_LIMIT = 30;


    /**
     * Computes the optimal trajectory to rendezvous with a destination. If an
     * object traverses this trajectory it will have the same velocity as the
     * destination at the point of arrival.
     * <P>
     * The vessel travels using its thrust engine, which accelerates the
     * vessel in whatever direction it is pointing. The planned trajectory
     * is expressed as a sequence of thrust applications, each describing
     * a trajectory leg:
     * <UL>
     * <LI>From time 0 to t1 apply thrust X
     * <LI>From time t1 to t2 apply thrust Y
     * <LI> ...
     * </UL>
     * <P>
     * The number of iterations needed by this implementation should be expected
     * to average below 10.
     * <P>
     * @param tp the trajectory parameters describing the travel to be achieved
     * @return an array with one or more trajectory legs (does not return null)
     * @throws IllegalArgumentException if the trajectory parameters describe a
     *  destination that has already been reached
     */
    public static TrajectoryLeg[] computeRendezvousTrajectory(TrajectoryParameters tp)
            throws IllegalArgumentException {
        TrajectoryLeg[] trajectory = new TrajectoryLeg[2];
        trajectory[0] = new TrajectoryLeg();
        trajectory[1] = new TrajectoryLeg();

        final double dV_magnitude = tp.deltaVelocity.length();
        final double distance = tp.deltaPosition.length();

        // check special case 1, dV = 0:
        if (dV_magnitude < GEOMETRIC_TOLERANCE) {
            if (distance < GEOMETRIC_TOLERANCE)
                throw new IllegalArgumentException("Already at destination");

            double t_tot = Math.sqrt(2*tp.mass*distance / tp.maxForce);
            trajectory[0].thrust.set(tp.deltaPosition).scale(tp.maxForce/distance);
            trajectory[1].thrust.set(trajectory[0].thrust).negate();
            trajectory[0].time = trajectory[1].time = t_tot / 2;
            return trajectory;
        }

        SpatialVector F_v = new SpatialVector();
        SpatialVector D_ttot = new SpatialVector();
        SpatialVector V_d_ttot = new SpatialVector();
        SpatialVector R_d = new SpatialVector();
        SpatialVector F_d = new SpatialVector();
        SpatialVector F_d2 = new SpatialVector();

        // pick f_v in (0, tp.maxForce) via f_v_ratio in (0, 1):
        double best_f_v_ratio = -1;
        double min_f_v_ratio = 0;
        double max_f_v_ratio = 1;
        double simple_f_v_ratio = calcSimpleRatio(tp);
        double f_v_ratio = simple_f_v_ratio;  // start value
        min_f_v_ratio = simple_f_v_ratio * .99;  // (account for rounding error)
        int nofIters = 0;
        do {
            nofIters++;
            double f_v = f_v_ratio * tp.maxForce;

            double t_tot = tp.mass / f_v * dV_magnitude;

            F_v.set(tp.deltaVelocity).scale(tp.mass / t_tot);
            D_ttot.set(tp.deltaVelocity).scale(t_tot/2).add(tp.deltaPosition);

            double dist_ttot = D_ttot.length();

            // check special case 2, dP_ttot = 0:
            if (dist_ttot < GEOMETRIC_TOLERANCE) {
                // done!  F1 = F2 = Fv (only one leg)  (such exact alignment of dV and dP is rare)
                // FUTURE: should we attempt to find more optimal trajectory in case f_v < maxForce?
                if (f_v_ratio < 0.5)
                    LOG.warn("Non-optimal trajectory for special case: dV and dP aligned: f_v_ratio=" + f_v_ratio);
                trajectory = new TrajectoryLeg[1];
                trajectory[0] = new TrajectoryLeg(F_v, t_tot);
                return trajectory;
            }

            V_d_ttot.set(D_ttot).scale(2/t_tot);
            R_d.set(D_ttot).scale(1/dist_ttot);  // normalized D_ttot

            double alpha = Math.PI - F_v.angle(R_d);  // angle between F_v and F_p1
            assert (alpha >= 0 && alpha <= Math.PI) : alpha + " not in (0, PI), F_v.dot(R_p1)=" + F_v.dot(R_d);

            double f_d;
            if (Math.PI - alpha < 0.00001) {
                // special case 3a, F_v and F_p1 are parallel in same direction
                f_d = tp.maxForce - f_v;
            }
            else if (alpha < 0.00001) {
                // special case 3b, F_v and F_p1 are parallel in opposite directions
                f_d = tp.maxForce + f_v;
            }
            else {
                double sin_alpha = Math.sin(alpha);
                f_d = tp.maxForce/sin_alpha * Math.sin(Math.PI - alpha - Math.asin(f_v/tp.maxForce*sin_alpha));
                assert (f_d > 0 && f_d < 2*tp.maxForce) : f_d + " not in (0, " + (2*tp.maxForce) + ")";
            }

            double t_1 = 2*tp.mass*dist_ttot / (t_tot*f_d);
            double t_2 = t_tot - t_1;
            if (t_2 < TIME_TOLERANCE) {
                // pick smaller f_v
                //LOG.debug(String.format("Iteration %2d:             f_v_ratio %f; t_2 %,7.3f", nofIters, f_v_ratio, t_2));
                max_f_v_ratio = f_v_ratio;
                f_v_ratio += (min_f_v_ratio - f_v_ratio) / 2;  // (divisor experimentally calibrated)
                continue;
            }

            F_d.set(R_d).scale(f_d);
            F_d2.set(F_d).scale(-t_1/t_2);  // since I_d = -I_d2
            assert (F_d.copy().scale( t_1/tp.mass).differenceMagnitude(V_d_ttot) < GEOMETRIC_TOLERANCE) : F_d;
            assert (F_d2.copy().scale(-t_2/tp.mass).differenceMagnitude(V_d_ttot) < GEOMETRIC_TOLERANCE) : F_d2;

            SpatialVector F_1 = F_d.add(F_v);  // NB: overwrites F_d instance
            SpatialVector F_2 = F_d2.add(F_v);  // NB: overwrites F_d2 instance
            assert (Math.abs(F_1.length()-tp.maxForce)/tp.maxForce < FORCE_TOLERANCE) : "f1=" + F_1.length() + " != f=" + tp.maxForce;
            assert verifyF1F2(tp, t_1, t_2, F_1, F_2) : "F1=" + F_1 + "; F2=" + F_2;

            double f_2 = F_2.length();
            if (f_2 > tp.maxForce) {
                // pick smaller f_v
                //LOG.debug(String.format("Iteration %2d:             f_v_ratio %f; f_2 diff %e", nofIters, f_v_ratio, (tp.maxForce-f_2)));
                max_f_v_ratio = f_v_ratio;
                f_v_ratio += (min_f_v_ratio - f_v_ratio) / 1.25;  // (divisor experimentally calibrated)
            }
            else {
                // best so far
                //LOG.debug(String.format("Iteration %2d: best so far f_v_ratio %f; f_2 diff %e; max_f_v_ratio %f", nofIters, f_v_ratio, (tp.maxForce-f_2), max_f_v_ratio));
                best_f_v_ratio = f_v_ratio;
                trajectory[0].set(F_1, t_1);
                trajectory[1].set(F_2, t_2);
                if (f_2 < (tp.maxForce*(1-RESULT_FORCE_TOLERANCE))) {
                    // pick greater f_v
                    min_f_v_ratio = f_v_ratio;
                    f_v_ratio += (max_f_v_ratio - f_v_ratio) / 4;  // (divisor experimentally calibrated)
                }
                else {
                    break;  // done!
                }
            }
        } while (nofIters < ITER_LIMIT);

        if (best_f_v_ratio >= 0) {
            return trajectory;
        }
        else {
            LOG.warn(String.format("Couldn't determine full trajectory for %s (nofIters %d) best_f_v_ratio=%.12f", tp, nofIters, best_f_v_ratio));
            trajectory = new TrajectoryLeg[1];
            trajectory[0] = new TrajectoryLeg(tp.deltaVelocity, dV_magnitude*tp.mass/tp.maxForce);
            trajectory[0].thrust.add(tp.deltaPosition).normalize().scale(tp.maxForce);  // set thrust direction to average of dP and dV
            return trajectory;
        }
    }

    private static boolean verifyF1F2(TrajectoryParameters tp, double t_1, double t_2, SpatialVector F_1, SpatialVector F_2) {
        final double REL_TOLERANCE = 1e-5;

        SpatialVector V_1 = F_1.copy().scale(t_1/tp.mass);
        SpatialVector V_2 = F_2.copy().scale(t_2/tp.mass);
        SpatialVector achievedDeltaVelocity = V_1.copy().add(V_2);
        double velDiff = achievedDeltaVelocity.differenceMagnitude(tp.deltaVelocity);
        assert (velDiff/tp.deltaVelocity.length() < REL_TOLERANCE) : velDiff/tp.deltaVelocity.length() +
            "; difference=" + velDiff + "; achievedDeltaVelocity=" + achievedDeltaVelocity + "; tp.deltaVelocity=" + tp.deltaVelocity;

        SpatialVector targetPosition = tp.deltaVelocity.copy().scale(t_1+t_2).add(tp.deltaPosition);
        SpatialVector achievedPosition = V_1.scale(t_1/2+t_2).add(V_2.scale(t_2/2));
        double posDiff = achievedPosition.differenceMagnitude(targetPosition);
        assert (posDiff/tp.deltaPosition.length() < REL_TOLERANCE) : posDiff/tp.deltaPosition.length() +
            "; difference=" + posDiff + "; achievedPosition=" + achievedPosition + "; targetPosition=" + targetPosition;

        return true;
    }


    private static double calcSimpleRatio(TrajectoryParameters tp) {
        // compute the f_v ratio of the simplest trajectory where we accelerate in two steps:
        // 1) reduce the velocity difference with the destination to 0 (time needed: t_v)
        // 2) traverse the distance to arrive at the destination (time needed: t_p)
        // (This usually takes longer than the optimal trajectory, since the distance traversal
        // does not utilize the full travel time period. (The greater travel period that
        // the distance traversal can use, the less impulse (acceleration) it needs.))

        double inv_acc = tp.mass / tp.maxForce;
        double t_v = inv_acc * tp.deltaVelocity.length();

        SpatialVector newDeltaPos = tp.deltaVelocity.copy().scale(t_v/2).add(tp.deltaPosition);
        double distance = newDeltaPos.length();
        double t_p = 2 * Math.sqrt(inv_acc * distance);

        double t_tot = t_v + t_p;
        double f_v_ratio = t_v / t_tot;
        return f_v_ratio;
    }



    /** Describes the starting conditions for a sought trajectory.
     * The members are:<BR>
     * deltaPosition<BR>
     * deltaVelocity<BR>
     * maxForce (ship's max thrust)<BR>
     * mass (ship's mass)
     */
    public static class TrajectoryParameters {
        public final SpatialVector deltaPosition;
        public final SpatialVector deltaVelocity;
        public final double maxForce;
        public final double mass;

        public TrajectoryParameters(SpatialTuple p0, SpatialTuple u, SpatialTuple q0, SpatialTuple v, double maxForce, double mass) {
            this(new SpatialVector(q0).sub(p0), new SpatialVector(v).sub(u), maxForce, mass);
        }

        public TrajectoryParameters(SpatialTuple deltaPosition, SpatialTuple deltaVelocity, double maxForce, double mass) {
            if (! deltaPosition.isFinite())
                throw new IllegalArgumentException("deltaPosition is not finite: " + deltaPosition);
            if (! deltaVelocity.isFinite())
                throw new IllegalArgumentException("deltaVelocity is not finite: " + deltaPosition);
            if (maxForce <= 0)
                throw new IllegalArgumentException("Force must be greater than zero (" + maxForce + ")");
            this.deltaPosition = new SpatialVector(deltaPosition);
            this.deltaVelocity = new SpatialVector(deltaVelocity);
            this.maxForce = maxForce;
            this.mass = mass;
        }

        @Override
        public String toString() {
            return String.format("[%s: deltaP=%s deltaV=%s maxForce=%,.1f mass=%,.0f]", getClass().getSimpleName(), deltaPosition, deltaVelocity, maxForce, mass);
        }
    }



    /** Describes a computed trajectory leg.
     * The members are:<BR>
     * thrust vector (length of which is the thrust force; less than or equal to maxForce)<BR>
     * time (length of time to apply the thrust)
     */
    public static class TrajectoryLeg {
        public SpatialVector thrust;
        public double time;

        public TrajectoryLeg() {
            this.thrust = new SpatialVector();
            this.time = 0;
        }
        public TrajectoryLeg(SpatialVector thrust, double time) {
            this.thrust = new SpatialVector(thrust);
            this.time = time;
        }

        public void set(SpatialVector thrust, double time) {
            this.thrust.set(thrust);
            this.time = time;
        }

        @Override
        public String toString() {
            return String.format("[%s: thrust=%s time=%,.1f]", getClass().getSimpleName(), thrust, time);
        }
    }
}

The vector and point classes it uses are very similar to Java 3D's vecmath and it would be straight-forward to modify this code to use vecmath instead.

Thanks for your great work and your blog post (http://mmoarch.blogspot.nl/2012/05/computing-space-travel.html) on the subject! This is precisely what I was looking for, I ported your code to Python and visualised it with vpython: https://gist.github.com/proycon/92a7b822f6269e178788 , it works marvellously!