diff --git a/src/push_solve.rs b/src/push_solve.rs index 5db32f0..1d24109 100644 --- a/src/push_solve.rs +++ b/src/push_solve.rs @@ -286,10 +286,24 @@ fn get_best_push_ray_and_indices( } } +fn get_touch_time(ray:&Ray,c:&Contact)->Planar64{ + (ray.origin-c.position).dot(c.normal)/(c.velocity-ray.direction).dot(c.normal) +} +fn get_first_touch(pnv_list:&Vec,ray:&Ray,indices:&Indices)->Option<(Planar64,usize)>{ + pnv_list.iter() + .enumerate() + .filter(|&(i,contact)| + !indices.contains(&i) + &&contact.relative_dot(ray.direction)< -EPSILON + ) + .map(|(i,contact)|(get_touch_time(ray,contact),i)) + .min_by_key(|&(t,_)|t) +} + pub fn push_solve(pnv_list:&Vec,point:Planar64Vec3)->Option{ let (mut ray,mut indices)=get_best_push_ray_and_indices_0(point)?; loop{ - let (next_index,next_t)=get_first_touch(pnv_list,ray,indices)?; + let (next_t,next_index)=get_first_touch(pnv_list,&ray,&indices)?; if Planar64::ZERO<=next_t{ return Some(ray.origin);