From 2e77366a17bce47834ddd9d667feecb4fda5913c Mon Sep 17 00:00:00 2001 From: Rhys Lloyd Date: Fri, 5 Dec 2025 09:55:56 -0800 Subject: [PATCH] handle non-canonnical multi-edge spanning edges --- engine/physics/src/minimum_difference.rs | 90 ++++++++++++++++++------ engine/physics/src/model.rs | 16 +---- 2 files changed, 71 insertions(+), 35 deletions(-) diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index 67cde17c..eb6bf7d1 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -529,6 +529,18 @@ enum EV{ Vert(MinkowskiVert), Edge(crate::model::MinkowskiEdge), } +impl From for FEV>{ + fn from(value:EV)->Self{ + match value{ + EV::Vert(minkowski_vert)=>FEV::Vert(minkowski_vert), + EV::Edge(minkowski_edge)=>FEV::Edge(minkowski_edge), + } + } +} + +trait Contains{ + fn contains(&self,point:Planar64Vec3)->bool; +} // convenience type to check if a point is within some threshold of a plane. struct ThickPlane{ @@ -548,18 +560,43 @@ impl ThickPlane{ let epsilon=normal.length().wrap_3()*3; Self{point,normal,epsilon} } +} +impl Contains for ThickPlane{ fn contains(&self,point:Planar64Vec3)->bool{ (point-self.point).dot(self.normal).abs()<=self.epsilon } } -struct EVFinder<'a>{ +struct ThickLine{ + point:Planar64Vec3, + dir:Planar64Vec3, + epsilon:Fixed<2,64>, +} +impl ThickLine{ + fn new(mesh:&MinkowskiMesh,[v0,v1]:Simplex<2>)->Self{ + let p0=mesh.vert(v0); + let p1=mesh.vert(v1); + let point=p0; + let dir=p1-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=dir.length_squared()*3; + Self{point,dir,epsilon} + } +} +impl Contains for ThickLine{ + fn contains(&self,point:Planar64Vec3)->bool{ + (point-self.point).cross(self.dir).length_squared()<=self.epsilon + } +} + +struct EVFinder<'a,C>{ mesh:&'a MinkowskiMesh<'a>, - plane:ThickPlane, + constraint:C, best_distance_squared:Fixed<2,64>, } -impl EVFinder<'_>{ +impl EVFinder<'_,C>{ fn next_transition_vert(&mut self,vert_id:MinkowskiVert,point:Planar64Vec3)->Transition{ let mut best_transition=Transition::Done; for &directed_edge_id in self.mesh.vert_edges(vert_id).as_ref(){ @@ -571,7 +608,7 @@ impl EVFinder<'_>{ let diff=point-test_pos; let distance_squared=diff.dot(diff); // ensure test_vert_id is coplanar to simplex - if distance_squared{ //test the edge let edge_nn=edge_n.dot(edge_n); // ensure edge contains closest point and directed_edge_id is coplanar to simplex - if !d.is_negative()&&d<=edge_nn&&self.plane.contains(test_pos){ + if !d.is_negative()&&d<=edge_nn&&self.constraint.contains(test_pos){ let distance_squared={ let c=diff.cross(edge_n); //wrap for speed @@ -615,6 +652,24 @@ impl EVFinder<'_>{ } } } +/// 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_ev(mesh:&MinkowskiMesh,simplex:Simplex<2>,point:Planar64Vec3)->EV{ + // 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 constraint=ThickLine::new(mesh,simplex); + let mut finder=EVFinder{constraint,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 + finder.crawl_boundaries(vert_id,point) +} + /// 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 @@ -625,8 +680,8 @@ fn crawl_to_closest_fev<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3>,point:Pla (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}; + let constraint=ThickPlane::new(mesh,simplex); + let mut finder=EVFinder{constraint,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 @@ -663,9 +718,7 @@ fn crawl_to_closest_fev<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3>,point:Pla } } -#[derive(Debug)] -pub struct OhNoes; -pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option>,OhNoes>>{ +pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option>>{ const ENABLE_FAST_FAIL:bool=false; // TODO: remove mesh negation minimum_difference::(&-mesh,point, @@ -677,20 +730,15 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O // Convert simplex to FEV // Vertices must be inverted since the mesh is inverted Some(match simplex{ - Simplex1_3::Simplex1([v0])=>Ok(FEV::Vert(-v0)), + Simplex1_3::Simplex1([v0])=>FEV::Vert(-v0), Simplex1_3::Simplex2([v0,v1])=>{ // invert let (v0,v1)=(-v0,-v1); - // TODO: handle non-canonnical multi-edge spanning edges - // dumbest stupidest brute force search - let v0e=mesh.vert_edges(v0); - for &v0e in v0e.as_ref(){ - // check opposite vertex to see if it is v1 - if mesh.edge_verts(v0e.as_undirected()).as_ref()[v0e.parity() as usize]==v1{ - return Some(Ok(FEV::Edge(v0e.as_undirected()))); - } + let ev=crawl_to_closest_ev(mesh,[v0,v1],point); + if !matches!(ev,EV::Edge(_)){ + println!("I can't believe it's not an edge!"); } - Err(OhNoes) + ev.into() }, Simplex1_3::Simplex3([v0,v1,v2])=>{ // invert @@ -702,7 +750,7 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O if !matches!(fev,FEV::Face(_)){ println!("I can't believe it's not a face!"); } - Ok(fev) + fev }, }) }, diff --git a/engine/physics/src/model.rs b/engine/physics/src/model.rs index 4ba59eef..4ed9d60f 100644 --- a/engine/physics/src/model.rs +++ b/engine/physics/src/model.rs @@ -672,24 +672,12 @@ impl MinkowskiMesh<'_>{ } } pub fn predict_collision_in(&self,relative_body:&Body,range:impl RangeBounds