Add a debug feature to both crates to allow access to the internal data.

.github/workflows/test.yml

@@ -25,8 +25,12 @@ jobs:
           toolchain: ${{ matrix.toolchain }}
       - run: cargo build
       - name: cargo test
-        run: cargo test --all-features --all-targets
+        run: cargo test --all-targets
       - name: cargo test --doc
+        run: cargo test --doc
+      - name: cargo test --all-features
+        run: cargo test --all-features --all-targets
+      - name: cargo test --all-features --doc
         run: cargo test --all-features --doc
   coverage:
     name: Coverage
@@ -55,6 +59,7 @@ jobs:
           submodules: true
       - uses: dtolnay/rust-toolchain@stable
       - run: cargo clippy -- -D warnings
+      - run: cargo clippy --features=debug -- -D warnings
   format:
     name: Format
     runs-on: ubuntu-latest

crates/dodgy_2d/Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 edition = "2021"
 name = "dodgy_2d"
-version = "0.5.2"
+version = "0.5.3"
 
 description = "An implementation of ORCA, a local collision avoidance algorithm for 2D."
 
@@ -16,3 +16,7 @@ keywords = ["orca", "rvo", "collision", "avoidance", "navigation"]
 [dependencies]
 glam = "0.28.0"
 rand = "0.8.5"
+
+[features]
+# Allows access to some of the internal data used to generate the final suggested velocity.
+debug = []

crates/dodgy_2d/src/debug.rs

@@ -0,0 +1,22 @@
+// Re-export Line so we can use it to provide debug data.
+pub use crate::linear_programming::Line;
+
+/// Internal data that is used to generate the final suggested velocity.
+pub enum DebugData {
+  /// The original problem (where the agent uses its current velocity) was
+  /// solved.
+  Satisfied {
+    /// The constraints that needed to be satisfied.
+    constraints: Vec<Line>,
+  },
+  /// The original problem (where the agent uses its current velocity) was
+  /// invalid, so the algorithm fell back to pretending the agent has a
+  /// zero-velocity, which is trivially satisfiable.
+  Fallback {
+    /// The constraints for the original problem.
+    original_constraints: Vec<Line>,
+    /// The constraints after falling back (pretending the agent has zero
+    /// velocity).
+    fallback_constraints: Vec<Line>,
+  },
+}

crates/dodgy_2d/src/lib.rs

@@ -25,6 +25,9 @@
 mod simulator;
 mod visibility_set;
 
+#[cfg(feature = "debug")]
+pub mod debug;
+
 use std::borrow::Cow;
 
 pub use glam::Vec2;
@@ -89,6 +92,52 @@
     time_step: f32,
     avoidance_options: &AvoidanceOptions,
   ) -> Vec2 {
+    let result = self.compute_avoiding_velocity_internal(
+      neighbours,
+      obstacles,
+      preferred_velocity,
+      max_speed,
+      time_step,
+      avoidance_options,
+    );
+    #[cfg(feature = "debug")]
+    return result.0;
+    #[cfg(not(feature = "debug"))]
+    result
+  }
+
+  #[cfg(feature = "debug")]
+  /// Same as [`Self::compute_avoiding_velocity`], but additionally provides
+  /// debug data.
+  pub fn compute_avoiding_velocity_with_debug(
+    &self,
+    neighbours: &[Cow<'_, Agent>],
+    obstacles: &[Cow<'_, Obstacle>],
+    preferred_velocity: Vec2,
+    max_speed: f32,
+    time_step: f32,
+    avoidance_options: &AvoidanceOptions,
+  ) -> (Vec2, debug::DebugData) {
+    self.compute_avoiding_velocity_internal(
+      neighbours,
+      obstacles,
+      preferred_velocity,
+      max_speed,
+      time_step,
+      avoidance_options,
+    )
+  }
+
+  /// The implementation of [`Self::compute_avoiding_velocity`].
+  fn compute_avoiding_velocity_internal(
+    &self,
+    neighbours: &[Cow<'_, Agent>],
+    obstacles: &[Cow<'_, Obstacle>],
+    preferred_velocity: Vec2,
+    max_speed: f32,
+    time_step: f32,
+    avoidance_options: &AvoidanceOptions,
+  ) -> AvoidingVelocityReturn {
     assert!(time_step > 0.0, "time_step must be positive, was {}", time_step);
 
     let lines = obstacles
@@ -120,6 +169,8 @@
       max_speed,
       preferred_velocity,
     ) {
+      #[cfg(feature = "debug")]
+      let result = (result, debug::DebugData::Satisfied { constraints: lines });
       return result;
     }
 
@@ -129,7 +180,7 @@
       clone
     };
 
-    let lines = obstacles
+    let zero_velocity_lines = obstacles
       .iter()
       .flat_map(|o| {
         get_lines_for_agent_to_obstacle(
@@ -153,15 +204,25 @@
 
     // Since each neighbour generates one line, the number of obstacle lines is
     // just the other lines.
-    let obstacle_line_count = lines.len() - neighbours.len();
+    let obstacle_line_count = zero_velocity_lines.len() - neighbours.len();
 
-    solve_linear_program(
-      &lines,
+    let result = solve_linear_program(
+      &zero_velocity_lines,
       obstacle_line_count,
       max_speed,
       preferred_velocity,
     )
-    .expect("The obstacle constraints should be trivially solvable.")
+    .expect("The obstacle constraints should be trivially solvable.");
+
+    #[cfg(feature = "debug")]
+    let result = (
+      result,
+      debug::DebugData::Fallback {
+        original_constraints: lines,
+        fallback_constraints: zero_velocity_lines,
+      },
+    );
+    result
   }
 
   /// Creates a line to describe the half-plane of valid velocities that should
@@ -332,6 +393,12 @@
   }
 }
 
+// Type alias so we can only construct the debug data if necessary.
+#[cfg(feature = "debug")]
+type AvoidingVelocityReturn = (Vec2, debug::DebugData);
+#[cfg(not(feature = "debug"))]
+type AvoidingVelocityReturn = Vec2;
+
 #[cfg(test)]
 #[path = "lib_test.rs"]
 mod test;

crates/dodgy_3d/Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 edition = "2021"
 name = "dodgy_3d"
-version = "0.5.1"
+version = "0.5.2"
 
 description = "An implementation of ORCA, a local collision avoidance algorithm for 3D."
 
@@ -16,3 +16,7 @@ keywords = ["orca", "rvo", "collision", "avoidance", "navigation"]
 [dependencies]
 glam = "0.28.0"
 rand = "0.8.5"
+
+[features]
+# Allows access to some of the internal data used to generate the final suggested velocity.
+debug = []

crates/dodgy_3d/src/lib.rs

@@ -24,11 +24,18 @@
 
 use std::borrow::Cow;
 
-use crate::linear_programming::{solve_linear_program, Plane};
+use crate::linear_programming::solve_linear_program;
 
 pub use glam::Vec3;
 pub use simulator::{AgentParameters, Simulator, SimulatorMargin};
 
+// Re-export Plane so we can use it to provide debug data.
+#[cfg(feature = "debug")]
+pub use crate::linear_programming::Plane;
+// Otherwise, just import it privately.
+#[cfg(not(feature = "debug"))]
+use crate::linear_programming::Plane;
+
 /// A single agent in the simulation.
 #[derive(Clone, PartialEq, Debug)]
 pub struct Agent {
@@ -71,6 +78,46 @@
     time_step: f32,
     avoidance_options: &AvoidanceOptions,
   ) -> Vec3 {
+    self
+      .compute_avoiding_velocity_internal(
+        neighbours,
+        preferred_velocity,
+        max_speed,
+        time_step,
+        avoidance_options,
+      )
+      .0
+  }
+
+  #[cfg(feature = "debug")]
+  /// Same as [`Self::compute_avoiding_velocity`], but additionally provides
+  /// debug data in the form of the plane constraints generated by each agent.
+  pub fn compute_avoiding_velocity_with_debug(
+    &self,
+    neighbours: &[Cow<'_, Agent>],
+    preferred_velocity: Vec3,
+    max_speed: f32,
+    time_step: f32,
+    avoidance_options: &AvoidanceOptions,
+  ) -> (Vec3, Vec<Plane>) {
+    self.compute_avoiding_velocity_internal(
+      neighbours,
+      preferred_velocity,
+      max_speed,
+      time_step,
+      avoidance_options,
+    )
+  }
+
+  /// The implementation of [`Self::compute_avoiding_velocity`].
+  fn compute_avoiding_velocity_internal(
+    &self,
+    neighbours: &[Cow<'_, Agent>],
+    preferred_velocity: Vec3,
+    max_speed: f32,
+    time_step: f32,
+    avoidance_options: &AvoidanceOptions,
+  ) -> (Vec3, Vec<Plane>) {
     assert!(time_step > 0.0, "time_step must be positive, was {}", time_step);
 
     let planes = neighbours
@@ -84,7 +131,7 @@
       })
       .collect::<Vec<Plane>>();
 
-    solve_linear_program(&planes, max_speed, preferred_velocity)
+    (solve_linear_program(&planes, max_speed, preferred_velocity), planes)
   }
 
   /// Creates a plane to describe the half-space of valid velocities that should