add unfortunate algorithm

engine/physics/src/minimum_difference.rs

@@ -518,6 +518,106 @@ pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{
 		||false
 	)
 }
+
+//infinity fev algorithm state transition
+#[derive(Debug)]
+enum Transition{
+	Done,//found closest vert, no edges are better
+	Vert(MinkowskiVert),//transition to vert
+}
+enum EV{
+	Vert(MinkowskiVert),
+	Edge(crate::model::MinkowskiEdge),
+}
+
+impl MinkowskiMesh<'_>{
+	fn next_transition_vert(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,point:Planar64Vec3)->Transition{
+		let mut best_transition=Transition::Done;
+		for &directed_edge_id in self.vert_edges(vert_id).as_ref(){
+			//is boundary uncrossable by a crawl from infinity
+			let edge_verts=self.edge_verts(directed_edge_id.as_undirected());
+			//select opposite vertex
+			let test_vert_id=edge_verts.as_ref()[directed_edge_id.parity() as usize];
+			//test if it's closer
+			let diff=point-self.vert(test_vert_id);
+			let distance_squared=diff.dot(diff);
+			if distance_squared<*best_distance_squared{
+				best_transition=Transition::Vert(test_vert_id);
+				*best_distance_squared=distance_squared;
+			}
+		}
+		best_transition
+	}
+	fn final_ev(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,point:Planar64Vec3)->EV{
+		let mut best_transition=EV::Vert(vert_id);
+		let diff=point-self.vert(vert_id);
+		for &directed_edge_id in self.vert_edges(vert_id).as_ref(){
+			let edge_n=self.directed_edge_n(directed_edge_id);
+			//is boundary uncrossable by a crawl from infinity
+			//check if time of collision is outside Time::MIN..Time::MAX
+			let d=edge_n.dot(diff);
+			//test the edge
+			let edge_nn=edge_n.dot(edge_n);
+			if !d.is_negative()&&d<=edge_nn{
+				let distance_squared={
+					let c=diff.cross(edge_n);
+					//wrap for speed
+					(c.dot(c)/edge_nn).divide().wrap_2()
+				};
+				if distance_squared<=*best_distance_squared{
+					best_transition=EV::Edge(directed_edge_id.as_undirected());
+					*best_distance_squared=distance_squared;
+				}
+			}
+		}
+		best_transition
+	}
+	fn crawl_boundaries(&self,mut vert_id:MinkowskiVert,mut best_distance_squared:Fixed<2,64>,point:Planar64Vec3)->EV{
+		loop{
+			match self.next_transition_vert(vert_id,&mut best_distance_squared,point){
+				Transition::Done=>return self.final_ev(vert_id,&mut best_distance_squared,point),
+				Transition::Vert(new_vert_id)=>vert_id=new_vert_id,
+			}
+		}
+	}
+	/// This function drops a vertex down to an edge or a face if the path from infinity did not cross any vertex-edge boundaries but the point is supposed to have already crossed a boundary down from a vertex
+	fn crawl_to_closest_fev(&self,simplex:Simplex<3>,point:Planar64Vec3)->FEV::<Self>{
+		let (vert_id,best_distance_squared)=simplex.into_iter().map(|vert_id|{
+			let diff=point-self.vert(vert_id);
+			(vert_id,diff.dot(diff))
+		}).min_by_key(|&(_,d)|d).unwrap();
+		//start on any vertex
+		//cross uncrossable vertex-edge boundaries until you find the closest vertex or edge
+		//cross edge-face boundary if it's uncrossable
+		match self.crawl_boundaries(vert_id,best_distance_squared,point){
+			//if a vert is returned, it is the closest point to the infinity point
+			EV::Vert(vert_id)=>FEV::Vert(vert_id),
+			EV::Edge(edge_id)=>{
+				//cross to face if the boundary is not crossable and we are on the wrong side
+				let edge_n=self.edge_n(edge_id);
+				// point is multiplied by two because vert_sum sums two vertices.
+				let delta_pos=point*2-{
+					let &[v0,v1]=self.edge_verts(edge_id).as_ref();
+					self.vert(v0)+self.vert(v1)
+				};
+				for (i,&face_id) in self.edge_faces(edge_id).as_ref().iter().enumerate(){
+					let face_n=self.face_nd(face_id).0;
+					//edge-face boundary nd, n facing out of the face towards the edge
+					let boundary_n=face_n.cross(edge_n)*(i as i64*2-1);
+					let boundary_d=boundary_n.dot(delta_pos);
+					//check if time of collision is outside Time::MIN..Time::MAX
+					//infinity_dir can always be treated as a velocity
+					if !boundary_d.is_positive(){
+						//both faces cannot pass this condition, return early if one does.
+						return FEV::Face(face_id);
+					}
+				}
+				FEV::Edge(edge_id)
+			},
+		}
+	}
+}
+
 #[derive(Debug)]
 pub struct OhNoes;
 pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option<Result<FEV<MinkowskiMesh<'a>>,OhNoes>>{
@@ -536,6 +636,7 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O
 				Simplex1_3::Simplex2([v0,v1])=>{
 					// invert
 					let (v0,v1)=(-v0,-v1);
+					// TODO: handle non-canonnical multi-edge spanning edges
 					// dumbest stupidest brute force search
 					let v0e=mesh.vert_edges(v0);
 					for &v0e in v0e.as_ref(){
@@ -549,25 +650,25 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O
 				Simplex1_3::Simplex3([v0,v1,v2])=>{
 					// invert
 					let (v0,v1,v2)=(-v0,-v1,-v2);
-					// dumbest stupidest brute force search
-					let v0e=mesh.vert_edges(v0);
-					for &v0e in v0e.as_ref(){
-						// check opposite vertex to see if it is v1
-						if mesh.edge_verts(v0e.as_undirected()).as_ref()[v0e.parity() as usize]==v1{
-							// check if a vertex of the face is v2
-							let ef=mesh.edge_faces(v0e.as_undirected());
-							for &ef in ef.as_ref(){
-								let fe=mesh.face_edges(ef);
-								for e in fe.as_ref(){
-									if mesh.edge_verts(e.as_undirected()).as_ref()[e.parity() as usize]==v2{
-										return Some(Ok(FEV::Face(ef)));
-									}
-								}
+					let p0=mesh.vert(v0);
+					let p1=mesh.vert(v1);
+					let p2=mesh.vert(v2);
+					// spanning simplex normal
+					let n=(p2-p0).cross(p1-p0);
+					// Scan opposite vertices for coplanar ones and find a coplanar face
+					for &v0e in mesh.vert_edges(v0).as_ref(){
+						// check if opposite vertex is coplanar
+						let o0=mesh.edge_verts(v0e.as_undirected()).as_ref()[v0e.parity() as usize];
+						let o0p=mesh.vert(o0);
+						if n.dot(o0p-p0).is_zero(){
+							// always take left face (or right idk just use the parity)
+							let f0=mesh.edge_faces(v0e.as_undirected()).as_ref()[v0e.parity() as usize];
+							{
+
 							}
-							// we found the connecting edge which is guaranteed to be unique, but could not find the connecting face
-							break;
 						}
 					}
+					// Shimmy to the side until you find a face that contains the closest point
 					Err(OhNoes)
 				},
 			})