diff --git a/src/push_solve.rs b/src/push_solve.rs index 2311c2d..1e8d952 100644 --- a/src/push_solve.rs +++ b/src/push_solve.rs @@ -6,6 +6,11 @@ struct Ray{ origin:Planar64Vec3, direction:Planar64Vec3, } +impl Ray{ + fn extrapolate(&self,t:Planar64)->Planar64Vec3{ + self.origin+self.direction*t + } +} /// Information about a contact restriction pub struct Contact{ @@ -236,8 +241,27 @@ fn get_best_push_ray_and_indices( } } -pub fn push_solve(pnv_list:&Vec,point:Planar64Vec3){ - // +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)?; + + if Planar64::ZERO<=next_t{ + return Some(ray.origin); + } + + //edit the indices + if indices.len()==4{ + indices.pop(); + } + indices.push(next_index); + + let meet_point=ray.extrapolate(next_t); + match get_best_push_ray_and_indices(meet_point,pnv_list,indices){ + Some((new_ray,new_indices))=>(ray,indices)=(new_ray,new_indices), + None=>return Some(meet_point), + } + } } pub fn push_solve_with_output(pnv_list:&Vec,point:Planar64Vec3,output:&mut Vec){