push solve!

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<Contact>,point:Planar64Vec3){
-	//
+pub fn push_solve(pnv_list:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
+	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<Contact>,point:Planar64Vec3,output:&mut Vec<Planar64Vec3>){