Refine timestep

1. Solve for 0-crossing in vertical velocity, vertical acceleration
2. Limit timestep to next event
3. Revert to using acceleration instead of jerk to limit timestep
4. Save calculations (eg calculate acceleration) in FlightData at start of timestep, results (eg velocity, position) at end of timestep

core/src/net/sf/openrocket/simulation/AbstractEulerStepper.java

@@ -47,25 +47,19 @@ public abstract class AbstractEulerStepper extends AbstractSimulationStepper {
 		
 		// Compute drag force
 		final double mach = airSpeed.length() / atmosphere.getMachSpeed();
-		final double dynP = (0.5 * atmosphere.getDensity() * airSpeed.length2());
+		final double CdA = getCD() * status.getConfiguration().getReferenceArea();
-		final double dragForce = getCD() * dynP * status.getConfiguration().getReferenceArea();
+		final double dragForce = 0.5 * CdA * atmosphere.getDensity() * airSpeed.length2();
 
 		final double rocketMass = calculateStructureMass(status).getMass();
 		final double motorMass = calculateMotorMass(status).getMass();
 		
 		final double mass = rocketMass + motorMass;
-
 		if (mass < MathUtil.EPSILON) {
 			status.abortSimulation(SimulationAbort.Cause.ACTIVE_MASS_ZERO);
 		}
 		
 		// Compute drag acceleration
-		Coordinate linearAcceleration;
+		Coordinate linearAcceleration = airSpeed.normalize().multiply(-dragForce / mass);
-		if (airSpeed.length() > 0.001) {
-			linearAcceleration = airSpeed.normalize().multiply(-dragForce / mass);
-		} else {
-			linearAcceleration = Coordinate.NUL;
-		}
 		
 		// Add effect of gravity
 		final double gravity = modelGravity(status);
@@ -80,38 +74,80 @@ public abstract class AbstractEulerStepper extends AbstractSimulationStepper {
 		// Select tentative time step
 		double timeStep = RECOVERY_TIME_STEP;
 
-		// adjust based on change in acceleration (ie jerk)
+		// adjust based on acceleration
-		final double jerk = Math.abs(linearAcceleration.sub(status.getRocketAcceleration()).multiply(1.0/status.getPreviousTimeStep()).length());
+		final double absAccel = linearAcceleration.length();
-		if (jerk > MathUtil.EPSILON) {
+		if (absAccel > MathUtil.EPSILON) {
-			timeStep = Math.min(timeStep, 1.0/jerk);
+			timeStep = Math.min(timeStep, 1.0/absAccel);
 		}
 
+		// Honor max step size passed in
+		timeStep = Math.min(timeStep, maxTimeStep);
+
 		// but don't let it get *too* small
 		timeStep = Math.max(timeStep, MIN_TIME_STEP);
 		log.trace("timeStep is " + timeStep);
 		
 		// Perform Euler integration
 		EulerValues newVals = eulerIntegrate(status.getRocketPosition(), status.getRocketVelocity(), linearAcceleration, timeStep);
-		/*
-		Coordinate newPosition = status.getRocketPosition().add(status.getRocketVelocity().multiply(timeStep)).
-		add(linearAcceleration.multiply(MathUtil.pow2(timeStep) / 2));*/
 
-		// If I've hit the ground, recalculate time step and position
+		// Check to see if z or either of its first two derivatives have changed sign and recalculate
+		// time step to point of change if so
+		// z   -- ground hit
+		// z'  -- apogee
+		// z'' -- possible oscillation building up in descent rate
+		// Note that it's virtually impossible for apogee to occur on the same
+		// step as either ground hit or descent rate inflection, and if we get a ground hit
+		// any descent rate inflection won't matter
+		final double a = linearAcceleration.z;
+		final double v = status.getRocketVelocity().z;
+		final double z = status.getRocketPosition().z;
+		double t = timeStep;
 		if (newVals.pos.z < 0) {
+			// If I've hit the ground, the new timestep is the solution of
+			// 1/2 at^2 + vt + z = 0
+			t = (-v - Math.sqrt(v*v - 2*a*z))/a;
+			log.trace("ground hit changes timeStep to " + t);
+		} else if (v * newVals.vel.z < 0) {
+			// If I've got apogee, the new timestep is the solution of
+			// v + at = 0
+			t = Math.abs(v / a);
+			log.trace("apogee changes timeStep to " + t);
+		} else {
+			// Use jerk to estimate accleration at end of time step.  Don't really need to redo all the atmospheric
+			// calculations to get it "right"; this will be close enough for our purposes.
+			// use chain rule to compute jerk
+			// dA/dT = dA/dV * dV/dT
+			final double dFdV = CdA * atmosphere.getDensity() * airSpeed.length();
+			final Coordinate dAdV = airSpeed.normalize().multiply(dFdV / mass);
+			final Coordinate jerk = linearAcceleration.multiply(dAdV);
+			final Coordinate newAcceleration = linearAcceleration.add(jerk.multiply(timeStep));
 
-			final double a = linearAcceleration.z;
+			// Only do this one if acceleration is appreciably different from 0
-			final double v = status.getRocketVelocity().z;
+			if (newAcceleration.z * linearAcceleration.z < -MathUtil.EPSILON) {
-			final double z0 = status.getRocketPosition().z;
+				// If acceleration oscillation is building up, the new timestep is the solution of
+				// a + j*t = 0
+				t = Math.abs(a / jerk.z);
+				log.trace("oscillation prevention changes timeStep to " + t);
+			}
+		}
+		
+		// once again, make sure new timestep isn't *too* small
+		t = Math.max(t, MIN_TIME_STEP);
 
-			// The new timestep is the solution of
+		// recalculate Euler integration for position and velocity if necessary.
-			// 1/2 at^2 + vt + z0 = 0
+		if (Math.abs(t - timeStep) > MathUtil.EPSILON) {
-			timeStep = (-v - Math.sqrt(v*v - 2*a*z0))/a;
+			timeStep = t;
-			log.trace("ground hit changes timeStep to " + timeStep);
+			
+			if (maxTimeStep - timeStep < MIN_TIME_STEP) {
+				timeStep = maxTimeStep;
+			}
 
 			newVals = eulerIntegrate(status.getRocketPosition(), status.getRocketVelocity(), linearAcceleration, timeStep);
 
-			// avoid rounding error in new altitude
+			// If we just landed chop off rounding error
-			newVals.pos = newVals.pos.setZ(0);
+			if (Math.abs(newVals.pos.z) < MathUtil.EPSILON) {
+				newVals.pos = newVals.pos.setZ(0);
+			}
 		}
 
 		status.setSimulationTime(status.getSimulationTime() + timeStep);

core/src/net/sf/openrocket/util/Coordinate.java

@@ -190,6 +190,16 @@ public final class Coordinate implements Cloneable, Serializable {
 		return new Coordinate(this.x * m, this.y * m, this.z * m, this.weight * m);
 	}
 	
+	/**
+	 * Multiply the <code>Coordinate</code> with another <Coordinate> component-by-component
+	 *
+	 * @param other the other Coordinate
+	 * @return   The product. 
+	 */
+	public Coordinate multiply(Coordinate other) {
+		return new Coordinate(this.x * other.x, this.y * other.y, this.z * other.z, this.weight * other.weight);
+	}
+	
 	/**
 	 * Dot product of two Coordinates, taken as vectors.  Equal to
 	 * x1*x2+y1*y2+z1*z2