diff --git a/Cargo.lock b/Cargo.lock index e242307..178f8ae 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2107,6 +2107,7 @@ checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" name = "strafe-client" version = "0.10.5" dependencies = [ + "arrayvec", "bytemuck", "configparser", "ddsfile", diff --git a/Cargo.toml b/Cargo.toml index b73148d..5e5f634 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,6 +15,7 @@ source = ["dep:strafesnet_deferred_loader", "dep:strafesnet_bsp_loader"] roblox = ["dep:strafesnet_deferred_loader", "dep:strafesnet_rbx_loader"] [dependencies] +arrayvec = "0.7.6" bytemuck = { version = "1.13.1", features = ["derive"] } configparser = "3.0.2" ddsfile = "0.5.1" diff --git a/src/main.rs b/src/main.rs index 337ba79..d779781 100644 --- a/src/main.rs +++ b/src/main.rs @@ -5,6 +5,7 @@ mod worker; mod physics; mod graphics; mod settings; +mod push_solve; mod face_crawler; mod compat_worker; mod model_physics; diff --git a/src/physics.rs b/src/physics.rs index eab5889..62cf46f 100644 --- a/src/physics.rs +++ b/src/physics.rs @@ -766,23 +766,31 @@ impl TouchingState{ a } fn constrain_velocity(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,velocity:&mut Planar64Vec3){ - //TODO: trey push solve - for contact in &self.contacts{ + let contacts=self.contacts.iter().map(|contact|{ let n=contact_normal(models,hitbox_mesh,contact); - let d=n.dot(*velocity); - if d.is_negative(){ - *velocity-=(n*d/n.length_squared()).divide().fix_1(); + crate::push_solve::Contact{ + position:Planar64Vec3::ZERO, + velocity:n, + normal:n, } + }).collect(); + match crate::push_solve::push_solve(&contacts,*velocity){ + Some(new_velocity)=>*velocity=new_velocity, + None=>println!("Algorithm silently failing :)"), } } fn constrain_acceleration(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,acceleration:&mut Planar64Vec3){ - //TODO: trey push solve - for contact in &self.contacts{ + let contacts=self.contacts.iter().map(|contact|{ let n=contact_normal(models,hitbox_mesh,contact); - let d=n.dot(*acceleration); - if d.is_negative(){ - *acceleration-=(n*d/n.length_squared()).divide().fix_1(); + crate::push_solve::Contact{ + position:Planar64Vec3::ZERO, + velocity:n, + normal:n, } + }).collect(); + match crate::push_solve::push_solve(&contacts,*acceleration){ + Some(new_acceleration)=>*acceleration=new_acceleration, + None=>println!("Algorithm silently failing :)"), } } fn predict_collision_end(&self,collector:&mut instruction::InstructionCollector,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,body:&Body,time:Time){ diff --git a/src/push_solve.rs b/src/push_solve.rs new file mode 100644 index 0000000..aa61ba9 --- /dev/null +++ b/src/push_solve.rs @@ -0,0 +1,339 @@ +use strafesnet_common::integer::{Planar64,Planar64Vec3}; + +// This algorithm is based on Lua code +// written by Trey Reynolds in 2021 + +// 1/2^10 +const EPSILON:Planar64=Planar64::raw(1<<(32-10)); + +// A stack-allocated variable-size list that holds up to 4 elements +// Direct references are used instead of indices i0, i1, i2, i3 +type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>; + +struct Ray{ + origin:Planar64Vec3, + direction:Planar64Vec3, +} +impl Ray{ + fn extrapolate(&self,t:Planar64)->Planar64Vec3{ + self.origin+self.direction*t + } +} + +/// Information about a contact restriction +pub struct Contact{ + pub position:Planar64Vec3, + pub velocity:Planar64Vec3, + pub normal:Planar64Vec3, +} +impl Contact{ + fn relative_to(&self,point:Planar64Vec3)->Self{ + Self{ + position:self.position-point, + velocity:self.velocity, + normal:self.normal, + } + } + fn relative_dot(&self,direction:Planar64Vec3)->Planar64{ + (direction-self.velocity).dot(self.normal) + } + /// Calculate the time of intersection. (previously get_touch_time) + fn solve(&self,ray:&Ray)->Planar64{ + (self.position-ray.origin).dot(self.normal)/(ray.direction-self.velocity).dot(self.normal) + } +} + +//note that this is horrible with fixed point arithmetic +fn solve1(c0:&Contact)->Option{ + let det=c0.normal.dot(c0.velocity); + if det.get().abs()Option{ + let u0_u1=c0.velocity.cross(c1.velocity); + let n0_n1=c0.normal.cross(c1.normal); + let det=u0_u1.dot(n0_n1); + if det.get().abs()Option{ + let n0_n1=c0.normal.cross(c1.normal); + let det=c2.normal.dot(n0_n1); + if det.get().abs()Option{ + let det=u0.dot(u0); + if det==Planar64::ZERO{ + return None; + } + let s0=u0.dot(point)/det; + Some(s0) +} +fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<(Planar64,Planar64)>{ + let u0_u1=u0.cross(u1); + let det=u0_u1.dot(u0_u1); + if det==Planar64::ZERO{ + return None; + } + let s0=u0_u1.dot(point.cross(u1))/det; + let s1=u0_u1.dot(u0.cross(point))/det; + Some((s0,s1)) +} +fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->Option<(Planar64,Planar64,Planar64)>{ + let det=u0.cross(u1).dot(u2); + if det==Planar64::ZERO{ + return None; + } + let s0=point.cross(u1).dot(u2)/det; + let s1=u0.cross(point).dot(u2)/det; + let s2=u0.cross(u1).dot(point)/det; + Some((s0,s1,s2)) +} + +fn is_space_enclosed_2( + a:Planar64Vec3, + b:Planar64Vec3, +)->bool{ + a.cross(b)==Planar64Vec3::ZERO + &&a.dot(b)bool{ + a.cross(b).dot(c)==Planar64::ZERO + &&{ + let det_abac=a.cross(b).dot(a.cross(c)); + let det_abbc=a.cross(b).dot(b.cross(c)); + let det_acbc=a.cross(c).dot(b.cross(c)); + return det_abac*det_abbc<=Planar64::ZERO + && det_abbc*det_acbc<=Planar64::ZERO + &&-det_acbc*det_abac<=Planar64::ZERO + ||is_space_enclosed_2(a,b) + ||is_space_enclosed_2(a,c) + ||is_space_enclosed_2(b,c) + } +} +fn is_space_enclosed_4( + a:Planar64Vec3, + b:Planar64Vec3, + c:Planar64Vec3, + d:Planar64Vec3, +)->bool{ + let det_abc=a.cross(b).dot(c); + let det_abd=a.cross(b).dot(d); + let det_acd=a.cross(c).dot(d); + let det_bcd=b.cross(c).dot(d); + return det_abc*det_abdOption{ + Some(Ray{origin:point,direction:Planar64Vec3::ZERO}) +} +fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option{ + let direction=solve1(c0)?; + let s0=decompose1(direction,c0.velocity)?; + if s0Option{ + let direction=solve2(c0,c1)?; + let (s0,s1)=decompose2(direction,c0.velocity,c1.velocity)?; + if s0Option{ + let direction=solve3(c0,c1,c2)?; + let (s0,s1,s2)=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?; + if s0(point:Planar64Vec3)->Option<(Ray,Conts<'a>)>{ + match get_push_ray_0(point){ + Some(ray)=>Some((ray,Conts::new_const())), + None=>None, + } +} +fn get_best_push_ray_and_conts_1(point:Planar64Vec3,c0:&Contact)->Option<(Ray,Conts)>{ + get_push_ray_1(point,c0) + .map(|ray|(ray,Conts::from_iter([c0]))) +} +fn get_best_push_ray_and_conts_2<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact)->Option<(Ray,Conts<'a>)>{ + if is_space_enclosed_2(c0.normal,c1.normal){ + return None; + } + if let Some(ray)=get_push_ray_2(point,c0,c1){ + return Some((ray,Conts::from_iter([c0,c1]))); + } + if let Some(ray)=get_push_ray_1(point,c0){ + if Planar64::ZERO<=c1.relative_dot(ray.direction){ + return Some((ray,Conts::from_iter([c0]))); + } + } + return None; +} +fn get_best_push_ray_and_conts_3<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact)->Option<(Ray,Conts<'a>)>{ + if is_space_enclosed_3(c0.normal,c1.normal,c2.normal){ + return None; + } + if let Some(ray)=get_push_ray_3(point,c0,c1,c2){ + return Some((ray,Conts::from_iter([c0,c1,c2]))); + } + if let Some(ray)=get_push_ray_2(point,c0,c1){ + if Planar64::ZERO<=c2.relative_dot(ray.direction){ + return Some((ray,Conts::from_iter([c0,c1]))); + } + } + if let Some(ray)=get_push_ray_2(point,c0,c2){ + if Planar64::ZERO<=c1.relative_dot(ray.direction){ + return Some((ray,Conts::from_iter([c0,c2]))); + } + } + if let Some(ray)=get_push_ray_1(point,c0){ + if Planar64::ZERO<=c1.relative_dot(ray.direction) + &&Planar64::ZERO<=c2.relative_dot(ray.direction){ + return Some((ray,Conts::from_iter([c0]))); + } + } + return None; +} +fn get_best_push_ray_and_conts_4<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact,c3:&'a Contact)->Option<(Ray,Conts<'a>)>{ + if is_space_enclosed_4(c0.normal,c1.normal,c2.normal,c3.normal){ + return None; + } + + let (ray012,conts012)=get_best_push_ray_and_conts_3(point,c0,c1,c2)?; + let (ray013,conts013)=get_best_push_ray_and_conts_3(point,c0,c1,c3)?; + let (ray023,conts023)=get_best_push_ray_and_conts_3(point,c0,c2,c3)?; + + let err012=c3.relative_dot(ray012.direction); + let err013=c2.relative_dot(ray013.direction); + let err023=c1.relative_dot(ray023.direction); + + let best_err=err012.max(err013).max(err023); + + if best_err==err012{ + return Some((ray012,conts012)) + }else if best_err==err013{ + return Some((ray013,conts013)) + }else if best_err==err023{ + return Some((ray023,conts023)) + } + unreachable!() +} + +fn get_best_push_ray_and_conts<'a>( + point:Planar64Vec3, + conts:Conts<'a>, +)->Option<(Ray,Conts<'a>)>{ + match conts.as_slice(){ + &[c0,c1,c2,c3]=>get_best_push_ray_and_conts_4(point,c0,c1,c2,c3), + &[c0,c1,c2]=>get_best_push_ray_and_conts_3(point,c0,c1,c2), + &[c0,c1]=>get_best_push_ray_and_conts_2(point,c0,c1), + &[c0]=>get_best_push_ray_and_conts_1(point,c0), + &[]=>get_best_push_ray_and_conts_0(point), + _=>unreachable!(), + } +} + +fn get_first_touch<'a>(contacts:&'a Vec,ray:&Ray,conts:&Conts)->Option<(Planar64,&'a Contact)>{ + contacts.iter() + .filter(|&contact| + !conts.iter().any(|&c|std::ptr::eq(c,contact)) + &&contact.relative_dot(ray.direction)< -EPSILON + ) + .map(|contact|(contact.solve(ray),contact)) + .min_by_key(|&(t,_)|t) +} + +pub fn push_solve(contacts:&Vec,point:Planar64Vec3)->Option{ + let (mut ray,mut conts)=get_best_push_ray_and_conts_0(point)?; + loop{ + let (next_t,next_cont)=match get_first_touch(contacts,&ray,&conts){ + Some((t,conts))=>(t,conts), + None=>return Some(ray.origin), + }; + + if Planar64::ZERO<=next_t{ + return Some(ray.origin); + } + + //push_front + if conts.len()==conts.capacity(){ + //this is a dead case, new_conts never has more than 3 elements + conts.rotate_right(1); + conts[0]=next_cont; + }else{ + conts.push(next_cont); + conts.rotate_right(1); + } + + let meet_point=ray.extrapolate(next_t); + match get_best_push_ray_and_conts(meet_point,conts){ + Some((new_ray,new_conts))=>(ray,conts)=(new_ray,new_conts), + None=>return Some(meet_point), + } + } +} + +#[cfg(test)] +mod tests{ + use super::*; + #[test] + fn test_push_solve(){ + let contacts=vec![ + Contact{ + position:Planar64Vec3::ZERO, + velocity:Planar64Vec3::Y, + normal:Planar64Vec3::Y, + } + ]; + assert_eq!( + Some(Planar64Vec3::ZERO), + push_solve(&contacts,Planar64Vec3::NEG_Y) + ); + } +}