New plane solver (#905)

Opening as a draft because it still needs a docs pass, benchmarking to ensure it's actually faster/scales better than the existing algorithm and removing the old code if so.

# Objective

The current plane solver is O(n^3) in the number of collision planes. It's not likely to break the bank but it'd be preferable to bring this down to O(n^2) or lower

## Solution

- I figured out a GJK-like algorithm for solving the underlying quadratic programming problem, and implemented it. Steps of the algorithm are O(n), and it should terminate in at most n steps, though I've never seen the step number exceed 4. I've tried to construct sets of normals that would take 5+ steps to converge and the arrangements of planes it would take seem very specific. Blog post fully explaining the details is in progress

## Testing

- Tested so far in the 3d move-and-slide example, still need to test in 2d as well
- ~~I'm intending to benchmark the new method against the existing one for some arrangements of maybe 1 to 10 normals? to try and establish scalings empirically~~
- ~~I think there are a few more micro-optimisations I could squeeze out, like imposing a specific ordering of the normals in `SimplicialCone::Wedge(n1, n2)` relative to the new search direction, which would save a few comparisons and dot products in the relevant branch of `SimplicialCone::project_point`.~~
- The `SimplicialCone` enum exists to keep the algorithm allocation-free (as opposed to keeping the current simplex points in a vec. Alternatively the first couple of iterations of the loop could be manually unrolled, which would get rid of the match and might speed things up a bit

---------

Co-authored-by: Joona Aalto <jondolf.dev@gmail.com>

Author: unpairedbracket

crates/avian3d/Cargo.toml

@@ -212,3 +212,8 @@ required-features = ["3d", "default-collider", "diagnostic_ui"]
 [[example]]
 name = "debugdump_3d"
 required-features = ["3d", "debug-plugin"]
+
+[[bench]]
+name = "velocity_projection"
+required-features = ["3d", "default-collider", "test"]
+harness = false

crates/avian3d/benches/velocity_projection.rs

@@ -0,0 +1,65 @@
+use bevy_math::{Dir3, Vec3};
+use core::hint::black_box;
+use criterion::{BenchmarkId, Criterion, PlotConfiguration, criterion_group, criterion_main};
+
+use avian3d::{
+    character_controller::move_and_slide::{
+        project_velocity, project_velocity_bruteforce, test::QuasiRandomDirection,
+    },
+    math::PI,
+};
+
+fn bench_velocity_projection(c: &mut Criterion) {
+    let mut group = c.benchmark_group("Velocity Projection");
+    let normals = &[
+        Dir3::Z,
+        Dir3::from_xyz(2.0, 0.0, 1.0).unwrap(),
+        Dir3::from_xyz(-2.0, 0.0, 1.0).unwrap(),
+        Dir3::from_xyz(0.0, 2.0, 1.0).unwrap(),
+        Dir3::from_xyz(0.0, -2.0, 1.0).unwrap(),
+        Dir3::from_xyz(1.5, 1.5, 1.0).unwrap(),
+        Dir3::from_xyz(1.5, -1.5, 1.0).unwrap(),
+        Dir3::from_xyz(-1.5, 1.5, 1.0).unwrap(),
+        Dir3::from_xyz(-1.5, -1.5, 1.0).unwrap(),
+        Dir3::from_xyz(1.0, 1.75, 1.0).unwrap(),
+        Dir3::from_xyz(1.0, -1.75, 1.0).unwrap(),
+        Dir3::from_xyz(-1.0, 1.75, 1.0).unwrap(),
+        Dir3::from_xyz(-1.0, -1.75, 1.0).unwrap(),
+        Dir3::from_xyz(1.75, 1.0, 1.0).unwrap(),
+        Dir3::from_xyz(1.75, -1.0, 1.0).unwrap(),
+        Dir3::from_xyz(-1.75, 1.0, 1.0).unwrap(),
+        Dir3::from_xyz(-1.75, -1.0, 1.0).unwrap(),
+    ];
+
+    // Performance is not the same for every input velocity,
+    // so ensure we're sampling the sphere evenly.
+    let mut velocities = QuasiRandomDirection::default();
+
+    for n in 1..=normals.len() {
+        velocities.reset();
+        group.bench_with_input(
+            BenchmarkId::new("brute-force", n),
+            &normals[..n],
+            |b, norms| {
+                b.iter_batched(
+                    || velocities.next().unwrap(),
+                    |v| project_velocity_bruteforce(black_box(v), black_box(norms)),
+                    criterion::BatchSize::SmallInput,
+                )
+            },
+        );
+        velocities.reset();
+        group.bench_with_input(BenchmarkId::new("gjk", n), &normals[..n], |b, norms| {
+            b.iter_batched(
+                || velocities.next().unwrap(),
+                |v| project_velocity(black_box(v), black_box(norms)),
+                criterion::BatchSize::SmallInput,
+            )
+        });
+    }
+    group
+        .plot_config(PlotConfiguration::default().summary_scale(criterion::AxisScale::Logarithmic));
+}
+
+criterion_group!(benches, bench_velocity_projection);
+criterion_main!(benches);

src/character_controller/mod.rs

@@ -1,6 +1,7 @@
 //! Utilities for implementing character controllers.
 
 pub mod move_and_slide;
+mod velocity_project;
 
 /// Re-exports common types related to character controller functionality.
 pub mod prelude {

src/character_controller/move_and_slide.rs

@@ -2,6 +2,8 @@
 //!
 //! See the documentation of [`MoveAndSlide`] for more information.
 
+pub use super::velocity_project::*;
+
 use crate::{collision::collider::contact_query::contact_manifolds, prelude::*};
 use bevy::{ecs::system::SystemParam, prelude::*};
 use core::time::Duration;
@@ -1082,97 +1084,6 @@ impl<'w, 's> MoveAndSlide<'w, 's> {
     /// does not try to continue moving into colliding geometry.
     #[must_use]
     pub fn project_velocity(v: Vector, normals: &[Dir]) -> Vector {
-        if normals.is_empty() {
-            return v;
-        }
-
-        // The halfspaces defined by the contact normals form a polyhedral cone.
-        // We want to find the closest point to v that lies inside this cone.
-        //
-        // There are three cases to consider:
-        // 1. v is already inside the cone -> return v
-        // 2. v is outside the cone
-        //    a. Project v onto each plane and check if the projection is inside the cone
-        //    b. Project v onto each edge (intersection of two planes) and check if the projection is inside the cone
-        // 3. If no valid projection is found, return the apex of the cone (the origin)
-
-        // Case 1: Check if v is inside the cone
-        if normals
-            .iter()
-            .all(|normal| normal.adjust_precision().dot(v) >= -DOT_EPSILON)
-        {
-            return v;
-        }
-
-        // Best candidate so far
-        let mut best_projection = Vector::ZERO;
-        let mut best_distance_sq = Scalar::INFINITY;
-
-        // Helper to test halfspace validity
-        let is_valid = |projection: Vector| {
-            normals
-                .iter()
-                .all(|n| projection.dot(n.adjust_precision()) >= -DOT_EPSILON)
-        };
-
-        // Case 2a: Face projections (single-plane active set)
-        for n in normals {
-            let n = n.adjust_precision();
-            let n_dot_v = n.dot(v);
-            if n_dot_v < -DOT_EPSILON {
-                // Project v onto the plane defined by n:
-                // projection = v - (v · n) n
-                let projection = v - n_dot_v * n;
-
-                // Check if better than previous best and valid
-                let distance_sq = v.distance_squared(projection);
-                if distance_sq < best_distance_sq && is_valid(projection) {
-                    best_distance_sq = distance_sq;
-                    best_projection = projection;
-                }
-            }
-        }
-
-        // Case 2b: Edge projections (two-plane active set)
-        // TODO: Can we optimize this from O(n^3) to O(n^2)?
-        #[cfg(feature = "3d")]
-        {
-            let n = normals.len();
-            for i in 0..n {
-                let ni = normals[i].adjust_precision();
-                for nj in normals
-                    .iter()
-                    .take(n)
-                    .skip(i + 1)
-                    .map(|n| n.adjust_precision())
-                {
-                    // Compute edge direction e = ni x nj
-                    let e = ni.cross(nj);
-                    let e_length_sq = e.length_squared();
-                    if e_length_sq < DOT_EPSILON {
-                        // Nearly parallel edge
-                        continue;
-                    }
-
-                    // Project v onto the line spanned by e:
-                    // projection = ((v · e) / |e|²) e
-                    let projection = e * (v.dot(e) / e_length_sq);
-
-                    // Check if better than previous best and valid
-                    let distance_sq = v.distance_squared(projection);
-                    if distance_sq < best_distance_sq && is_valid(projection) {
-                        best_distance_sq = distance_sq;
-                        best_projection = projection;
-                    }
-                }
-            }
-        }
-
-        // Case 3: If no candidate is found, the projection is at the apex (the origin)
-        if best_distance_sq.is_infinite() {
-            Vector::ZERO
-        } else {
-            best_projection
-        }
+        project_velocity(v, normals)
     }
 }