From 0da6da24ac2ffccc91667802a5bbf2f07ccae96a Mon Sep 17 00:00:00 2001
From: Quaternions <krakow20@gmail.com>
Date: Thu, 16 Nov 2023 17:03:12 -0800
Subject: [PATCH] implement final algorithm with infinity tech

---
 src/face_crawler.rs  | 20 +++++++++++++++++---
 src/integer.rs       |  2 ++
 src/model_physics.rs | 24 ++++++++++++++++--------
 src/physics.rs       | 18 ++++++++++++++++++
 4 files changed, 53 insertions(+), 11 deletions(-)

diff --git a/src/face_crawler.rs b/src/face_crawler.rs
index 41de9ec..1c4b23a 100644
--- a/src/face_crawler.rs
+++ b/src/face_crawler.rs
@@ -9,7 +9,7 @@ enum Transition<F,E:DirectedEdge,V>{
 	Hit(F,Time),
 }
 
-	pub fn next_transition_body<F:Copy,E:Copy+DirectedEdge,V:Copy>(fev:&FEV<F,E,V>,time:Time,mesh:&impl MeshQuery<F,E,V>,body:&Body,time_limit:Time)->Transition<F,E,V>{
+	fn next_transition<F:Copy,E:Copy+DirectedEdge,V:Copy>(fev:&FEV<F,E,V>,time:Time,mesh:&impl MeshQuery<F,E,V>,body:&Body,time_limit:Time)->Transition<F,E,V>{
 		//conflicting derivative means it crosses in the wrong direction.
 		//if the transition time is equal to an already tested transition, do not replace the current best.
 		let mut best_time=time_limit;
@@ -101,13 +101,27 @@ enum Transition<F,E:DirectedEdge,V>{
 		}
 		best_transtition
 	}
-pub fn crawl_fev_body<F:Copy,E:Copy+DirectedEdge,V:Copy>(mut fev:FEV<F,E,V>,mesh:&impl MeshQuery<F,E,V>,relative_body:&Body,time_limit:Time)->Option<(F,Time)>{	
+pub fn crawl_fev<F:Copy,E:Copy+DirectedEdge,V:Copy>(mut fev:FEV<F,E,V>,mesh:&impl MeshQuery<F,E,V>,relative_body:&Body,time_limit:Time)->Option<(F,Time)>{	
 	let mut time=relative_body.time;
 	loop{
-		match next_transition_body(&fev,time,mesh,relative_body,time_limit){
+		match next_transition(&fev,time,mesh,relative_body,time_limit){
 			Transition::Miss=>return None,
 			Transition::Next(next_fev,next_time)=>(fev,time)=(next_fev,next_time),
 			Transition::Hit(face,time)=>return Some((face,time)),
 		}
 	}
 }
+pub enum CrawlResult<F,E:DirectedEdge,V>{
+	Closest(FEV<F,E,V>),
+	Hit(F,Time),
+}
+pub fn crawl_fev_from_negative_infinity<F:Copy,E:Copy+DirectedEdge,V:Copy>(mut fev:FEV<F,E,V>,mesh:&impl MeshQuery<F,E,V>,relative_body:&Body)->CrawlResult<F,E,V>{	
+	let mut time=Time::MIN;
+	loop{
+		match next_transition(&fev,time,mesh,relative_body,relative_body.time){
+			Transition::Miss=>return CrawlResult::Closest(fev),
+			Transition::Next(next_fev,next_time)=>(fev,time)=(next_fev,next_time),
+			Transition::Hit(face,time)=>return CrawlResult::Hit(face,time),//algorithm breaks down inside without full fat voronoi
+		}
+	}
+}
diff --git a/src/integer.rs b/src/integer.rs
index 9ac3a14..29755b7 100644
--- a/src/integer.rs
+++ b/src/integer.rs
@@ -2,6 +2,8 @@
 #[derive(Clone,Copy,Hash,Eq,PartialEq,PartialOrd,Debug)]
 pub struct Time(i64);
 impl Time{
+	pub const MIN:Self=Self(i64::MIN);
+	pub const MAX:Self=Self(i64::MAX);
 	pub const ZERO:Self=Self(0);
 	pub const ONE_SECOND:Self=Self(1_000_000_000);
 	pub const ONE_MILLISECOND:Self=Self(1_000_000);
diff --git a/src/model_physics.rs b/src/model_physics.rs
index 4233750..a291e21 100644
--- a/src/model_physics.rs
+++ b/src/model_physics.rs
@@ -352,15 +352,23 @@ impl MinkowskiMesh<'_>{
 	fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{
 		MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir))
 	}
-	fn closest_fev(&self,point:Planar64Vec3)->FEV<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>{
-		//put some genius code right here instead of this
-		//assume that point is outside the mesh and nonzero
-		//find vertex on mesh0 farthest in point direction
-		let fev=FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Vert(self.farthest_vert(point));
-		crate::face_crawler::crawl_fev_dot(fev,self,point)
-	}
 	pub fn predict_collision_in(&self,relative_body:&crate::physics::Body,time_limit:crate::integer::Time)->Option<(MinkowskiFace,crate::integer::Time)>{
-		crate::face_crawler::crawl_fev_body(self.closest_fev(relative_body.position),self,relative_body,time_limit)
+		let start_vert=FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Vert(self.farthest_vert((-relative_body.clone()).infinity_dir()));
+		match crate::face_crawler::crawl_fev_from_negative_infinity(start_vert,self,relative_body){
+			crate::face_crawler::CrawlResult::Closest(fev)=>{
+				crate::face_crawler::crawl_fev(fev,self,relative_body,time_limit)
+			},
+			crate::face_crawler::CrawlResult::Hit(_,_)=>None,//already in or passed
+		}
+	}
+	pub fn predict_collision_out(&self,relative_body:&crate::physics::Body,time_limit:crate::integer::Time)->Option<(MinkowskiFace,crate::integer::Time)>{
+		let start_vert=FEV::<MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert>::Vert(self.farthest_vert(relative_body.infinity_dir()));
+		match crate::face_crawler::crawl_fev_from_negative_infinity(start_vert,self,&-relative_body.clone()){
+			crate::face_crawler::CrawlResult::Closest(fev)=>{
+				crate::face_crawler::crawl_fev(fev,self,&-relative_body.clone(),-time_limit).map(|t|(t.0,-t.1))
+			},
+			crate::face_crawler::CrawlResult::Hit(face,time)=>Some((face,-time)),
+		}
 	}
 	pub fn predict_collision_face_out(&self,relative_body:&crate::physics::Body,time_limit:crate::integer::Time,contact_face_id:MinkowskiFace)->Option<(MinkowskiEdge,crate::integer::Time)>{
 		//no algorithm needed, there is only one state and two cases (Edge,None)
diff --git a/src/physics.rs b/src/physics.rs
index 3415a6b..d62a37e 100644
--- a/src/physics.rs
+++ b/src/physics.rs
@@ -43,6 +43,17 @@ pub struct Body{
 	pub acceleration:Planar64Vec3,//I64 where 2^32 = 1 u/s/s
 	pub time:Time,//nanoseconds x xxxxD!
 }
+impl std::ops::Neg for Body{
+	type Output=Self;
+	fn neg(self)->Self::Output{
+		Self{
+			position:self.position,
+			velocity:-self.velocity,
+			acceleration:self.acceleration,
+			time:-self.time,
+		}
+	}
+}
 
 //hey dumbass just use a delta
 #[derive(Clone,Debug)]
@@ -955,6 +966,13 @@ impl Body{
 		self.velocity=self.extrapolated_velocity(time);
 		self.time=time;
 	}
+	pub fn infinity_dir(&self)->Planar64Vec3{
+		if self.acceleration==Planar64Vec3::ZERO{
+			self.velocity
+		}else{
+			self.acceleration
+		}
+	}
 }
 impl std::fmt::Display for Body{
 	fn fmt(&self,f:&mut std::fmt::Formatter<'_>)->std::fmt::Result{