diff --git a/src/face_crawler.rs b/src/face_crawler.rs index cae28bb..40cee50 100644 --- a/src/face_crawler.rs +++ b/src/face_crawler.rs @@ -1,31 +1,32 @@ use crate::physics::Body; -use crate::model_physics::{FEV,MeshQuery,DirectedEdge}; -use strafesnet_common::integer::{Time,Fixed,Planar64}; +use crate::model_physics::{FEV,MeshQuery,DirectedEdge,MinkowskiMesh,MinkowskiFace,MinkowskiDirectedEdge,MinkowskiVert}; +use strafesnet_common::integer::{Time,Fixed,Ratio}; enum Transition{ Miss, - Next(FEV,Time), - Hit(F,Time), + Next(FEV,Ratio,Fixed<4,128>>), + Hit(F,Ratio,Fixed<4,128>>), } - fn next_transition(fev:&FEV,time:Time,mesh:&impl MeshQuery,body:&Body,time_limit:Time)->Transition{ +type MinkowskiFEV=FEV; +type MinkowskiTransition=Transition; + + fn next_transition(fev:&MinkowskiFEV,body_time:Ratio,Fixed<4,128>>,mesh:&MinkowskiMesh,body:&Body,mut best_time:Ratio,Fixed<4,128>>)->MinkowskiTransition{ //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; - let mut best_transtition=Transition::Miss; + let mut best_transtition=MinkowskiTransition::Miss; match fev{ - &FEV::::Face(face_id)=>{ + &MinkowskiFEV::Face(face_id)=>{ //test own face collision time, ignoring roots with zero or conflicting derivative //n=face.normal d=face.dot //n.a t^2+n.v t+n.p-d==0 let (n,d)=mesh.face_nd(face_id); //TODO: use higher precision d value? //use the mesh transform translation instead of baking it into the d value. - for t in Fixed::<2,64>::zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ - let t=body.time+Time::from(t); - if time<=t&&t::zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ + if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ + best_time=dt; + best_transtition=MinkowskiTransition::Hit(face_id,dt); break; } } @@ -35,18 +36,18 @@ enum Transition{ let n=n.cross(edge_n); let verts=mesh.edge_verts(directed_edge_id.as_undirected()); //WARNING: d is moved out of the *2 block because of adding two vertices! - for t in zeroes2(n.dot(body.position*2-(mesh.vert(verts[0])+mesh.vert(verts[1]))),n.dot(body.velocity)*2,n.dot(body.acceleration)){ - let t=body.time+Time::from(t); - if time<=t&&t::Edge(directed_edge_id.as_undirected()),t); + //WARNING: precision is swept under the rug! + for dt in Fixed::<4,128>::zeroes2(n.dot(body.position*2-(mesh.vert(verts[0])+mesh.vert(verts[1]))).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){ + if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ + best_time=dt; + best_transtition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt); break; } } } //if none: }, - &FEV::::Edge(edge_id)=>{ + &MinkowskiFEV::Edge(edge_id)=>{ //test each face collision time, ignoring roots with zero or conflicting derivative let edge_n=mesh.edge_n(edge_id); let edge_verts=mesh.edge_verts(edge_id); @@ -56,11 +57,10 @@ enum Transition{ //edge_n gets parity from the order of edge_faces let n=face_n.cross(edge_n)*((i as i64)*2-1); //WARNING yada yada d *2 - for t in zeroes2(n.dot(delta_pos),n.dot(body.velocity)*2,n.dot(body.acceleration)){ - let t=body.time+Time::from(t); - if time<=t&&t::Face(edge_face_id),t); + for dt in Fixed::<4,128>::zeroes2(n.dot(delta_pos).fix_4(),n.dot(body.velocity).fix_4()*2,n.dot(body.acceleration).fix_4()){ + if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ + best_time=dt; + best_transtition=MinkowskiTransition::Next(MinkowskiFEV::Face(edge_face_id),dt); break; } } @@ -69,27 +69,27 @@ enum Transition{ for (i,&vert_id) in edge_verts.iter().enumerate(){ //vertex normal gets parity from vert index let n=edge_n*(1-2*(i as i64)); - for t in zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ - let t=body.time+Time::from(t); - if time<=t&&t::Vert(vert_id),t); + for dt in Fixed::<2,64>::zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ + if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ + let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4()); + best_time=dt; + best_transtition=MinkowskiTransition::Next(MinkowskiFEV::Vert(vert_id),dt); break; } } } //if none: }, - &FEV::::Vert(vert_id)=>{ + &MinkowskiFEV::Vert(vert_id)=>{ //test each edge collision time, ignoring roots with zero or conflicting derivative for &directed_edge_id in mesh.vert_edges(vert_id).iter(){ //edge is directed away from vertex, but we want the dot product to turn out negative let n=-mesh.directed_edge_n(directed_edge_id); - for t in zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ - let t=body.time+Time::from(t); - if time<=t&&t::Edge(directed_edge_id.as_undirected()),t); + for dt in Fixed::<2,64>::zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){ + if body_time.le_ratio(dt)&&dt.lt_ratio(best_time)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){ + let dt=Ratio::new(dt.num.fix_4(),dt.den.fix_4()); + best_time=dt; + best_transtition=MinkowskiTransition::Next(MinkowskiFEV::Edge(directed_edge_id.as_undirected()),dt); break; } } @@ -101,14 +101,22 @@ enum Transition{ } pub enum CrawlResult{ Miss(FEV), - Hit(F,Time), + Hit(F,Ratio,Fixed<4,128>>), } -pub fn crawl_fev(mut fev:FEV,mesh:&impl MeshQuery,relative_body:&Body,start_time:Time,time_limit:Time)->CrawlResult{ - let mut time=start_time; +type MinkowskiCrawlResult=CrawlResult; +pub fn crawl_fev(mut fev:MinkowskiFEV,mesh:&MinkowskiMesh,relative_body:&Body,start_time:Time,time_limit:Time)->MinkowskiCrawlResult{ + let mut body_time={ + let r=(start_time-relative_body.time).to_ratio(); + Ratio::new(r.num.fix_4(),r.den.fix_4()) + }; + let time_limit={ + let r=(time_limit-relative_body.time).to_ratio(); + Ratio::new(r.num.fix_4(),r.den.fix_4()) + }; for _ in 0..20{ - match next_transition(&fev,time,mesh,relative_body,time_limit){ + match next_transition(&fev,body_time,mesh,relative_body,time_limit){ Transition::Miss=>return CrawlResult::Miss(fev), - Transition::Next(next_fev,next_time)=>(fev,time)=(next_fev,next_time), + Transition::Next(next_fev,next_time)=>(fev,body_time)=(next_fev,next_time), Transition::Hit(face,time)=>return CrawlResult::Hit(face,time), } }