From d98dca548a575ca80c494cabfb5e51e6722d55e0 Mon Sep 17 00:00:00 2001 From: Rhys Lloyd Date: Thu, 27 Nov 2025 13:37:47 -0800 Subject: [PATCH] rewrite MeshQuery --- engine/physics/src/face_crawler.rs | 22 ++-- engine/physics/src/lib.rs | 1 + engine/physics/src/mesh_query.rs | 56 ++++++++++ engine/physics/src/minimum_difference.rs | 78 +++++++++----- engine/physics/src/minkowski.rs | 109 ++++++++++++-------- engine/physics/src/model.rs | 125 ++++++++++------------- engine/physics/src/physics.rs | 3 +- 7 files changed, 245 insertions(+), 149 deletions(-) create mode 100644 engine/physics/src/mesh_query.rs diff --git a/engine/physics/src/face_crawler.rs b/engine/physics/src/face_crawler.rs index 3cc3e1cf..dc2d9541 100644 --- a/engine/physics/src/face_crawler.rs +++ b/engine/physics/src/face_crawler.rs @@ -1,20 +1,21 @@ -use crate::model::{into_giga_time,GigaTime,FEV,MeshQuery,DirectedEdge}; -use strafesnet_common::integer::{Fixed,Ratio,vec3::Vector3}; +use crate::model::{into_giga_time,GigaTime}; +use strafesnet_common::integer::{Fixed,Ratio,vec3::Vector3,Planar64Vec3}; use crate::physics::{Time,Trajectory}; +use crate::mesh_query::{FEV,DirectedEdge,MeshQuery,MeshTopology}; use core::ops::Bound; -enum Transition{ +enum Transition{ Miss, Next(FEV,GigaTime), Hit(M::Face,GigaTime), } -pub enum CrawlResult{ +pub enum CrawlResult{ Miss(FEV), Hit(M::Face,GigaTime), } -impl CrawlResult{ +impl CrawlResult{ pub fn hit(self)->Option<(M::Face,GigaTime)>{ match self{ CrawlResult::Miss(_)=>None, @@ -64,11 +65,12 @@ where } } -impl,Offset=Fixed<4,128>>> FEV +impl,Offset=Fixed<4,128>,Position=Planar64Vec3,Direction=Planar64Vec3>> FEV where // This is hardcoded for MinkowskiMesh lol M::Face:Copy, M::Edge:Copy, + M::DirectedEdge:Copy, M::Vert:Copy, F:core::ops::Mul,Output=Fixed<4,128>>, >>::Output:core::iter::Sum, @@ -94,7 +96,7 @@ impl,Offset=Fixed<4,128>>> FEV } } //test each edge collision time, ignoring roots with zero or conflicting derivative - for &directed_edge_id in mesh.face_edges(face_id).as_ref(){ + mesh.for_each_face_edge(face_id,|directed_edge_id|{ let edge_n=mesh.directed_edge_n(directed_edge_id); let n=n.cross(edge_n); let &[v0,v1]=mesh.edge_verts(directed_edge_id.as_undirected()).as_ref(); @@ -108,7 +110,7 @@ impl,Offset=Fixed<4,128>>> FEV break; } } - } + }); //if none: }, &FEV::Edge(edge_id)=>{ @@ -149,7 +151,7 @@ impl,Offset=Fixed<4,128>>> FEV }, &FEV::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).as_ref(){ + mesh.for_each_vert_edge(vert_id,|directed_edge_id|{ //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 dt in Fixed::<2,64>::zeroes2((n.dot(trajectory.position-mesh.vert(vert_id)))*2,n.dot(trajectory.velocity)*2,n.dot(trajectory.acceleration)){ @@ -160,7 +162,7 @@ impl,Offset=Fixed<4,128>>> FEV break; } } - } + }); //if none: }, } diff --git a/engine/physics/src/lib.rs b/engine/physics/src/lib.rs index ec5e84ad..1c0f3ce6 100644 --- a/engine/physics/src/lib.rs +++ b/engine/physics/src/lib.rs @@ -1,5 +1,6 @@ mod body; mod face_crawler; +mod mesh_query; mod minkowski; mod model; mod push_solve; diff --git a/engine/physics/src/mesh_query.rs b/engine/physics/src/mesh_query.rs new file mode 100644 index 00000000..20fddf1c --- /dev/null +++ b/engine/physics/src/mesh_query.rs @@ -0,0 +1,56 @@ +pub enum FEV{ + Vert(M::Vert), + Edge(M::Edge), + Face(M::Face), +} + +pub trait UndirectedEdge{ + type DirectedEdge:DirectedEdge; + fn as_directed(self,parity:bool)->Self::DirectedEdge; +} +pub trait DirectedEdge{ + type UndirectedEdge:UndirectedEdge; + fn as_undirected(self)->Self::UndirectedEdge; + fn parity(&self)->bool; + fn reverse(self)->Self + where + Self:Sized + { + let parity=!self.parity(); + self.as_undirected().as_directed(parity) + } +} + +pub trait MeshTopology{ + type Face; + type Edge:UndirectedEdge; + type DirectedEdge:DirectedEdge; + type Vert; + fn for_each_vert_edge(&self,vert_id:Self::Vert,f:impl FnMut(Self::DirectedEdge)); + fn for_each_vert_face(&self,vert_id:Self::Vert,f:impl FnMut(Self::Face)); + fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>; + fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>; + #[expect(unused)] + fn for_each_face_vert(&self,face_id:Self::Face,f:impl FnMut(Self::Vert)); + fn for_each_face_edge(&self,face_id:Self::Face,f:impl FnMut(Self::DirectedEdge)); +} + +// Make face_nd d value relative +// euclidean point? +// Simplex physics +// Directed edge necessary? +// recursive for_each function calls +// define faces from vertices (Fixed<2> vs Fixed<3>) +pub trait MeshQuery:MeshTopology{ + type Position; + type Direction; + type Normal; + type Offset; + fn vert(&self,vert_id:Self::Vert)->Self::Position; + fn farthest_vert(&self,dir:Self::Direction)->Self::Vert; + /// This must return a point inside the mesh. + fn hint_point(&self)->Self::Position; + fn face_nd(&self,face_id:Self::Face)->(Self::Normal,Self::Offset); + fn edge_n(&self,edge_id:Self::Edge)->Self::Direction; + fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction; +} diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index cbc40114..2dfd0878 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -2,7 +2,7 @@ use strafesnet_common::integer::vec3; use strafesnet_common::integer::vec3::Vector3; use strafesnet_common::integer::{Fixed,Planar64,Planar64Vec3}; -use crate::model::{DirectedEdge,FEV,MeshQuery}; +use crate::mesh_query::{FEV,DirectedEdge,MeshQuery,MeshTopology}; // TODO: remove mesh invert use crate::minkowski::{MinkowskiMesh,MinkowskiVert}; @@ -46,7 +46,7 @@ local function absDet(r, u, v, w) end */ impl Simplex2_4{ - fn det_is_zero>(self,mesh:&M)->bool{ + fn det_is_zero>(self,mesh:&M)->bool{ match self{ Self::Simplex4([p0,p1,p2,p3])=>{ let p0=mesh.vert(p0); @@ -131,11 +131,13 @@ fn narrow_dir3(dir:Vector3>)->Planar64Vec3{ }.narrow_1().unwrap() } -fn reduce1( +fn reduce1>( [v0]:Simplex<1,M::Vert>, mesh:&M, point:Planar64Vec3, -)->Reduced{ +)->Reduced + where M::Vert:Copy, +{ // --debug.profilebegin("reduceSimplex0") // local a = a1 - a0 let p0=mesh.vert(v0); @@ -160,11 +162,14 @@ fn reduce1( } // local function reduceSimplex1(a0, a1, b0, b1) -fn reduce2( +fn reduce2>( [v0,v1]:Simplex<2,M::Vert>, mesh:&M, point:Planar64Vec3, -)->Reduced{ +)->Reduced + where + M::Vert:Copy +{ // --debug.profilebegin("reduceSimplex1") // local a = a1 - a0 // local b = b1 - b0 @@ -217,11 +222,14 @@ fn reduce2( } // local function reduceSimplex2(a0, a1, b0, b1, c0, c1) -fn reduce3( +fn reduce3>( [v0,mut v1,v2]:Simplex<3,M::Vert>, mesh:&M, point:Planar64Vec3, -)->Reduced{ +)->Reduced + where + M::Vert:Copy +{ // --debug.profilebegin("reduceSimplex2") // local a = a1 - a0 // local b = b1 - b0 @@ -326,11 +334,14 @@ fn reduce3( } // local function reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1) -fn reduce4( +fn reduce4>( [v0,mut v1,mut v2,v3]:Simplex<4,M::Vert>, mesh:&M, point:Planar64Vec3, -)->Reduce{ +)->Reduce + where + M::Vert:Copy +{ // --debug.profilebegin("reduceSimplex3") // local a = a1 - a0 // local b = b1 - b0 @@ -516,7 +527,10 @@ enum Reduce{ } impl Simplex2_4{ - fn reduce>(self,mesh:&M,point:Planar64Vec3)->Reduce{ + fn reduce>(self,mesh:&M,point:Planar64Vec3)->Reduce + where + M::Vert:Copy + { match self{ Self::Simplex2(simplex)=>Reduce::Reduced(reduce2(simplex,mesh,point)), Self::Simplex3(simplex)=>Reduce::Reduced(reduce3(simplex,mesh,point)), @@ -531,11 +545,11 @@ enum Transition{ Done,//found closest vert, no edges are better Vert(Vert),//transition to vert } -enum EV{ +enum EV{ Vert(M::Vert), - Edge(::UndirectedEdge), + Edge(M::Edge), } -impl From> for FEV{ +impl From> for FEV{ fn from(value:EV)->Self{ match value{ EV::Vert(minkowski_vert)=>FEV::Vert(minkowski_vert), @@ -555,7 +569,7 @@ struct ThickPlane{ epsilon:Fixed<3,96>, } impl ThickPlane{ - fn new(mesh:&M,[v0,v1,v2]:Simplex<3,M::Vert>)->Self{ + fn new>(mesh:&M,[v0,v1,v2]:Simplex<3,M::Vert>)->Self{ let p0=mesh.vert(v0); let p1=mesh.vert(v1); let p2=mesh.vert(v2); @@ -579,7 +593,7 @@ struct ThickLine{ epsilon:Fixed<4,128>, } impl ThickLine{ - fn new(mesh:&M,[v0,v1]:Simplex<2,M::Vert>)->Self{ + fn new>(mesh:&M,[v0,v1]:Simplex<2,M::Vert>)->Self{ let p0=mesh.vert(v0); let p1=mesh.vert(v1); let point=p0; @@ -602,10 +616,14 @@ struct EVFinder<'a,M,C>{ best_distance_squared:Fixed<2,64>, } -impl EVFinder<'_,M,C>{ +impl,C:Contains> EVFinder<'_,M,C> + where + M::Vert:Copy, + M::DirectedEdge:Copy, +{ fn next_transition_vert(&mut self,vert_id:M::Vert,point:Planar64Vec3)->Transition{ let mut best_transition=Transition::Done; - for &directed_edge_id in self.mesh.vert_edges(vert_id).as_ref(){ + self.mesh.for_each_vert_edge(vert_id,|directed_edge_id|{ //test if this edge's opposite vertex closer let edge_verts=self.mesh.edge_verts(directed_edge_id.as_undirected()); //select opposite vertex @@ -618,14 +636,14 @@ impl EVFinder<'_,M,C>{ best_transition=Transition::Vert(test_vert_id); self.best_distance_squared=distance_squared; } - } + }); best_transition } fn final_ev(&mut self,vert_id:M::Vert,point:Planar64Vec3)->EV{ let mut best_transition=EV::Vert(vert_id); let vert_pos=self.mesh.vert(vert_id); let diff=point-vert_pos; - for &directed_edge_id in self.mesh.vert_edges(vert_id).as_ref(){ + self.mesh.for_each_vert_edge(vert_id,|directed_edge_id|{ //test if this edge is closer let edge_verts=self.mesh.edge_verts(directed_edge_id.as_undirected()); let test_vert_id=edge_verts.as_ref()[directed_edge_id.parity() as usize]; @@ -646,10 +664,13 @@ impl EVFinder<'_,M,C>{ self.best_distance_squared=distance_squared; } } - } + }); best_transition } - fn crawl_boundaries(&mut self,mut vert_id:M::Vert,point:Planar64Vec3)->EV{ + fn crawl_boundaries(&mut self,mut vert_id:M::Vert,point:Planar64Vec3)->EV + where + M::Vert:Copy + { loop{ match self.next_transition_vert(vert_id,point){ Transition::Done=>return self.final_ev(vert_id,point), @@ -659,7 +680,11 @@ impl EVFinder<'_,M,C>{ } } /// This function hops along parallel vertices until it finds the EV which contains the closest point to `point`. -fn crawl_to_closest_ev(mesh:&M,simplex:Simplex<2,M::Vert>,point:Planar64Vec3)->EV{ +fn crawl_to_closest_ev>(mesh:&M,simplex:Simplex<2,M::Vert>,point:Planar64Vec3)->EV + where + M::Vert:Copy, + M::DirectedEdge:Copy, +{ // 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. @@ -787,13 +812,16 @@ pub fn contains_point(mesh:&MinkowskiMesh<'_>,point:Planar64Vec3)->bool{ // queryQ, radiusQ, // exitRadius, testIntersection // ) -fn minimum_difference( +fn minimum_difference>( mesh:&M, point:Planar64Vec3, on_exact:impl FnOnce(bool,Simplex1_3)->T, on_escape:impl FnOnce(Simplex<4,M::Vert>)->T, on_fast_fail:impl FnOnce()->T, -)->T{ +)->T + where + M::Vert:Copy +{ // local initialAxis = queryQ() - queryP() // local new_point_p = queryP(initialAxis) // local new_point_q = queryQ(-initialAxis) diff --git a/engine/physics/src/minkowski.rs b/engine/physics/src/minkowski.rs index 913ca8dd..bbb87486 100644 --- a/engine/physics/src/minkowski.rs +++ b/engine/physics/src/minkowski.rs @@ -3,7 +3,7 @@ use core::ops::{Bound,RangeBounds}; use strafesnet_common::integer::{Planar64Vec3,Ratio,Fixed,vec3::Vector3}; use crate::model::into_giga_time; use crate::model::{SubmeshVertId,SubmeshEdgeId,SubmeshDirectedEdgeId,SubmeshFaceId,TransformedMesh,GigaTime}; -use crate::model::{MeshQuery,DirectedEdge,UndirectedEdge}; +use crate::mesh_query::{MeshQuery,MeshTopology,DirectedEdge,UndirectedEdge}; use crate::physics::{Time,Trajectory}; struct AsRefHelper(T); @@ -13,7 +13,6 @@ impl AsRef for AsRefHelper{ } } - //Note that a face on a minkowski mesh refers to a pair of fevs on the meshes it's summed from //(face,vertex) //(edge,edge) @@ -134,7 +133,7 @@ impl MinkowskiMesh<'_>{ let mut best_time=range.end_bound().map(|&t|into_giga_time(t,trajectory.time)); let mut best_edge=None; let face_n=self.face_nd(contact_face_id).0; - for &directed_edge_id in self.face_edges(contact_face_id).as_ref(){ + self.for_each_face_edge(contact_face_id,|directed_edge_id|{ let edge_n=self.directed_edge_n(directed_edge_id); //f x e points in let n=face_n.cross(edge_n); @@ -150,7 +149,7 @@ impl MinkowskiMesh<'_>{ break; } } - } + }); best_edge } pub fn contains_point(&self,point:Planar64Vec3)->bool{ @@ -158,9 +157,8 @@ impl MinkowskiMesh<'_>{ } } impl MeshQuery for MinkowskiMesh<'_>{ - type Face=MinkowskiFace; - type Edge=MinkowskiDirectedEdge; - type Vert=MinkowskiVert; + type Direction=Planar64Vec3; + type Position=Planar64Vec3; type Normal=Vector3>; type Offset=Fixed<4,128>; // TODO: relative d @@ -199,21 +197,40 @@ impl MeshQuery for MinkowskiMesh<'_>{ fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{ MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir)) } - fn vert_edges(&self,vert_id:MinkowskiVert)->impl AsRef<[MinkowskiDirectedEdge]>{ + fn edge_n(&self,edge_id:Self::Edge)->Self::Direction{ + let &[v0,v1]=self.edge_verts(edge_id).as_ref(); + self.vert(v1)-self.vert(v0) + } + fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction{ + let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref(); + (self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1) + } +} +impl MeshTopology for MinkowskiMesh<'_>{ + type Face=MinkowskiFace; + type Edge=MinkowskiEdge; + type DirectedEdge=MinkowskiDirectedEdge; + type Vert=MinkowskiVert; + fn for_each_vert_edge(&self,vert_id:Self::Vert,mut f:impl FnMut(Self::DirectedEdge)){ match vert_id{ MinkowskiVert::VertVert(v0,v1)=>{ - let mut edges=Vec::new(); //detect shared volume when the other mesh is mirrored along a test edge dir - let v0f_thing=self.mesh0.vert_faces(v0); - let v1f_thing=self.mesh1.vert_faces(v1); - let v0f=v0f_thing.as_ref(); - let v1f=v1f_thing.as_ref(); + let v0f={ + let mut faces=Vec::new(); + self.mesh0.for_each_vert_face(v0,|face|faces.push(face)); + faces + }; + let v1f={ + let mut faces=Vec::new(); + self.mesh1.for_each_vert_face(v1,|face|faces.push(face)); + faces + }; let v0f_n:Vec<_>=v0f.iter().map(|&face_id|self.mesh0.face_nd(face_id).0).collect(); let v1f_n:Vec<_>=v1f.iter().map(|&face_id|self.mesh1.face_nd(face_id).0).collect(); // scratch vector let mut face_normals=Vec::with_capacity(v0f.len()+v1f.len()); face_normals.clone_from(&v0f_n); - for &directed_edge_id in self.mesh0.vert_edges(v0).as_ref(){ + self.mesh0.for_each_vert_edge(v0,|directed_edge_id|{ let n=self.mesh0.directed_edge_n(directed_edge_id); let nn=n.dot(n); // TODO: there's gotta be a better way to do this @@ -226,11 +243,11 @@ impl MeshQuery for MinkowskiMesh<'_>{ face_normals.push(*face_n-(n*face_n.dot(n)*2/nn).divide().wrap_3()); } if is_empty_volume(&face_normals){ - edges.push(MinkowskiDirectedEdge::EdgeVert(directed_edge_id,v1)); + f(MinkowskiDirectedEdge::EdgeVert(directed_edge_id,v1)); } - } + }); face_normals.clone_from(&v1f_n); - for &directed_edge_id in self.mesh1.vert_edges(v1).as_ref(){ + self.mesh1.for_each_vert_edge(v1,|directed_edge_id|{ let n=self.mesh1.directed_edge_n(directed_edge_id); let nn=n.dot(n); // drop faces beyond v1f_n @@ -241,30 +258,31 @@ impl MeshQuery for MinkowskiMesh<'_>{ face_normals.push(*face_n-(n*face_n.dot(n)*2/nn).divide().wrap_3()); } if is_empty_volume(&face_normals){ - edges.push(MinkowskiDirectedEdge::VertEdge(v0,directed_edge_id)); + f(MinkowskiDirectedEdge::VertEdge(v0,directed_edge_id)); } - } - edges + }); }, } } - fn vert_faces(&self,_vert_id:MinkowskiVert)->impl AsRef<[MinkowskiFace]>{ - unimplemented!(); - #[expect(unreachable_code)] - Vec::new() + fn for_each_vert_face(&self,_vert_id:Self::Vert,_f:impl FnMut(Self::Face)){ + unimplemented!() } - fn edge_faces(&self,edge_id:MinkowskiEdge)->impl AsRef<[MinkowskiFace;2]>{ + fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>{ match edge_id{ MinkowskiEdge::VertEdge(v0,e1)=>{ //faces are listed backwards from the minkowski mesh - let v0e=self.mesh0.vert_edges(v0); + let v0e={ + let mut edges=Vec::new(); + self.mesh0.for_each_vert_edge(v0,|edge|edges.push(edge)); + edges + }; let &[e1f0,e1f1]=self.mesh1.edge_faces(e1).as_ref(); AsRefHelper([(e1f1,false),(e1f0,true)].map(|(edge_face_id1,face_parity)|{ let mut best_edge=None; let mut best_d:Ratio,Fixed<8,256>>=Ratio::new(Fixed::ZERO,Fixed::ONE); let edge_face1_n=self.mesh1.face_nd(edge_face_id1).0; let edge_face1_nn=edge_face1_n.dot(edge_face1_n); - for &directed_edge_id0 in v0e.as_ref(){ + for &directed_edge_id0 in &v0e{ let edge0_n=self.mesh0.directed_edge_n(directed_edge_id0); //must be behind other face. let d=edge_face1_n.dot(edge0_n); @@ -287,14 +305,18 @@ impl MeshQuery for MinkowskiMesh<'_>{ }, MinkowskiEdge::EdgeVert(e0,v1)=>{ //tracking index with an external variable because .enumerate() is not available - let v1e=self.mesh1.vert_edges(v1); + let v1e={ + let mut edges=Vec::new(); + self.mesh1.for_each_vert_edge(v1,|edge|edges.push(edge)); + edges + }; let &[e0f0,e0f1]=self.mesh0.edge_faces(e0).as_ref(); AsRefHelper([(e0f0,true),(e0f1,false)].map(|(edge_face_id0,face_parity)|{ let mut best_edge=None; let mut best_d:Ratio,Fixed<8,256>>=Ratio::new(Fixed::ZERO,Fixed::ONE); let edge_face0_n=self.mesh0.face_nd(edge_face_id0).0; let edge_face0_nn=edge_face0_n.dot(edge_face0_n); - for &directed_edge_id1 in v1e.as_ref(){ + for &directed_edge_id1 in &v1e{ let edge1_n=self.mesh1.directed_edge_n(directed_edge_id1); let d=edge_face0_n.dot(edge1_n); if d.is_negative(){ @@ -314,7 +336,7 @@ impl MeshQuery for MinkowskiMesh<'_>{ }, } } - fn edge_verts(&self,edge_id:MinkowskiEdge)->impl AsRef<[MinkowskiVert;2]>{ + fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>{ AsRefHelper(match edge_id{ MinkowskiEdge::VertEdge(v0,e1)=>self.mesh1.edge_verts(e1).as_ref().map(|vert_id1| MinkowskiVert::VertVert(v0,vert_id1) @@ -324,29 +346,30 @@ impl MeshQuery for MinkowskiMesh<'_>{ ), }) } - fn face_edges(&self,face_id:MinkowskiFace)->impl AsRef<[MinkowskiDirectedEdge]>{ + fn for_each_face_vert(&self,_face_id:Self::Face,_f:impl FnMut(Self::Vert)){ + unimplemented!() + } + fn for_each_face_edge(&self,face_id:Self::Face,mut f:impl FnMut(Self::DirectedEdge)){ match face_id{ MinkowskiFace::VertFace(v0,f1)=>{ - self.mesh1.face_edges(f1).as_ref().iter().map(|&edge_id1| - MinkowskiDirectedEdge::VertEdge(v0,edge_id1.reverse()) - ).collect() + self.mesh1.for_each_face_edge(f1,|edge_id1| + f(MinkowskiDirectedEdge::VertEdge(v0,edge_id1.reverse())) + ) }, MinkowskiFace::EdgeEdge(e0,e1,parity)=>{ let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).as_ref(); let &[e1v0,e1v1]=self.mesh1.edge_verts(e1).as_ref(); //could sort this if ordered edges are needed //probably just need to reverse this list according to parity - vec![ - MinkowskiDirectedEdge::VertEdge(e0v0,e1.as_directed(parity)), - MinkowskiDirectedEdge::EdgeVert(e0.as_directed(!parity),e1v0), - MinkowskiDirectedEdge::VertEdge(e0v1,e1.as_directed(!parity)), - MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),e1v1), - ] + f(MinkowskiDirectedEdge::VertEdge(e0v0,e1.as_directed(parity))); + f(MinkowskiDirectedEdge::EdgeVert(e0.as_directed(!parity),e1v0)); + f(MinkowskiDirectedEdge::VertEdge(e0v1,e1.as_directed(!parity))); + f(MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),e1v1)); }, MinkowskiFace::FaceVert(f0,v1)=>{ - self.mesh0.face_edges(f0).as_ref().iter().map(|&edge_id0| - MinkowskiDirectedEdge::EdgeVert(edge_id0,v1) - ).collect() + self.mesh0.for_each_face_edge(f0,|edge_id0| + f(MinkowskiDirectedEdge::EdgeVert(edge_id0,v1)) + ) }, } } diff --git a/engine/physics/src/model.rs b/engine/physics/src/model.rs index c05403dc..8c8b6822 100644 --- a/engine/physics/src/model.rs +++ b/engine/physics/src/model.rs @@ -3,6 +3,7 @@ use strafesnet_common::integer::vec3::Vector3; use strafesnet_common::model::{self,MeshId,PolygonIter}; use strafesnet_common::integer::{self,vec3,Fixed,Planar64,Planar64Vec3,Ratio}; use strafesnet_common::physics::Time; +use crate::mesh_query::{MeshQuery,MeshTopology,DirectedEdge,UndirectedEdge}; struct AsRefHelper(T); impl AsRef for AsRefHelper{ @@ -11,20 +12,6 @@ impl AsRef for AsRefHelper{ } } -pub trait UndirectedEdge{ - type DirectedEdge:Copy+DirectedEdge; - fn as_directed(self,parity:bool)->Self::DirectedEdge; -} -pub trait DirectedEdge:Copy{ - type UndirectedEdge:Copy+std::fmt::Debug+UndirectedEdge; - fn as_undirected(self)->Self::UndirectedEdge; - fn parity(&self)->bool; - //this is stupid but may work fine - fn reverse(self)-><::UndirectedEdge as UndirectedEdge>::DirectedEdge{ - self.as_undirected().as_directed(!self.parity()) - } -} - #[derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq)] pub struct MeshVertId(u32); #[derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq)] @@ -56,14 +43,6 @@ impl DirectedEdge for SubmeshDirectedEdgeId{ } } -//Vertex <-> Edge <-> Face -> Collide -#[derive(Debug)] -pub enum FEV{ - Face(M::Face), - Edge(::UndirectedEdge), - Vert(M::Vert), -} - //use Unit32 #[repr(C)] for map files #[derive(Clone,Copy,Debug,Hash,Eq,PartialEq)] struct Face{ @@ -72,32 +51,6 @@ struct Face{ } #[derive(Debug)] struct Vert(Planar64Vec3); -pub trait MeshQuery{ - type Face:Copy; - type Edge:Copy+DirectedEdge; - type Vert:Copy; - // Vertex must be Planar64Vec3 because it represents an actual position - type Normal; - type Offset; - fn edge_n(&self,edge_id:::UndirectedEdge)->Planar64Vec3{ - let &[v0,v1]=self.edge_verts(edge_id).as_ref(); - self.vert(v1)-self.vert(v0) - } - fn directed_edge_n(&self,directed_edge_id:Self::Edge)->Planar64Vec3{ - let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref(); - (self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1) - } - /// This must return a point inside the mesh. - fn hint_point(&self)->Planar64Vec3; - fn farthest_vert(&self,dir:Planar64Vec3)->Self::Vert; - fn vert(&self,vert_id:Self::Vert)->Planar64Vec3; - fn face_nd(&self,face_id:Self::Face)->(Self::Normal,Self::Offset); - fn face_edges(&self,face_id:Self::Face)->impl AsRef<[Self::Edge]>; - fn edge_faces(&self,edge_id:::UndirectedEdge)->impl AsRef<[Self::Face;2]>; - fn edge_verts(&self,edge_id:::UndirectedEdge)->impl AsRef<[Self::Vert;2]>; - fn vert_edges(&self,vert_id:Self::Vert)->impl AsRef<[Self::Edge]>; - fn vert_faces(&self,vert_id:Self::Vert)->impl AsRef<[Self::Face]>; -} #[derive(Debug)] struct FaceRefs{ // I didn't write it down, but I assume the edges are directed @@ -442,9 +395,8 @@ pub struct PhysicsMeshView<'a>{ topology:&'a PhysicsMeshTopology, } impl MeshQuery for PhysicsMeshView<'_>{ - type Face=SubmeshFaceId; - type Edge=SubmeshDirectedEdgeId; - type Vert=SubmeshVertId; + type Position=Planar64Vec3; + type Direction=Planar64Vec3; type Normal=Planar64Vec3; type Offset=Planar64; fn face_nd(&self,face_id:SubmeshFaceId)->(Planar64Vec3,Planar64){ @@ -472,20 +424,37 @@ impl MeshQuery for PhysicsMeshView<'_>{ let vert_idx=self.topology.verts[vert_id.get() as usize].get() as usize; self.data.verts[vert_idx].0 } - fn vert_edges(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshDirectedEdgeId]>{ - self.topology.vert_topology[vert_id.get() as usize].edges.as_slice() + fn edge_n(&self,edge_id:Self::Edge)->Self::Direction{ + let &[v0,v1]=self.edge_verts(edge_id).as_ref(); + self.vert(v1)-self.vert(v0) } - fn vert_faces(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshFaceId]>{ - self.topology.vert_topology[vert_id.get() as usize].faces.as_slice() + fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction{ + let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref(); + (self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1) } - fn edge_faces(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshFaceId;2]>{ +} +impl MeshTopology for PhysicsMeshView<'_>{ + type Face=SubmeshFaceId; + type Edge=SubmeshEdgeId; + type DirectedEdge=SubmeshDirectedEdgeId; + type Vert=SubmeshVertId; + fn for_each_vert_edge(&self,vert_id:Self::Vert,f:impl FnMut(Self::DirectedEdge)){ + self.topology.vert_topology[vert_id.get() as usize].edges.iter().copied().for_each(f); + } + fn for_each_vert_face(&self,vert_id:Self::Vert,f:impl FnMut(Self::Face)){ + self.topology.vert_topology[vert_id.get() as usize].faces.iter().copied().for_each(f); + } + fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>{ AsRefHelper(self.topology.edge_topology[edge_id.get() as usize].faces) } - fn edge_verts(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshVertId;2]>{ + fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>{ AsRefHelper(self.topology.edge_topology[edge_id.get() as usize].verts) } - fn face_edges(&self,face_id:SubmeshFaceId)->impl AsRef<[SubmeshDirectedEdgeId]>{ - self.topology.face_topology[face_id.get() as usize].edges.as_slice() + fn for_each_face_vert(&self,_face_id:Self::Face,_f:impl FnMut(Self::Vert)){ + unimplemented!() + } + fn for_each_face_edge(&self,face_id:Self::Face,f:impl FnMut(Self::DirectedEdge)){ + self.topology.face_topology[face_id.get() as usize].edges.iter().copied().for_each(f); } } @@ -525,9 +494,8 @@ impl TransformedMesh<'_>{ } } impl MeshQuery for TransformedMesh<'_>{ - type Face=SubmeshFaceId; - type Edge=SubmeshDirectedEdgeId; - type Vert=SubmeshVertId; + type Direction=Planar64Vec3; + type Position=Planar64Vec3; type Normal=Vector3>; type Offset=Fixed<4,128>; fn face_nd(&self,face_id:SubmeshFaceId)->(Self::Normal,Self::Offset){ @@ -555,25 +523,42 @@ impl MeshQuery for TransformedMesh<'_>{ .unwrap().0 as u32 ) } + fn edge_n(&self,edge_id:Self::Edge)->Self::Direction{ + let &[v0,v1]=self.edge_verts(edge_id).as_ref(); + self.vert(v1)-self.vert(v0) + } + fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction{ + let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref(); + (self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1) + } +} +impl MeshTopology for TransformedMesh<'_>{ + type Face=SubmeshFaceId; + type Edge=SubmeshEdgeId; + type DirectedEdge=SubmeshDirectedEdgeId; + type Vert=SubmeshVertId; #[inline] - fn vert_edges(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshDirectedEdgeId]>{ - self.view.vert_edges(vert_id) + fn for_each_vert_edge(&self,vert_id:Self::Vert,f:impl FnMut(Self::DirectedEdge)){ + self.view.for_each_vert_edge(vert_id,f) } #[inline] - fn vert_faces(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshFaceId]>{ - self.view.vert_faces(vert_id) + fn for_each_vert_face(&self,vert_id:Self::Vert,f:impl FnMut(Self::Face)){ + self.view.for_each_vert_face(vert_id,f) } #[inline] - fn edge_faces(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshFaceId;2]>{ + fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>{ self.view.edge_faces(edge_id) } #[inline] - fn edge_verts(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshVertId;2]>{ + fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>{ self.view.edge_verts(edge_id) } + fn for_each_face_vert(&self,face_id:Self::Face,f:impl FnMut(Self::Vert)){ + self.view.for_each_face_vert(face_id,f) + } #[inline] - fn face_edges(&self,face_id:SubmeshFaceId)->impl AsRef<[SubmeshDirectedEdgeId]>{ - self.view.face_edges(face_id) + fn for_each_face_edge(&self,face_id:Self::Face,f:impl FnMut(Self::DirectedEdge)){ + self.view.for_each_face_edge(face_id,f) } } diff --git a/engine/physics/src/physics.rs b/engine/physics/src/physics.rs index 8308ac04..3b495b99 100644 --- a/engine/physics/src/physics.rs +++ b/engine/physics/src/physics.rs @@ -1,6 +1,7 @@ use std::collections::{HashMap,HashSet}; +use crate::mesh_query::MeshQuery; use crate::minkowski::{MinkowskiMesh,MinkowskiFace}; -use crate::model::{self as model_physics,PhysicsMesh,PhysicsMeshTransform,TransformedMesh,MeshQuery,PhysicsMeshId,PhysicsSubmeshId}; +use crate::model::{self as model_physics,PhysicsMesh,PhysicsMeshTransform,TransformedMesh,PhysicsMeshId,PhysicsSubmeshId}; use strafesnet_common::bvh; use strafesnet_common::map; use strafesnet_common::run; -- 2.49.1