Add hysteresis to physics timestep count per frame

Add new class _TimerSync to manage timestep calculations.
The new class handles the decisions about simulation progression
previously handled by main::iteration(). It is fed the current timer
ticks and determines how many physics updates are to be run and what
the delta argument to the _process() functions should be.

The new class tries to keep the number of physics updates per frame as
constant as possible from frame to frame. Ideally, it would be N steps
every render frame, but even with perfectly regular rendering, the
general case is that N or N+1 steps are required per frame, for some
fixed N. The best guess for N is stored in typical_physics_steps.

When determining the number of steps to take, no restrictions are
imposed between the choice of typical_physics_steps and
typical_physics_steps+1 steps. Should more or less steps than that be
required, the accumulated remaining time (as before, stored in
time_accum) needs to surpass its boundaries by some minimal threshold.
Once surpassed, typical_physics_steps is updated to allow the new step
count for future updates.

Care is taken that the modified calculation of the number of physics
steps is not observable from game code that only checks the delta
parameters to the _process and _physics_process functions; in addition
to modifying the number of steps, the _process argument is modified as
well to stay in expected bounds. Extra care is taken that the accumulated
steps still sum up to roughly the real elapsed time, up to a maximum
tolerated difference.

To allow the hysteresis code to work correctly on higher refresh
monitors, the number of typical physics steps is not only recorded and
kept consistent for single render frames, but for groups of them.
Currently, up to 12 frames are grouped that way.

The engine parameter physics_jitter_fix controls both the maximum
tolerated difference between wall clock time and summed up _process
arguments and the threshold for changing typical_physics_steps. It is
given in units of the real physics frame slice 1/physics_fps. Set
physics_jitter_fix to 0 to disable the effects of the new code here.
It starts to be effective against the random physics jitter at around
0.02 to 0.05. at values greater than 1 it starts having ill effects on
the engine's ability to react sensibly to dropped frames and framerate
changes.

1 files changed, 238 insertions, 15 deletions

diff --git a/main/main.cpp b/main/main.cpp
index a59ca3da3b..a01ef00cd0 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -955,6 +955,7 @@ Error Main::setup(const char *execpath, int argc, char *argv[], bool p_second_ph
 	}
 
 	Engine::get_singleton()->set_iterations_per_second(GLOBAL_DEF("physics/common/physics_fps", 60));
+	Engine::get_singleton()->set_physics_jitter_fix(GLOBAL_DEF("physics/common/physics_jitter_fix", 0.5));
 	Engine::get_singleton()->set_target_fps(GLOBAL_DEF("debug/settings/fps/force_fps", 0));
 
 	GLOBAL_DEF("debug/settings/stdout/print_fps", false);
@@ -1225,6 +1226,229 @@ Error Main::setup2(Thread::ID p_main_tid_override) {
 	return OK;
 }
 
+// everything the main loop needs to know about frame timings
+struct _FrameTime {
+	float animation_step; // time to advance animations for (argument to process())
+	int physics_steps; // number of times to iterate the physics engine
+
+	void clamp_animation(float min_animation_step, float max_animation_step) {
+		if (animation_step < min_animation_step) {
+			animation_step = min_animation_step;
+		} else if (animation_step > max_animation_step) {
+			animation_step = max_animation_step;
+		}
+	}
+};
+
+class _TimerSync {
+	// wall clock time measured on the main thread
+	uint64_t last_cpu_ticks_usec;
+	uint64_t current_cpu_ticks_usec;
+
+	// logical game time since last physics timestep
+	float time_accum;
+
+	// current difference between wall clock time and reported sum of animation_steps
+	float time_deficit;
+
+	// number of frames back for keeping accumulated physics steps roughly constant.
+	// value of 12 chosen because that is what is required to make 144 Hz monitors
+	// behave well with 60 Hz physics updates. The only worse commonly available refresh
+	// would be 85, requiring CONTROL_STEPS = 17.
+	static const int CONTROL_STEPS = 12;
+
+	// sum of physics steps done over the last (i+1) frames
+	int accumulated_physics_steps[CONTROL_STEPS];
+
+	// typical value for accumulated_physics_steps[i] is either this or this plus one
+	int typical_physics_steps[CONTROL_STEPS];
+
+protected:
+	// returns the fraction of p_frame_slice required for the timer to overshoot
+	// before advance_core considers changing the physics_steps return from
+	// the typical values as defined by typical_physics_steps
+	float get_physics_jitter_fix() {
+		return Engine::get_singleton()->get_physics_jitter_fix();
+	}
+
+	// gets our best bet for the average number of physics steps per render frame
+	// return value: number of frames back this data is consistent
+	int get_average_physics_steps(float &p_min, float &p_max) {
+		p_min = typical_physics_steps[0];
+		p_max = p_min + 1;
+
+		for (int i = 1; i < CONTROL_STEPS; ++i) {
+			const float typical_lower = typical_physics_steps[i];
+			const float current_min = typical_lower / (i + 1);
+			if (current_min > p_max)
+				return i; // bail out of further restrictions would void the interval
+			else if (current_min > p_min)
+				p_min = current_min;
+			const float current_max = (typical_lower + 1) / (i + 1);
+			if (current_max < p_min)
+				return i;
+			else if (current_max < p_max)
+				p_max = current_max;
+		}
+
+		return CONTROL_STEPS;
+	}
+
+	// advance physics clock by p_animation_step, return appropriate number of steps to simulate
+	_FrameTime advance_core(float p_frame_slice, int p_iterations_per_second, float p_animation_step) {
+		_FrameTime ret;
+
+		ret.animation_step = p_animation_step;
+
+		// simple determination of number of physics iteration
+		time_accum += ret.animation_step;
+		ret.physics_steps = floor(time_accum * p_iterations_per_second);
+
+		int min_typical_steps = typical_physics_steps[0];
+		int max_typical_steps = min_typical_steps + 1;
+
+		// given the past recorded steps and typcial steps to match, calculate bounds for this
+		// step to be typical
+		bool update_typical = false;
+
+		for (int i = 0; i < CONTROL_STEPS - 1; ++i) {
+			int steps_left_to_match_typical = typical_physics_steps[i + 1] - accumulated_physics_steps[i];
+			if (steps_left_to_match_typical > max_typical_steps ||
+					steps_left_to_match_typical + 1 < min_typical_steps) {
+				update_typical = true;
+				break;
+			}
+
+			if (steps_left_to_match_typical > min_typical_steps)
+				min_typical_steps = steps_left_to_match_typical;
+			if (steps_left_to_match_typical + 1 < max_typical_steps)
+				max_typical_steps = steps_left_to_match_typical + 1;
+		}
+
+		// try to keep it consistent with previous iterations
+		if (ret.physics_steps < min_typical_steps) {
+			const int max_possible_steps = floor((time_accum)*p_iterations_per_second + get_physics_jitter_fix());
+			if (max_possible_steps < min_typical_steps) {
+				ret.physics_steps = max_possible_steps;
+				update_typical = true;
+			} else {
+				ret.physics_steps = min_typical_steps;
+			}
+		} else if (ret.physics_steps > max_typical_steps) {
+			const int min_possible_steps = floor((time_accum)*p_iterations_per_second - get_physics_jitter_fix());
+			if (min_possible_steps > max_typical_steps) {
+				ret.physics_steps = min_possible_steps;
+				update_typical = true;
+			} else {
+				ret.physics_steps = max_typical_steps;
+			}
+		}
+
+		time_accum -= ret.physics_steps * p_frame_slice;
+
+		// keep track of accumulated step counts
+		for (int i = CONTROL_STEPS - 2; i >= 0; --i) {
+			accumulated_physics_steps[i + 1] = accumulated_physics_steps[i] + ret.physics_steps;
+		}
+		accumulated_physics_steps[0] = ret.physics_steps;
+
+		if (update_typical) {
+			for (int i = CONTROL_STEPS - 1; i >= 0; --i) {
+				if (typical_physics_steps[i] > accumulated_physics_steps[i]) {
+					typical_physics_steps[i] = accumulated_physics_steps[i];
+				} else if (typical_physics_steps[i] < accumulated_physics_steps[i] - 1) {
+					typical_physics_steps[i] = accumulated_physics_steps[i] - 1;
+				}
+			}
+		}
+
+		return ret;
+	}
+
+	// calls advance_core, keeps track of deficit it adds to animaption_step, make sure the deficit sum stays close to zero
+	_FrameTime advance_checked(float p_frame_slice, int p_iterations_per_second, float p_animation_step) {
+		if (fixed_fps != -1)
+			p_animation_step = 1.0 / fixed_fps;
+
+		// compensate for last deficit
+		p_animation_step += time_deficit;
+
+		_FrameTime ret = advance_core(p_frame_slice, p_iterations_per_second, p_animation_step);
+
+		// we will do some clamping on ret.animation_step and need to sync those changes to time_accum,
+		// that's easiest if we just remember their fixed difference now
+		const double animation_minus_accum = ret.animation_step - time_accum;
+
+		// first, least important clamping: keep ret.animation_step consistent with typical_physics_steps.
+		// this smoothes out the animation steps and culls small but quick variations.
+		{
+			float min_average_physics_steps, max_average_physics_steps;
+			int consistent_steps = get_average_physics_steps(min_average_physics_steps, max_average_physics_steps);
+			if (consistent_steps > 3) {
+				ret.clamp_animation(min_average_physics_steps * p_frame_slice, max_average_physics_steps * p_frame_slice);
+			}
+		}
+
+		// second clamping: keep abs(time_deficit) < jitter_fix * frame_slise
+		float max_clock_deviation = get_physics_jitter_fix() * p_frame_slice;
+		ret.clamp_animation(p_animation_step - max_clock_deviation, p_animation_step + max_clock_deviation);
+
+		// last clamping: make sure time_accum is between 0 and p_frame_slice for consistency between physics and animation
+		ret.clamp_animation(animation_minus_accum, animation_minus_accum + p_frame_slice);
+
+		// restore time_accum
+		time_accum = ret.animation_step - animation_minus_accum;
+
+		// track deficit
+		time_deficit = p_animation_step - ret.animation_step;
+
+		return ret;
+	}
+
+	// determine wall clock step since last iteration
+	float get_cpu_animation_step() {
+		uint64_t cpu_ticks_elapsed = current_cpu_ticks_usec - last_cpu_ticks_usec;
+		last_cpu_ticks_usec = current_cpu_ticks_usec;
+
+		return cpu_ticks_elapsed / 1000000.0;
+	}
+
+public:
+	explicit _TimerSync() :
+			last_cpu_ticks_usec(0),
+			current_cpu_ticks_usec(0),
+			time_accum(0),
+			time_deficit(0) {
+		for (int i = CONTROL_STEPS - 1; i >= 0; --i) {
+			typical_physics_steps[i] = i;
+			accumulated_physics_steps[i] = i;
+		}
+	}
+
+	// start the clock
+	void init(uint64_t p_cpu_ticks_usec) {
+		current_cpu_ticks_usec = last_cpu_ticks_usec = p_cpu_ticks_usec;
+	}
+
+	// set measured wall clock time
+	void set_cpu_ticks_usec(uint64_t p_cpu_ticks_usec) {
+		current_cpu_ticks_usec = p_cpu_ticks_usec;
+	}
+
+	// advance one frame, return timesteps to take
+	_FrameTime advance(float p_frame_slice, int p_iterations_per_second) {
+		float cpu_animation_step = get_cpu_animation_step();
+
+		return advance_checked(p_frame_slice, p_iterations_per_second, cpu_animation_step);
+	}
+
+	void before_start_render() {
+		VisualServer::get_singleton()->sync();
+	}
+};
+
+static _TimerSync _timer_sync;
+
 bool Main::start() {
 
 	ERR_FAIL_COND_V(!_start_success, false);
@@ -1239,6 +1463,8 @@ bool Main::start() {
 	String _export_preset;
 	bool export_debug = false;
 
+	_timer_sync.init(OS::get_singleton()->get_ticks_usec());
+
 	List<String> args = OS::get_singleton()->get_cmdline_args();
 	for (int i = 0; i < args.size(); i++) {
 		//parameters that do not have an argument to the right
@@ -1704,7 +1930,6 @@ bool Main::start() {
 
 uint64_t Main::last_ticks = 0;
 uint64_t Main::target_ticks = 0;
-float Main::time_accum = 0;
 uint32_t Main::frames = 0;
 uint32_t Main::frame = 0;
 bool Main::force_redraw_requested = false;
@@ -1717,14 +1942,15 @@ bool Main::iteration() {
 
 	uint64_t ticks = OS::get_singleton()->get_ticks_usec();
 	Engine::get_singleton()->_frame_ticks = ticks;
+	_timer_sync.set_cpu_ticks_usec(ticks);
 
 	uint64_t ticks_elapsed = ticks - last_ticks;
 
-	double step = (double)ticks_elapsed / 1000000.0;
-	if (fixed_fps != -1)
-		step = 1.0 / fixed_fps;
+	int physics_fps = Engine::get_singleton()->get_iterations_per_second();
+	float frame_slice = 1.0 / physics_fps;
 
-	float frame_slice = 1.0 / Engine::get_singleton()->get_iterations_per_second();
+	_FrameTime advance = _timer_sync.advance(frame_slice, physics_fps);
+	double step = advance.animation_step;
 
 	Engine::get_singleton()->_frame_step = step;
 
@@ -1740,20 +1966,19 @@ bool Main::iteration() {
 
 	last_ticks = ticks;
 
-	if (fixed_fps == -1 && step > frame_slice * 8)
-		step = frame_slice * 8;
-
-	time_accum += step;
+	static const int max_physics_steps = 8;
+	if (fixed_fps == -1 && advance.physics_steps > max_physics_steps) {
+		step -= (advance.physics_steps - max_physics_steps) * frame_slice;
+		advance.physics_steps = max_physics_steps;
+	}
 
 	float time_scale = Engine::get_singleton()->get_time_scale();
 
 	bool exit = false;
 
-	int iters = 0;
-
 	Engine::get_singleton()->_in_physics = true;
 
-	while (time_accum > frame_slice) {
+	for (int iters = 0; iters < advance.physics_steps; ++iters) {
 
 		uint64_t physics_begin = OS::get_singleton()->get_ticks_usec();
 
@@ -1775,12 +2000,10 @@ bool Main::iteration() {
 		Physics2DServer::get_singleton()->end_sync();
 		Physics2DServer::get_singleton()->step(frame_slice * time_scale);
 
-		time_accum -= frame_slice;
 		message_queue->flush();
 
 		physics_process_ticks = MAX(physics_process_ticks, OS::get_singleton()->get_ticks_usec() - physics_begin); // keep the largest one for reference
 		physics_process_max = MAX(OS::get_singleton()->get_ticks_usec() - physics_begin, physics_process_max);
-		iters++;
 		Engine::get_singleton()->_physics_frames++;
 	}
 
@@ -1791,7 +2014,7 @@ bool Main::iteration() {
 	OS::get_singleton()->get_main_loop()->idle(step * time_scale);
 	message_queue->flush();
 
-	VisualServer::get_singleton()->sync(); //sync if still drawing from previous frames.
+	_timer_sync.before_start_render(); //sync if still drawing from previous frames.
 
 	if (OS::get_singleton()->can_draw() && !disable_render_loop) {