diff --git a/src/push_solve.rs b/src/push_solve.rs index c47af3a..05435c3 100644 --- a/src/push_solve.rs +++ b/src/push_solve.rs @@ -4,8 +4,8 @@ use strafesnet_common::integer::{Planar64,Planar64Vec3}; const EPSILON:Planar64=Planar64::raw(1<<(32-10)); // A stack-allocated variable-size list that holds up to 4 elements -// This is used for i0, i1, i2, i3 -type Indices=arrayvec::ArrayVec; +// Direct references are used instead of indices i0, i1, i2, i3 +type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>; struct Ray{ 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,index:usize)->CI<'_>{ - CI{ - contact:&conts[index], - index - } - } -} - //note that this is horrible with fixed point arithmetic fn solve1(c0:&Contact)->Option{ 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}) } -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){ - Some(ray)=>Some((ray,Indices::new_const())), + Some(ray)=>Some((ray,Conts::new_const())), None=>None, } } -fn get_best_push_ray_and_indices_1(point:Planar64Vec3,ci0:CI)->Option<(Ray,Indices)>{ - get_push_ray_1(point,ci0.contact) - .map(|ray|(ray,Indices::from_iter([ci0.index]))) +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_indices_2(point:Planar64Vec3,ci0:CI,ci1:CI)->Option<(Ray,Indices)>{ - if is_space_enclosed_2(ci0.contact.normal,ci1.contact.normal){ +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,ci0.contact,ci1.contact){ - return Some((ray,Indices::from_iter([ci0.index,ci1.index]))); + 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,ci0.contact){ - if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){ - return Some((ray,Indices::from_iter([ci0.index]))); + 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_indices_3(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI)->Option<(Ray,Indices)>{ - if is_space_enclosed_3(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal){ +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,ci0.contact,ci1.contact,ci2.contact){ - return Some((ray,Indices::from_iter([ci0.index,ci1.index,ci2.index]))); + 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,ci0.contact,ci1.contact){ - if Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){ - return Some((ray,Indices::from_iter([ci0.index,ci1.index]))); + 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,ci0.contact,ci2.contact){ - if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction){ - return Some((ray,Indices::from_iter([ci0.index,ci2.index]))); + 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,ci0.contact){ - if Planar64::ZERO<=ci1.contact.relative_dot(ray.direction) - &&Planar64::ZERO<=ci2.contact.relative_dot(ray.direction){ - return Some((ray,Indices::from_iter([ci0.index]))); + 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_indices_4(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI,ci3:CI)->Option<(Ray,Indices)>{ - if is_space_enclosed_4(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal,ci3.contact.normal){ +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,indices012)=get_best_push_ray_and_indices_3(point,ci0,ci1,ci2)?; - let (ray013,indices013)=get_best_push_ray_and_indices_3(point,ci0,ci1,ci3)?; - let (ray023,indices023)=get_best_push_ray_and_indices_3(point,ci0,ci2,ci3)?; + 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=ci3.contact.relative_dot(ray012.direction); - let err013=ci2.contact.relative_dot(ray013.direction); - let err023=ci1.contact.relative_dot(ray023.direction); + 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,indices012)) + return Some((ray012,conts012)) }else if best_err==err013{ - return Some((ray013,indices013)) + return Some((ray013,conts013)) }else if best_err==err023{ - return Some((ray023,indices023)) + return Some((ray023,conts023)) } unreachable!() } -fn get_best_push_ray_and_indices( +fn get_best_push_ray_and_conts<'a>( point:Planar64Vec3, - conts:&Vec, - indices:Indices, -)->Option<(Ray,Indices)>{ - match indices.as_slice(){ - &[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)), - &[i0,i1,i2]=>get_best_push_ray_and_indices_3(point,CI::new(conts,i0),CI::new(conts,i1),CI::new(conts,i2)), - &[i0,i1]=>get_best_push_ray_and_indices_2(point,CI::new(conts,i0),CI::new(conts,i1)), - &[i0]=>get_best_push_ray_and_indices_1(point,CI::new(conts,i0)), - &[]=>get_best_push_ray_and_indices_0(point), + 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(conts:&Vec,ray:&Ray,indices:&Indices)->Option<(Planar64,usize)>{ - conts.iter() - .enumerate() - .filter(|&(i,contact)| - !indices.contains(&i) +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(|(i,contact)|(contact.solve(ray),i)) + .map(|contact|(contact.solve(ray),contact)) .min_by_key(|&(t,_)|t) } -pub fn push_solve(conts:&Vec,point:Planar64Vec3)->Option{ - let (mut ray,mut indices)=get_best_push_ray_and_indices_0(point)?; +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_index)=get_first_touch(conts,&ray,&indices)?; + let (next_t,next_cont)=get_first_touch(contacts,&ray,&conts)?; if Planar64::ZERO<=next_t{ return Some(ray.origin); } //edit the indices - if indices.len()==4{ - indices.pop(); + if conts.len()==4{ + conts.pop(); } - indices.push(next_index); + conts.push(next_cont); let meet_point=ray.extrapolate(next_t); - match get_best_push_ray_and_indices(meet_point,conts,indices){ - Some((new_ray,new_indices))=>(ray,indices)=(new_ray,new_indices), + 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), } }