document failure case

engine/physics/src/model.rs

@@ -90,6 +90,16 @@ pub trait MeshQuery{
 		let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
 		(self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1)
 	}
+	/// Returns an iterator over the vertices in the direction of the directed edges.
+	/// Intended to be used to find adjacent vertices:
+	/// `self.directed_verts(self.vert_edges(vert_id).as_ref())`
+	/// TODO: rewrite this function as `adjacent_vertices`
+	fn directed_verts(&self,edges:&[Self::Edge])->impl Iterator<Item=Self::Vert>{
+		edges.iter().map(|e|{
+			let edge_verts=self.edge_verts(e.as_undirected());
+			edge_verts.as_ref()[e.parity() as usize]
+		})
+	}
 	fn vert(&self,vert_id:Self::Vert)->Planar64Vec3;
 	fn face_nd(&self,face_id:Self::Face)->(Self::Normal,Self::Offset);
 	fn face_edges(&self,face_id:Self::Face)->impl AsRef<[Self::Edge]>;
@@ -452,6 +462,8 @@ impl MeshQuery for PhysicsMeshView<'_>{
 		let vert_idx=self.topology.verts[vert_id.get() as usize].get() as usize;
 		self.data.verts[vert_idx].0
 	}
+	/// Directed edges going clockwise when looking in the direction of the face normal.
+	/// (Edit this documentation if this is wrong!)
 	fn face_edges(&self,face_id:SubmeshFaceId)->impl AsRef<[SubmeshDirectedEdgeId]>{
 		self.topology.face_topology[face_id.get() as usize].edges.as_slice()
 	}
@@ -617,17 +629,6 @@ pub struct MinkowskiMesh<'a>{
 	mesh1:TransformedMesh<'a>,
 }
 
-//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(MinkowskiEdge),
-}
-
 pub type GigaTime=Ratio<Fixed<4,128>,Fixed<4,128>>;
 pub fn into_giga_time(time:Time,relative_to:Time)->GigaTime{
 	let r=(time-relative_to).to_ratio();
@@ -644,122 +645,195 @@ impl MinkowskiMesh<'_>{
 	fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{
 		MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir))
 	}
-	fn next_transition_vert(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,infinity_dir:Planar64Vec3,point:Planar64Vec3)->Transition{
-		let mut best_transition=Transition::Done;
-		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
-			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);
-			if edge_n.dot(infinity_dir).is_zero(){
-				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>,infinity_dir:Planar64Vec3,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
-			if edge_n.dot(infinity_dir).is_zero(){
-				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,infinity_dir:Planar64Vec3,point:Planar64Vec3)->EV{
+	fn closest_fev_not_inside(&self,relative_position:Planar64Vec3)->Option<FEV<MinkowskiMesh>>{
+		// Make a fast guess as to what the closest point will be.
+		let MinkowskiVert::VertVert(mut v0,mut v1)=self.farthest_vert(relative_position);
+		// TODO: alternate naive vertex searches to improve the robustness
+		// in the "tall bipyramid" failure case
+		let mut m0v=self.mesh0.vert(v0);
+		let mut m1v=self.mesh1.vert(v1);
 		let mut best_distance_squared={
-			let diff=point-self.vert(vert_id);
+			let diff=relative_position+m1v-m0v;
 			diff.dot(diff)
 		};
+		let mut v0e=self.mesh0.vert_edges(v0);
+		let mut v1e=self.mesh1.vert_edges(v1);
+		let mut v0e_ref=v0e.as_ref();
+		let mut v1e_ref=v1e.as_ref();
+		// repeatedly check adjacent vertex permutations to see if they are closer
 		loop{
-			match self.next_transition_vert(vert_id,&mut best_distance_squared,infinity_dir,point){
-				Transition::Done=>return self.final_ev(vert_id,&mut best_distance_squared,infinity_dir,point),
-				Transition::Vert(new_vert_id)=>vert_id=new_vert_id,
+			let mut best_v0=None;
+			let mut best_v1=None;
+
+			// check vertices adjacent to v1 against v0
+			for m1_test_vert in self.mesh1.directed_verts(v1e_ref){
+				let m1v_test=self.mesh1.vert(m1_test_vert);
+				let diff=relative_position+m1v_test-m0v;
+				let d=diff.dot(diff);
+				if d<best_distance_squared{
+					best_distance_squared=d;
+					best_v0=None;
+					best_v1=Some(m1_test_vert);
+				}
 			}
-		}
-	}
-	/// 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 infinity_fev(&self,infinity_dir:Planar64Vec3,point:Planar64Vec3)->FEV::<MinkowskiMesh>{
-		//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(self.farthest_vert(infinity_dir),infinity_dir,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()&&boundary_n.dot(infinity_dir).is_zero(){
-						//both faces cannot pass this condition, return early if one does.
-						return FEV::Face(face_id);
+
+			// check vertices adjacent to v0 against v1
+			for m0_test_vert in self.mesh0.directed_verts(v0e_ref){
+				let m0v_test=self.mesh0.vert(m0_test_vert);
+				let diff=relative_position+m1v-m0v_test;
+				let d=diff.dot(diff);
+				if d<best_distance_squared{
+					best_distance_squared=d;
+					best_v0=Some(m0_test_vert);
+					best_v1=None;
+				}
+			}
+
+			// check permutations of adjacent vertices
+			for m0_test_vert in self.mesh0.directed_verts(v0e_ref){
+				let m0v_test=self.mesh0.vert(m0_test_vert);
+				for m1_test_vert in self.mesh1.directed_verts(v1e_ref){
+					let m1v_test=self.mesh1.vert(m1_test_vert);
+					let diff=relative_position+m1v_test-m0v_test;
+					let d=diff.dot(diff);
+					if d<best_distance_squared{
+						best_distance_squared=d;
+						best_v0=Some(m0_test_vert);
+						best_v1=Some(m1_test_vert);
 					}
 				}
-				FEV::Edge(edge_id)
-			},
+			}
+
+			// end condition
+			let v0_changed=match best_v0{
+				Some(new_v0)=>{
+					v0=new_v0;
+					m0v=self.mesh0.vert(v0);
+					v0e=self.mesh0.vert_edges(v0);
+					v0e_ref=v0e.as_ref();
+					true
+				},
+				None=>false,
+			};
+			let v1_changed=match best_v1{
+				Some(new_v1)=>{
+					v1=new_v1;
+					m1v=self.mesh1.vert(v1);
+					v1e=self.mesh1.vert_edges(v1);
+					v1e_ref=v1e.as_ref();
+					true
+				},
+				None=>false,
+			};
+			// if neither vertex changes, we found the closest two vertices.
+			if !(v0_changed&&v1_changed){
+				break;
+			}
 		}
-	}
-	// TODO: fundamentally improve this algorithm.
-	// All it needs to do is find the closest point on the mesh
-	// and return the FEV which the point resides on.
-	//
-	// What it actually does is use the above functions to trace a ray in from infinity,
-	// crawling the closest point along the mesh surface until the ray reaches
-	// the starting point to discover the final FEV.
-	//
-	// The actual collision prediction probably does a single test
-	// and then immediately returns with 0 FEV transitions on average,
-	// because of the strict time_limit constraint.
-	//
-	// Most of the calculation time is just calculating the starting point
-	// for the "actual" crawling algorithm below (predict_collision_{in|out}).
-	fn closest_fev_not_inside(&self,mut infinity_body:Body,start_time:Bound<&Time>)->Option<FEV<MinkowskiMesh>>{
-		infinity_body.infinity_dir().and_then(|dir|{
-			let infinity_fev=self.infinity_fev(-dir,infinity_body.position);
-			//a line is simpler to solve than a parabola
-			infinity_body.velocity=dir;
-			infinity_body.acceleration=vec3::ZERO;
-			//crawl in from negative infinity along a tangent line to get the closest fev
-			infinity_fev.crawl(self,&infinity_body,Bound::Unbounded,start_time).miss()
-		})
+		// now you have the two closest vertices
+		let mut best_fev=FEV::Vert(MinkowskiVert::VertVert(v0,v1));
+		// ==== FEV::Edge ====
+		// test VertEdges
+		for &e1 in v1e_ref{
+			let edge_verts=self.mesh1.edge_verts(e1.as_undirected());
+			let &[ev0_id,ev1_id]=edge_verts.as_ref();
+			let (ev0,ev1)=(self.mesh1.vert(ev0_id),self.mesh1.vert(ev1_id));
+			let edge_n=ev1-ev0;
+			// use relative coordinates to make including relative_position easier
+			let diff=relative_position+m0v-ev0;
+			let d=diff.dot(edge_n);
+			// is test point between edge vertices
+			let edge_nn=edge_n.dot(edge_n);
+			if d.is_positive()&&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_distance_squared=distance_squared;
+					best_fev=FEV::Edge(MinkowskiEdge::VertEdge(v0,e1.as_undirected()))
+				}
+			}
+		}
+		// test EdgeVerts
+		for &e0 in v0e_ref{
+			let edge_verts=self.mesh0.edge_verts(e0.as_undirected());
+			let &[ev0_id,ev1_id]=edge_verts.as_ref();
+			let (ev0,ev1)=(self.mesh0.vert(ev0_id),self.mesh0.vert(ev1_id));
+			let edge_n=ev1-ev0;
+			// use relative coordinates to make including relative_position easier
+			let diff=m1v-relative_position-ev0;
+			let d=diff.dot(edge_n);
+			// is test point between edge vertices
+			let edge_nn=edge_n.dot(edge_n);
+			if d.is_positive()&&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_distance_squared=distance_squared;
+					best_fev=FEV::Edge(MinkowskiEdge::EdgeVert(e0.as_undirected(),v1))
+				}
+			}
+		}
+
+		// ==== FEV::Face ====
+		// test VertFaces
+		for &f1 in self.mesh1.vert_faces(v1).as_ref(){
+			let (n,d)=self.mesh1.face_nd(f1);
+			// Test the face's voronoi column
+			for &e1 in self.mesh1.face_edges(f1).as_ref(){
+				let edge_n=self.mesh1.directed_edge_n(e1);
+				let boundary_n=n.cross(edge_n);
+				let &[ev0_id,ev1_id]=self.mesh1.edge_verts(e1.as_undirected()).as_ref();
+				let (ev0,ev1)=(self.mesh1.vert(ev0_id),self.mesh1.vert(ev1_id));
+				let diff=(relative_position+m0v)*2-(ev0+ev1);
+				if boundary_n.dot(diff).is_negative(){
+					// The test point is outside the face's voronoi column.
+					continue;
+				}
+			}
+			// Calculate distance
+			let d=n.dot(relative_position+m0v)-d;
+			// Wrap for speed
+			let distance_squared=(d*d).wrap_2();
+			if distance_squared<best_distance_squared{
+				best_distance_squared=distance_squared;
+				best_fev=FEV::Face(MinkowskiFace::VertFace(v0,f1));
+			}
+		}
+		// test FaceVerts
+		for &f0 in self.mesh0.vert_faces(v0).as_ref(){
+			let (n,d)=self.mesh0.face_nd(f0);
+			// Test the face's voronoi column
+			for &e0 in self.mesh0.face_edges(f0).as_ref(){
+				let edge_n=self.mesh0.directed_edge_n(e0);
+				let boundary_n=n.cross(edge_n);
+				let &[ev0_id,ev1_id]=self.mesh0.edge_verts(e0.as_undirected()).as_ref();
+				let (ev0,ev1)=(self.mesh0.vert(ev0_id),self.mesh0.vert(ev1_id));
+				let diff=(m1v-relative_position)*2-(ev0+ev1);
+				if boundary_n.dot(diff).is_negative(){
+					// The test point is outside the face's voronoi column.
+					continue;
+				}
+			}
+			// Calculate distance
+			let d=n.dot(relative_position+m0v)-d;
+			// Wrap for speed
+			let distance_squared=(d*d).wrap_2();
+			if distance_squared<best_distance_squared{
+				best_distance_squared=distance_squared;
+				best_fev=FEV::Face(MinkowskiFace::FaceVert(f0,v1));
+			}
+		}
+		// test EdgeEdges
+		Some(best_fev)
 	}
 	pub fn predict_collision_in(&self,relative_body:&Body,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
-		self.closest_fev_not_inside(*relative_body,range.start_bound()).and_then(|fev|{
+		self.closest_fev_not_inside(relative_body.position).and_then(|fev|{
 			//continue forwards along the body parabola
 			fev.crawl(self,relative_body,range.start_bound(),range.end_bound()).hit()
 		})
@@ -768,10 +842,9 @@ impl MinkowskiMesh<'_>{
 		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));
-		let infinity_body=-relative_body;
-		self.closest_fev_not_inside(infinity_body,lower_bound.as_ref()).and_then(|fev|{
+		self.closest_fev_not_inside(relative_body.position).and_then(|fev|{
 			//continue backwards along the body parabola
-			fev.crawl(self,&infinity_body,lower_bound.as_ref(),upper_bound.as_ref()).hit()
+			fev.crawl(self,&-relative_body,lower_bound.as_ref(),upper_bound.as_ref()).hit()
 				//no need to test -time<time_limit because of the first step
 				.map(|(face,time)|(face,-time))
 		})
@@ -807,20 +880,10 @@ impl MinkowskiMesh<'_>{
 		}
 		best_edge
 	}
-	fn infinity_in(&self,infinity_body:Body)->Option<(MinkowskiFace,GigaTime)>{
-		let infinity_fev=self.infinity_fev(-infinity_body.velocity,infinity_body.position);
-		// Bound::Included means that the surface of the mesh is included in the mesh
-		infinity_fev.crawl(self,&infinity_body,Bound::Unbounded,Bound::Included(&infinity_body.time)).hit()
-	}
 	pub fn is_point_in_mesh(&self,point:Planar64Vec3)->bool{
-		let infinity_body=Body::new(point,vec3::Y,vec3::ZERO,Time::ZERO);
-		//movement must escape the mesh forwards and backwards in time,
-		//otherwise the point is not inside the mesh
-		self.infinity_in(infinity_body)
-    	.is_some_and(|_|
-			self.infinity_in(-infinity_body)
-    		.is_some()
-     	)
+		// TODO
+		println!("Unimplemented is_point_in_mesh called! {point}");
+		false
 	}
 }
 impl MeshQuery for MinkowskiMesh<'_>{