interpreterK/strafe-project
1
Fork 0
forked from StrafesNET/strafe-project
Code Pull Requests Activity

fixed wide

Browse Source
This commit is contained in:
Quaternions 2025-01-08 19:07:13 -08:00
parent ff5d954cfb
commit ca8035cdfc
2 changed files with 76 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
strafe-client/src/physics.rs
View File

@ -768,7 +768,7 @@ impl TouchingState{
let contacts=self.contacts.iter().map(|contact|{ let contacts=self.contacts.iter().map(|contact|{
let n=contact_normal(models,hitbox_mesh,contact); let n=contact_normal(models,hitbox_mesh,contact);
crate::push_solve::Contact{ crate::push_solve::Contact{
position:Planar64Vec3::ZERO, position:vec3::ZERO,
velocity:n, velocity:n,
normal:n, normal:n,
} }
@ -782,7 +782,7 @@ impl TouchingState{
let contacts=self.contacts.iter().map(|contact|{ let contacts=self.contacts.iter().map(|contact|{
let n=contact_normal(models,hitbox_mesh,contact); let n=contact_normal(models,hitbox_mesh,contact);
crate::push_solve::Contact{ crate::push_solve::Contact{
position:Planar64Vec3::ZERO, position:vec3::ZERO,
velocity:n, velocity:n,
normal:n, normal:n,
} }

134
strafe-client/src/push_solve.rs
View File

@ -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 // This algorithm is based on Lua code
// written by Trey Reynolds in 2021 // written by Trey Reynolds in 2021
// 1/2^10 // EPSILON=1/2^10
const EPSILON:Planar64=Planar64::raw(1<<(32-10));
// A stack-allocated variable-size list that holds up to 4 elements // A stack-allocated variable-size list that holds up to 4 elements
// Direct references are used instead of indices i0, i1, i2, i3 // Direct references are used instead of indices i0, i1, i2, i3
type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>; type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>;
// hack to allow comparing ratios to zero
const RATIO_ZERO:Ratio<Fixed<1,32>,Fixed<1,32>>=Ratio::new(Fixed::ZERO,Fixed::EPSILON);
struct Ray{ struct Ray{
origin:Planar64Vec3, origin:Planar64Vec3,
direction:Planar64Vec3, direction:Planar64Vec3,
} }
impl Ray{ impl Ray{
fn extrapolate(&self,t:Planar64)->Planar64Vec3{ fn extrapolate<Num,Den,N1,T1>(&self,t:Ratio<Num,Den>)->Planar64Vec3
self.origin+self.direction*t where
Num:Copy,
Den:Copy,
Num:core::ops::Mul<Planar64,Output=N1>,
Planar64:core::ops::Mul<Den,Output=N1>,
N1:integer::Divide<Den,Output=T1>,
T1:integer::Fix<Planar64>,
{
self.origin+self.direction.map(|elem|(t*elem).divide().fix())
} }
} }
@ -34,39 +44,42 @@ impl Contact{
normal:self.normal, 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) (direction-self.velocity).dot(self.normal)
} }
/// Calculate the time of intersection. (previously get_touch_time) /// Calculate the time of intersection. (previously get_touch_time)
fn solve(&self,ray:&Ray)->Planar64{ fn solve(&self,ray:&Ray)->Ratio<Fixed<2,64>,Fixed<2,64>>{
(self.position-ray.origin).dot(self.normal)/(ray.direction-self.velocity).dot(self.normal) (self.position-ray.origin).dot(self.normal)/(ray.direction-self.velocity).dot(self.normal)
} }
} }
//note that this is horrible with fixed point arithmetic //note that this is horrible with fixed point arithmetic
fn solve1(c0:&Contact)->Option<Planar64Vec3>{ fn solve1(c0:&Contact)->Option<Ratio<Vector3<Fixed<3,96>>,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); let det=c0.normal.dot(c0.velocity);
if det.get().abs()<EPSILON.get(){ if det.abs()<EPSILON{
return None; return None;
} }
let d0=c0.normal.dot(c0.position); let d0=c0.normal.dot(c0.position);
Some(c0.normal*d0/det) Some(c0.normal*d0/det)
} }
fn solve2(c0:&Contact,c1:&Contact)->Option<Planar64Vec3>{ fn solve2(c0:&Contact,c1:&Contact)->Option<Ratio<Vector3<Fixed<5,160>>,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 u0_u1=c0.velocity.cross(c1.velocity);
let n0_n1=c0.normal.cross(c1.normal); let n0_n1=c0.normal.cross(c1.normal);
let det=u0_u1.dot(n0_n1); let det=u0_u1.dot(n0_n1);
if det.get().abs()<EPSILON.get(){ if det.abs()<EPSILON{
return None; return None;
} }
let d0=c0.normal.dot(c0.position); let d0=c0.normal.dot(c0.position);
let d1=c1.normal.dot(c1.position); let d1=c1.normal.dot(c1.position);
Some((c1.normal.cross(u0_u1)*d0+u0_u1.cross(c0.normal)*d1)/det) Some((c1.normal.cross(u0_u1)*d0+u0_u1.cross(c0.normal)*d1)/det)
} }
fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option<Planar64Vec3>{ fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option<Ratio<Vector3<Fixed<4,128>>,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 n0_n1=c0.normal.cross(c1.normal);
let det=c2.normal.dot(n0_n1); let det=c2.normal.dot(n0_n1);
if det.get().abs()<EPSILON.get(){ if det.abs()<EPSILON{
return None; return None;
} }
let d0=c0.normal.dot(c0.position); let d0=c0.normal.dot(c0.position);
@ -75,55 +88,55 @@ fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option<Planar64Vec3>{
Some((c1.normal.cross(c2.normal)*d0+c2.normal.cross(c0.normal)*d1+c0.normal.cross(c1.normal)*d2)/det) 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<Planar64>{ fn decompose1(point:Planar64Vec3,u0:Planar64Vec3)->Option<[Ratio<Fixed<2,64>,Fixed<2,64>>;1]>{
let det=u0.dot(u0); let det=u0.dot(u0);
if det==Planar64::ZERO{ if det==Fixed::ZERO{
return None; return None;
} }
let s0=u0.dot(point)/det; 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>,Fixed<4,128>>;2]>{
let u0_u1=u0.cross(u1); let u0_u1=u0.cross(u1);
let det=u0_u1.dot(u0_u1); let det=u0_u1.dot(u0_u1);
if det==Planar64::ZERO{ if det==Fixed::ZERO{
return None; return None;
} }
let s0=u0_u1.dot(point.cross(u1))/det; let s0=u0_u1.dot(point.cross(u1))/det;
let s1=u0_u1.dot(u0.cross(point))/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>,Fixed<3,96>>;3]>{
let det=u0.cross(u1).dot(u2); let det=u0.cross(u1).dot(u2);
if det==Planar64::ZERO{ if det==Fixed::ZERO{
return None; return None;
} }
let s0=point.cross(u1).dot(u2)/det; let s0=point.cross(u1).dot(u2)/det;
let s1=u0.cross(point).dot(u2)/det; let s1=u0.cross(point).dot(u2)/det;
let s2=u0.cross(u1).dot(point)/det; let s2=u0.cross(u1).dot(point)/det;
Some((s0,s1,s2)) Some([s0,s1,s2])
} }
fn is_space_enclosed_2( fn is_space_enclosed_2(
a:Planar64Vec3, a:Planar64Vec3,
b:Planar64Vec3, b:Planar64Vec3,
)->bool{ )->bool{
a.cross(b)==Planar64Vec3::ZERO a.cross(b)==Vector3::new([Fixed::ZERO;3])
&&a.dot(b)<Planar64::ZERO &&a.dot(b).is_negative()
} }
fn is_space_enclosed_3( fn is_space_enclosed_3(
a:Planar64Vec3, a:Planar64Vec3,
b:Planar64Vec3, b:Planar64Vec3,
c:Planar64Vec3 c:Planar64Vec3
)->bool{ )->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_abac=a.cross(b).dot(a.cross(c));
let det_abbc=a.cross(b).dot(b.cross(c)); let det_abbc=a.cross(b).dot(b.cross(c));
let det_acbc=a.cross(c).dot(b.cross(c)); let det_acbc=a.cross(c).dot(b.cross(c));
return det_abac*det_abbc<=Planar64::ZERO return!( det_abac*det_abbc).is_positive()
&& det_abbc*det_acbc<=Planar64::ZERO &&!( det_abbc*det_acbc).is_positive()
&&-det_acbc*det_abac<=Planar64::ZERO &&!(-det_acbc*det_abac).is_positive()
||is_space_enclosed_2(a,b) ||is_space_enclosed_2(a,b)
||is_space_enclosed_2(a,c) ||is_space_enclosed_2(a,c)
||is_space_enclosed_2(b,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_abd=a.cross(b).dot(d);
let det_acd=a.cross(c).dot(d); let det_acd=a.cross(c).dot(d);
let det_bcd=b.cross(c).dot(d); let det_bcd=b.cross(c).dot(d);
return det_abc*det_abd<Planar64::ZERO return( det_abc*det_abd).is_negative()
&&-det_abc*det_acd<Planar64::ZERO &&(-det_abc*det_acd).is_negative()
&& det_abd*det_acd<Planar64::ZERO &&( det_abd*det_acd).is_negative()
&& det_abc*det_bcd<Planar64::ZERO &&( det_abc*det_bcd).is_negative()
&&-det_abd*det_bcd<Planar64::ZERO &&(-det_abd*det_bcd).is_negative()
&& det_acd*det_bcd<Planar64::ZERO &&( det_acd*det_bcd).is_negative()
||is_space_enclosed_3(a,b,c) ||is_space_enclosed_3(a,b,c)
||is_space_enclosed_3(a,b,d) ||is_space_enclosed_3(a,b,d)
||is_space_enclosed_3(a,c,d) ||is_space_enclosed_3(a,c,d)
@ -152,42 +165,42 @@ fn is_space_enclosed_4(
} }
const fn get_push_ray_0(point:Planar64Vec3)->Option<Ray>{ const fn get_push_ray_0(point:Planar64Vec3)->Option<Ray>{
Some(Ray{origin:point,direction:Planar64Vec3::ZERO}) Some(Ray{origin:point,direction:vec3::ZERO})
} }
fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option<Ray>{ fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option<Ray>{
let direction=solve1(c0)?; let direction=solve1(c0)?.divide().fix_1();
let s0=decompose1(direction,c0.velocity)?; let [s0]=decompose1(direction,c0.velocity)?;
if s0<Planar64::ZERO{ if s0.lt_ratio(RATIO_ZERO){
return None; return None;
} }
let origin=point+solve1( let origin=point+solve1(
&c0.relative_to(point), &c0.relative_to(point),
)?; )?.divide().fix_1();
Some(Ray{origin,direction}) Some(Ray{origin,direction})
} }
fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option<Ray>{ fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option<Ray>{
let direction=solve2(c0,c1)?; let direction=solve2(c0,c1)?.divide().fix_1();
let (s0,s1)=decompose2(direction,c0.velocity,c1.velocity)?; let [s0,s1]=decompose2(direction,c0.velocity,c1.velocity)?;
if s0<Planar64::ZERO||s1<Planar64::ZERO{ if s0.lt_ratio(RATIO_ZERO)||s1.lt_ratio(RATIO_ZERO){
return None; return None;
} }
let origin=point+solve2( let origin=point+solve2(
&c0.relative_to(point), &c0.relative_to(point),
&c1.relative_to(point), &c1.relative_to(point),
)?; )?.divide().fix_1();
Some(Ray{origin,direction}) Some(Ray{origin,direction})
} }
fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Option<Ray>{ fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Option<Ray>{
let direction=solve3(c0,c1,c2)?; let direction=solve3(c0,c1,c2)?.divide().fix_1();
let (s0,s1,s2)=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?; let [s0,s1,s2]=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?;
if s0<Planar64::ZERO||s1<Planar64::ZERO||s2<Planar64::ZERO{ if s0.lt_ratio(RATIO_ZERO)||s1.lt_ratio(RATIO_ZERO)||s2.lt_ratio(RATIO_ZERO){
return None; return None;
} }
let origin=point+solve3( let origin=point+solve3(
&c0.relative_to(point), &c0.relative_to(point),
&c1.relative_to(point), &c1.relative_to(point),
&c2.relative_to(point), &c2.relative_to(point),
)?; )?.divide().fix_1();
Some(Ray{origin,direction}) Some(Ray{origin,direction})
} }
@ -209,7 +222,7 @@ fn get_best_push_ray_and_conts_2<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Co
return Some((ray,Conts::from_iter([c0,c1]))); return Some((ray,Conts::from_iter([c0,c1])));
} }
if let Some(ray)=get_push_ray_1(point,c0){ 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]))); 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]))); return Some((ray,Conts::from_iter([c0,c1,c2])));
} }
if let Some(ray)=get_push_ray_2(point,c0,c1){ 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]))); return Some((ray,Conts::from_iter([c0,c1])));
} }
} }
if let Some(ray)=get_push_ray_2(point,c0,c2){ 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]))); return Some((ray,Conts::from_iter([c0,c2])));
} }
} }
if let Some(ray)=get_push_ray_1(point,c0){ 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()
&&Planar64::ZERO<=c2.relative_dot(ray.direction){ &&!c2.relative_dot(ray.direction).is_negative(){
return Some((ray,Conts::from_iter([c0]))); 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<Contact>,ray:&Ray,conts:&Conts)->Option<(Planar64,&'a Contact)>{ fn get_first_touch<'a>(contacts:&'a Vec<Contact>,ray:&Ray,conts:&Conts)->Option<(Ratio<Fixed<2,64>,Fixed<2,64>>,&'a Contact)>{
contacts.iter() contacts.iter()
.filter(|&contact| .filter(|&contact|
!conts.iter().any(|&c|std::ptr::eq(c,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)) .map(|contact|(contact.solve(ray),contact))
.min_by_key(|&(t,_)|t) .min_by_key(|&(t,_)|t)
} }
pub fn push_solve(contacts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{ pub fn push_solve(contacts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
const ZERO:Ratio<Fixed<1,32>,Fixed<1,32>>=Ratio::new(Fixed::ZERO,Fixed::EPSILON);
let (mut ray,mut conts)=get_best_push_ray_and_conts_0(point)?; let (mut ray,mut conts)=get_best_push_ray_and_conts_0(point)?;
loop{ loop{
let (next_t,next_cont)=match get_first_touch(contacts,&ray,&conts){ let (next_t,next_cont)=match get_first_touch(contacts,&ray,&conts){
@ -297,7 +311,7 @@ pub fn push_solve(contacts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec
None=>return Some(ray.origin), None=>return Some(ray.origin),
}; };
if Planar64::ZERO<=next_t{ if ZERO.le_ratio(next_t){
return Some(ray.origin); return Some(ray.origin);
} }
@ -326,14 +340,14 @@ mod tests{
fn test_push_solve(){ fn test_push_solve(){
let contacts=vec![ let contacts=vec![
Contact{ Contact{
position:Planar64Vec3::ZERO, position:vec3::ZERO,
velocity:Planar64Vec3::Y, velocity:vec3::Y,
normal:Planar64Vec3::Y, normal:vec3::Y,
} }
]; ];
assert_eq!( assert_eq!(
Some(Planar64Vec3::ZERO), Some(vec3::ZERO),
push_solve(&contacts,Planar64Vec3::NEG_Y) push_solve(&contacts,vec3::NEG_Y)
); );
} }
} }