diff --git a/src/push_solve.rs b/src/push_solve.rs index e0d0283..2311c2d 100644 --- a/src/push_solve.rs +++ b/src/push_solve.rs @@ -25,6 +25,19 @@ impl Contact{ (direction-self.velocity).dot(self.normal) } } +#[derive(Clone,Copy)] +struct CI<'a>{ + contact:&'a Contact, + index:usize, +} +impl CI<'_>{ + fn new(pnv_list:&Vec,index:usize)->CI<'_>{ + CI{ + contact:&pnv_list[index], + index + } + } +} const fn solve0()->Planar64Vec3{ Planar64Vec3::ZERO @@ -112,9 +125,9 @@ fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option{ )?; Some(Ray{origin,direction}) } -fn get_best_push_ray_and_indices_1(point:Planar64Vec3,pnv_list:&Vec,i0:usize)->Option<(Ray,Indices)>{ - get_push_ray_1(point,&pnv_list[i0]) - .map(|ray|(ray,Indices::from_iter([i0]))) +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_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option{ @@ -129,16 +142,16 @@ fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option{ )?; Some(Ray{origin,direction}) } -fn get_best_push_ray_and_indices_2(point:Planar64Vec3,pnv_list:&Vec,i0:usize,i1:usize)->Option<(Ray,Indices)>{ - if is_space_enclosed_2(pnv_list[i0].normal,pnv_list[i1].normal){ +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){ return None; } - if let Some(ray)=get_push_ray_2(point,&pnv_list[i0],&pnv_list[i1]){ - return Some((ray,Indices::from_iter([i0,i1]))); + 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_1(point,&pnv_list[i0]){ - if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction){ - return Some((ray,Indices::from_iter([i0]))); + 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]))); } } return None; @@ -157,44 +170,44 @@ fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Optio )?; Some(Ray{origin,direction}) } -fn get_best_push_ray_and_indices_3(point:Planar64Vec3,pnv_list:&Vec,i0:usize,i1:usize,i2:usize)->Option<(Ray,Indices)>{ - if is_space_enclosed_3(pnv_list[i0].normal,pnv_list[i1].normal,pnv_list[i2].normal){ +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){ return None; } - if let Some(ray)=get_push_ray_3(point,&pnv_list[i0],&pnv_list[i1],&pnv_list[i2]){ - return Some((ray,Indices::from_iter([i0,i1,i2]))); + 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_2(point,&pnv_list[i0],&pnv_list[i1]){ - if Planar64::ZERO<=pnv_list[i2].relative_dot(ray.direction){ - return Some((ray,Indices::from_iter([i0,i1]))); + 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,&pnv_list[i0],&pnv_list[i2]){ - if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction){ - return Some((ray,Indices::from_iter([i0,i2]))); + 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_1(point,&pnv_list[i0]){ - if Planar64::ZERO<=pnv_list[i1].relative_dot(ray.direction) - &&Planar64::ZERO<=pnv_list[i2].relative_dot(ray.direction){ - return Some((ray,Indices::from_iter([i0]))); + 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]))); } } return None; } -fn get_best_push_ray_and_indices_4(point:Planar64Vec3,pnv_list:&Vec,i0:usize,i1:usize,i2:usize,i3:usize)->Option<(Ray,Indices)>{ - if is_space_enclosed_4(pnv_list[i0].normal,pnv_list[i1].normal,pnv_list[i2].normal,pnv_list[i3].normal){ +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){ return None; } - let (ray012,indices012)=get_best_push_ray_and_indices_3(point,pnv_list,i0,i1,i2)?; - let (ray013,indices013)=get_best_push_ray_and_indices_3(point,pnv_list,i0,i1,i3)?; - let (ray023,indices023)=get_best_push_ray_and_indices_3(point,pnv_list,i0,i2,i3)?; + 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 err012=pnv_list[i3].relative_dot(ray012.direction); - let err013=pnv_list[i2].relative_dot(ray013.direction); - let err023=pnv_list[i1].relative_dot(ray023.direction); + 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 best_err=err012.max(err013).max(err023); @@ -214,10 +227,10 @@ fn get_best_push_ray_and_indices( indices:Indices, )->Option<(Ray,Indices)>{ match indices.as_slice(){ - &[i0,i1,i2,i3]=>get_best_push_ray_and_indices_4(point,pnv_list,i0,i1,i2,i3), - &[i0,i1,i2]=>get_best_push_ray_and_indices_3(point,pnv_list,i0,i1,i2), - &[i0,i1]=>get_best_push_ray_and_indices_2(point,pnv_list,i0,i1), - &[i0]=>get_best_push_ray_and_indices_1(point,pnv_list,i0), + &[i0,i1,i2,i3]=>get_best_push_ray_and_indices_4(point,CI::new(pnv_list,i0),CI::new(pnv_list,i1),CI::new(pnv_list,i2),CI::new(pnv_list,i3)), + &[i0,i1,i2]=>get_best_push_ray_and_indices_3(point,CI::new(pnv_list,i0),CI::new(pnv_list,i1),CI::new(pnv_list,i2)), + &[i0,i1]=>get_best_push_ray_and_indices_2(point,CI::new(pnv_list,i0),CI::new(pnv_list,i1)), + &[i0]=>get_best_push_ray_and_indices_1(point,CI::new(pnv_list,i0)), &[]=>get_best_push_ray_and_indices_0(point), _=>unreachable!(), }