From 5b93532d03f418cb087a6a8222d7be545463cb2b Mon Sep 17 00:00:00 2001
From: Megamouse <studienricky89@googlemail.com>
Date: Thu, 12 Dec 2024 02:42:57 +0100
Subject: [PATCH] move tracker: ignore sporadic invalid results

The tracker may fail for a couple of frames occasionally.
Just report the last known position instead.
---
 rpcs3/Input/ps_move_tracker.cpp | 32 +++++++++++++++++++++++++++++---
 rpcs3/Input/ps_move_tracker.h   |  3 +++
 2 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/rpcs3/Input/ps_move_tracker.cpp b/rpcs3/Input/ps_move_tracker.cpp
index fe49501334..062731a052 100644
--- a/rpcs3/Input/ps_move_tracker.cpp
+++ b/rpcs3/Input/ps_move_tracker.cpp
@@ -40,6 +40,26 @@ ps_move_tracker<DiagnosticsEnabled>::~ps_move_tracker()
 	}
 }
 
+template <bool DiagnosticsEnabled>
+void ps_move_tracker<DiagnosticsEnabled>::set_valid(ps_move_info& info, u32 index, bool valid)
+{
+	u32& fail_count = ::at32(m_fail_count, index);
+
+	if (info.valid && !valid)
+	{
+		// Ignore a couple of untracked frames. This reduces noise.
+		if (++fail_count >= 3)
+		{
+			info.valid = valid;
+		}
+
+		return;
+	}
+
+	info.valid = valid;
+	fail_count = 0; // Reset fail count
+};
+
 template <bool DiagnosticsEnabled>
 void ps_move_tracker<DiagnosticsEnabled>::set_image_data(const void* buf, u64 size, u32 width, u32 height, s32 format)
 {
@@ -136,7 +156,7 @@ void ps_move_tracker<DiagnosticsEnabled>::init_workers()
 
 				// Find contours
 				ps_move_info& info = m_info[index];
-				ps_move_info new_info{};
+				ps_move_info new_info = info;
 				process_contours(new_info, index);
 
 				if (new_info.valid)
@@ -179,6 +199,7 @@ void ps_move_tracker<DiagnosticsEnabled>::process_image()
 		{
 			ps_move_info& info = m_info[index];
 			info.valid = false;
+			m_fail_count[index] = 0;
 		}
 	}
 
@@ -303,7 +324,6 @@ void ps_move_tracker<DiagnosticsEnabled>::process_contours(ps_move_info& info, u
 	const u32 height = m_height;
 	const bool wrapped_hue = config.min_hue > config.max_hue; // e.g. min=355, max=5 (red)
 
-	info.valid = false;
 	info.x_max = width;
 	info.y_max = height;
 
@@ -362,7 +382,10 @@ void ps_move_tracker<DiagnosticsEnabled>::process_contours(ps_move_info& info, u
 	cv::findContours(binary, all_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
 
 	if (all_contours.empty())
+	{
+		set_valid(info, index, false);
 		return;
+	}
 
 	std::vector<std::vector<cv::Point>> contours;
 	contours.reserve(all_contours.size());
@@ -406,7 +429,10 @@ void ps_move_tracker<DiagnosticsEnabled>::process_contours(ps_move_info& info, u
 	}
 
 	if (best_index == umax)
+	{
+		set_valid(info, index, false);
 		return;
+	}
 
 	// Calculate distance from sphere to camera
 	const f32 sphere_radius_pixels = radii[best_index];
@@ -417,7 +443,7 @@ void ps_move_tracker<DiagnosticsEnabled>::process_contours(ps_move_info& info, u
 	const f32 distance_mm = (focal_length_pixels * CELL_GEM_SPHERE_RADIUS_MM) / sphere_radius_pixels;
 
 	// Set results
-	info.valid = true;
+	set_valid(info, index, true);
 	info.distance_mm = distance_mm;
 	info.radius = sphere_radius_pixels;
 	info.x_pos = std::clamp(static_cast<u32>(centers[best_index].x), 0u, width);
diff --git a/rpcs3/Input/ps_move_tracker.h b/rpcs3/Input/ps_move_tracker.h
index be1b598852..00f5155541 100644
--- a/rpcs3/Input/ps_move_tracker.h
+++ b/rpcs3/Input/ps_move_tracker.h
@@ -71,6 +71,8 @@ private:
 		void calculate_values();
 	};
 
+	void set_valid(ps_move_info& info, u32 index, bool valid);
+
 	u32 m_width = 0;
 	u32 m_height = 0;
 	s32 m_format = 0;
@@ -90,6 +92,7 @@ private:
 
 	std::array<std::vector<u8>, CELL_GEM_MAX_NUM> m_image_hue_filtered{};
 	std::array<std::vector<u8>, CELL_GEM_MAX_NUM> m_image_binary{};
+	std::array<u32, CELL_GEM_MAX_NUM> m_fail_count{};
 
 	std::array<u32, 360> m_hues{};
 	std::array<ps_move_info, CELL_GEM_MAX_NUM> m_info{};