StrafesNET/strafe-project
7
Fork 4
Code Issues 8 Pull Requests 1 Packages Projects Releases 2 Wiki Activity

refactor algorithm to use a struct

Browse Source
This commit is contained in:
Rhys Lloyd
2025-12-04 11:21:08 -08:00
parent 6f8c2692ca
commit 261bc5c845
1 changed files with 86 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
engine/physics/src/minimum_difference.rs
View File

@@ -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<Fixed<2,64>>,
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()<self.epsilon
}
}
struct EVFinder<'a>{
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<self.best_distance_squared{
best_transition=Transition::Vert(test_vert_id);
*best_distance_squared=distance_squared;
self.best_distance_squared=distance_squared;
}
}
best_transition
}
fn final_ev(&self,vert_id:MinkowskiVert,best_distance_squared:&mut Fixed<2,64>,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::<Self>{
// 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::<MinkowskiMesh<'a>>{
// 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!");
}