diff --git a/src/model_physics.rs b/src/model_physics.rs index b045aae..72f10c8 100644 --- a/src/model_physics.rs +++ b/src/model_physics.rs @@ -597,6 +597,8 @@ enum EV{ Edge(MinkowskiEdge), } +pub type GigaTime=Ratio,Fixed<4,128>>; + impl MinkowskiMesh<'_>{ pub fn minkowski_sum<'a>(mesh0:TransformedMesh<'a>,mesh1:TransformedMesh<'a>)->MinkowskiMesh<'a>{ MinkowskiMesh{ @@ -709,7 +711,7 @@ impl MinkowskiMesh<'_>{ } }) } - pub fn predict_collision_in(&self,relative_body:&crate::physics::Body,time_limit:integer::Time)->Option<(MinkowskiFace,integer::Time)>{ + pub fn predict_collision_in(&self,relative_body:&crate::physics::Body,time_limit:integer::Time)->Option<(MinkowskiFace,GigaTime)>{ self.closest_fev_not_inside(relative_body.clone()).map_or(None,|fev|{ //continue forwards along the body parabola match crate::face_crawler::crawl_fev(fev,self,relative_body,relative_body.time,time_limit){ @@ -718,7 +720,7 @@ impl MinkowskiMesh<'_>{ } }) } - pub fn predict_collision_out(&self,relative_body:&crate::physics::Body,time_limit:integer::Time)->Option<(MinkowskiFace,integer::Time)>{ + pub fn predict_collision_out(&self,relative_body:&crate::physics::Body,time_limit:integer::Time)->Option<(MinkowskiFace,GigaTime)>{ //create an extrapolated body at time_limit let infinity_body=crate::physics::Body::new( relative_body.extrapolated_position(time_limit), @@ -734,10 +736,13 @@ impl MinkowskiMesh<'_>{ } }) } - pub fn predict_collision_face_out(&self,relative_body:&crate::physics::Body,time_limit:integer::Time,contact_face_id:MinkowskiFace)->Option<(MinkowskiEdge,integer::Time)>{ + pub fn predict_collision_face_out(&self,relative_body:&crate::physics::Body,time_limit:integer::Time,contact_face_id:MinkowskiFace)->Option<(MinkowskiEdge,GigaTime)>{ //no algorithm needed, there is only one state and two cases (Edge,None) //determine when it passes an edge ("sliding off" case) - let mut best_time=time_limit; + let mut best_time={ + let r=(time_limit-relative_body.time).to_ratio(); + Ratio::new(r.num.fix_4(),r.den.fix_4()) + }; let mut best_edge=None; let face_n=self.face_nd(contact_face_id).0; for &directed_edge_id in self.face_edges(contact_face_id).iter(){ @@ -747,10 +752,10 @@ impl MinkowskiMesh<'_>{ let verts=self.edge_verts(directed_edge_id.as_undirected()); let d=n.dot(self.vert(verts[0])+self.vert(verts[1])); //WARNING! d outside of *2 - for t in Fixed::<5,160>::zeroes2((n.dot(relative_body.position))*2-d,n.dot(relative_body.velocity)*2,n.dot(relative_body.acceleration)){ - let t=relative_body.time.to_ratio().add_ratio(t); - if relative_body.time.to_ratio().lt_ratio(t)&&t.lt_ratio(best_time)&&n.dot(relative_body.extrapolated_velocity(t)).is_negative(){ - best_time=t; + //WARNING: truncated precision + for dt in Fixed::<4,128>::zeroes2(((n.dot(relative_body.position))*2-d).fix_4(),n.dot(relative_body.velocity).fix_4()*2,n.dot(relative_body.acceleration).fix_4()){ + if dt.num.is_positive()&&dt.lt_ratio(best_time)&&n.dot(relative_body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ + best_time=dt; best_edge=Some(directed_edge_id); break; } @@ -758,7 +763,7 @@ impl MinkowskiMesh<'_>{ } best_edge.map(|e|(e.as_undirected(),best_time)) } - fn infinity_in(&self,infinity_body:crate::physics::Body)->Option<(MinkowskiFace,integer::Time)>{ + fn infinity_in(&self,infinity_body:crate::physics::Body)->Option<(MinkowskiFace,GigaTime)>{ let infinity_fev=self.infinity_fev(-infinity_body.velocity,infinity_body.position); match crate::face_crawler::crawl_fev(infinity_fev,self,&infinity_body,integer::Time::MIN,infinity_body.time){ crate::face_crawler::CrawlResult::Miss(_)=>None,