diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index 0882d624..ee712b66 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -530,31 +530,60 @@ enum EV{ Edge(crate::model::MinkowskiEdge), } -impl MinkowskiMesh<'_>{ - fn next_transition_vert(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,point:Planar64Vec3)->Transition{ +// convenience type to check if a point is within some threshold of a plane. +struct ThickPlane{ + point:Planar64Vec3, + normal:Vector3>, + epsilon:Fixed<3,96>, +} +impl ThickPlane{ + fn new(mesh:&MinkowskiMesh,[v0,v1,v2]:Simplex<3>)->Self{ + let p0=mesh.vert(v0); + let p1=mesh.vert(v1); + let p2=mesh.vert(v2); + let point=p0; + let normal=(p1-p0).cross(p2-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=normal.length().wrap_3()*3; + Self{point,normal,epsilon} + } + fn contains(&self,point:Planar64Vec3)->bool{ + (point-self.point).dot(self.normal).abs(){ + mesh:&'a MinkowskiMesh<'a>, + plane:ThickPlane, + best_distance_squared:Fixed<2,64>, +} + +impl EVFinder<'_>{ + fn next_transition_vert(&mut self,vert_id:MinkowskiVert,point:Planar64Vec3)->Transition{ // TODO: ensure test_vert_id is coplanar to simplex let mut best_transition=Transition::Done; - for &directed_edge_id in self.vert_edges(vert_id).as_ref(){ + for &directed_edge_id in self.mesh.vert_edges(vert_id).as_ref(){ //is boundary uncrossable by a crawl from infinity - let edge_verts=self.edge_verts(directed_edge_id.as_undirected()); + let edge_verts=self.mesh.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); + let diff=point-self.mesh.vert(test_vert_id); let distance_squared=diff.dot(diff); - if distance_squared<*best_distance_squared{ + if distance_squared,point:Planar64Vec3)->EV{ + fn final_ev(&mut self,vert_id:MinkowskiVert,point:Planar64Vec3)->EV{ // TODO: ensure directed_edge_id is coplanar to simplex 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); + let diff=point-self.mesh.vert(vert_id); + for &directed_edge_id in self.mesh.vert_edges(vert_id).as_ref(){ + let edge_n=self.mesh.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 let d=edge_n.dot(diff); @@ -566,60 +595,63 @@ impl MinkowskiMesh<'_>{ //wrap for speed (c.dot(c)/edge_nn).divide().wrap_2() }; - if distance_squared<=*best_distance_squared{ + if distance_squared<=self.best_distance_squared{ best_transition=EV::Edge(directed_edge_id.as_undirected()); - *best_distance_squared=distance_squared; + self.best_distance_squared=distance_squared; } } } best_transition } - fn crawl_boundaries(&self,mut vert_id:MinkowskiVert,mut best_distance_squared:Fixed<2,64>,point:Planar64Vec3)->EV{ + fn crawl_boundaries(&mut self,mut vert_id:MinkowskiVert,point:Planar64Vec3)->EV{ loop{ - match self.next_transition_vert(vert_id,&mut best_distance_squared,point){ - Transition::Done=>return self.final_ev(vert_id,&mut best_distance_squared,point), + match self.next_transition_vert(vert_id,point){ + Transition::Done=>return self.final_ev(vert_id,point), Transition::Vert(new_vert_id)=>vert_id=new_vert_id, } } } - /// 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(&self,simplex:Simplex<3>,point:Planar64Vec3)->FEV::{ - // 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-self.vert(vert_id); - (vert_id,diff.dot(diff)) - }).min_by_key(|&(_,d)|d).unwrap(); - //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(vert_id,best_distance_squared,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(){ - //both faces cannot pass this condition, return early if one does. - return FEV::Face(face_id); - } +} +/// 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::>{ + // 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 plane=ThickPlane::new(mesh,simplex); + let mut finder=EVFinder{plane,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 + match finder.crawl_boundaries(vert_id,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=mesh.edge_n(edge_id); + // point is multiplied by two because vert_sum sums two vertices. + let delta_pos=point*2-{ + let &[v0,v1]=mesh.edge_verts(edge_id).as_ref(); + mesh.vert(v0)+mesh.vert(v1) + }; + for (i,&face_id) in mesh.edge_faces(edge_id).as_ref().iter().enumerate(){ + let face_n=mesh.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(){ + //both faces cannot pass this condition, return early if one does. + return FEV::Face(face_id); } - FEV::Edge(edge_id) - }, - } + } + FEV::Edge(edge_id) + }, } } @@ -658,7 +690,7 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O // Shimmy to the side until you find a face that contains the closest point // it's ALWAYS representable as a face, but this algorithm may // return E or V in edge cases but I don't think that will break the face crawler - let fev=mesh.crawl_to_closest_fev([v0,v1,v2],point); + let fev=crawl_to_closest_fev(mesh,[v0,v1,v2],point); if !matches!(fev,FEV::Face(_)){ println!("I can't believe it's not a face!"); }