Computing Space Travel - Implementation

This is the Java code implementing the space travel solution I described in the previous post.

(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.)

	/**
	* 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);
	}
	}
	}

view raw TrajectoryCalculator.java hosted with ❤ by GitHub