Add angle rounding for heatseeker

Author: VirxEC

src/simulation/ball.rs

@@ -263,6 +263,8 @@ impl Ball {
                     .pitch
                     .clamp(-heatseeker::MAX_TURN_PITCH, heatseeker::MAX_TURN_PITCH);
 
+                new_angle = new_angle.round();
+
                 // Determine new interpolated speed
                 let current_state = ((vel - heatseeker::INITIAL_TARGET_SPEED)
                     / heatseeker::TARGET_SPEED_INCREMENT)
@@ -273,7 +275,6 @@ impl Ball {
                 let new_speed = vel + ((target_speed - vel) * heatseeker::SPEED_BLEND);
 
                 // Update velocity
-                // dbg!(new_angle);
                 self.velocity = new_angle.get_forward_vec() * new_speed;
             }
 

src/simulation/geometry.rs

@@ -61,6 +61,23 @@ impl Angle {
         delta.normalize_fix();
         delta
     }
+
+    #[allow(clippy::cast_precision_loss)]
+    #[allow(clippy::cast_possible_truncation)]
+    #[must_use]
+    pub fn round(self) -> Self {
+        const TO_INTS: f32 = (1 << 15) as f32 / PI;
+        const BACK_TO_RADIANS: f32 = (1. / TO_INTS) * 4.;
+        const ROUNDING_MASK: i32 = 0x4000 - 1;
+
+        let r_yaw = ((self.yaw * TO_INTS) as i32 >> 2) & ROUNDING_MASK;
+        let r_pitch = ((self.pitch * TO_INTS) as i32 >> 2) & ROUNDING_MASK;
+
+        Self {
+            yaw: r_yaw as f32 * BACK_TO_RADIANS,
+            pitch: r_pitch as f32 * BACK_TO_RADIANS,
+        }
+    }
 }
 
 #[cfg(feature = "heatseeker")]