public

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

  • Download Gist
TrajectoryCalculator.java
Java
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341
/**
* 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.

Please sign in to comment on this gist.

Something went wrong with that request. Please try again.