Instantly share code, notes, and snippets.

# christerswahn/TrajectoryCalculator.java Created May 16, 2012

What would you like to do?
Computes the optimal trajectory from A to B in a solar system using classical mechanics.
 /** * 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. *

* 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. *

* 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. *

* 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. *

* Input: An instance of TrajectoryParameters that contains:
* deltaPosition
* deltaVelocity
* maxForce (ship's max thrust)
* mass (ship's mass) *

* Output: An array of TrajectoryLeg instances, each containing:
* thrust vector (length of which is the thrust force; less than or equal to maxForce)
* time (length of time to apply the thrust) *

* 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! 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. * * @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. *

* 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: *

*
• From time 0 to t1 apply thrust X *
• From time t1 to t2 apply thrust Y *
• ... *
*

* The number of iterations needed by this implementation should be expected * to average below 10. *

* @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:
* deltaPosition
* deltaVelocity
* maxForce (ship's max thrust)
* 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:
* thrust vector (length of which is the thrust force; less than or equal to maxForce)
* 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); } } }

Owner

### christerswahn commented May 16, 2012

 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.

### proycon commented Apr 17, 2015

 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!