From a7c205281d86f3af47c5348168d9215312118045 Mon Sep 17 00:00:00 2001
From: JoePfeiffer <joseph@pfeifferfamily.net>
Date: Wed, 4 Jan 2023 14:33:19 -0700
Subject: [PATCH] Solve time step for ground hit in tumble stepper

---
 .../simulation/BasicTumbleStepper.java        | 21 +++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/core/src/net/sf/openrocket/simulation/BasicTumbleStepper.java b/core/src/net/sf/openrocket/simulation/BasicTumbleStepper.java
index 65c37d4d7..cd8750e23 100644
--- a/core/src/net/sf/openrocket/simulation/BasicTumbleStepper.java
+++ b/core/src/net/sf/openrocket/simulation/BasicTumbleStepper.java
@@ -69,6 +69,27 @@ public class BasicTumbleStepper extends AbstractSimulationStepper {
 		double timeStep = MathUtil.min(0.5 / linearAcceleration.length(), RECOVERY_TIME_STEP);
 		
 		// Perform Euler integration
+		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
+		if (newPosition.z < 0) {
+
+			final double a = linearAcceleration.z;
+			final double v = status.getRocketVelocity().z;
+			final double z0 = status.getRocketPosition().z;
+
+			// The new timestep is the solution of
+			// 1/2 at^2 + vt + z0 = 0
+			timeStep = (-v - Math.sqrt(v*v - 2*a*z0))/a;
+			
+			newPosition = status.getRocketPosition().add(status.getRocketVelocity().multiply(timeStep)).
+				add(linearAcceleration.multiply(MathUtil.pow2(timeStep) / 2));
+
+			// avoid rounding error in new altitude
+			newPosition = newPosition.setZ(0);
+		}
+
 		status.setRocketPosition(status.getRocketPosition().add(status.getRocketVelocity().multiply(timeStep)).
 				add(linearAcceleration.multiply(MathUtil.pow2(timeStep) / 2)));
 		status.setRocketVelocity(status.getRocketVelocity().add(linearAcceleration.multiply(timeStep)));