diff --git a/.gitignore b/.gitignore index fe70245..c5eadb5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,10 @@ -.vscode/ +/.vscode /target +/venv ball_prediction.json Cargo.lock *.svg perf* *.dump +*.json +/collision_meshes diff --git a/Cargo.toml b/Cargo.toml index 8e8416f..75c70ae 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3,29 +3,35 @@ name = "rl_ball_sym" version = "4.0.0" authors = ["VirxEC"] edition = "2021" -description = "Rust implementation of ball path prediction for Rocket League; Inspired by Samuel (Chip) P. Mish's C++ utils called RLUtilities" +description = "rl_ball_sym is a Rust implementation of Rocket League's ball physics" readme = "README.md" repository = "https://github.com/VirxEC/rl_ball_sym" license = "MIT" keywords = ["rocket-league", "rlbot", "physics", "simulation"] categories = ["science", "simulation", "mathematics"] include = ["src/", "assets/"] -rust-version = "1.65" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html +[lints.rust] +unsafe_code = "forbid" + +[lints.clippy] +all = "warn" + [dev-dependencies] rand = "0.8" once_cell = "1.17.1" criterion = { version = "0.5.0", features = ["html_reports"] } serde_json = "1.0.107" colored = "2.0.4" +rocketsim_rs = { version = "0.28.0", features = ["glam"] } [dependencies] byteorder = "1" -glam = "0.25.0" +glam = "0.27.0" radsort = "0.1" -combo_vec = { version = "0.7.1", default_features = false } +combo_vec = { version = "0.7.1", default-features = false } include-flate = { version = "0.3.0", optional = true } serde = { version = "1.0.188", features = ["derive"], optional = true } @@ -55,3 +61,7 @@ lto = true name = "benchmarks" path = "benches/benchmarks.rs" harness = false + +[[example]] +name = "accuracy" +required-features = ["serde"] diff --git a/README.md b/README.md index c5da386..e79010c 100644 --- a/README.md +++ b/README.md @@ -6,10 +6,9 @@ Rust implementation of Rocket League's ball physics; Inspired by Samuel P. Mish's C++ utils called [RLUtilities](https://github.com/samuelpmish/RLUtilities) +with accuracy improvements from [RocketSim](https://github.com/ZealanL/RocketSim) and other miscellaneous performance improvements. -This crate also contains fixes to discovered errors stemming from the original repo. - ## Running Make sure you have Rust/Cargo installed, then just run `cargo test --release` in the terminal. @@ -38,11 +37,11 @@ Numbers _will_ vary depending on your system. + `load_hoops`: Loads 15732 triangles, executes in around `1.30ms` + `load_dropshot`: Loads 3616 triangles, executes in around `300µs` + `load_standard_throwback`: Loads 9272 triangles, executes in around `805µs` -+ `get_ball_prediction_struct_for_time`: standard + 8 seconds, executes in around `130µs` -+ `get_ball_prediction`: standard + 6 seconds, executes in around `90µs` -+ `get_ball_prediction`: Hoops + 6 seconds, executes in around `120µs` -+ `get_ball_prediction`: Dropshot + 6 seconds, executes in around `110µs` -+ `get_ball_prediction`: standard + Throwback Stadium + 6 seconds, executes in around `100µs` ++ `get_ball_prediction_struct_for_time`: standard + 8 seconds, executes in around `220µs` ++ `get_ball_prediction`: standard + 6 seconds, executes in around `165µs` ++ `get_ball_prediction`: Hoops + 6 seconds, executes in around `160µs` ++ `get_ball_prediction`: Dropshot + 6 seconds, executes in around `140µs` ++ `get_ball_prediction`: standard + Throwback Stadium + 6 seconds, executes in around `175µs` ## Features diff --git a/analysis/accuracy.py b/analysis/accuracy.py new file mode 100644 index 0000000..3806ded --- /dev/null +++ b/analysis/accuracy.py @@ -0,0 +1,84 @@ +import json +from pathlib import Path + +import matplotlib.pyplot as plt + +# run `cargo r --example accuracy --features=serde` first! + +with open(Path(__file__).parent / "accuracy.json") as f: + data = json.load(f) + +rocketsim = [[], [], [], []] +rl_ball_sym = [[], [], [], []] + +for ball in data["rocketsim"]: + rocketsim[0].append(ball["time"]) + rocketsim[1].append(ball["location"]) + rocketsim[2].append(ball["velocity"]) + rocketsim[3].append(ball["angular_velocity"]) + +for ball in data["rl_ball_sym"]: + rl_ball_sym[0].append(ball["time"]) + rl_ball_sym[1].append(ball["location"]) + rl_ball_sym[2].append(ball["velocity"]) + rl_ball_sym[3].append(ball["angular_velocity"]) + +start_time = 0 * 120 +# time_limit = 250 +time_limit = len(rocketsim[0]) + +# start_time = 7 * 120 +# time_limit = 8 * 120 + +location_diff = [] + +for i in range(start_time, time_limit): + location_diff.append( + ( + (rocketsim[1][i][0] - rl_ball_sym[1][i][0]) ** 2 + + (rocketsim[1][i][1] - rl_ball_sym[1][i][1]) ** 2 + + (rocketsim[1][i][2] - rl_ball_sym[1][i][2]) ** 2 + ) + ** 0.5 + ) + + if location_diff[-1] > 0: + print(rl_ball_sym[0][i]) + +velocity_diff = [] + +for i in range(start_time, time_limit): + velocity_diff.append( + ( + (rocketsim[2][i][0] - rl_ball_sym[2][i][0]) ** 2 + + (rocketsim[2][i][1] - rl_ball_sym[2][i][1]) ** 2 + + (rocketsim[2][i][2] - rl_ball_sym[2][i][2]) ** 2 + ) + ** 0.5 + ) + +angular_velocity_diff = [] + +for i in range(start_time, time_limit): + angular_velocity_diff.append( + ( + (rocketsim[3][i][0] - rl_ball_sym[3][i][0]) ** 2 + + (rocketsim[3][i][1] - rl_ball_sym[3][i][1]) ** 2 + + (rocketsim[3][i][2] - rl_ball_sym[3][i][2]) ** 2 + ) + ** 0.5 + ) + +ax = plt.figure().add_subplot() + +ax.plot(rocketsim[0][start_time:time_limit], location_diff, label="location") +ax.plot(rocketsim[0][start_time:time_limit], velocity_diff, label="velocity") +ax.plot(rocketsim[0][start_time:time_limit], angular_velocity_diff, label="angular velocity") + +ax.legend() +ax.set_title("Time vs Difference") +ax.set_xlabel("Time") +ax.set_ylabel("Magnitude of difference") + +plt.show() + diff --git a/analysis/angular_velocity.py b/analysis/angular_velocity.py new file mode 100644 index 0000000..c5420d6 --- /dev/null +++ b/analysis/angular_velocity.py @@ -0,0 +1,31 @@ +import json +import matplotlib.pyplot as plt + +with open("accuracy.json") as f: + data = json.load(f) + +rocketsim = [[], [], []] +rl_ball_sym = [[], [], []] + +for ball in data["rocketsim"]: + rocketsim[0].append(ball["angular_velocity"][0]) + rocketsim[1].append(ball["angular_velocity"][1]) + rocketsim[2].append(ball["angular_velocity"][2]) + +for ball in data["rl_ball_sym"]: + rl_ball_sym[0].append(ball["angular_velocity"][0]) + rl_ball_sym[1].append(ball["angular_velocity"][1]) + rl_ball_sym[2].append(ball["angular_velocity"][2]) + +ax = plt.figure().add_subplot(projection='3d') + +ax.plot(rocketsim[0], rocketsim[1], rocketsim[2], label="rocketsim") +ax.plot(rl_ball_sym[0], rl_ball_sym[1], rl_ball_sym[2], label="rl_ball_sym") + +ax.legend() +ax.set_title("Angular Velocity") +ax.set_xlabel("X") +ax.set_ylabel("Z") +ax.set_zlabel("Y") + +plt.show() diff --git a/analysis/location.py b/analysis/location.py new file mode 100644 index 0000000..56d7f61 --- /dev/null +++ b/analysis/location.py @@ -0,0 +1,31 @@ +import json +import matplotlib.pyplot as plt + +with open("accuracy.json") as f: + data = json.load(f) + +rocketsim = [[], [], []] +rl_ball_sym = [[], [], []] + +for ball in data["rocketsim"]: + rocketsim[0].append(ball["location"][0]) + rocketsim[1].append(ball["location"][1]) + rocketsim[2].append(ball["location"][2]) + +for ball in data["rl_ball_sym"]: + rl_ball_sym[0].append(ball["location"][0]) + rl_ball_sym[1].append(ball["location"][1]) + rl_ball_sym[2].append(ball["location"][2]) + +ax = plt.figure().add_subplot(projection='3d') + +ax.plot(rocketsim[0], rocketsim[1], rocketsim[2], label="rocketsim") +ax.plot(rl_ball_sym[0], rl_ball_sym[1], rl_ball_sym[2], label="rl_ball_sym") + +ax.legend() +ax.set_title("Location") +ax.set_xlabel("X") +ax.set_ylabel("Z") +ax.set_zlabel("Y") + +plt.show() diff --git a/analysis/velocity.py b/analysis/velocity.py new file mode 100644 index 0000000..eddaf3f --- /dev/null +++ b/analysis/velocity.py @@ -0,0 +1,31 @@ +import json +import matplotlib.pyplot as plt + +with open("accuracy.json") as f: + data = json.load(f) + +rocketsim = [[], [], []] +rl_ball_sym = [[], [], []] + +for ball in data["rocketsim"]: + rocketsim[0].append(ball["velocity"][0]) + rocketsim[1].append(ball["velocity"][1]) + rocketsim[2].append(ball["velocity"][2]) + +for ball in data["rl_ball_sym"]: + rl_ball_sym[0].append(ball["velocity"][0]) + rl_ball_sym[1].append(ball["velocity"][1]) + rl_ball_sym[2].append(ball["velocity"][2]) + +ax = plt.figure().add_subplot(projection='3d') + +ax.plot(rocketsim[0], rocketsim[1], rocketsim[2], label="rocketsim") +ax.plot(rl_ball_sym[0], rl_ball_sym[1], rl_ball_sym[2], label="rl_ball_sym") + +ax.legend() +ax.set_title("Velocity") +ax.set_xlabel("X") +ax.set_ylabel("Z") +ax.set_zlabel("Y") + +plt.show() diff --git a/assets/hoops/hoops_corner_ids.bin b/assets/hoops/hoops_corner_ids.bin index 4447102..a6de0fb 100644 Binary files a/assets/hoops/hoops_corner_ids.bin and b/assets/hoops/hoops_corner_ids.bin differ diff --git a/assets/hoops/hoops_corner_vertices.bin b/assets/hoops/hoops_corner_vertices.bin index c624a83..ae866c6 100644 Binary files a/assets/hoops/hoops_corner_vertices.bin and b/assets/hoops/hoops_corner_vertices.bin differ diff --git a/assets/hoops/hoops_net_ids.bin b/assets/hoops/hoops_net_ids.bin index 3e2c63f..a894b98 100644 Binary files a/assets/hoops/hoops_net_ids.bin and b/assets/hoops/hoops_net_ids.bin differ diff --git a/assets/hoops/hoops_net_vertices.bin b/assets/hoops/hoops_net_vertices.bin index 8deb096..429a1f7 100644 Binary files a/assets/hoops/hoops_net_vertices.bin and b/assets/hoops/hoops_net_vertices.bin differ diff --git a/assets/hoops/hoops_ramps_0_ids.bin b/assets/hoops/hoops_ramps_0_ids.bin index 6c366e4..9823072 100644 Binary files a/assets/hoops/hoops_ramps_0_ids.bin and b/assets/hoops/hoops_ramps_0_ids.bin differ diff --git a/assets/hoops/hoops_ramps_0_vertices.bin b/assets/hoops/hoops_ramps_0_vertices.bin index e188073..6aafb9f 100644 Binary files a/assets/hoops/hoops_ramps_0_vertices.bin and b/assets/hoops/hoops_ramps_0_vertices.bin differ diff --git a/assets/hoops/hoops_ramps_1_ids.bin b/assets/hoops/hoops_ramps_1_ids.bin index 7bdef5a..4f9221e 100644 Binary files a/assets/hoops/hoops_ramps_1_ids.bin and b/assets/hoops/hoops_ramps_1_ids.bin differ diff --git a/assets/hoops/hoops_ramps_1_vertices.bin b/assets/hoops/hoops_ramps_1_vertices.bin index fa41062..ee0ec40 100644 Binary files a/assets/hoops/hoops_ramps_1_vertices.bin and b/assets/hoops/hoops_ramps_1_vertices.bin differ diff --git a/assets/hoops/hoops_rim_ids.bin b/assets/hoops/hoops_rim_ids.bin index c1565eb..33a775f 100644 Binary files a/assets/hoops/hoops_rim_ids.bin and b/assets/hoops/hoops_rim_ids.bin differ diff --git a/assets/hoops/hoops_rim_vertices.bin b/assets/hoops/hoops_rim_vertices.bin index fb61183..bc1d766 100644 Binary files a/assets/hoops/hoops_rim_vertices.bin and b/assets/hoops/hoops_rim_vertices.bin differ diff --git a/assets/standard/standard_corner_vertices.bin b/assets/standard/standard_corner_vertices.bin index 001481a..e13b11a 100644 Binary files a/assets/standard/standard_corner_vertices.bin and b/assets/standard/standard_corner_vertices.bin differ diff --git a/assets/standard/standard_goal_ids.bin b/assets/standard/standard_goal_ids.bin index 2841dde..a1e83d5 100644 Binary files a/assets/standard/standard_goal_ids.bin and b/assets/standard/standard_goal_ids.bin differ diff --git a/assets/standard/standard_goal_vertices.bin b/assets/standard/standard_goal_vertices.bin index f231f39..6dfc975 100644 Binary files a/assets/standard/standard_goal_vertices.bin and b/assets/standard/standard_goal_vertices.bin differ diff --git a/assets/standard/standard_ramps_0_ids.bin b/assets/standard/standard_ramps_0_ids.bin index 105b541..c3c30dd 100644 Binary files a/assets/standard/standard_ramps_0_ids.bin and b/assets/standard/standard_ramps_0_ids.bin differ diff --git a/assets/standard/standard_ramps_0_vertices.bin b/assets/standard/standard_ramps_0_vertices.bin index fe43ca0..423d3df 100644 Binary files a/assets/standard/standard_ramps_0_vertices.bin and b/assets/standard/standard_ramps_0_vertices.bin differ diff --git a/assets/standard/standard_ramps_1_ids.bin b/assets/standard/standard_ramps_1_ids.bin index 225d658..3970397 100644 Binary files a/assets/standard/standard_ramps_1_ids.bin and b/assets/standard/standard_ramps_1_ids.bin differ diff --git a/assets/standard/standard_ramps_1_vertices.bin b/assets/standard/standard_ramps_1_vertices.bin index 430c3b9..42d2b23 100644 Binary files a/assets/standard/standard_ramps_1_vertices.bin and b/assets/standard/standard_ramps_1_vertices.bin differ diff --git a/benches/benchmarks.rs b/benches/benchmarks.rs index 6c11d2f..e36c066 100644 --- a/benches/benchmarks.rs +++ b/benches/benchmarks.rs @@ -37,7 +37,7 @@ fn get_ball_prediction_struct_benchmark(c: &mut Criterion) { ball.velocity = BALL_VEL; c.bench_function("get_ball_prediction/standard", |b| { - b.iter(|| ball.get_ball_prediction_struct(black_box(&game))) + b.iter(|| ball.get_ball_prediction_struct(black_box(&game))); }); } @@ -46,7 +46,7 @@ fn get_ball_prediction_struct_hoops_benchmark(c: &mut Criterion) { ball.velocity = BALL_VEL; c.bench_function("get_ball_prediction/hoops", |b| { - b.iter(|| ball.get_ball_prediction_struct(black_box(&game))) + b.iter(|| ball.get_ball_prediction_struct(black_box(&game))); }); } @@ -55,7 +55,7 @@ fn get_ball_prediction_struct_dropshot(c: &mut Criterion) { ball.velocity = BALL_VEL; c.bench_function("get_ball_prediction/dropshot", |b| { - b.iter(|| ball.get_ball_prediction_struct(black_box(&game))) + b.iter(|| ball.get_ball_prediction_struct(black_box(&game))); }); } @@ -64,7 +64,7 @@ fn get_ball_prediction_struct_throwback(c: &mut Criterion) { ball.velocity = BALL_VEL; c.bench_function("get_ball_prediction/throwback", |b| { - b.iter(|| ball.get_ball_prediction_struct(black_box(&game))) + b.iter(|| ball.get_ball_prediction_struct(black_box(&game))); }); } diff --git a/examples/accuracy.rs b/examples/accuracy.rs new file mode 100644 index 0000000..d5ef436 --- /dev/null +++ b/examples/accuracy.rs @@ -0,0 +1,80 @@ +use byteorder::{LittleEndian, ReadBytesExt}; +use colored::Colorize; +use rl_ball_sym::{load_standard, Ball}; +use std::{fs, io}; + +fn read_balls(file_name: &str, mut ball: Ball) -> io::Result> { + let mut file = fs::File::open(file_name)?; + let num_balls = file.read_u16::()? as usize; + + let mut balls = Vec::with_capacity(num_balls); + + for _ in 0..num_balls { + ball.time = file.read_f32::()?; + ball.location.x = file.read_f32::()?; + ball.location.y = file.read_f32::()?; + ball.location.z = file.read_f32::()?; + ball.velocity.x = file.read_f32::()?; + ball.velocity.y = file.read_f32::()?; + ball.velocity.z = file.read_f32::()?; + ball.angular_velocity.x = file.read_f32::()?; + ball.angular_velocity.y = file.read_f32::()?; + ball.angular_velocity.z = file.read_f32::()?; + + balls.push(ball); + } + + Ok(balls) +} + +#[derive(serde::Serialize)] +struct BallDump { + rocketsim: Vec, + rl_ball_sym: Vec, +} + +fn main() -> io::Result<()> { + let (game, mut ball) = load_standard(); + + let rocketsim = read_balls("examples/ball.dump", ball)?; + + println!("Starting config:"); + println!("Location: {}", rocketsim[0].location); + println!("Velocity: {}", rocketsim[0].velocity); + println!("Angular Velocity: {}", rocketsim[0].angular_velocity); + + println!(); + + let mut rl_ball_sym = Vec::with_capacity(rocketsim.len()); + rl_ball_sym.push(rocketsim[0]); + + ball = rocketsim[0]; + for cball in rocketsim.iter().copied().take(5) { + print_error(ball, cball); + + println!("{}", "[START OF TICK]".green()); + ball = cball; + ball.step(&game, 1. / 120.); + rl_ball_sym.push(ball); + println!("{}", "[END OF TICK]".red()); + } + + fs::write( + "analysis/accuracy.json", + serde_json::to_string(&BallDump { rocketsim, rl_ball_sym }).unwrap(), + )?; + + Ok(()) +} + +fn print_error(ball: Ball, cball: Ball) { + println!("{} | {}", ball.location / 50., cball.location / 50.); + println!("{} | {}", ball.velocity / 50., cball.velocity / 50.); + println!( + "Error: {}, {}, {}", + ball.location.distance(cball.location), + ball.velocity.distance(cball.velocity), + ball.angular_velocity.distance(cball.angular_velocity) + ); + println!("Vel error: {}", ball.velocity - cball.velocity); +} diff --git a/examples/compare_perf.rs b/examples/compare_perf.rs new file mode 100644 index 0000000..e768473 --- /dev/null +++ b/examples/compare_perf.rs @@ -0,0 +1,52 @@ +use glam::Vec3A; +use rocketsim_rs::{ + autocxx::WithinUniquePtr, + init, + math::Vec3, + sim::{Arena, ArenaConfig, BallState, GameMode}, +}; +use std::time::Instant; + +const STEPS: u32 = 120 * 60 * 60 * 5; + +fn main() { + init(None); + + let mut arena = Arena::new( + GameMode::SOCCAR, + ArenaConfig { + no_ball_rot: false, + ..Default::default() + }, + 120., + ) + .within_unique_ptr(); + + arena.pin_mut().set_ball(BallState { + pos: Vec3::new(0., 0., 200.), + vel: Vec3::new(2000., 2000., 2000.), + ..Default::default() + }); + + let start_time = Instant::now(); + + arena.pin_mut().step(STEPS as i32); + + let elapsed = start_time.elapsed(); + + println!("RocketSim simulated 5 hours in {elapsed:?}"); + + let (game, mut ball) = rl_ball_sym::load_standard(); + ball.location = Vec3A::new(0., 0., 200.); + ball.velocity = Vec3A::new(2000., 2000., 2000.); + + let start_time = Instant::now(); + + for _ in 0..STEPS { + ball.step(&game, 1. / 120.); + } + + let elapsed = start_time.elapsed(); + + println!("RLBallSym simulated 5 hours in {elapsed:?}"); +} diff --git a/examples/find_max_error.rs b/examples/find_max_error.rs new file mode 100644 index 0000000..36b9a63 --- /dev/null +++ b/examples/find_max_error.rs @@ -0,0 +1,124 @@ +use byteorder::{LittleEndian, WriteBytesExt}; +use rand::Rng; +use rl_ball_sym::load_standard; +use rocketsim_rs::{ + math::Vec3, + sim::{Arena, BallState}, +}; +use std::{ + fs, + io::{self, Write}, +}; + +fn write_ball(file: &mut fs::File, ball: BallState, time: f32) -> io::Result<()> { + file.write_f32::(time)?; + file.write_f32::(ball.pos.x)?; + file.write_f32::(ball.pos.y)?; + file.write_f32::(ball.pos.z)?; + file.write_f32::(ball.vel.x)?; + file.write_f32::(ball.vel.y)?; + file.write_f32::(ball.vel.z)?; + file.write_f32::(ball.ang_vel.x)?; + file.write_f32::(ball.ang_vel.y)?; + file.write_f32::(ball.ang_vel.z)?; + Ok(()) +} + +const TRIES: usize = 1_000_000; +const NUM_TICKS: i32 = 120; + +fn main() -> io::Result<()> { + rocketsim_rs::init(None); + + let mut arena = Arena::default_standard(); + let (game, mut ball) = load_standard(); + + let mut rng = rand::thread_rng(); + + let mut max_error = 0.; + let mut error_values = [Vec3::ZERO; 3]; + + 'outer: for i in 0..TRIES { + let location = Vec3::new( + rng.gen_range(0.0..2900.), + rng.gen_range(0.0..3900.), + rng.gen_range(100.0..1900.), + ); + let velocity = Vec3::new( + rng.gen_range(0.0..2000.), + rng.gen_range(0.0..2000.), + rng.gen_range(-2000.0..2000.), + ); + let angular_velocity = Vec3::new(rng.gen_range(-3.0..3.), rng.gen_range(-3.0..3.), rng.gen_range(-3.0..3.)); + + let mut rocketsim_ball = arena.pin_mut().get_ball(); + rocketsim_ball.pos = location; + rocketsim_ball.vel = velocity; + rocketsim_ball.ang_vel = angular_velocity; + arena.pin_mut().set_ball(rocketsim_ball); + + ball.update(0., location.into(), velocity.into(), angular_velocity.into()); + for _ in 0..NUM_TICKS { + let last_ball = arena.pin_mut().get_ball(); + ball.step(&game, 1. / 120.); + arena.pin_mut().step(1); + + let rocketsim_ball = arena.pin_mut().get_ball(); + + let pos_error = ball.location.distance(rocketsim_ball.pos.into()); + let vel_error = ball.velocity.distance(rocketsim_ball.vel.into()); + let error = pos_error / 2. + vel_error / 2.; + if error > max_error && error < 100. { + max_error = error; + error_values[0] = last_ball.pos; + error_values[1] = last_ball.vel; + error_values[2] = last_ball.ang_vel; + + if (max_error > 30. && i > TRIES / 2) || max_error > 80. { + println!("Breaking early with an error of {max_error}; Location error: {pos_error}, Velocity error: {vel_error}"); + break 'outer; + } + + break; + } + + ball.location = rocketsim_ball.pos.into(); + ball.velocity = rocketsim_ball.vel.into(); + ball.angular_velocity = rocketsim_ball.ang_vel.into(); + } + + if i % 1000 == 0 { + print!("Max error in {i} / {TRIES}: {max_error:.0}\r"); + io::stdout().flush()?; + } + } + + println!("\n"); + println!( + "Location: ({}, {}, {})", + error_values[0].x, error_values[0].y, error_values[0].z + ); + println!( + "Velocity: ({}, {}, {})", + error_values[1].x, error_values[1].y, error_values[1].z + ); + println!( + "Angular velocity: ({}, {}, {})", + error_values[2].x, error_values[2].y, error_values[2].z + ); + + let mut ball = arena.pin_mut().get_ball(); + ball.pos = error_values[0]; + ball.vel = error_values[1]; + ball.ang_vel = error_values[2]; + arena.pin_mut().set_ball(ball); + + let mut file = fs::File::create("examples/ball.dump")?; + file.write_u16::(2)?; + write_ball(&mut file, ball, 0.)?; + arena.pin_mut().step(1); + let ball = arena.pin_mut().get_ball(); + write_ball(&mut file, ball, 1. / 120.)?; + + Ok(()) +} diff --git a/examples/flamegraph.rs b/examples/flamegraph.rs index 6e50ad5..7ec43f5 100644 --- a/examples/flamegraph.rs +++ b/examples/flamegraph.rs @@ -13,6 +13,7 @@ fn main() { Vec3A::new(600., 1550., -1200.), Vec3A::new(0., 0., 0.), ); - black_box(ball.get_ball_prediction_struct_for_time(&game, 60.)); + let predictions = ball.get_ball_prediction_struct_for_time(&game, 60.); + black_box(predictions); } } diff --git a/examples/out_of_bounds_count.rs b/examples/out_of_bounds_count.rs index 302fac2..895b7f5 100644 --- a/examples/out_of_bounds_count.rs +++ b/examples/out_of_bounds_count.rs @@ -40,7 +40,7 @@ fn main() { } } - if i % 500 == 0 { + if i % 1000 == 0 { print!( "\rNumber of out of bounds: {count}/{i} ({:.6}%)", count as f32 / i as f32 * 100. diff --git a/src/compressed.rs b/src/compressed.rs index ea04271..0dbd084 100644 --- a/src/compressed.rs +++ b/src/compressed.rs @@ -39,7 +39,7 @@ pub fn load_standard() -> (Game, Ball) { "assets/standard/standard_ramps_1_vertices.bin" ); - let triangle_collisions = initialize_standard(&standard_corner, &standard_goal, &standard_ramps_0, &standard_ramps_1); + let triangle_collisions = initialize_standard(standard_corner, standard_goal, standard_ramps_0, standard_ramps_1); (Game::new(triangle_collisions), Ball::initialize_standard()) } @@ -60,7 +60,7 @@ pub fn load_hoops() -> (Game, Ball) { "assets/hoops/hoops_ramps_1_vertices.bin" ); - let triangle_collisions = initialize_hoops(hoops_corner, &hoops_net, &hoops_rim, hoops_ramps_0, hoops_ramps_1); + let triangle_collisions = initialize_hoops(hoops_corner, hoops_net, hoops_rim, hoops_ramps_0, hoops_ramps_1); (Game::new(triangle_collisions), Ball::initialize_hoops()) } @@ -71,7 +71,7 @@ pub fn load_hoops() -> (Game, Ball) { pub fn load_dropshot() -> (Game, Ball) { let dropshot = include_mesh!("assets/dropshot/dropshot_ids.bin", "assets/dropshot/dropshot_vertices.bin"); - let triangle_collisions = initialize_dropshot(&dropshot); + let triangle_collisions = initialize_dropshot(dropshot); (Game::new(triangle_collisions), Ball::initialize_dropshot()) } diff --git a/src/lib.rs b/src/lib.rs index bf2a009..230f0ed 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,4 +1,5 @@ -#![warn(missing_docs, clippy::pedantic, clippy::all)] +#![warn(missing_docs, clippy::pedantic, clippy::nursery)] +#![allow(clippy::suboptimal_flops)] //! `rl_ball_sym` is a Rust implementation of a simulation of the Rocket League ball inside it's field. //! It loads the real geometry from the game and simulates the ball's movement in nanoseconds. diff --git a/src/linear_algebra/mat.rs b/src/linear_algebra/mat.rs deleted file mode 100644 index 8437eaf..0000000 --- a/src/linear_algebra/mat.rs +++ /dev/null @@ -1,30 +0,0 @@ -use glam::Mat3A; - -pub(crate) trait MatrixExt { - fn dot(&self, other: Self) -> Self; -} - -impl MatrixExt for Mat3A { - #[inline] - fn dot(&self, other: Self) -> Self { - // For some reason, glam does matrix multiplication in column order. So, - // we have to transpose both matrices prior to multiplying them. Then, - // transpose the final result. - (self.transpose() * other.transpose()).transpose() - } -} - -#[cfg(test)] -mod test { - use super::MatrixExt; - use glam::Mat3A; - - const A: Mat3A = Mat3A::from_cols_array_2d(&[[-1., 2., 3.], [4., 5., 6.], [7., 8., 9.]]); - const B: Mat3A = Mat3A::from_cols_array_2d(&[[9., 8., 7.], [6., -5., 4.], [3., 2., 1.]]); - const C: Mat3A = Mat3A::from_cols_array_2d(&[[12.0, -12.0, 4.0], [84.0, 19.0, 54.0], [138.0, 34.0, 90.0]]); - - #[test] - fn mat3_dot() { - assert_eq!(A.dot(B), C); - } -} diff --git a/src/linear_algebra/math.rs b/src/linear_algebra/math.rs deleted file mode 100644 index 81b1ee3..0000000 --- a/src/linear_algebra/math.rs +++ /dev/null @@ -1,45 +0,0 @@ -use glam::{Mat3A, Vec3A}; - -#[inline] -/// Round a vector to a given precision -pub fn round_vec_bullet(vec: &mut Vec3A, scale: f32, precision: f32) { - *vec = (*vec / scale / precision).round() * precision * scale; -} - -#[must_use] -#[inline] -/// Convert an axis-angle vector to a rotation matrix. -pub fn axis_to_rotation(axis: Vec3A) -> Mat3A { - Mat3A::from_axis_angle(axis.into(), axis.length()) -} - -#[must_use] -#[inline] -/// Returns the dot product of the matrix and the vector. -pub fn dot(matrix: Mat3A, vector: Vec3A) -> Vec3A { - matrix.transpose() * vector -} - -#[cfg(test)] -mod test { - use super::dot; - use glam::{Mat3A, Vec3A}; - - #[allow(clippy::approx_constant)] - const MAT: Mat3A = Mat3A::from_cols_array_2d(&[ - [-0.0, -0.166_666_67, -0.166_666_67], - [0.166_666_67, 0.083_333_336, 0.083_333_336], - [0.0, -0.707_106_8, 0.707_106_8], - ]); - const VEC: Vec3A = Vec3A::new(3., -1., -1.); - const RES: Vec3A = Vec3A::new(0.333_333_34, 0.333_333_3, 0.0); - - #[test] - fn test_dot() { - assert_eq!(dot(MAT, VEC), RES); - assert_eq!( - dot(MAT, VEC), - Vec3A::new(VEC.dot(MAT.x_axis), VEC.dot(MAT.y_axis), VEC.dot(MAT.z_axis)) - ); - } -} diff --git a/src/linear_algebra/mod.rs b/src/linear_algebra/mod.rs deleted file mode 100644 index 78e3ce9..0000000 --- a/src/linear_algebra/mod.rs +++ /dev/null @@ -1,2 +0,0 @@ -pub(crate) mod mat; -pub(crate) mod math; diff --git a/src/simulation/ball.rs b/src/simulation/ball.rs index 29b478e..0391eac 100644 --- a/src/simulation/ball.rs +++ b/src/simulation/ball.rs @@ -1,6 +1,9 @@ //! Tools for simulation a Rocket League ball. -use super::{game::Game, geometry::Sphere}; +use super::{ + game::{Constraints, Game}, + geometry::Sphere, +}; use glam::Vec3A; /// Represents the game's ball @@ -32,16 +35,13 @@ impl Default for Ball { pub type Predictions = Vec; impl Ball { - const RESTITUTION: f32 = 0.6; const DRAG: f32 = 0.03; - const MU: f32 = 2.; const V_MAX: f32 = 6000.; const W_MAX: f32 = 6.; const M: f32 = 30.; - const INV_M: f32 = 1. / Self::M; - const RESTITUTION_M: f32 = -(1. + Self::RESTITUTION) * Self::M; + pub(crate) const INV_M: f32 = 1. / Self::M; const STANDARD_RADIUS: f32 = 91.25; const HOOPS_RADIUS: f32 = 96.38307; @@ -140,6 +140,16 @@ impl Ball { Sphere::new(self.location, self.radius) } + #[inline] + pub(crate) fn get_velocity_in_local_point(&self, rel_pos: Vec3A) -> Vec3A { + self.velocity + self.angular_velocity.cross(rel_pos) + } + + #[inline] + pub(crate) fn get_velocity_in_local_point_no_delta(&self, rel_pos: Vec3A, external_force_impulse: Vec3A) -> Vec3A { + self.velocity + external_force_impulse + self.angular_velocity.cross(rel_pos) + } + /// Simulate the ball for one game tick /// /// `dt` - The delta time (game tick length) @@ -147,44 +157,25 @@ impl Ball { self.time += dt; if self.velocity.length_squared() != 0. || self.angular_velocity.length_squared() != 0. { - if let Some(contact) = game.triangle_collisions.collide(self.hitbox()) { - let p = contact.start; - let n = contact.direction; - - let loc = p - self.location; - - let m_reduced = 1. / (Self::INV_M + loc.length_squared() * self.inv_inertia); - - let v_perp = n * self.velocity.dot(n).min(0.); - let v_para = self.velocity - v_perp - loc.cross(self.angular_velocity); - - let ratio = v_perp.length() / v_para.length().max(0.0001); - - let j_perp = v_perp * Self::RESTITUTION_M; - let j_para = -(Self::MU * ratio).min(1.) * m_reduced * v_para; - - let j = j_perp + j_para; - - self.velocity *= (1. - Self::DRAG).powf(dt); - self.velocity += game.gravity * dt; - self.velocity += j * Self::INV_M; + self.velocity *= (1. - Self::DRAG).powf(dt); + let external_force_impulse = game.gravity * dt; - self.location += self.velocity * dt; + let contacts = game.triangle_collisions.collide(self.hitbox()); + if !contacts.is_empty() { + let mut constraints = Constraints::new(Self::INV_M, external_force_impulse); + constraints.add_contacts(contacts, self, dt); - self.angular_velocity += loc.cross(j) * self.inv_inertia; + let (delta_velocity, push_velocity) = constraints.solve(self); - let penetration = self.radius + Sphere::CONTACT_BREAKING_THRESHOLD - (self.location - p).dot(n); - if penetration > 0. { - self.location += 1.001 * penetration * n; - } - } else { - self.velocity *= (1. - Self::DRAG).powf(dt); - self.velocity += game.gravity * dt; - self.location += self.velocity * dt; + self.velocity += delta_velocity.linear; + self.angular_velocity += delta_velocity.angular; + self.location += push_velocity * dt; } - self.angular_velocity *= (Self::W_MAX * self.angular_velocity.length_recip()).min(1.); - self.velocity *= (Self::V_MAX * self.velocity.length_recip()).min(1.); + self.velocity += external_force_impulse; + self.location += self.velocity * dt; + self.angular_velocity = self.angular_velocity.clamp_length_max(Self::W_MAX); + self.velocity = self.velocity.clamp_length_max(Self::V_MAX); } } diff --git a/src/simulation/bvh.rs b/src/simulation/bvh.rs index fcda63b..2ed63ee 100644 --- a/src/simulation/bvh.rs +++ b/src/simulation/bvh.rs @@ -74,8 +74,8 @@ pub fn global_aabb(boxes: &[Aabb]) -> Aabb { } #[cfg(test)] +#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] mod test { - #![allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] use super::*; use glam::Vec3A; diff --git a/src/simulation/field.rs b/src/simulation/field.rs index 31ae728..16c28d2 100644 --- a/src/simulation/field.rs +++ b/src/simulation/field.rs @@ -14,8 +14,8 @@ fn z_axis_to_rotation(axis: f32) -> Mat3A { #[inline] fn quad(p: Vec3A, e1: Vec3A, e2: Vec3A) -> Mesh { Mesh::new( - vec![0, 1, 3, 1, 2, 3], - vec![p + e1 + e2, p - e1 + e2, p - e1 - e2, p + e1 - e2], + Box::from([0, 1, 3, 1, 2, 3]), + Box::from([p + e1 + e2, p - e1 + e2, p - e1 - e2, p + e1 - e2]), ) } @@ -45,34 +45,34 @@ pub fn get_standard_walls() -> [Mesh; 4] { #[must_use] /// Get a BVH generated from the given standard field meshes. pub fn initialize_standard( - standard_corner: &Mesh, - standard_goal: &Mesh, - standard_ramps_0: &Mesh, - standard_ramps_1: &Mesh, + standard_corner: Mesh, + standard_goal: Mesh, + standard_ramps_0: Mesh, + standard_ramps_1: Mesh, ) -> TriangleBvh { - const SCALE: f32 = 100.; - const S: Mat3A = Mat3A::from_diagonal(Vec3::splat(SCALE)); - - const Y_OFFSET: Vec3A = Vec3A::new(0., -5120., 0.); + const Y_OFFSET: f32 = -5120.; - let standard_corner_tf = standard_corner.transform(S); - let standard_goal_tf = standard_goal.transform(S); - let standard_ramps_0_tf = standard_ramps_0.transform(S); - let standard_ramps_1_tf = standard_ramps_1.transform(S); + let standard_goal_tf = standard_goal.translate_y(Y_OFFSET); let [floor, ceiling, side_wall_0, side_wall_1] = get_standard_walls(); let field_mesh = Mesh::combine([ - standard_corner_tf.transform(FLIP_X), - standard_corner_tf.transform(FLIP_Y), - standard_corner_tf.transform(FLIP_X * FLIP_Y), - standard_corner_tf, - standard_goal_tf.translate(Y_OFFSET), - standard_goal_tf.translate(Y_OFFSET).transform(FLIP_Y), - standard_ramps_0_tf.transform(FLIP_X), - standard_ramps_0_tf, - standard_ramps_1_tf.transform(FLIP_X), - standard_ramps_1_tf, + standard_corner.clone().transform(FLIP_X), + standard_corner.clone().transform(FLIP_Y), + standard_corner.clone().transform(FLIP_X * FLIP_Y), + standard_corner, + standard_goal_tf.clone().transform(FLIP_X), + standard_goal_tf.clone().transform(FLIP_Y), + standard_goal_tf.clone().transform(FLIP_X * FLIP_Y), + standard_goal_tf, + standard_ramps_0.clone().transform(FLIP_X), + standard_ramps_0.clone().transform(FLIP_Y), + standard_ramps_0.clone().transform(FLIP_X * FLIP_Y), + standard_ramps_0, + standard_ramps_1.clone().transform(FLIP_X), + standard_ramps_1.clone().transform(FLIP_Y), + standard_ramps_1.clone().transform(FLIP_X * FLIP_Y), + standard_ramps_1, floor, ceiling, side_wall_0, @@ -119,33 +119,33 @@ pub fn get_hoops_walls() -> [Mesh; 6] { /// Get a BVH generated from the given hoops field meshes. pub fn initialize_hoops( hoops_corner: Mesh, - hoops_net: &Mesh, - hoops_rim: &Mesh, + hoops_net: Mesh, + hoops_rim: Mesh, hoops_ramps_0: Mesh, hoops_ramps_1: Mesh, ) -> TriangleBvh { const SCALE: f32 = 0.9; const S: Mat3A = Mat3A::from_diagonal(Vec3::splat(SCALE)); - const Y_OFFSET: Vec3A = Vec3A::new(0., 431.664, 0.); + const Y_OFFSET: f32 = 431.664; - let hoops_net_tf = hoops_net.transform(S).translate(Y_OFFSET); - let hoops_rim_tf = hoops_rim.transform(S).translate(Y_OFFSET); + let hoops_net_tf = hoops_net.transform(S).translate_y(Y_OFFSET); + let hoops_rim_tf = hoops_rim.transform(S).translate_y(Y_OFFSET); let [floor, ceiling, side_wall_0, side_wall_1, back_wall_0, back_wall_1] = get_hoops_walls(); let field_mesh = Mesh::combine([ - hoops_corner.transform(FLIP_X), - hoops_corner.transform(FLIP_Y), - hoops_corner.transform(FLIP_X * FLIP_Y), + hoops_corner.clone().transform(FLIP_X), + hoops_corner.clone().transform(FLIP_Y), + hoops_corner.clone().transform(FLIP_X * FLIP_Y), hoops_corner, - hoops_net_tf.transform(FLIP_Y), + hoops_net_tf.clone().transform(FLIP_Y), hoops_net_tf, - hoops_rim_tf.transform(FLIP_Y), + hoops_rim_tf.clone().transform(FLIP_Y), hoops_rim_tf, - hoops_ramps_0.transform(FLIP_X), + hoops_ramps_0.clone().transform(FLIP_X), hoops_ramps_0, - hoops_ramps_1.transform(FLIP_Y), + hoops_ramps_1.clone().transform(FLIP_Y), hoops_ramps_1, floor, ceiling, @@ -160,11 +160,10 @@ pub fn initialize_hoops( #[must_use] /// Get a BVH generated from the given dropshot field meshes. -pub fn initialize_dropshot(dropshot: &Mesh) -> TriangleBvh { +pub fn initialize_dropshot(dropshot: Mesh) -> TriangleBvh { const SCALE: f32 = 0.393; const S: Mat3A = Mat3A::from_diagonal(Vec3::splat(SCALE)); const Z_OFFSET: f32 = -207.565; - const DZ: Vec3A = Vec3A::new(0., 0., Z_OFFSET); const Z: Vec3A = Vec3A::new(0., 0., 1010.); let q = z_axis_to_rotation(FRAC_PI_6); @@ -188,7 +187,7 @@ pub fn initialize_dropshot(dropshot: &Mesh) -> TriangleBvh { }; let field_mesh = Mesh::combine([ - dropshot.transform(q * S).translate(DZ), + dropshot.transform(q * S).translate_z(Z_OFFSET), floor, ceiling, // it is critical that this operation only happens 6 times @@ -279,35 +278,35 @@ pub fn initialize_throwback( let corner_wall_2_tf = corner_wall_2.transform(S); let field_mesh = Mesh::combine([ - corner_ramps_lower_tf.transform(FLIP_X), - corner_ramps_lower_tf.transform(FLIP_Y), - corner_ramps_lower_tf.transform(FLIP_Y).transform(FLIP_X), + corner_ramps_lower_tf.clone().transform(FLIP_X), + corner_ramps_lower_tf.clone().transform(FLIP_Y), + corner_ramps_lower_tf.clone().transform(FLIP_X * FLIP_Y), corner_ramps_lower_tf, - corner_ramps_upper_tf.transform(FLIP_X), - corner_ramps_upper_tf.transform(FLIP_Y), - corner_ramps_upper_tf.transform(FLIP_Y).transform(FLIP_X), + corner_ramps_upper_tf.clone().transform(FLIP_X), + corner_ramps_upper_tf.clone().transform(FLIP_Y), + corner_ramps_upper_tf.clone().transform(FLIP_X * FLIP_Y), corner_ramps_upper_tf, - goal_tf.transform(FLIP_Y), + goal_tf.clone().transform(FLIP_Y), goal_tf, - side_ramps_lower_tf.transform(FLIP_X), + side_ramps_lower_tf.clone().transform(FLIP_X), side_ramps_lower_tf, - side_ramps_upper_tf.transform(FLIP_X), + side_ramps_upper_tf.clone().transform(FLIP_X), side_ramps_upper_tf, - back_ramps_lower_tf.transform(FLIP_Y), + back_ramps_lower_tf.clone().transform(FLIP_Y), back_ramps_lower_tf, - back_ramps_upper_tf.transform(FLIP_Y), + back_ramps_upper_tf.clone().transform(FLIP_Y), back_ramps_upper_tf, - corner_wall_0_tf.transform(FLIP_X), - corner_wall_0_tf.transform(FLIP_Y), - corner_wall_0_tf.transform(FLIP_Y).transform(FLIP_X), + corner_wall_0_tf.clone().transform(FLIP_X), + corner_wall_0_tf.clone().transform(FLIP_Y), + corner_wall_0_tf.clone().transform(FLIP_X * FLIP_Y), corner_wall_0_tf, - corner_wall_1_tf.transform(FLIP_X), - corner_wall_1_tf.transform(FLIP_Y), - corner_wall_1_tf.transform(FLIP_Y).transform(FLIP_X), + corner_wall_1_tf.clone().transform(FLIP_X), + corner_wall_1_tf.clone().transform(FLIP_Y), + corner_wall_1_tf.clone().transform(FLIP_X * FLIP_Y), corner_wall_1_tf, - corner_wall_2_tf.transform(FLIP_X), - corner_wall_2_tf.transform(FLIP_Y), - corner_wall_2_tf.transform(FLIP_Y).transform(FLIP_X), + corner_wall_2_tf.clone().transform(FLIP_X), + corner_wall_2_tf.clone().transform(FLIP_Y), + corner_wall_2_tf.clone().transform(FLIP_X * FLIP_Y), corner_wall_2_tf, floor, ceiling, diff --git a/src/simulation/game.rs b/src/simulation/game.rs index 010fec0..8859d80 100644 --- a/src/simulation/game.rs +++ b/src/simulation/game.rs @@ -1,7 +1,11 @@ //! All the data about the game to simulate it. -use super::tri_bvh::TriangleBvh; +use super::{ball::Ball, geometry::Contact, tri_bvh::TriangleBvh}; +use combo_vec::ReArr; use glam::Vec3A; +use std::f32::consts::FRAC_1_SQRT_2; + +const COEFF_FRICTION: f32 = 0.35; /// All of the game information. #[derive(Clone, Debug)] @@ -13,6 +17,9 @@ pub struct Game { } impl Game { + const RESTITUTION: f32 = 0.6; + const RESTITUTION_VELOCITY_THRESHOLD: f32 = 0.2; + #[inline] pub(crate) const fn new(triangle_collisions: TriangleBvh) -> Self { Self { @@ -20,4 +27,405 @@ impl Game { triangle_collisions, } } + + pub(crate) fn plane_space_1(n: Vec3A) -> Vec3A { + if n.z.abs() > FRAC_1_SQRT_2 { + // choose p in y-z plane + let a = n.y * n.y + n.z * n.z; + let k = 1. / a.sqrt(); + Vec3A::new(0., -n.z * k, n.y * k) + } else { + // choose p in x-y plane + let a = n.x * n.x + n.y * n.y; + let k = 1. / a.sqrt(); + Vec3A::new(-n.y * k, n.x * k, 0.) + } + } + + #[inline] + pub(crate) fn restitution_curve(rel_vel: f32) -> f32 { + if -rel_vel > Self::RESTITUTION_VELOCITY_THRESHOLD { + Self::RESTITUTION * -rel_vel + } else { + 0. + } + } +} + +#[derive(Clone, Debug)] +pub struct Constraint { + pub contact_normal: Vec3A, + pub rel_pos_cross_normal: Vec3A, + pub angular_component: Vec3A, + pub rhs: f32, + pub rhs_penetration: f32, + pub lower_limit: f32, + pub upper_limit: f32, + pub jac_diag_ab_inv: f32, + pub applied_impulse: f32, + pub applied_push_impulse: f32, +} + +impl Constraint { + // gResolveSingleConstraintRowLowerLimit_sse2 + fn resolve_single_constraint_row_lower_limit(&mut self, deltas: &mut VelocityPair, inv_mass: f32) -> f32 { + let mut delta_impulse: f32 = self.rhs; + + let delta_vel_1_dot_n = self.contact_normal.dot(deltas.linear) + self.rel_pos_cross_normal.dot(deltas.angular); + delta_impulse -= delta_vel_1_dot_n * self.jac_diag_ab_inv; + + let sum = self.applied_impulse + delta_impulse; + let low_min_applied = self.lower_limit - self.applied_impulse; + delta_impulse = if sum < self.lower_limit { + low_min_applied + } else { + delta_impulse + }; + + self.applied_impulse = sum.max(self.lower_limit); + + let linear_component_a = self.contact_normal * inv_mass; + let impulse_magnitude = Vec3A::splat(delta_impulse); + + deltas.linear += linear_component_a * impulse_magnitude; + deltas.angular += self.angular_component * impulse_magnitude; + + delta_impulse / self.jac_diag_ab_inv + } + + // gResolveSingleConstraintRowGeneric_sse2 + fn resolve_single_constraint_row_generic(&mut self, deltas: &mut VelocityPair, inv_mass: f32) -> f32 { + let applied_impulse = self.applied_impulse; + + let mut delta_impulse = self.rhs; + let delta_vel_1_dot_n = self.contact_normal.dot(deltas.linear) + self.rel_pos_cross_normal.dot(deltas.angular); + delta_impulse -= delta_vel_1_dot_n * self.jac_diag_ab_inv; + + let sum = applied_impulse + delta_impulse; + delta_impulse = if sum < self.lower_limit { + self.lower_limit - applied_impulse + } else { + delta_impulse + }; + + self.applied_impulse = sum.max(self.lower_limit); + + let upper_min_applied = self.upper_limit - applied_impulse; + delta_impulse = if sum < self.upper_limit { + delta_impulse + } else { + upper_min_applied + }; + + self.applied_impulse = if sum < self.upper_limit { + self.applied_impulse + } else { + self.upper_limit + }; + + let linear_component_a = self.contact_normal * inv_mass; + let impulse_magnitude = Vec3A::splat(delta_impulse); + + deltas.linear += linear_component_a * impulse_magnitude; + deltas.angular += self.angular_component * impulse_magnitude; + + delta_impulse / self.jac_diag_ab_inv + } + + // gResolveSplitPenetrationImpulse_sse2 + fn resolve_split_penetration_impulse(&mut self, velocities: &mut VelocityPair, inv_mass: f32) -> f32 { + if self.rhs_penetration == 0. { + return 0.; + } + + let mut delta_impulse = self.rhs_penetration; + + let delta_vel_1_dot_n = + self.contact_normal.dot(velocities.linear) + self.rel_pos_cross_normal.dot(velocities.angular); + delta_impulse -= delta_vel_1_dot_n * self.jac_diag_ab_inv; + + let sum = self.applied_push_impulse + delta_impulse; + delta_impulse = if sum < self.lower_limit { + self.lower_limit - self.applied_push_impulse + } else { + delta_impulse + }; + + self.applied_push_impulse = sum.max(self.lower_limit); + + let linear_component_a = self.contact_normal * inv_mass; + let impulse_magnitude = Vec3A::splat(delta_impulse); + + velocities.linear += linear_component_a * impulse_magnitude; + velocities.angular += self.angular_component * impulse_magnitude; + + delta_impulse / self.jac_diag_ab_inv + } +} + +#[derive(Clone, Debug)] +pub struct ConstraintPair { + pub contact: Constraint, + pub friction: Constraint, +} + +impl ConstraintPair { + fn solve_single_iteration(&mut self, deltas: &mut VelocityPair, inv_mass: f32) -> f32 { + let residual = self.contact.resolve_single_constraint_row_lower_limit(deltas, inv_mass); + let mut least_squares_residual = residual * residual; + + let total_impulse = self.contact.applied_impulse; + if total_impulse > 0. { + self.friction.lower_limit = -COEFF_FRICTION * total_impulse; + self.friction.upper_limit = COEFF_FRICTION * total_impulse; + + let residual = self.friction.resolve_single_constraint_row_generic(deltas, inv_mass); + least_squares_residual = least_squares_residual.max(residual * residual); + } + + least_squares_residual + } +} + +#[derive(Clone, Copy, Debug)] +pub struct VelocityPair { + pub linear: Vec3A, + pub angular: Vec3A, +} + +impl VelocityPair { + pub const ZERO: Self = Self::new(Vec3A::ZERO, Vec3A::ZERO); + + #[inline] + #[must_use] + pub const fn new(linear: Vec3A, angular: Vec3A) -> Self { + Self { linear, angular } + } +} + +#[derive(Clone, Debug)] +pub struct Constraints { + contacts: Vec, + normal_sum: Vec3A, + depth_sum: f32, + count: u8, + inv_mass: f32, + external_force_impulse: Vec3A, +} + +impl Constraints { + pub const MAX_CONTACTS: usize = 4; + + const NUM_ITERATIONS: usize = 10; + const RELAXATION: f32 = 1.; + const ERP2: f32 = 0.8; + + #[inline] + #[must_use] + pub const fn new(inv_mass: f32, external_force_impulse: Vec3A) -> Self { + Self { + contacts: Vec::new(), + normal_sum: Vec3A::ZERO, + depth_sum: 0., + count: 0, + inv_mass, + external_force_impulse, + } + } + + pub fn add_contacts(&mut self, contacts: ReArr, ball: &Ball, dt: f32) { + self.contacts.reserve(contacts.len()); + + for contact in contacts { + self.normal_sum += contact.triangle_normal; + self.depth_sum += contact.local_position.length(); + self.count += 1; + self.contacts.push(self.setup_contact_constraint(ball, contact, dt)); + } + } + + // setupContactConstraint + fn setup_contact_constraint(&self, ball: &Ball, contact: Contact, dt: f32) -> Constraint { + let torque_axis = contact.local_position.cross(contact.triangle_normal); + let angular_component = ball.inv_inertia * torque_axis; + let vec = angular_component.cross(contact.local_position); + + let denom = self.inv_mass + contact.triangle_normal.dot(vec); + let jac_diag_ab_inv = Self::RELAXATION / denom; + let rel_pos_cross_normal = torque_axis; + + let abs_vel = ball.get_velocity_in_local_point(contact.local_position); + let rel_vel = contact.triangle_normal.dot(abs_vel); + let restitution = Game::restitution_curve(rel_vel); + + let rel_vel = contact.triangle_normal.dot(ball.velocity + self.external_force_impulse) + + rel_pos_cross_normal.dot(ball.angular_velocity); + + let velocity_error = restitution - rel_vel; + + let penetration_impulse = if contact.depth > 0. { + 0. + } else { + -contact.depth * Self::ERP2 / dt * jac_diag_ab_inv + }; + + let velocity_impulse = velocity_error * jac_diag_ab_inv; + + Constraint { + contact_normal: contact.triangle_normal, + rel_pos_cross_normal, + angular_component, + rhs: velocity_impulse, + rhs_penetration: penetration_impulse, + lower_limit: 0., + upper_limit: 1e10, + jac_diag_ab_inv, + applied_impulse: 0., + applied_push_impulse: 0., + } + } + + fn setup_special_contact_constraint(&self, ball: &Ball, normal_world_on_b: Vec3A, rel_pos: Vec3A) -> Constraint { + let torque_axis = rel_pos.cross(normal_world_on_b); + let angular_component = ball.inv_inertia * torque_axis; + let vec = angular_component.cross(rel_pos); + + let denom = self.inv_mass + normal_world_on_b.dot(vec); + let jac_diag_ab_inv = Self::RELAXATION / denom; + + let abs_vel = ball.get_velocity_in_local_point(rel_pos); + let rel_vel = normal_world_on_b.dot(abs_vel); + + let restitution = Game::restitution_curve(rel_vel); + + let rel_vel = + normal_world_on_b.dot(ball.velocity + self.external_force_impulse) + torque_axis.dot(ball.angular_velocity); + + let velocity_error = restitution - rel_vel; + let velocity_impulse = velocity_error * jac_diag_ab_inv; + + Constraint { + contact_normal: normal_world_on_b, + rel_pos_cross_normal: torque_axis, + angular_component, + rhs: velocity_impulse, + rhs_penetration: 0., + lower_limit: 0., + upper_limit: 1e10, + jac_diag_ab_inv, + applied_impulse: 0., + applied_push_impulse: 0., + } + } + + // setupFrictionConstraint + fn setup_friction_constraint(&self, ball: &Ball, normal_axis: Vec3A, rel_pos: Vec3A) -> Constraint { + let contact_normal = normal_axis; + let rel_pos_cross_normal = rel_pos.cross(contact_normal); + let angular_component = ball.inv_inertia * rel_pos_cross_normal; + + let vec = angular_component.cross(rel_pos); + let denom: f32 = self.inv_mass + normal_axis.dot(vec); + + let jac_diag_ab_inv = Self::RELAXATION / denom; + + let rel_vel = contact_normal.dot(ball.velocity + self.external_force_impulse) + + rel_pos_cross_normal.dot(ball.angular_velocity); + let velocity_error = -rel_vel; + let velocity_impulse = velocity_error * jac_diag_ab_inv; + + Constraint { + contact_normal, + rel_pos_cross_normal, + angular_component, + rhs: velocity_impulse, + rhs_penetration: 0., + lower_limit: -COEFF_FRICTION, + upper_limit: COEFF_FRICTION, + jac_diag_ab_inv, + applied_impulse: 0., + applied_push_impulse: 0., + } + } + + // convertContactSpecial + fn process_special_contact(&self, ball: &Ball) -> ConstraintPair { + debug_assert!(self.count > 0); + + let count = Vec3A::splat(f32::from(self.count)); + let average_normal = self.normal_sum / count; + let average_distance = self.depth_sum / count; + + let rel_pos = average_normal * -average_distance; + let vel: Vec3A = ball.get_velocity_in_local_point_no_delta(rel_pos, self.external_force_impulse); + let rel_vel = average_normal.dot(vel); + + let contact_constraint = self.setup_special_contact_constraint(ball, average_normal, rel_pos); + + let mut lateral_friction_dir = vel - average_normal * rel_vel; + let lat_rel_vel = lateral_friction_dir.length_squared(); + + if lat_rel_vel > f32::EPSILON { + lateral_friction_dir /= lat_rel_vel.sqrt(); + } else { + lateral_friction_dir = Game::plane_space_1(average_normal); + } + + let friction_constraint = self.setup_friction_constraint(ball, lateral_friction_dir, rel_pos); + + ConstraintPair { + contact: contact_constraint, + friction: friction_constraint, + } + } + + #[must_use] + pub fn solve(mut self, ball: &Ball) -> (VelocityPair, Vec3A) { + let mut average_constraint = self.process_special_contact(ball); + + let velocities = self.solve_split_impulse_iterations(); + + let mut deltas = VelocityPair::ZERO; + + for _ in 0..Self::NUM_ITERATIONS { + let least_squares_residual = average_constraint.solve_single_iteration(&mut deltas, self.inv_mass); + + if least_squares_residual <= f32::EPSILON { + break; + } + } + + (deltas, velocities) + } + + // solveGroupCacheFriendlySplitImpulseIterations + fn solve_split_impulse_iterations(&mut self) -> Vec3A { + let mut velocities = VelocityPair::ZERO; + let mut should_runs = [true; Self::MAX_CONTACTS]; + + for _ in 0..Self::NUM_ITERATIONS { + let mut any_run_next = false; + + for (should_run, contact) in should_runs.iter_mut().zip(&mut self.contacts) { + if !*should_run { + continue; + } + + let residual = contact.resolve_split_penetration_impulse(&mut velocities, self.inv_mass); + + if residual * residual <= f32::EPSILON { + *should_run = false; + } else { + any_run_next = true; + } + } + + if !any_run_next { + break; + } + } + + // we skip calculating the ball's rotation so just return the linear velocity + velocities.linear + } } diff --git a/src/simulation/geometry.rs b/src/simulation/geometry.rs index 9f81891..a6cfa3d 100644 --- a/src/simulation/geometry.rs +++ b/src/simulation/geometry.rs @@ -1,7 +1,145 @@ //! Various geometrical objects and tools. +use crate::simulation::game::Constraints; +use combo_vec::ReArr; use glam::Vec3A; -use std::ops::{Add, AddAssign}; +use std::ops::Add; + +#[derive(Clone, Copy, Debug)] +pub struct Contact { + pub local_position: Vec3A, + pub triangle_normal: Vec3A, + pub depth: f32, +} + +#[derive(Debug)] +#[repr(transparent)] +pub struct Hits(ReArr); + +impl Hits { + #[inline] + pub const fn new() -> Self { + Self(ReArr::new()) + } + + #[inline] + fn get_res(new_contact_local: Vec3A, point1: Vec3A, point2: Vec3A, point3: Vec3A) -> f32 { + (new_contact_local - point1).cross(point2 - point3).length_squared() + } + + #[inline] + fn get_res_0(&self, new_contact_local: Vec3A) -> f32 { + Self::get_res( + new_contact_local, + self.0[1].local_position, + self.0[3].local_position, + self.0[2].local_position, + ) + } + + #[inline] + fn get_res_1(&self, new_contact_local: Vec3A) -> f32 { + Self::get_res( + new_contact_local, + self.0[0].local_position, + self.0[3].local_position, + self.0[2].local_position, + ) + } + + #[inline] + fn get_res_2(&self, new_contact_local: Vec3A) -> f32 { + Self::get_res( + new_contact_local, + self.0[0].local_position, + self.0[3].local_position, + self.0[1].local_position, + ) + } + + #[inline] + fn get_res_3(&self, new_contact_local: Vec3A) -> f32 { + Self::get_res( + new_contact_local, + self.0[0].local_position, + self.0[2].local_position, + self.0[1].local_position, + ) + } + + // sortCachedPoints + fn replacement_index(&self, new_contact: &Contact) -> usize { + let mut max_penetration_index = self.0.len(); + let mut max_penetration = new_contact.depth; + for (i, contact) in self.0.iter().enumerate() { + if contact.depth < max_penetration { + max_penetration_index = i; + max_penetration = contact.depth; + } + } + + let res = match max_penetration_index { + 0 => [ + 0., + self.get_res_1(new_contact.local_position), + self.get_res_2(new_contact.local_position), + self.get_res_3(new_contact.local_position), + ], + 1 => [ + self.get_res_0(new_contact.local_position), + 0., + self.get_res_2(new_contact.local_position), + self.get_res_3(new_contact.local_position), + ], + 2 => [ + self.get_res_0(new_contact.local_position), + self.get_res_1(new_contact.local_position), + 0., + self.get_res_3(new_contact.local_position), + ], + 3 => [ + self.get_res_0(new_contact.local_position), + self.get_res_1(new_contact.local_position), + self.get_res_2(new_contact.local_position), + 0., + ], + _ => [ + self.get_res_0(new_contact.local_position), + self.get_res_1(new_contact.local_position), + self.get_res_2(new_contact.local_position), + self.get_res_3(new_contact.local_position), + ], + }; + + let (mut biggest_area, mut biggest_area_index) = if res[1] > res[0] { (res[1], 1) } else { (res[0], 0) }; + + if res[2] > biggest_area { + biggest_area = res[2]; + biggest_area_index = 2; + } + + if res[3] > biggest_area { + biggest_area_index = 3; + } + + biggest_area_index + } + + // addManifoldPoint + pub fn push(&mut self, contact: Contact) { + if self.0.len() == Constraints::MAX_CONTACTS { + let index = self.replacement_index(&contact); + self.0[index] = contact; + } else { + self.0.push(contact); + } + } + + #[inline] + pub const fn inner(self) -> ReArr { + self.0 + } +} #[repr(transparent)] /// A triangle made from 3 points. @@ -23,23 +161,11 @@ impl Tri { Self::new([iter.next().unwrap(), iter.next().unwrap(), iter.next().unwrap()]) } - #[must_use] - /// Get the normal of the triangle. - pub fn unit_normal(self) -> Vec3A { - (self.points[1] - self.points[0]) - .cross(self.points[2] - self.points[0]) - .normalize() - } - /// Check if a point projected onto the same place as the triangle /// is within the bounds of it. /// This is used instead of bullet's method because it's much faster: /// - fn face_contains(&self, point: Vec3A) -> bool { - let u = self.points[1] - self.points[0]; - let v = self.points[2] - self.points[0]; - let n = u.cross(v); - let w = point - self.points[0]; + fn face_contains(u: Vec3A, v: Vec3A, n: Vec3A, w: Vec3A) -> bool { let gamma = u.cross(w).dot(n) / n.dot(n); let beta = w.cross(v).dot(n) / n.dot(n); let alpha = 1. - gamma - beta; @@ -51,11 +177,7 @@ impl Tri { /// Instead of using bullet's method, /// we use the method described here which is much faster: /// - fn closest_point(&self, point: Vec3A) -> Vec3A { - let ab = self.points[1] - self.points[0]; - let ac = self.points[2] - self.points[0]; - let ap = point - self.points[0]; - + fn closest_point(&self, point: Vec3A, ab: Vec3A, ac: Vec3A, ap: Vec3A) -> Vec3A { let d1 = ab.dot(ap); let d2 = ac.dot(ap); if d1 <= 0. && d2 <= 0. { @@ -102,35 +224,39 @@ impl Tri { #[must_use] /// Check if a sphere intersects the triangle. - pub fn intersect_sphere(&self, obj: Sphere) -> Option { - let mut normal = self.unit_normal(); + pub fn intersect_sphere(&self, obj: Sphere) -> Option { + let u = self.points[1] - self.points[0]; + let v = self.points[2] - self.points[0]; + let n = u.cross(v); + let mut triangle_normal = n.normalize(); let p1_to_center = obj.center - self.points[0]; - let mut distance_from_plane = p1_to_center.dot(normal); + let mut distance_from_plane = p1_to_center.dot(triangle_normal); if distance_from_plane < 0. { distance_from_plane *= -1.; - normal *= -1.; + triangle_normal *= -1.; } if distance_from_plane >= obj.radius_with_threshold { return None; } - let contact_point = if self.face_contains(obj.center) { - Some(obj.center - normal * distance_from_plane) + let w = obj.center - self.points[0]; + + let contact_point = if Self::face_contains(u, v, n, w) { + obj.center - triangle_normal * distance_from_plane } else { let min_dist_sqr = obj.radius_with_threshold.powi(2); - - let closest_point = self.closest_point(obj.center); + let closest_point = self.closest_point(obj.center, u, v, w); let distance_sqr = (closest_point - obj.center).length_squared(); if distance_sqr < min_dist_sqr { - Some(closest_point) + closest_point } else { - None + return None; } - }?; + }; let contact_to_center = obj.center - contact_point; let distance_sqr = contact_to_center.length_squared(); @@ -139,13 +265,22 @@ impl Tri { return None; } - let result_normal = if distance_sqr > f32::EPSILON { - contact_to_center / distance_sqr.sqrt() + let (result_normal, depth) = if distance_sqr > f32::EPSILON { + let distance = distance_sqr.sqrt(); + (contact_to_center / distance, -(obj.radius - distance)) } else { - normal + (triangle_normal, -obj.radius) }; - Some(Ray::new(contact_point, result_normal)) + // println!("CONTACT"); + + let point_in_world = contact_point + result_normal * depth; + + Some(Contact { + local_position: point_in_world - obj.center, + triangle_normal, + depth, + }) } } @@ -247,36 +382,12 @@ impl From for Aabb { } } -/// A ray starting at `start` and going in `direction`. -#[derive(Clone, Copy, Debug, Default)] -pub struct Ray { - /// Starting location of the ray. - pub start: Vec3A, - /// Direction the ray is pointing. - pub direction: Vec3A, -} - -impl AddAssign for Ray { - #[inline] - fn add_assign(&mut self, rhs: Self) { - self.start += rhs.start; - self.direction += rhs.direction; - } -} - -impl Ray { - #[inline] - pub const fn new(start: Vec3A, direction: Vec3A) -> Self { - Self { start, direction } - } -} - /// A Sphere-like object. #[derive(Clone, Copy, Debug, Default)] pub struct Sphere { - pub center: Vec3A, - pub radius: f32, - pub radius_with_threshold: f32, + center: Vec3A, + radius: f32, + radius_with_threshold: f32, } impl Sphere { @@ -296,7 +407,6 @@ impl Sphere { #[cfg(test)] mod test { use super::*; - use glam::Vec3A; const TRI: Tri = Tri::new([ Vec3A::new(-1.0, 5.0, 0.0), diff --git a/src/simulation/mesh.rs b/src/simulation/mesh.rs index ada125c..825fc01 100644 --- a/src/simulation/mesh.rs +++ b/src/simulation/mesh.rs @@ -3,20 +3,27 @@ use super::geometry::Tri; use byteorder::{LittleEndian, ReadBytesExt}; use glam::{Mat3A, Vec3A}; -use std::io::{Cursor, Read}; +use std::io::Cursor; + +#[inline] +fn extract_usize(cursor: &mut Cursor<&[u8]>) -> usize { + cursor + .read_u32::() + .unwrap_or_else(|e| unreachable!("Problem parsing ***_ids.dat: {e:?}")) as usize +} #[inline] fn extract_f32(cursor: &mut Cursor<&[u8]>) -> f32 { cursor .read_f32::() - .unwrap_or_else(|e| panic!("Problem parsing ***_vertices.dat: {e:?}")) + .unwrap_or_else(|e| unreachable!("Problem parsing ***_vertices.dat: {e:?}")) } /// A collection of inter-connected triangles. #[derive(Clone, Debug, Default)] pub struct Mesh { - ids: Vec, - vertices: Vec, + ids: Box<[usize]>, + vertices: Box<[Vec3A]>, } impl Mesh { @@ -26,12 +33,7 @@ impl Mesh { let ids_len = ids_dat.len() / 4; let mut ids_cursor = Cursor::new(ids_dat); - (0..ids_len) - .map(|_| match ids_cursor.read_u32::() { - Ok(id) => id as usize, - Err(e) => panic!("Problem parsing ***_ids.dat: {e:?}"), - }) - .collect::>() + (0..ids_len).map(|_| extract_usize(&mut ids_cursor)).collect() }; let vertices = { @@ -41,12 +43,12 @@ impl Mesh { (0..vertices_len / 3) .map(|_| { Vec3A::new( - extract_f32(vertices_cursor.by_ref()), - extract_f32(vertices_cursor.by_ref()), - extract_f32(vertices_cursor.by_ref()), + extract_f32(&mut vertices_cursor), + extract_f32(&mut vertices_cursor), + extract_f32(&mut vertices_cursor), ) }) - .collect::>() + .collect() }; Self { ids, vertices } @@ -55,7 +57,7 @@ impl Mesh { #[must_use] #[inline] /// Create a new Mesh from a list of ids and vertices. - pub const fn new(ids: Vec, vertices: Vec) -> Self { + pub const fn new(ids: Box<[usize]>, vertices: Box<[Vec3A]>) -> Self { Self { ids, vertices } } @@ -70,52 +72,69 @@ impl Mesh { let (ids, vertices) = other_meshes.into_iter().fold( (Vec::with_capacity(n_ids), Vec::with_capacity(n_verts)), |(mut ids, mut vertices), m| { - ids.extend(m.ids.into_iter().map(|id| id + id_offset)); + ids.extend(m.ids.iter().map(|id| id + id_offset)); id_offset += m.vertices.len(); - vertices.extend(m.vertices); + vertices.extend(m.vertices.iter()); (ids, vertices) }, ); - Self { ids, vertices } + Self { + ids: ids.into_boxed_slice(), + vertices: vertices.into_boxed_slice(), + } } #[must_use] /// Transform the mesh by the given matrix. - pub fn transform(&self, mut a: Mat3A) -> Self { + pub fn transform(mut self, a: Mat3A) -> Self { debug_assert_eq!(self.ids.len() % 3, 0); - a = a.transpose(); - let vertices = self.vertices.iter().map(|&vertex| a * vertex).collect::>(); + for vertex in self.vertices.iter_mut() { + *vertex = a * *vertex; + } // for transformations that flip things // inside-out, change triangle winding - let ids = if a.determinant() < 0. { - self.ids.chunks_exact(3).flat_map(|ids| [ids[1], ids[0], ids[2]]).collect() - } else { - self.ids.clone() - }; + if a.determinant() < 0. { + for ids in self.ids.chunks_exact_mut(3) { + ids.swap(1, 2); + } + } - Self { ids, vertices } + self } #[must_use] #[inline] - /// Translate the mesh by the given vector. - pub fn translate(&self, p: Vec3A) -> Self { - Self { - ids: self.ids.clone(), - vertices: self.vertices.iter().map(|&vertex| vertex + p).collect(), + /// Translate the mesh on the z axis by the given value. + pub fn translate_z(mut self, p: f32) -> Self { + for vertex in self.vertices.iter_mut() { + vertex.z += p; + } + + self + } + + #[must_use] + #[inline] + /// Translate the mesh on the y axis by the given value. + pub fn translate_y(mut self, p: f32) -> Self { + for vertex in self.vertices.iter_mut() { + vertex.y += p; } + + self } #[must_use] /// Convert the mesh to a list of triangles. - pub fn into_triangles(self) -> Vec { + pub fn into_triangles(self) -> Box<[Tri]> { debug_assert_eq!(self.ids.len() % 3, 0); (0..self.ids.len() / 3) - .map(|i| Tri::from_points_iter((0..3).map(|j| self.vertices[self.ids[i * 3 + j]]))) + .map(|i| i * 3) + .map(|i| Tri::from_points_iter(self.ids[i..i + 3].iter().map(|&j| self.vertices[j]))) .collect() } } diff --git a/src/simulation/morton.rs b/src/simulation/morton.rs index 1f80ae2..ba0166f 100644 --- a/src/simulation/morton.rs +++ b/src/simulation/morton.rs @@ -56,9 +56,8 @@ impl Morton { } #[cfg(test)] +#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] mod test { - #![allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] - use super::Morton; use crate::simulation::geometry::Aabb; use glam::Vec3A; diff --git a/src/simulation/tri_bvh.rs b/src/simulation/tri_bvh.rs index 96b4d37..08ca846 100644 --- a/src/simulation/tri_bvh.rs +++ b/src/simulation/tri_bvh.rs @@ -2,11 +2,11 @@ use super::{ bvh::{global_aabb, Branch, Leaf, Node}, - geometry::{Aabb, Ray, Sphere, Tri}, + game::Constraints, + geometry::{Aabb, Contact, Hits, Sphere, Tri}, morton::Morton, }; use combo_vec::{re_arr, ReArr}; -use std::convert::Into; /// A bounding volume hierarchy. #[derive(Clone, Debug)] @@ -14,17 +14,17 @@ pub struct TriangleBvh { /// The root of the BVH. root: Node, /// The primitive triangles that the BVH is built from. - primitives: Vec, + primitives: Box<[Tri]>, } impl TriangleBvh { #[must_use] /// Creates a new BVH from a list of primitives. - pub fn new(primitives: Vec) -> Self { - let aabbs: Vec = primitives.iter().copied().map(Into::into).collect(); + pub fn new(primitives: Box<[Tri]>) -> Self { + let aabbs: Box<[Aabb]> = primitives.iter().copied().map(Into::into).collect(); let morton = Morton::new(global_aabb(&aabbs)); - let mut sorted_leaves: Vec<_> = aabbs.iter().map(|aabb| morton.get_code(aabb)).enumerate().collect(); + let mut sorted_leaves: Box<[_]> = aabbs.iter().map(|aabb| morton.get_code(aabb)).enumerate().collect(); radsort::sort_by_key(&mut sorted_leaves, |leaf| leaf.1); let root = Self::generate_hierarchy(&sorted_leaves, &aabbs); @@ -52,21 +52,22 @@ impl TriangleBvh { } #[must_use] - /// Returns a Vec of the collision triangles intersecting with the `query_object`. - pub fn intersect(&self, query_object: Sphere) -> Vec { + /// Returns a Vec of the collision rays and triangle normals from the triangles intersecting with the `query_object`. + pub fn collide(&self, query_object: Sphere) -> ReArr { const STACK: ReArr<&Branch, 12> = re_arr![]; + const HITS: Hits = Hits::new(); - let mut hits = Vec::with_capacity(4); + let mut hits = HITS; // Traverse nodes starting from the root // If the node is a leaf, check for intersection let mut node = match &self.root { Node::Branch(branch) => branch, Node::Leaf(leaf) => { - if let Some(contact) = self.primitives[leaf.idx].intersect_sphere(query_object) { - hits.push(contact); + if let Some(hit) = self.primitives[leaf.idx].intersect_sphere(query_object) { + hits.push(hit); } - return hits; + return hits.inner(); } }; @@ -83,8 +84,8 @@ impl TriangleBvh { if left.aabb().intersect_self(&query_box) { match left { Node::Leaf(left) => { - if let Some(contact) = self.primitives[left.idx].intersect_sphere(query_object) { - hits.push(contact); + if let Some(info) = self.primitives[left.idx].intersect_sphere(query_object) { + hits.push(info); } } Node::Branch(left) => traverse_left = Some(left), @@ -95,8 +96,8 @@ impl TriangleBvh { if right.aabb().intersect_self(&query_box) { match right { Node::Leaf(right) => { - if let Some(contact) = self.primitives[right.idx].intersect_sphere(query_object) { - hits.push(contact); + if let Some(info) = self.primitives[right.idx].intersect_sphere(query_object) { + hits.push(info); } } Node::Branch(right) => { @@ -120,42 +121,13 @@ impl TriangleBvh { }; } - hits - } - - #[must_use] - /// Returns the calculated ray-intersection of the given Sphere and the BVH. - /// Returns None if no intersecting objects were found in the BVH. - pub fn collide(&self, obj: Sphere) -> Option { - let tris_hit = self.intersect(obj); - if tris_hit.is_empty() { - return None; - } - - let mut contact_points = tris_hit.into_iter(); - - let Some(mut contact_point) = contact_points.next() else { - return None; - }; - - let mut count = 1.; - - for point in contact_points { - count += 1.; - contact_point += point; - } - - contact_point.start /= count; - contact_point.direction = contact_point.direction.normalize_or_zero(); - - Some(contact_point) + hits.inner() } } #[cfg(test)] +#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] mod test { - #![allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] - use super::*; use crate::{load_dropshot, load_hoops, load_standard, load_standard_throwback}; use criterion::black_box; @@ -177,7 +149,7 @@ mod test { [2, 3, 6], ]; - fn generate_tris() -> Vec { + fn generate_tris() -> Box<[Tri]> { let verts = &[ Vec3A::new(-4096.0, -5120.0, 0.0), Vec3A::new(-4096.0, -5120.0, 2044.0), @@ -209,29 +181,29 @@ mod test { { // Sphere hits nothing let sphere = Sphere::new(Vec3A::new(0., 0., 1022.), 100.); - let hits = bvh.intersect(sphere); + let hits = bvh.collide(sphere); assert_eq!(hits.len(), 0); } { // Sphere hits one Tri let sphere = Sphere::new(Vec3A::new(4096. / 2., 5120. / 2., 99.9), 100.); - let hits = bvh.intersect(sphere); + let hits = bvh.collide(sphere); assert_eq!(hits.len(), 1); } { // Middle of two Tris let sphere = Sphere::new(Vec3A::ZERO, 100.); - let hits = bvh.intersect(sphere); + let hits = bvh.collide(sphere); assert_eq!(hits.len(), 2); } { // Sphere is in a corner let sphere = Sphere::new(Vec3A::new(4096., 5120., 0.), 100.); - let hits = bvh.intersect(sphere); + let hits = bvh.collide(sphere); - assert_eq!(hits.len(), 5); + assert_eq!(hits.len(), 4); } } @@ -245,54 +217,70 @@ mod test { // Sphere hits nothing let sphere = Sphere::new(Vec3A::new(0., 0., 1022.), 100.); - let ray = bvh.intersect(sphere); + let ray = bvh.collide(sphere); assert!(ray.is_empty()); } { // Sphere hits one Tri - let sphere = Sphere::new(Vec3A::new(4096. / 2., 5120. / 2., 99.), 100.); + let center = Vec3A::new(4096. / 2., 5120. / 2., 99.); + let sphere = Sphere::new(center, 100.); - let rays = bvh.intersect(sphere); + let rays = bvh.collide(sphere); assert!(!rays.is_empty()); - let ray = rays[0]; + let position = rays[0].local_position + center; + dbg!(position); - assert!((ray.start.x - 2048.).abs() < f32::EPSILON); - assert!((ray.start.y - 2560.).abs() < f32::EPSILON); - assert!((ray.start.z - 0.).abs() < f32::EPSILON); - assert!((ray.direction.x - 0.0).abs() < f32::EPSILON); - assert!((ray.direction.y - 0.0).abs() < f32::EPSILON); - assert!((ray.direction.z - 1.0).abs() < f32::EPSILON); + assert!((position.x - 2048.).abs() < f32::EPSILON); + assert!((position.y - 2560.).abs() < f32::EPSILON); + assert!((position.z - -1.).abs() < f32::EPSILON); + assert!((rays[0].triangle_normal.x - 0.0).abs() < f32::EPSILON); + assert!((rays[0].triangle_normal.y - 0.0).abs() < f32::EPSILON); + assert!((rays[0].triangle_normal.z - 1.0).abs() < f32::EPSILON); } { // Middle of two Tris - let sphere = Sphere::new(Vec3A::Z, 100.); + let center = Vec3A::Z; + let sphere = Sphere::new(center, 2.); - let rays = bvh.intersect(sphere); + let rays = bvh.collide(sphere); assert!(!rays.is_empty()); - let ray = rays[0]; + let position = rays[0].local_position + center; + dbg!(position); - assert!((ray.start.x - 0.0).abs() < f32::EPSILON); - assert!((ray.start.y - 0.0).abs() < f32::EPSILON); - assert!((ray.start.z - 0.0).abs() < f32::EPSILON); - assert!((ray.direction.x - 0.0).abs() < f32::EPSILON); - assert!((ray.direction.y - 0.0).abs() < f32::EPSILON); - assert!((ray.direction.z - 1.0).abs() < f32::EPSILON); + assert!((position.x - 0.0).abs() < f32::EPSILON); + assert!((position.y - 0.0).abs() < f32::EPSILON); + assert!((position.z - -1.0).abs() < f32::EPSILON); + assert!((rays[0].triangle_normal.x - 0.0).abs() < f32::EPSILON); + assert!((rays[0].triangle_normal.y - 0.0).abs() < f32::EPSILON); + assert!((rays[0].triangle_normal.z - 1.0).abs() < f32::EPSILON); } { // Sphere is in a corner - let sphere = Sphere::new(Vec3A::new(4095., 5119., 5.), 100.); + let center = Vec3A::new(4095., 5119., 5.); + let sphere = Sphere::new(center, 6.); - let rays = bvh.intersect(sphere); + let rays = bvh.collide(sphere); assert!(!rays.is_empty()); - let first_ray = rays[0]; - dbg!(first_ray); - assert!((first_ray.start.x - 4093.8838).abs() < f32::EPSILON); - assert!((first_ray.start.y - 5120.).abs() < f32::EPSILON); - assert!((first_ray.start.z - 0.528_076_2).abs() < f32::EPSILON); + let position = rays[0].local_position + center; + dbg!(position); + + assert!((position.x - 4095.).abs() < f32::EPSILON); + assert!((position.y - 5119.).abs() < f32::EPSILON); + assert!((position.z - -1.).abs() < f32::EPSILON); + + let ray_normal = rays.iter().skip(1).fold(rays[0].triangle_normal, |mut acc, ray| { + acc += ray.triangle_normal; + acc + }); + let direction = ray_normal.normalize(); + + assert!((direction.x - -0.816_496_55).abs() < f32::EPSILON); + assert!((direction.y - -0.408_248_28).abs() < f32::EPSILON); + assert!((direction.z - 0.408_248_28).abs() < f32::EPSILON); } } @@ -304,7 +292,7 @@ mod test { { let sphere = Sphere::new(Vec3A::new(0., 0., 92.15 - f32::EPSILON), 92.15); - let ray = bvh.intersect(sphere); + let ray = bvh.collide(sphere); assert_eq!(ray.len(), 2); } diff --git a/src/uncompressed.rs b/src/uncompressed.rs index bdec4bb..137a673 100644 --- a/src/uncompressed.rs +++ b/src/uncompressed.rs @@ -31,7 +31,7 @@ pub fn load_standard() -> (Game, Ball) { "../assets/standard/standard_ramps_1_vertices.bin" ); - let triangle_collisions = initialize_standard(&standard_corner, &standard_goal, &standard_ramps_0, &standard_ramps_1); + let triangle_collisions = initialize_standard(standard_corner, standard_goal, standard_ramps_0, standard_ramps_1); (Game::new(triangle_collisions), Ball::initialize_standard()) } @@ -55,7 +55,7 @@ pub fn load_hoops() -> (Game, Ball) { "../assets/hoops/hoops_ramps_1_vertices.bin" ); - let triangle_collisions = initialize_hoops(hoops_corner, &hoops_net, &hoops_rim, hoops_ramps_0, hoops_ramps_1); + let triangle_collisions = initialize_hoops(hoops_corner, hoops_net, hoops_rim, hoops_ramps_0, hoops_ramps_1); (Game::new(triangle_collisions), Ball::initialize_hoops()) } @@ -69,7 +69,7 @@ pub fn load_dropshot() -> (Game, Ball) { "../assets/dropshot/dropshot_vertices.bin" ); - let triangle_collisions = initialize_dropshot(&dropshot); + let triangle_collisions = initialize_dropshot(dropshot); (Game::new(triangle_collisions), Ball::initialize_dropshot()) } diff --git a/tests/tests.rs b/tests/tests.rs index 20c71f9..27d11a2 100644 --- a/tests/tests.rs +++ b/tests/tests.rs @@ -89,7 +89,7 @@ fn rand_vec() -> Vec3A { fn check_for_nans_ball() { let (game, mut ball) = load_standard(); - for _ in 0..500 { + for _ in 0..100 { ball.update(0., rand_vec(), rand_vec(), rand_vec()); let ball_prediction = ball.get_ball_prediction_struct(&game);