StrafesNET/strafe-client
5
Fork 4
Code Issues 2 Pull Requests 1 Packages Projects Releases 1 Wiki Activity

factor out indices, use only direct references

Browse Source
This commit is contained in:
Quaternions 2024-08-22 12:50:40 -07:00
parent 1265926c98
commit 78202beb53
1 changed files with 61 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
src/push_solve.rs
View File

@ -4,8 +4,8 @@ use strafesnet_common::integer::{Planar64,Planar64Vec3};
const EPSILON:Planar64=Planar64::raw(1<<(32-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
// This is used for i0, i1, i2, i3 // Direct references are used instead of indices i0, i1, i2, i3
type Indices=arrayvec::ArrayVec<usize,4>; type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>;
struct Ray{ struct Ray{
origin:Planar64Vec3, origin:Planar64Vec3,
@ -40,21 +40,6 @@ impl Contact{
} }
} }
//A reference to a Contact and its respective index, paired together
#[derive(Clone,Copy)]
struct CI<'a>{
contact:&'a Contact,
index:usize,
}
impl CI<'_>{
fn new(conts:&Vec<Contact>,index:usize)->CI<'_>{
CI{
contact:&conts[index],
index
}
}
}
//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<Planar64Vec3>{
let det=c0.normal.dot(c0.velocity); let det=c0.normal.dot(c0.velocity);
@ -203,124 +188,122 @@ fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Optio
Some(Ray{origin,direction}) Some(Ray{origin,direction})
} }
const fn get_best_push_ray_and_indices_0(point:Planar64Vec3)->Option<(Ray,Indices)>{ const fn get_best_push_ray_and_conts_0<'a>(point:Planar64Vec3)->Option<(Ray,Conts<'a>)>{
match get_push_ray_0(point){ match get_push_ray_0(point){
Some(ray)=>Some((ray,Indices::new_const())), Some(ray)=>Some((ray,Conts::new_const())),
None=>None, None=>None,
} }
} }
fn get_best_push_ray_and_indices_1(point:Planar64Vec3,ci0:CI)->Option<(Ray,Indices)>{ fn get_best_push_ray_and_conts_1(point:Planar64Vec3,c0:&Contact)->Option<(Ray,Conts)>{
get_push_ray_1(point,ci0.contact) get_push_ray_1(point,c0)
.map(|ray|(ray,Indices::from_iter([ci0.index]))) .map(|ray|(ray,Conts::from_iter([c0])))
} }
fn get_best_push_ray_and_indices_2(point:Planar64Vec3,ci0:CI,ci1:CI)->Option<(Ray,Indices)>{ 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(ci0.contact.normal,ci1.contact.normal){ if is_space_enclosed_2(c0.normal,c1.normal){
return None; return None;
} }
if let Some(ray)=get_push_ray_2(point,ci0.contact,ci1.contact){ if let Some(ray)=get_push_ray_2(point,c0,c1){
return Some((ray,Indices::from_iter([ci0.index,ci1.index]))); return Some((ray,Conts::from_iter([c0,c1])));
} }
if let Some(ray)=get_push_ray_1(point,ci0.contact){ if let Some(ray)=get_push_ray_1(point,c0){
if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){ if Planar64::ZERO<=c1.relative_dot(ray.direction){
return Some((ray,Indices::from_iter([ci0.index]))); return Some((ray,Conts::from_iter([c0])));
} }
} }
return None; return None;
} }
fn get_best_push_ray_and_indices_3(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI)->Option<(Ray,Indices)>{ 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(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal){ if is_space_enclosed_3(c0.normal,c1.normal,c2.normal){
return None; return None;
} }
if let Some(ray)=get_push_ray_3(point,ci0.contact,ci1.contact,ci2.contact){ if let Some(ray)=get_push_ray_3(point,c0,c1,c2){
return Some((ray,Indices::from_iter([ci0.index,ci1.index,ci2.index]))); return Some((ray,Conts::from_iter([c0,c1,c2])));
} }
if let Some(ray)=get_push_ray_2(point,ci0.contact,ci1.contact){ if let Some(ray)=get_push_ray_2(point,c0,c1){
if Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){ if Planar64::ZERO<=c2.relative_dot(ray.direction){
return Some((ray,Indices::from_iter([ci0.index,ci1.index]))); return Some((ray,Conts::from_iter([c0,c1])));
} }
} }
if let Some(ray)=get_push_ray_2(point,ci0.contact,ci2.contact){ if let Some(ray)=get_push_ray_2(point,c0,c2){
if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){ if Planar64::ZERO<=c1.relative_dot(ray.direction){
return Some((ray,Indices::from_iter([ci0.index,ci2.index]))); return Some((ray,Conts::from_iter([c0,c2])));
} }
} }
if let Some(ray)=get_push_ray_1(point,ci0.contact){ if let Some(ray)=get_push_ray_1(point,c0){
if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction) if Planar64::ZERO<=c1.relative_dot(ray.direction)
&&Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){ &&Planar64::ZERO<=c2.relative_dot(ray.direction){
return Some((ray,Indices::from_iter([ci0.index]))); return Some((ray,Conts::from_iter([c0])));
} }
} }
return None; return None;
} }
fn get_best_push_ray_and_indices_4(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI,ci3:CI)->Option<(Ray,Indices)>{ 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(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal,ci3.contact.normal){ if is_space_enclosed_4(c0.normal,c1.normal,c2.normal,c3.normal){
return None; return None;
} }
let (ray012,indices012)=get_best_push_ray_and_indices_3(point,ci0,ci1,ci2)?; let (ray012,conts012)=get_best_push_ray_and_conts_3(point,c0,c1,c2)?;
let (ray013,indices013)=get_best_push_ray_and_indices_3(point,ci0,ci1,ci3)?; let (ray013,conts013)=get_best_push_ray_and_conts_3(point,c0,c1,c3)?;
let (ray023,indices023)=get_best_push_ray_and_indices_3(point,ci0,ci2,ci3)?; let (ray023,conts023)=get_best_push_ray_and_conts_3(point,c0,c2,c3)?;
let err012=ci3.contact.relative_dot(ray012.direction); let err012=c3.relative_dot(ray012.direction);
let err013=ci2.contact.relative_dot(ray013.direction); let err013=c2.relative_dot(ray013.direction);
let err023=ci1.contact.relative_dot(ray023.direction); let err023=c1.relative_dot(ray023.direction);
let best_err=err012.max(err013).max(err023); let best_err=err012.max(err013).max(err023);
if best_err==err012{ if best_err==err012{
return Some((ray012,indices012)) return Some((ray012,conts012))
}else if best_err==err013{ }else if best_err==err013{
return Some((ray013,indices013)) return Some((ray013,conts013))
}else if best_err==err023{ }else if best_err==err023{
return Some((ray023,indices023)) return Some((ray023,conts023))
} }
unreachable!() unreachable!()
} }
fn get_best_push_ray_and_indices( fn get_best_push_ray_and_conts<'a>(
point:Planar64Vec3, point:Planar64Vec3,
conts:&Vec<Contact>, conts:Conts<'a>,
indices:Indices, )->Option<(Ray,Conts<'a>)>{
)->Option<(Ray,Indices)>{ match conts.as_slice(){
match indices.as_slice(){ &[c0,c1,c2,c3]=>get_best_push_ray_and_conts_4(point,c0,c1,c2,c3),
&[i0,i1,i2,i3]=>get_best_push_ray_and_indices_4(point,CI::new(conts,i0),CI::new(conts,i1),CI::new(conts,i2),CI::new(conts,i3)), &[c0,c1,c2]=>get_best_push_ray_and_conts_3(point,c0,c1,c2),
&[i0,i1,i2]=>get_best_push_ray_and_indices_3(point,CI::new(conts,i0),CI::new(conts,i1),CI::new(conts,i2)), &[c0,c1]=>get_best_push_ray_and_conts_2(point,c0,c1),
&[i0,i1]=>get_best_push_ray_and_indices_2(point,CI::new(conts,i0),CI::new(conts,i1)), &[c0]=>get_best_push_ray_and_conts_1(point,c0),
&[i0]=>get_best_push_ray_and_indices_1(point,CI::new(conts,i0)), &[]=>get_best_push_ray_and_conts_0(point),
&[]=>get_best_push_ray_and_indices_0(point),
_=>unreachable!(), _=>unreachable!(),
} }
} }
fn get_first_touch(conts:&Vec<Contact>,ray:&Ray,indices:&Indices)->Option<(Planar64,usize)>{ fn get_first_touch<'a>(contacts:&'a Vec<Contact>,ray:&Ray,conts:&Conts)->Option<(Planar64,&'a Contact)>{
conts.iter() contacts.iter()
.enumerate() .filter(|&contact|
.filter(|&(i,contact)| !conts.iter().any(|&c|std::ptr::eq(c,contact))
!indices.contains(&i)
&&contact.relative_dot(ray.direction)< -EPSILON &&contact.relative_dot(ray.direction)< -EPSILON
) )
.map(|(i,contact)|(contact.solve(ray),i)) .map(|contact|(contact.solve(ray),contact))
.min_by_key(|&(t,_)|t) .min_by_key(|&(t,_)|t)
} }
pub fn push_solve(conts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{ pub fn push_solve(contacts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
let (mut ray,mut indices)=get_best_push_ray_and_indices_0(point)?; let (mut ray,mut conts)=get_best_push_ray_and_conts_0(point)?;
loop{ loop{
let (next_t,next_index)=get_first_touch(conts,&ray,&indices)?; let (next_t,next_cont)=get_first_touch(contacts,&ray,&conts)?;
if Planar64::ZERO<=next_t{ if Planar64::ZERO<=next_t{
return Some(ray.origin); return Some(ray.origin);
} }
//edit the indices //edit the indices
if indices.len()==4{ if conts.len()==4{
indices.pop(); conts.pop();
} }
indices.push(next_index); conts.push(next_cont);
let meet_point=ray.extrapolate(next_t); let meet_point=ray.extrapolate(next_t);
match get_best_push_ray_and_indices(meet_point,conts,indices){ match get_best_push_ray_and_conts(meet_point,conts){
Some((new_ray,new_indices))=>(ray,indices)=(new_ray,new_indices), Some((new_ray,new_conts))=>(ray,conts)=(new_ray,new_conts),
None=>return Some(meet_point), None=>return Some(meet_point),
} }
} }