diff --git a/src/push_solve.rs b/src/push_solve.rs index 1e8d952..8b3f583 100644 --- a/src/push_solve.rs +++ b/src/push_solve.rs @@ -112,13 +112,6 @@ fn is_space_enclosed_4( const fn get_push_ray_0(point:Planar64Vec3)->Option{ Some(Ray{origin:point,direction:Planar64Vec3::ZERO}) } -const fn get_best_push_ray_and_indices_0(point:Planar64Vec3)->Option<(Ray,Indices)>{ - match get_push_ray_0(point){ - Some(ray)=>Some((ray,Indices::new_const())), - None=>None, - } -} - fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option{ let direction=solve1(c0)?; let s0=decompose1(direction,c0.velocity)?; @@ -130,11 +123,6 @@ fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option{ )?; Some(Ray{origin,direction}) } -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{ let direction=solve2(c0,c1)?; let (s0,s1)=decompose2(direction,c0.velocity,c1.velocity)?; @@ -147,6 +135,30 @@ fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option{ )?; Some(Ray{origin,direction}) } +fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Option{ + let direction=solve3(c0,c1,c2)?; + let (s0,s1,s2)=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?; + if s0Option<(Ray,Indices)>{ + match get_push_ray_0(point){ + Some(ray)=>Some((ray,Indices::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_indices_2(point:Planar64Vec3,ci0:CI,ci1:CI)->Option<(Ray,Indices)>{ if is_space_enclosed_2(ci0.contact.normal,ci1.contact.normal){ return None; @@ -161,20 +173,6 @@ fn get_best_push_ray_and_indices_2(point:Planar64Vec3,ci0:CI,ci1:CI)->Option<(Ra } return None; } - -fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Option{ - let direction=solve3(c0,c1,c2)?; - let (s0,s1,s2)=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?; - if s0Option<(Ray,Indices)>{ if is_space_enclosed_3(ci0.contact.normal,ci1.contact.normal,ci2.contact.normal){ return None; @@ -200,7 +198,6 @@ fn get_best_push_ray_and_indices_3(point:Planar64Vec3,ci0:CI,ci1:CI,ci2:CI)->Opt } 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){ return None;