handle non-canonnical multi-edge spanning edges

engine/physics/src/minimum_difference.rs

@@ -529,6 +529,18 @@ enum EV{
 	Vert(MinkowskiVert),
 	Edge(crate::model::MinkowskiEdge),
 }
+impl From<EV> for FEV<MinkowskiMesh<'_>>{
+	fn from(value:EV)->Self{
+		match value{
+			EV::Vert(minkowski_vert)=>FEV::Vert(minkowski_vert),
+			EV::Edge(minkowski_edge)=>FEV::Edge(minkowski_edge),
+		}
+	}
+}
+
+trait Contains{
+	fn contains(&self,point:Planar64Vec3)->bool;
+}
 
 // convenience type to check if a point is within some threshold of a plane.
 struct ThickPlane{
@@ -548,18 +560,43 @@ impl ThickPlane{
 		let epsilon=normal.length().wrap_3()*3;
 		Self{point,normal,epsilon}
 	}
+}
+impl Contains for ThickPlane{
 	fn contains(&self,point:Planar64Vec3)->bool{
 		(point-self.point).dot(self.normal).abs()<=self.epsilon
 	}
 }
 
-struct EVFinder<'a>{
+struct ThickLine{
+	point:Planar64Vec3,
+	dir:Planar64Vec3,
+	epsilon:Fixed<2,64>,
+}
+impl ThickLine{
+	fn new(mesh:&MinkowskiMesh,[v0,v1]:Simplex<2>)->Self{
+		let p0=mesh.vert(v0);
+		let p1=mesh.vert(v1);
+		let point=p0;
+		let dir=p1-p0;
+		// Allow ~ 2*sqrt(3) units of thickness on the plane
+		// This is to account for the variance of two voxels across the longest diagonal
+		let epsilon=dir.length_squared()*3;
+		Self{point,dir,epsilon}
+	}
+}
+impl Contains for ThickLine{
+	fn contains(&self,point:Planar64Vec3)->bool{
+		(point-self.point).cross(self.dir).length_squared()<=self.epsilon
+	}
+}
+
+struct EVFinder<'a,C>{
 	mesh:&'a MinkowskiMesh<'a>,
-	plane:ThickPlane,
+	constraint:C,
 	best_distance_squared:Fixed<2,64>,
 }
 
-impl EVFinder<'_>{
+impl<C:Contains> EVFinder<'_,C>{
 	fn next_transition_vert(&mut self,vert_id:MinkowskiVert,point:Planar64Vec3)->Transition{
 		let mut best_transition=Transition::Done;
 		for &directed_edge_id in self.mesh.vert_edges(vert_id).as_ref(){
@@ -571,7 +608,7 @@ impl EVFinder<'_>{
 			let diff=point-test_pos;
 			let distance_squared=diff.dot(diff);
 			// ensure test_vert_id is coplanar to simplex
-			if distance_squared<self.best_distance_squared&&self.plane.contains(test_pos){
+			if distance_squared<self.best_distance_squared&&self.constraint.contains(test_pos){
 				best_transition=Transition::Vert(test_vert_id);
 				self.best_distance_squared=distance_squared;
 			}
@@ -592,7 +629,7 @@ impl EVFinder<'_>{
 			//test the edge
 			let edge_nn=edge_n.dot(edge_n);
 			// ensure edge contains closest point and directed_edge_id is coplanar to simplex
-			if !d.is_negative()&&d<=edge_nn&&self.plane.contains(test_pos){
+			if !d.is_negative()&&d<=edge_nn&&self.constraint.contains(test_pos){
 				let distance_squared={
 					let c=diff.cross(edge_n);
 					//wrap for speed
@@ -615,6 +652,24 @@ impl EVFinder<'_>{
 		}
 	}
 }
+/// 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_ev(mesh:&MinkowskiMesh,simplex:Simplex<2>,point:Planar64Vec3)->EV{
+	// naively start at the closest vertex
+	// the closest vertex is not necessarily the one with the fewest boundary hops
+	// but it doesn't matter, we will get there regardless.
+	let (vert_id,best_distance_squared)=simplex.into_iter().map(|vert_id|{
+		let diff=point-mesh.vert(vert_id);
+		(vert_id,diff.dot(diff))
+	}).min_by_key(|&(_,d)|d).unwrap();
+
+	let constraint=ThickLine::new(mesh,simplex);
+	let mut finder=EVFinder{constraint,mesh,best_distance_squared};
+	//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
+	finder.crawl_boundaries(vert_id,point)
+}
+
 /// 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<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3>,point:Planar64Vec3)->FEV::<MinkowskiMesh<'a>>{
 	// naively start at the closest vertex
@@ -625,8 +680,8 @@ fn crawl_to_closest_fev<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3>,point:Pla
 		(vert_id,diff.dot(diff))
 	}).min_by_key(|&(_,d)|d).unwrap();
 
-	let plane=ThickPlane::new(mesh,simplex);
-	let mut finder=EVFinder{plane,mesh,best_distance_squared};
+	let constraint=ThickPlane::new(mesh,simplex);
+	let mut finder=EVFinder{constraint,mesh,best_distance_squared};
 	//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
@@ -663,9 +718,7 @@ fn crawl_to_closest_fev<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3>,point:Pla
 	}
 }
 
-#[derive(Debug)]
-pub struct OhNoes;
-pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option<Result<FEV<MinkowskiMesh<'a>>,OhNoes>>{
+pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option<FEV<MinkowskiMesh<'a>>>{
 	const ENABLE_FAST_FAIL:bool=false;
 	// TODO: remove mesh negation
 	minimum_difference::<ENABLE_FAST_FAIL,_>(&-mesh,point,
@@ -677,20 +730,15 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O
 			// Convert simplex to FEV
 			// Vertices must be inverted since the mesh is inverted
 			Some(match simplex{
-				Simplex1_3::Simplex1([v0])=>Ok(FEV::Vert(-v0)),
+				Simplex1_3::Simplex1([v0])=>FEV::Vert(-v0),
 				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(){
-						// check opposite vertex to see if it is v1
-						if mesh.edge_verts(v0e.as_undirected()).as_ref()[v0e.parity() as usize]==v1{
-							return Some(Ok(FEV::Edge(v0e.as_undirected())));
-						}
+					let ev=crawl_to_closest_ev(mesh,[v0,v1],point);
+					if !matches!(ev,EV::Edge(_)){
+						println!("I can't believe it's not an edge!");
 					}
-					Err(OhNoes)
+					ev.into()
 				},
 				Simplex1_3::Simplex3([v0,v1,v2])=>{
 					// invert
@@ -702,7 +750,7 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O
 					if !matches!(fev,FEV::Face(_)){
 						println!("I can't believe it's not a face!");
 					}
-					Ok(fev)
+					fev
 				},
 			})
 		},

engine/physics/src/model.rs

@@ -672,24 +672,12 @@ impl MinkowskiMesh<'_>{
 		}
 	}
 	pub fn predict_collision_in(&self,relative_body:&Body,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
-		let fev=match crate::minimum_difference::closest_fev_not_inside(self,relative_body.position)?{
-			Ok(fev)=>fev,
-			Err(_)=>{
-				println!("oh noes");
-				return None;
-			}
-		};
+		let fev=crate::minimum_difference::closest_fev_not_inside(self,relative_body.position)?;
 		//continue forwards along the body parabola
 		fev.crawl(self,relative_body,range.start_bound(),range.end_bound()).hit()
 	}
 	pub fn predict_collision_out(&self,relative_body:&Body,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
-		let fev=match crate::minimum_difference::closest_fev_not_inside(self,relative_body.position)?{
-			Ok(fev)=>fev,
-			Err(_)=>{
-				println!("oh noes");
-				return None;
-			}
-		};
+		let fev=crate::minimum_difference::closest_fev_not_inside(self,relative_body.position)?;
 		let (lower_bound,upper_bound)=(range.start_bound(),range.end_bound());
 		// swap and negate bounds to do a time inversion
 		let (lower_bound,upper_bound)=(upper_bound.map(|&t|-t),lower_bound.map(|&t|-t));