1074 lines
36 KiB
Rust
1074 lines
36 KiB
Rust
use rapier2d::prelude::*;
|
||
|
||
use crate::protocol::*;
|
||
|
||
const MAX_SIM_TIME: f32 = 30.0;
|
||
const REST_SPEED: f32 = 0.04;
|
||
const REST_ANGULAR_SPEED: f32 = 0.05;
|
||
const G: f32 = 9.80665;
|
||
|
||
/// Initial |ω| for full curl: 5 rotations over 14 s (rad/s).
|
||
/// Sign follows curl_sign; clockwise (curl>0) uses +ω0 in spawn (see apply_curl).
|
||
pub const INITIAL_OMEGA: f32 = 5.0 * 2.0 * std::f32::consts::PI / 14.0;
|
||
|
||
/// Target spin duration (s) matching the 5-rev / 14 s design.
|
||
/// Client animation only starts at the hog (~9 s in); damping must leave
|
||
/// tangible |ω| past that, or stones look frozen on screen.
|
||
pub const SPIN_HOLD_S: f32 = 14.0;
|
||
|
||
/// Scale for lateral speed: `v_lat = CURL_LAT_K * µ(speed) * friction_scalar` (m/s).
|
||
/// Applied ⊥ **instantaneous velocity** heading as continuous normal dynamics
|
||
/// (`a_n = v_lat / CURL_LAT_TAU`, integrated each substep) so we do not stack
|
||
/// a fixed geometric rotation of atan(v_lat/v) per 1/120 s tick.
|
||
/// Calibrated so a full-curl DRAW_VELOCITY throw to the tee drifts ≈ 4 feet.
|
||
/// Clockwise curl_sign > 0 → right of velocity (+x when moving +y).
|
||
pub const CURL_LAT_K: f32 = 0.683;
|
||
|
||
/// Time constant (s) mapping target v_lat → normal acceleration: a_n = v_lat / TAU.
|
||
pub const CURL_LAT_TAU: f32 = 1.0;
|
||
|
||
/// Target lateral displacement (m) for a full-curl draw to the tee line.
|
||
#[allow(dead_code)] // used by unit tests + docs; keeps calibration goal explicit
|
||
pub const CURL_DRAW_LATERAL_M: f32 = 4.0 * FEET_TO_METERS;
|
||
|
||
/// Calibrated initial speed (m/s) for a mid draw that stops near the tee line
|
||
/// with friction_scalar = 1.0, curl = 0, broom aimed at HOUSE_CENTER.
|
||
#[allow(dead_code)] // used by unit tests / clients; sim accepts arbitrary velocity
|
||
pub const DRAW_VELOCITY: f32 = 2.38;
|
||
|
||
/// Ice friction coefficient µ as a function of speed (m/s).
|
||
/// Piecewise-linear interpolation of the binding table; µ(v≥2.5) = 0.0081.
|
||
pub fn mu(v: f32) -> f32 {
|
||
// Binding µ(v) table (v_m/s, µ)
|
||
const KNOTS: [(f32, f32); 7] = [
|
||
(0.0, 0.016),
|
||
(0.1482, 0.014),
|
||
(0.3005, 0.0116),
|
||
(0.4486, 0.0098),
|
||
(0.7371, 0.0079),
|
||
(1.0098, 0.0073),
|
||
(2.5, 0.0081),
|
||
];
|
||
|
||
let speed = v.abs();
|
||
if speed >= 2.5 {
|
||
return 0.0081;
|
||
}
|
||
for i in 0..KNOTS.len() - 1 {
|
||
let (v0, mu0) = KNOTS[i];
|
||
let (v1, mu1) = KNOTS[i + 1];
|
||
if speed >= v0 && speed <= v1 {
|
||
let t = if (v1 - v0).abs() < f32::EPSILON {
|
||
0.0
|
||
} else {
|
||
(speed - v0) / (v1 - v0)
|
||
};
|
||
return mu0 + t * (mu1 - mu0);
|
||
}
|
||
}
|
||
0.016
|
||
}
|
||
|
||
pub struct PhysicsWorld {
|
||
gravity: Vector,
|
||
integration_parameters: IntegrationParameters,
|
||
pipeline: PhysicsPipeline,
|
||
islands: IslandManager,
|
||
broad_phase: DefaultBroadPhase,
|
||
narrow_phase: NarrowPhase,
|
||
bodies: RigidBodySet,
|
||
colliders: ColliderSet,
|
||
impulse_joints: ImpulseJointSet,
|
||
multibody_joints: MultibodyJointSet,
|
||
ccd_solver: CCDSolver,
|
||
/// Next stone number per team within the current end (1..=8).
|
||
next_n_team1: u8,
|
||
next_n_team2: u8,
|
||
/// (id, handle, team, curl_sign, friction_scalar)
|
||
stone_handles: Vec<(StoneId, RigidBodyHandle, Team, i8, f32)>,
|
||
}
|
||
|
||
impl Default for PhysicsWorld {
|
||
fn default() -> Self {
|
||
Self::new()
|
||
}
|
||
}
|
||
|
||
impl PhysicsWorld {
|
||
pub fn new() -> Self {
|
||
let mut integration_parameters = IntegrationParameters::default();
|
||
integration_parameters.dt = PHYSICS_DT;
|
||
integration_parameters.num_solver_iterations = 8;
|
||
|
||
let mut world = Self {
|
||
gravity: Vector::new(0.0, 0.0),
|
||
integration_parameters,
|
||
pipeline: PhysicsPipeline::new(),
|
||
islands: IslandManager::new(),
|
||
broad_phase: DefaultBroadPhase::new(),
|
||
narrow_phase: NarrowPhase::new(),
|
||
bodies: RigidBodySet::new(),
|
||
colliders: ColliderSet::new(),
|
||
impulse_joints: ImpulseJointSet::new(),
|
||
multibody_joints: MultibodyJointSet::new(),
|
||
ccd_solver: CCDSolver::new(),
|
||
next_n_team1: 1,
|
||
next_n_team2: 1,
|
||
stone_handles: Vec::new(),
|
||
};
|
||
world.build_sheet();
|
||
world
|
||
}
|
||
|
||
pub fn reset(&mut self) {
|
||
self.bodies = RigidBodySet::new();
|
||
self.colliders = ColliderSet::new();
|
||
self.islands = IslandManager::new();
|
||
self.broad_phase = DefaultBroadPhase::new();
|
||
self.narrow_phase = NarrowPhase::new();
|
||
self.impulse_joints = ImpulseJointSet::new();
|
||
self.multibody_joints = MultibodyJointSet::new();
|
||
self.ccd_solver = CCDSolver::new();
|
||
self.stone_handles.clear();
|
||
self.build_sheet();
|
||
}
|
||
|
||
/// Reset per-team stone numbers for a new end (n starts at 1 again).
|
||
pub fn reset_stone_ids(&mut self) {
|
||
self.next_n_team1 = 1;
|
||
self.next_n_team2 = 1;
|
||
}
|
||
|
||
/// No wall colliders: stones leave play via prune_out_of_play only.
|
||
fn build_sheet(&mut self) {
|
||
// Intentionally empty — open boundaries (no left/right/back bounce).
|
||
}
|
||
|
||
fn alloc_stone_id(&mut self, team: Team) -> Result<StoneId, String> {
|
||
let n = match team {
|
||
Team::Team1 => self.next_n_team1,
|
||
Team::Team2 => self.next_n_team2,
|
||
};
|
||
if n > STONES_PER_TEAM {
|
||
return Err(format!("no stones remaining for {}", team));
|
||
}
|
||
match team {
|
||
Team::Team1 => self.next_n_team1 = n.saturating_add(1),
|
||
Team::Team2 => self.next_n_team2 = n.saturating_add(1),
|
||
}
|
||
Ok(StoneId { team, n })
|
||
}
|
||
|
||
/// Throw a stone with pure initial velocity (m/s).
|
||
/// `friction_scalar` is clamped to 0.5..=1.5 and multiplies µ.
|
||
pub fn throw(
|
||
&mut self,
|
||
team: Team,
|
||
broom_x: f32,
|
||
broom_y: f32,
|
||
velocity: f32,
|
||
curl: i8,
|
||
friction_scalar: f32,
|
||
) -> Result<Vec<StonePath>, String> {
|
||
let speed = velocity.max(0.0);
|
||
let dx = broom_x;
|
||
let dy = broom_y - HACK_Y;
|
||
let len = (dx * dx + dy * dy).sqrt().max(0.01);
|
||
let vx = dx / len * speed;
|
||
let vy = dy / len * speed;
|
||
|
||
let curl_sign = if curl < 0 {
|
||
-1
|
||
} else if curl > 0 {
|
||
1
|
||
} else {
|
||
0
|
||
};
|
||
let friction_scalar = friction_scalar.clamp(0.5, 1.5);
|
||
|
||
self.spawn_stone(team, 0.0, HACK_Y, vx, vy, curl_sign, friction_scalar)
|
||
}
|
||
|
||
fn spawn_stone(
|
||
&mut self,
|
||
team: Team,
|
||
x: f32,
|
||
y: f32,
|
||
vx: f32,
|
||
vy: f32,
|
||
curl_sign: i8,
|
||
friction_scalar: f32,
|
||
) -> Result<Vec<StonePath>, String> {
|
||
let id = self.alloc_stone_id(team)?;
|
||
|
||
// Clockwise curl (curl_sign > 0) → positive ω0; lateral model maps that to +x.
|
||
let omega0 = curl_sign as f32 * INITIAL_OMEGA;
|
||
let body = RigidBodyBuilder::dynamic()
|
||
.translation(Vector::new(x, y))
|
||
.linvel(Vector::new(vx, vy))
|
||
.angvel(omega0)
|
||
.linear_damping(0.0)
|
||
.angular_damping(0.0)
|
||
.ccd_enabled(true)
|
||
.can_sleep(false)
|
||
.build();
|
||
|
||
let handle = self.bodies.insert(body);
|
||
|
||
let collider = ColliderBuilder::ball(STONE_RADIUS)
|
||
.friction(STONE_FRICTION)
|
||
.friction_combine_rule(CoefficientCombineRule::Average)
|
||
.restitution(STONE_RESTITUTION)
|
||
.restitution_combine_rule(CoefficientCombineRule::Average)
|
||
.density(STONE_MASS / (std::f32::consts::PI * STONE_RADIUS * STONE_RADIUS))
|
||
.build();
|
||
|
||
self.colliders
|
||
.insert_with_parent(collider, handle, &mut self.bodies);
|
||
|
||
self.stone_handles
|
||
.push((id, handle, team, curl_sign, friction_scalar));
|
||
|
||
self.simulate_until_rest(id)
|
||
}
|
||
|
||
fn simulate_until_rest(&mut self, _thrown_id: StoneId) -> Result<Vec<StonePath>, String> {
|
||
// Path samples are [x, y, theta]. Client time is sample_index / SAMPLE_RATE_HZ.
|
||
// All stones share the same sample clock from the thrown stone's release.
|
||
let sample_step = 1.0 / SAMPLE_RATE_HZ as f32;
|
||
let mut sample_accum: f32 = 0.0;
|
||
let mut time: f32 = 0.0;
|
||
|
||
// Pre-allocate a path buffer for every stone currently in the world.
|
||
let mut paths: Vec<(StoneId, Team, RigidBodyHandle, Vec<[f32; 3]>)> = self
|
||
.stone_handles
|
||
.iter()
|
||
.map(|(id, handle, team, _, _)| (*id, *team, *handle, Vec::new()))
|
||
.collect();
|
||
|
||
// Record the initial sample for every stone.
|
||
for (id, _, handle, path) in &mut paths {
|
||
if let Some(body) = self.bodies.get(*handle) {
|
||
let pos = body.translation();
|
||
let theta = body.rotation().angle();
|
||
path.push([pos.x, pos.y, theta]);
|
||
} else {
|
||
return Err(format!("stone {:?}/{} has no rigid body", id.team, id.n));
|
||
}
|
||
}
|
||
|
||
loop {
|
||
self.step();
|
||
self.apply_ice_friction();
|
||
self.apply_curl();
|
||
time += PHYSICS_DT;
|
||
sample_accum += PHYSICS_DT;
|
||
|
||
if sample_accum >= sample_step {
|
||
sample_accum -= sample_step;
|
||
for (_, _, handle, path) in &mut paths {
|
||
if let Some(body) = self.bodies.get(*handle) {
|
||
let pos = body.translation();
|
||
let theta = body.rotation().angle();
|
||
// Don't grow client animation paths once the stone is clearly off-sheet.
|
||
if Self::position_clearly_out_of_play(pos.x, pos.y) {
|
||
continue;
|
||
}
|
||
path.push([pos.x, pos.y, theta]);
|
||
}
|
||
}
|
||
}
|
||
|
||
if self.all_stones_settled_or_out() || time > MAX_SIM_TIME {
|
||
break;
|
||
}
|
||
}
|
||
|
||
self.prune_out_of_play();
|
||
|
||
Ok(paths
|
||
.into_iter()
|
||
.map(|(id, team, handle, trajectory)| {
|
||
let rotation = self
|
||
.bodies
|
||
.get(handle)
|
||
.map(|b| b.rotation().angle())
|
||
.or_else(|| trajectory.last().map(|s| s[2]))
|
||
.unwrap_or(0.0);
|
||
StonePath {
|
||
stone_id: id,
|
||
rotation,
|
||
team,
|
||
trajectory,
|
||
}
|
||
})
|
||
.collect())
|
||
}
|
||
|
||
/// Apply a = −µ_eff * g * unit(v) after each physics step.
|
||
/// µ_eff = mu(|v|) * friction_scalar. If velocity would reverse, stop.
|
||
fn apply_ice_friction(&mut self) {
|
||
for (_, handle, _, _, friction_scalar) in &self.stone_handles {
|
||
let body = match self.bodies.get_mut(*handle) {
|
||
Some(b) => b,
|
||
None => continue,
|
||
};
|
||
|
||
let v = body.linvel();
|
||
let speed = (v.x * v.x + v.y * v.y).sqrt();
|
||
if speed < 1e-6 {
|
||
body.set_linvel(Vector::new(0.0, 0.0), true);
|
||
continue;
|
||
}
|
||
|
||
let mu_eff = mu(speed) * *friction_scalar;
|
||
let a = mu_eff * G;
|
||
let dv = a * PHYSICS_DT;
|
||
if dv >= speed {
|
||
body.set_linvel(Vector::new(0.0, 0.0), true);
|
||
} else {
|
||
let scale = (speed - dv) / speed;
|
||
body.set_linvel(Vector::new(v.x * scale, v.y * scale), true);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Spin-curl model after drag:
|
||
/// - Angular damping designed for ~SPIN_HOLD_S hold (not µmg/R, which killed
|
||
/// spin in ~4 s — before the client ever drew the stone past the hog)
|
||
/// - Instantaneous velocity heading; right = CW perp (uy, -ux)
|
||
/// - v_lat = curl_sign * CURL_LAT_K * µ(speed) * friction_scalar
|
||
/// - Continuous normal dynamics: a_n = v_lat / CURL_LAT_TAU, v += a_n * right * dt
|
||
/// - Clockwise curl_sign > 0 → right of velocity (+x when moving +y)
|
||
fn apply_curl(&mut self) {
|
||
const MIN_CURL_SPEED: f32 = 0.08;
|
||
|
||
for (_, handle, _, curl_sign, friction_scalar) in &self.stone_handles {
|
||
let body = match self.bodies.get_mut(*handle) {
|
||
Some(b) => b,
|
||
None => continue,
|
||
};
|
||
|
||
let v = body.linvel();
|
||
let speed = (v.x * v.x + v.y * v.y).sqrt();
|
||
|
||
// Decay |ω| so it lasts ~SPIN_HOLD_S at friction_scalar=1; scale by scalar.
|
||
// Old α = µ g / R wiped spin pre-hog so FE never showed rotation.
|
||
let omega = body.angvel();
|
||
if speed < REST_SPEED {
|
||
body.set_angvel(0.0, true);
|
||
} else if omega.abs() > 1e-8 {
|
||
let alpha = (INITIAL_OMEGA / SPIN_HOLD_S) * *friction_scalar;
|
||
let domega = alpha * PHYSICS_DT;
|
||
let new_omega = if domega >= omega.abs() {
|
||
0.0
|
||
} else {
|
||
omega - omega.signum() * domega
|
||
};
|
||
body.set_angvel(new_omega, true);
|
||
}
|
||
|
||
if *curl_sign == 0 || speed < MIN_CURL_SPEED {
|
||
continue;
|
||
}
|
||
|
||
// Instantaneous velocity heading and body-right (CW 90°).
|
||
let ux = v.x / speed;
|
||
let uy = v.y / speed;
|
||
let rx = uy;
|
||
let ry = -ux;
|
||
|
||
// v_lat = k * µ(speed) * friction_scalar (same µ table as ice friction).
|
||
let v_lat =
|
||
(*curl_sign as f32) * CURL_LAT_K * mu(speed) * *friction_scalar;
|
||
// Continuous: a_n = v_lat / τ → integrates without per-tick geometric stack.
|
||
let a_n = v_lat / CURL_LAT_TAU;
|
||
body.set_linvel(
|
||
Vector::new(v.x + rx * a_n * PHYSICS_DT, v.y + ry * a_n * PHYSICS_DT),
|
||
true,
|
||
);
|
||
}
|
||
}
|
||
|
||
fn prune_out_of_play(&mut self) {
|
||
let mut keep = Vec::new();
|
||
for (id, handle, team, curl, friction_scalar) in self.stone_handles.drain(..) {
|
||
if let Some(body) = self.bodies.get(handle) {
|
||
let pos = body.translation();
|
||
let beyond_back = pos.y > BACK_LINE_Y;
|
||
let short_of_hog = pos.y < HOG_LINE_Y;
|
||
let outside = pos.x.abs() > SHEET_WIDTH / 2.0;
|
||
if beyond_back || short_of_hog || outside {
|
||
self.bodies.remove(
|
||
handle,
|
||
&mut self.islands,
|
||
&mut self.colliders,
|
||
&mut self.impulse_joints,
|
||
&mut self.multibody_joints,
|
||
true,
|
||
);
|
||
} else {
|
||
keep.push((id, handle, team, curl, friction_scalar));
|
||
}
|
||
}
|
||
}
|
||
self.stone_handles = keep;
|
||
}
|
||
|
||
fn step(&mut self) {
|
||
self.pipeline.step(
|
||
self.gravity,
|
||
&self.integration_parameters,
|
||
&mut self.islands,
|
||
&mut self.broad_phase,
|
||
&mut self.narrow_phase,
|
||
&mut self.bodies,
|
||
&mut self.colliders,
|
||
&mut self.impulse_joints,
|
||
&mut self.multibody_joints,
|
||
&mut self.ccd_solver,
|
||
&(),
|
||
&(),
|
||
);
|
||
}
|
||
|
||
fn position_clearly_out_of_play(x: f32, y: f32) -> bool {
|
||
y > BACK_LINE_Y || x.abs() > SHEET_WIDTH / 2.0
|
||
}
|
||
|
||
/// End sim when every stone is at rest or already past back/sidelines.
|
||
/// Avoids MAX_SIM_TIME client animations for long overthrows.
|
||
fn all_stones_settled_or_out(&self) -> bool {
|
||
for (_, handle, _, _, _) in &self.stone_handles {
|
||
if let Some(body) = self.bodies.get(*handle) {
|
||
let pos = body.translation();
|
||
if Self::position_clearly_out_of_play(pos.x, pos.y) {
|
||
continue;
|
||
}
|
||
let v = body.linvel();
|
||
let speed = (v.x * v.x + v.y * v.y).sqrt();
|
||
if speed > REST_SPEED || body.angvel().abs() > REST_ANGULAR_SPEED {
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
true
|
||
}
|
||
|
||
pub fn current_stones(&self) -> Vec<StoneState> {
|
||
let mut states = Vec::new();
|
||
for (id, handle, team, _, _) in &self.stone_handles {
|
||
if let Some(body) = self.bodies.get(*handle) {
|
||
let pos = body.translation();
|
||
states.push(StoneState {
|
||
id: *id,
|
||
team: *team,
|
||
x: pos.x,
|
||
y: pos.y,
|
||
rotation: body.rotation().angle(),
|
||
});
|
||
}
|
||
}
|
||
states
|
||
}
|
||
|
||
pub fn stone_states_for_scoring(&self) -> Vec<(StoneId, Team, f32, f32)> {
|
||
let mut out = Vec::new();
|
||
for (id, handle, team, _, _) in &self.stone_handles {
|
||
if let Some(body) = self.bodies.get(*handle) {
|
||
let pos = body.translation();
|
||
out.push((*id, *team, pos.x, pos.y));
|
||
}
|
||
}
|
||
out
|
||
}
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
fn final_y(world: &PhysicsWorld, id: StoneId) -> f32 {
|
||
world
|
||
.stone_handles
|
||
.iter()
|
||
.find(|(sid, _, _, _, _)| *sid == id)
|
||
.map(|(_, h, _, _, _)| {
|
||
let b = &world.bodies[*h];
|
||
b.translation().y
|
||
})
|
||
.unwrap_or(f32::NAN)
|
||
}
|
||
|
||
fn final_x(world: &PhysicsWorld, id: StoneId) -> f32 {
|
||
world
|
||
.stone_handles
|
||
.iter()
|
||
.find(|(sid, _, _, _, _)| *sid == id)
|
||
.map(|(_, h, _, _, _)| {
|
||
let b = &world.bodies[*h];
|
||
b.translation().x
|
||
})
|
||
.unwrap_or(f32::NAN)
|
||
}
|
||
|
||
fn last_thrown_id(world: &PhysicsWorld, team: Team) -> StoneId {
|
||
world
|
||
.stone_handles
|
||
.iter()
|
||
.rev()
|
||
.find(|(_, _, t, _, _)| *t == team)
|
||
.map(|(id, _, _, _, _)| *id)
|
||
.unwrap_or_else(|| {
|
||
// Pruned: reconstruct from counters (last allocated n - 1)
|
||
let n = match team {
|
||
Team::Team1 => world.next_n_team1.saturating_sub(1),
|
||
Team::Team2 => world.next_n_team2.saturating_sub(1),
|
||
};
|
||
StoneId { team, n }
|
||
})
|
||
}
|
||
|
||
#[test]
|
||
fn mu_at_rest_is_0_016() {
|
||
assert!((mu(0.0) - 0.016).abs() < 1e-6);
|
||
}
|
||
|
||
#[test]
|
||
fn mu_interpolates_between_knots() {
|
||
// Midpoint between 0.1482 (0.014) and 0.3005 (0.0116)
|
||
let v = (0.1482 + 0.3005) / 2.0;
|
||
let expected = (0.014 + 0.0116) / 2.0;
|
||
let got = mu(v);
|
||
assert!(
|
||
(got - expected).abs() < 1e-5,
|
||
"mu({}) = {}, expected ~{}",
|
||
v,
|
||
got,
|
||
expected
|
||
);
|
||
// High-speed plateau
|
||
assert!((mu(2.5) - 0.0081).abs() < 1e-6);
|
||
assert!((mu(5.0) - 0.0081).abs() < 1e-6);
|
||
// Exact knot
|
||
assert!((mu(1.0098) - 0.0073).abs() < 1e-6);
|
||
}
|
||
|
||
#[test]
|
||
fn velocity_draw_lands_on_tee_line() {
|
||
let mut world = PhysicsWorld::new();
|
||
// curl=0 so lateral drift does not push the stone OOB before rest.
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
let id = last_thrown_id(&world, Team::Team1);
|
||
let y = final_y(&world, id);
|
||
println!("DRAW_VELOCITY={} final y={}", DRAW_VELOCITY, y);
|
||
assert!(
|
||
(y - HOUSE_CENTER.1).abs() <= 0.8,
|
||
"draw shot should finish near tee line, got y={} (tee={})",
|
||
y,
|
||
HOUSE_CENTER.1
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn high_friction_or_low_v_prunes_before_hog() {
|
||
// Low velocity + high friction_scalar ⇒ short of hog, pruned.
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, 1.0, 0, 1.5)
|
||
.unwrap();
|
||
let stones = world.current_stones();
|
||
assert!(
|
||
stones.is_empty(),
|
||
"low-v high-friction throw should be pruned short of hog"
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn sideline_aim_goes_out_not_bounce() {
|
||
// Aim so the stone crosses |x| > SHEET_WIDTH/2; with open boundaries it
|
||
// must be pruned (not bounce off a wall and remain in play).
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 4.0, 15.0, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
let stones = world.current_stones();
|
||
assert!(
|
||
stones.is_empty(),
|
||
"sideline-bound stone should be pruned, not bounce; remaining={:?}",
|
||
stones
|
||
.iter()
|
||
.map(|s| (s.x, s.y))
|
||
.collect::<Vec<_>>()
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn curl_direction_mirrors_x_offset() {
|
||
// Use trajectory last sample (pre-prune): strong curl can exit the sheet.
|
||
let mut right = PhysicsWorld::new();
|
||
let right_traj = right
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
|
||
.unwrap();
|
||
let right_x = right_traj[0]
|
||
.trajectory
|
||
.last()
|
||
.map(|p| p[0])
|
||
.unwrap_or(f32::NAN);
|
||
|
||
let mut left = PhysicsWorld::new();
|
||
let left_traj = left
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, -1, 1.0)
|
||
.unwrap();
|
||
let left_x = left_traj[0]
|
||
.trajectory
|
||
.last()
|
||
.map(|p| p[0])
|
||
.unwrap_or(f32::NAN);
|
||
|
||
println!("right curl final x={} left curl final x={}", right_x, left_x);
|
||
assert!(
|
||
right_x > left_x + 0.05,
|
||
"right curl should finish to the right of left curl: right={} left={}",
|
||
right_x,
|
||
left_x
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn initial_angvel_magnitude_matches_5_rot_per_14s() {
|
||
let expected = 5.0 * 2.0 * std::f32::consts::PI / 14.0;
|
||
assert!(
|
||
(INITIAL_OMEGA - expected).abs() < 1e-5,
|
||
"INITIAL_OMEGA={} expected {}",
|
||
INITIAL_OMEGA,
|
||
expected
|
||
);
|
||
|
||
// Early path dθ/dt should be near |ω0| before damping eats much spin.
|
||
let mut world = PhysicsWorld::new();
|
||
let traj = world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
|
||
.unwrap();
|
||
let path = &traj[0].trajectory;
|
||
assert!(path.len() >= 3, "need samples to estimate ω");
|
||
let dt = 1.0 / SAMPLE_RATE_HZ as f32;
|
||
let omega_est = (path[1][2] - path[0][2]) / dt;
|
||
assert!(
|
||
(omega_est.abs() - expected).abs() < expected * 0.35,
|
||
"early |ω|≈{} should be near {} (5 rot / 14s)",
|
||
omega_est.abs(),
|
||
expected
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn clockwise_curl_moves_right() {
|
||
// Clockwise curl (curl > 0) must finish to the right of counterclockwise.
|
||
let mut cw = PhysicsWorld::new();
|
||
let cw_traj = cw
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
|
||
.unwrap();
|
||
let right_x = cw_traj[0]
|
||
.trajectory
|
||
.last()
|
||
.map(|p| p[0])
|
||
.unwrap_or(f32::NAN);
|
||
|
||
let mut ccw = PhysicsWorld::new();
|
||
let ccw_traj = ccw
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, -1, 1.0)
|
||
.unwrap();
|
||
let left_x = ccw_traj[0]
|
||
.trajectory
|
||
.last()
|
||
.map(|p| p[0])
|
||
.unwrap_or(f32::NAN);
|
||
|
||
println!(
|
||
"clockwise final x={} counterclockwise final x={}",
|
||
right_x, left_x
|
||
);
|
||
assert!(
|
||
right_x > left_x + 0.05,
|
||
"clockwise curl should move right: right_x={} left_x={}",
|
||
right_x,
|
||
left_x
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn path_samples_include_nonzero_theta_when_spinning() {
|
||
let mut world = PhysicsWorld::new();
|
||
let traj = world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
|
||
.unwrap();
|
||
let path = &traj[0].trajectory;
|
||
let max_abs_theta = path
|
||
.iter()
|
||
.map(|s| s[2].abs())
|
||
.fold(0.0_f32, f32::max);
|
||
assert!(
|
||
max_abs_theta > 0.05,
|
||
"spinning stone path should include nonzero theta, max|θ|={}",
|
||
max_abs_theta
|
||
);
|
||
}
|
||
|
||
/// FE trims trajectories to the hog; spin must still change θ after that.
|
||
#[test]
|
||
fn theta_keeps_changing_after_hog_when_curling() {
|
||
let mut world = PhysicsWorld::new();
|
||
let traj = world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
|
||
.unwrap();
|
||
let path = &traj[0].trajectory;
|
||
let post_hog: Vec<[f32; 3]> = path
|
||
.iter()
|
||
.copied()
|
||
.filter(|s| s[1] >= HOG_LINE_Y)
|
||
.collect();
|
||
assert!(
|
||
post_hog.len() > 10,
|
||
"need a post-hog path to animate, got {}",
|
||
post_hog.len()
|
||
);
|
||
|
||
// Unwrap sample-to-sample Δθ (Rapier angle is in [-π, π]).
|
||
let mut travel = 0.0_f32;
|
||
let mut prev = post_hog[0][2];
|
||
for s in post_hog.iter().skip(1) {
|
||
let mut d = s[2] - prev;
|
||
if d > std::f32::consts::PI {
|
||
d -= 2.0 * std::f32::consts::PI;
|
||
}
|
||
if d < -std::f32::consts::PI {
|
||
d += 2.0 * std::f32::consts::PI;
|
||
}
|
||
travel += d.abs();
|
||
prev = s[2];
|
||
}
|
||
assert!(
|
||
travel > 0.75,
|
||
"stone should rotate past the hog (client-visible), |Δθ|sum={travel} rad"
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn stones_persist_after_multiple_throws() {
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
// Slight lateral aim so stones don't stack identically; still in-bounds.
|
||
world
|
||
.throw(Team::Team1, 0.3, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
|
||
let stones = world.current_stones();
|
||
assert_eq!(stones.len(), 2, "both stones should remain in the physics world");
|
||
assert_eq!(
|
||
stones[0].id,
|
||
StoneId {
|
||
team: Team::Team1,
|
||
n: 1
|
||
}
|
||
);
|
||
assert_eq!(
|
||
stones[1].id,
|
||
StoneId {
|
||
team: Team::Team1,
|
||
n: 2
|
||
}
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn stone_ids_are_per_team_and_reset_each_end() {
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
world
|
||
.throw(Team::Team2, 0.2, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
let stones = world.current_stones();
|
||
let t1 = stones.iter().find(|s| s.team == Team::Team1).unwrap();
|
||
let t2 = stones.iter().find(|s| s.team == Team::Team2).unwrap();
|
||
assert_eq!(t1.id.n, 1);
|
||
assert_eq!(t2.id.n, 1);
|
||
|
||
world.reset();
|
||
world.reset_stone_ids();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
let again = world.current_stones();
|
||
assert_eq!(again[0].id.n, 1, "stone numbers reset each end");
|
||
}
|
||
|
||
#[test]
|
||
fn out_of_play_stone_is_pruned() {
|
||
// A very slow, high-friction throw should stop short of the hog line and be removed.
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, 0.8, 0, 1.5)
|
||
.unwrap();
|
||
let stones = world.current_stones();
|
||
assert!(
|
||
stones.is_empty(),
|
||
"stones short of the hog line should be pruned"
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn collision_records_trajectories_for_both_stones() {
|
||
// Place a stationary stone on the center line and throw a second stone
|
||
// straight at it so they collide. Both stones must have sampled paths.
|
||
let takeout_v = DRAW_VELOCITY * 1.4;
|
||
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
let first_id = last_thrown_id(&world, Team::Team1);
|
||
|
||
let target_y = final_y(&world, first_id);
|
||
let target_x = final_x(&world, first_id);
|
||
world
|
||
.throw(Team::Team2, target_x, target_y, takeout_v, 0, 1.0)
|
||
.unwrap();
|
||
let second_id = last_thrown_id(&world, Team::Team2);
|
||
|
||
// Re-run the collision throw and capture trajectories.
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
let first_id = last_thrown_id(&world, Team::Team1);
|
||
let target_y = final_y(&world, first_id);
|
||
let target_x = final_x(&world, first_id);
|
||
let trajectories = world
|
||
.throw(Team::Team2, target_x, target_y, takeout_v, 0, 1.0)
|
||
.unwrap();
|
||
|
||
let by_id: std::collections::HashMap<StoneId, Vec<[f32; 3]>> = trajectories
|
||
.into_iter()
|
||
.map(|st| (st.stone_id, st.trajectory))
|
||
.collect();
|
||
|
||
assert!(
|
||
by_id.contains_key(&first_id),
|
||
"trajectories should contain the first stone (id={:?})",
|
||
first_id
|
||
);
|
||
assert!(
|
||
by_id.contains_key(&second_id),
|
||
"trajectories should contain the thrown stone (id={:?})",
|
||
second_id
|
||
);
|
||
|
||
let first_path = by_id.get(&first_id).unwrap();
|
||
let second_path = by_id.get(&second_id).unwrap();
|
||
assert!(
|
||
first_path.len() > 1,
|
||
"first stone path should have multiple samples, got {}",
|
||
first_path.len()
|
||
);
|
||
assert!(
|
||
second_path.len() > 1,
|
||
"thrown stone path should have multiple samples, got {}",
|
||
second_path.len()
|
||
);
|
||
|
||
// Paths are [x, y, theta]; both stones must have a sample at release (index 0).
|
||
assert!(
|
||
first_path[0][2].is_finite(),
|
||
"first stone path should include finite theta"
|
||
);
|
||
assert!(
|
||
second_path[0][2].is_finite(),
|
||
"thrown stone path should include finite theta"
|
||
);
|
||
}
|
||
|
||
/// Head-on takeout with nearly elastic restitution must launch the sitters
|
||
/// and keep both moving along the impact (down-sheet) direction — not a
|
||
/// plastic "stick and dump" limp.
|
||
#[test]
|
||
fn near_elastic_takeout_launches_both_downsheet() {
|
||
let takeout_v = DRAW_VELOCITY * 1.6;
|
||
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
let first_id = last_thrown_id(&world, Team::Team1);
|
||
let rest_x = final_x(&world, first_id);
|
||
let rest_y = final_y(&world, first_id);
|
||
|
||
let trajectories = world
|
||
.throw(Team::Team2, rest_x, rest_y, takeout_v, 0, 1.0)
|
||
.unwrap();
|
||
let second_id = last_thrown_id(&world, Team::Team2);
|
||
|
||
let by_id: std::collections::HashMap<StoneId, Vec<[f32; 3]>> = trajectories
|
||
.into_iter()
|
||
.map(|st| (st.stone_id, st.trajectory))
|
||
.collect();
|
||
|
||
let first_path = by_id.get(&first_id).expect("struck stone path");
|
||
let second_path = by_id.get(&second_id).expect("shooter path");
|
||
|
||
let first_start_y = first_path[0][1];
|
||
let first_max_y = first_path.iter().map(|s| s[1]).fold(f32::NEG_INFINITY, f32::max);
|
||
let first_launch = first_max_y - first_start_y;
|
||
|
||
// Inelastic e≈0.05 only nudges the sitters; nearly elastic takes them meters.
|
||
assert!(
|
||
first_launch > 1.5,
|
||
"struck stone should be launched down-sheet, launch={first_launch} rest_y={rest_y}"
|
||
);
|
||
|
||
// Both should still be moving +y at some point after contact (sample peak
|
||
// leftmost/rightmost velocity proxy: later samples farther down than early).
|
||
let second_start_y = second_path[0][1];
|
||
let second_max_y = second_path.iter().map(|s| s[1]).fold(f32::NEG_INFINITY, f32::max);
|
||
assert!(
|
||
second_max_y > second_start_y + 10.0,
|
||
"shooter must travel down-sheet, Δy={}",
|
||
second_max_y - second_start_y
|
||
);
|
||
|
||
// Impact direction is primarily +y; struck stone's net lateral drift after
|
||
// a head-on should stay small compared to longitudinal launch.
|
||
let first_end = first_path.last().expect("non-empty struck path");
|
||
let lateral = (first_end[0] - rest_x).abs();
|
||
assert!(
|
||
lateral < first_launch * 0.5,
|
||
"head-on should keep both mostly along impact axis: lateral={lateral} launch={first_launch}"
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn stone_path_samples_are_xyz_arrays() {
|
||
let mut world = PhysicsWorld::new();
|
||
let paths = world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 0, 1.0)
|
||
.unwrap();
|
||
assert_eq!(paths[0].stone_id.n, 1);
|
||
assert_eq!(paths[0].team, Team::Team1);
|
||
assert!(!paths[0].trajectory.is_empty());
|
||
let sample = paths[0].trajectory[0];
|
||
assert_eq!(sample.len(), 3);
|
||
assert!(sample[0].is_finite() && sample[1].is_finite() && sample[2].is_finite());
|
||
}
|
||
|
||
#[test]
|
||
fn weight5_ui_velocity_leaves_stone_in_play() {
|
||
let v = crate::protocol::MIN_SPEED
|
||
+ 4.0 / 9.0 * (crate::protocol::MAX_SPEED - crate::protocol::MIN_SPEED);
|
||
assert!(
|
||
(v - DRAW_VELOCITY).abs() < 0.02,
|
||
"weight-5 velocity {v} should ≈ DRAW_VELOCITY {DRAW_VELOCITY}"
|
||
);
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, v, 0, 1.0)
|
||
.unwrap();
|
||
assert_eq!(world.current_stones().len(), 1);
|
||
}
|
||
|
||
#[test]
|
||
fn min_ui_speed_reaches_past_hog() {
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(
|
||
Team::Team1,
|
||
0.0,
|
||
HOUSE_CENTER.1,
|
||
crate::protocol::MIN_SPEED,
|
||
0,
|
||
1.0,
|
||
)
|
||
.unwrap();
|
||
let stones = world.current_stones();
|
||
assert_eq!(stones.len(), 1);
|
||
assert!(stones[0].y >= HOG_LINE_Y);
|
||
}
|
||
|
||
#[test]
|
||
fn full_curl_draw_stays_on_sheet() {
|
||
// UI default was curl=±1; old k/v model pruned every curled throw.
|
||
let v = crate::protocol::MIN_SPEED
|
||
+ 4.0 / 9.0 * (crate::protocol::MAX_SPEED - crate::protocol::MIN_SPEED);
|
||
for curl in [1i8, -1] {
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, v, curl, 1.0)
|
||
.unwrap();
|
||
let stones = world.current_stones();
|
||
assert_eq!(
|
||
stones.len(),
|
||
1,
|
||
"curl={curl} must leave a stone in play, got {}",
|
||
stones.len()
|
||
);
|
||
assert!(stones[0].y >= HOG_LINE_Y && stones[0].y <= BACK_LINE_Y);
|
||
assert!(stones[0].x.abs() <= SHEET_WIDTH / 2.0);
|
||
}
|
||
}
|
||
|
||
#[test]
|
||
fn draw_to_tee_full_curl_drifts_four_feet() {
|
||
// v_lat = CURL_LAT_K * µ(v); k calibrated so |x| ≈ 4 ft on a tee-line draw.
|
||
let mut world = PhysicsWorld::new();
|
||
world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, 1, 1.0)
|
||
.unwrap();
|
||
let stones = world.current_stones();
|
||
assert_eq!(stones.len(), 1);
|
||
let s = &stones[0];
|
||
assert!(
|
||
(s.y - HOUSE_CENTER.1).abs() < 1.0,
|
||
"should stop near tee line, y={}",
|
||
s.y
|
||
);
|
||
assert!(
|
||
(s.x - CURL_DRAW_LATERAL_M).abs() < 0.25,
|
||
"full curl should drift ~4 ft ({} m), got x={} m ({:.2} ft)",
|
||
CURL_DRAW_LATERAL_M,
|
||
s.x,
|
||
s.x / FEET_TO_METERS
|
||
);
|
||
// Opposite curl is mirror-image.
|
||
let mut world2 = PhysicsWorld::new();
|
||
world2
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, DRAW_VELOCITY, -1, 1.0)
|
||
.unwrap();
|
||
let s2 = &world2.current_stones()[0];
|
||
assert!(
|
||
(s2.x + CURL_DRAW_LATERAL_M).abs() < 0.25,
|
||
"ccw curl should drift ~-4 ft, got x={}",
|
||
s2.x
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn fast_overshoot_does_not_run_full_max_sim_path() {
|
||
let mut world = PhysicsWorld::new();
|
||
let paths = world
|
||
.throw(Team::Team1, 0.0, HOUSE_CENTER.1, 4.0, 0, 1.0)
|
||
.unwrap();
|
||
let n = paths[0].trajectory.len();
|
||
assert!(
|
||
n < 900,
|
||
"overshoot path should end when past back line, got {n} samples"
|
||
);
|
||
assert!(world.current_stones().is_empty());
|
||
}
|
||
}
|
||
|