From ca8035cdfcfa72497c78bc4c5f36d3cfdbbf4c3e Mon Sep 17 00:00:00 2001 From: Quaternions Date: Wed, 8 Jan 2025 19:07:13 -0800 Subject: [PATCH] fixed wide --- strafe-client/src/physics.rs | 4 +- strafe-client/src/push_solve.rs | 134 ++++++++++++++++++-------------- 2 files changed, 76 insertions(+), 62 deletions(-) diff --git a/strafe-client/src/physics.rs b/strafe-client/src/physics.rs index cc9b18b..c6c4491 100644 --- a/strafe-client/src/physics.rs +++ b/strafe-client/src/physics.rs @@ -768,7 +768,7 @@ impl TouchingState{ let contacts=self.contacts.iter().map(|contact|{ let n=contact_normal(models,hitbox_mesh,contact); crate::push_solve::Contact{ - position:Planar64Vec3::ZERO, + position:vec3::ZERO, velocity:n, normal:n, } @@ -782,7 +782,7 @@ impl TouchingState{ let contacts=self.contacts.iter().map(|contact|{ let n=contact_normal(models,hitbox_mesh,contact); crate::push_solve::Contact{ - position:Planar64Vec3::ZERO, + position:vec3::ZERO, velocity:n, normal:n, } diff --git a/strafe-client/src/push_solve.rs b/strafe-client/src/push_solve.rs index aa61ba9..62f1ff9 100644 --- a/strafe-client/src/push_solve.rs +++ b/strafe-client/src/push_solve.rs @@ -1,22 +1,32 @@ -use strafesnet_common::integer::{Planar64,Planar64Vec3}; +use strafesnet_common::integer::{self,vec3::{self,Vector3},Fixed,Planar64,Planar64Vec3,Ratio}; // This algorithm is based on Lua code // written by Trey Reynolds in 2021 -// 1/2^10 -const EPSILON:Planar64=Planar64::raw(1<<(32-10)); +// EPSILON=1/2^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>; +// hack to allow comparing ratios to zero +const RATIO_ZERO:Ratio,Fixed<1,32>>=Ratio::new(Fixed::ZERO,Fixed::EPSILON); + struct Ray{ origin:Planar64Vec3, direction:Planar64Vec3, } impl Ray{ - fn extrapolate(&self,t:Planar64)->Planar64Vec3{ - self.origin+self.direction*t + fn extrapolate(&self,t:Ratio)->Planar64Vec3 + where + Num:Copy, + Den:Copy, + Num:core::ops::Mul, + Planar64:core::ops::Mul, + N1:integer::Divide, + T1:integer::Fix, + { + self.origin+self.direction.map(|elem|(t*elem).divide().fix()) } } @@ -34,39 +44,42 @@ impl Contact{ normal:self.normal, } } - fn relative_dot(&self,direction:Planar64Vec3)->Planar64{ + fn relative_dot(&self,direction:Planar64Vec3)->Fixed<2,64>{ (direction-self.velocity).dot(self.normal) } /// Calculate the time of intersection. (previously get_touch_time) - fn solve(&self,ray:&Ray)->Planar64{ + fn solve(&self,ray:&Ray)->Ratio,Fixed<2,64>>{ (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{ +fn solve1(c0:&Contact)->Option>,Fixed<2,64>>>{ + const EPSILON:Fixed<2,64>=Fixed::from_bits(Fixed::<2,64>::ONE.to_bits().shr(10)); let det=c0.normal.dot(c0.velocity); - if det.get().abs()Option{ +fn solve2(c0:&Contact,c1:&Contact)->Option>,Fixed<4,128>>>{ + const EPSILON:Fixed<4,128>=Fixed::from_bits(Fixed::<4,128>::ONE.to_bits().shr(10)); 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{ +fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option>,Fixed<3,96>>>{ + const EPSILON:Fixed<3,96>=Fixed::from_bits(Fixed::<3,96>::ONE.to_bits().shr(10)); let n0_n1=c0.normal.cross(c1.normal); let det=c2.normal.dot(n0_n1); - if det.get().abs()Option{ Some((c1.normal.cross(c2.normal)*d0+c2.normal.cross(c0.normal)*d1+c0.normal.cross(c1.normal)*d2)/det) } -fn decompose1(point:Planar64Vec3,u0:Planar64Vec3)->Option{ +fn decompose1(point:Planar64Vec3,u0:Planar64Vec3)->Option<[Ratio,Fixed<2,64>>;1]>{ let det=u0.dot(u0); - if det==Planar64::ZERO{ + if det==Fixed::ZERO{ return None; } let s0=u0.dot(point)/det; - Some(s0) + Some([s0]) } -fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<(Planar64,Planar64)>{ +fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<[Ratio,Fixed<4,128>>;2]>{ let u0_u1=u0.cross(u1); let det=u0_u1.dot(u0_u1); - if det==Planar64::ZERO{ + if det==Fixed::ZERO{ return None; } let s0=u0_u1.dot(point.cross(u1))/det; let s1=u0_u1.dot(u0.cross(point))/det; - Some((s0,s1)) + Some([s0,s1]) } -fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->Option<(Planar64,Planar64,Planar64)>{ +fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->Option<[Ratio,Fixed<3,96>>;3]>{ let det=u0.cross(u1).dot(u2); - if det==Planar64::ZERO{ + if det==Fixed::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)) + 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 + a.cross(b).dot(c)==Fixed::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 + return!( det_abac*det_abbc).is_positive() + &&!( det_abbc*det_acbc).is_positive() + &&!(-det_acbc*det_abac).is_positive() ||is_space_enclosed_2(a,b) ||is_space_enclosed_2(a,c) ||is_space_enclosed_2(b,c) @@ -139,12 +152,12 @@ fn is_space_enclosed_4( 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}) + Some(Ray{origin:point,direction:vec3::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,c0:&'a Contact,c1:&'a Co 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){ + if !c1.relative_dot(ray.direction).is_negative(){ return Some((ray,Conts::from_iter([c0]))); } } @@ -223,18 +236,18 @@ fn get_best_push_ray_and_conts_3<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Co 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){ + if !c2.relative_dot(ray.direction).is_negative(){ 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){ + if !c1.relative_dot(ray.direction).is_negative(){ 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){ + if !c1.relative_dot(ray.direction).is_negative() + &&!c2.relative_dot(ray.direction).is_negative(){ return Some((ray,Conts::from_iter([c0]))); } } @@ -279,17 +292,18 @@ fn get_best_push_ray_and_conts<'a>( } } -fn get_first_touch<'a>(contacts:&'a Vec,ray:&Ray,conts:&Conts)->Option<(Planar64,&'a Contact)>{ +fn get_first_touch<'a>(contacts:&'a Vec,ray:&Ray,conts:&Conts)->Option<(Ratio,Fixed<2,64>>,&'a Contact)>{ contacts.iter() .filter(|&contact| !conts.iter().any(|&c|std::ptr::eq(c,contact)) - &&contact.relative_dot(ray.direction)< -EPSILON + &&contact.relative_dot(ray.direction).is_negative() ) .map(|contact|(contact.solve(ray),contact)) .min_by_key(|&(t,_)|t) } pub fn push_solve(contacts:&Vec,point:Planar64Vec3)->Option{ + const ZERO:Ratio,Fixed<1,32>>=Ratio::new(Fixed::ZERO,Fixed::EPSILON); 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){ @@ -297,7 +311,7 @@ pub fn push_solve(contacts:&Vec,point:Planar64Vec3)->Optionreturn Some(ray.origin), }; - if Planar64::ZERO<=next_t{ + if ZERO.le_ratio(next_t){ return Some(ray.origin); } @@ -326,14 +340,14 @@ mod tests{ fn test_push_solve(){ let contacts=vec![ Contact{ - position:Planar64Vec3::ZERO, - velocity:Planar64Vec3::Y, - normal:Planar64Vec3::Y, + position:vec3::ZERO, + velocity:vec3::Y, + normal:vec3::Y, } ]; assert_eq!( - Some(Planar64Vec3::ZERO), - push_solve(&contacts,Planar64Vec3::NEG_Y) + Some(vec3::ZERO), + push_solve(&contacts,vec3::NEG_Y) ); } }