From fcc2348eb0b9fa3529b026a2e3155bac4e13d7cf Mon Sep 17 00:00:00 2001 From: Rhys Lloyd Date: Tue, 2 Dec 2025 09:21:42 -0800 Subject: [PATCH] add unfortunate algorithm --- engine/physics/src/minimum_difference.rs | 133 ++++++++++++++++++++--- 1 file changed, 117 insertions(+), 16 deletions(-) diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index 639ae306..033bd567 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -518,6 +518,106 @@ pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{ ||false ) } + +//infinity fev algorithm state transition +#[derive(Debug)] +enum Transition{ + Done,//found closest vert, no edges are better + Vert(MinkowskiVert),//transition to vert +} +enum EV{ + Vert(MinkowskiVert), + Edge(crate::model::MinkowskiEdge), +} + +impl MinkowskiMesh<'_>{ + fn next_transition_vert(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,point:Planar64Vec3)->Transition{ + let mut best_transition=Transition::Done; + for &directed_edge_id in self.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()); + //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 distance_squared=diff.dot(diff); + if distance_squared<*best_distance_squared{ + best_transition=Transition::Vert(test_vert_id); + *best_distance_squared=distance_squared; + } + } + best_transition + } + fn final_ev(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,point:Planar64Vec3)->EV{ + 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); + //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); + //test the edge + let edge_nn=edge_n.dot(edge_n); + if !d.is_negative()&&d<=edge_nn{ + let distance_squared={ + let c=diff.cross(edge_n); + //wrap for speed + (c.dot(c)/edge_nn).divide().wrap_2() + }; + if distance_squared<=*best_distance_squared{ + best_transition=EV::Edge(directed_edge_id.as_undirected()); + *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{ + 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), + 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::{ + 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); + } + } + FEV::Edge(edge_id) + }, + } + } +} + #[derive(Debug)] pub struct OhNoes; pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option>,OhNoes>>{ @@ -536,6 +636,7 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O 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(){ @@ -549,25 +650,25 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O Simplex1_3::Simplex3([v0,v1,v2])=>{ // invert let (v0,v1,v2)=(-v0,-v1,-v2); - // 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{ - // check if a vertex of the face is v2 - let ef=mesh.edge_faces(v0e.as_undirected()); - for &ef in ef.as_ref(){ - let fe=mesh.face_edges(ef); - for e in fe.as_ref(){ - if mesh.edge_verts(e.as_undirected()).as_ref()[e.parity() as usize]==v2{ - return Some(Ok(FEV::Face(ef))); - } - } + let p0=mesh.vert(v0); + let p1=mesh.vert(v1); + let p2=mesh.vert(v2); + // spanning simplex normal + let n=(p2-p0).cross(p1-p0); + // Scan opposite vertices for coplanar ones and find a coplanar face + for &v0e in mesh.vert_edges(v0).as_ref(){ + // check if opposite vertex is coplanar + let o0=mesh.edge_verts(v0e.as_undirected()).as_ref()[v0e.parity() as usize]; + let o0p=mesh.vert(o0); + if n.dot(o0p-p0).is_zero(){ + // always take left face (or right idk just use the parity) + let f0=mesh.edge_faces(v0e.as_undirected()).as_ref()[v0e.parity() as usize]; + { + } - // we found the connecting edge which is guaranteed to be unique, but could not find the connecting face - break; } } + // Shimmy to the side until you find a face that contains the closest point Err(OhNoes) }, })