obstacle-math: add test for handling missed bins logic

src/lib/collision_prevention/ObstacleMathTest.cpp

@@ -33,6 +33,7 @@
 
 #include <gtest/gtest.h>
 #include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
 #include "ObstacleMath.hpp"
 
 using namespace matrix;
@@ -158,3 +159,42 @@ TEST(ObstacleMathTest, OffsetBinIndex)
 	// THEN: the new bin index should be correct
 	EXPECT_EQ(new_bin_index, 0);
 }
+
+TEST(ObstacleMathTest, HandleMissedBins)
+{
+	// GIVEN: measurements, current bin, previous bin, bin width, and field of view offset
+	float measurements[8] = {0, 0, 1, 0, 0, 2, 0, 0};
+	int   current_bin     = 2;
+	int   previous_bin    = 5;
+	int   bin_width       = 45.0f;
+	float fov             = 270.0f;
+	float fov_offset      = 360.0f - fov / 2;
+
+	float measurement     = measurements[current_bin];
+
+	// WHEN: we handle missed bins
+	int current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin, bin_width, fov_offset);
+	int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, bin_width, fov_offset);
+
+	int start = math::min(current_bin_offset, previous_bin_offset) + 1;
+	int end   = math::max(current_bin_offset, previous_bin_offset);
+
+	EXPECT_EQ(start, 1);
+	EXPECT_EQ(end,   5);
+
+	for (uint16_t i = start; i < end; i++) {
+		uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, bin_width, -fov_offset);
+		measurements[bin_index] = measurement;
+	}
+
+	// THEN: the missed bins should be populated with the measurement
+	EXPECT_EQ(measurements[0], 1);
+	EXPECT_EQ(measurements[1], 1);
+	EXPECT_EQ(measurements[2], 1);
+	EXPECT_EQ(measurements[3], 0);
+	EXPECT_EQ(measurements[4], 0);
+	EXPECT_EQ(measurements[5], 2);
+	EXPECT_EQ(measurements[6], 1);
+	EXPECT_EQ(measurements[7], 1);
+
+}