diff --git a/src/model_physics.rs b/src/model_physics.rs index 2bbdf93..573d439 100644 --- a/src/model_physics.rs +++ b/src/model_physics.rs @@ -483,9 +483,9 @@ enum MinkowskiEdge{ } #[derive(Debug,Clone,Copy,Hash,Eq,PartialEq)] pub enum MinkowskiFace{ - FaceVert(FaceId,VertId), - EdgeEdge(EdgeId,EdgeId), VertFace(VertId,FaceId), + EdgeEdge(EdgeId,EdgeId), + FaceVert(FaceId,VertId), } pub struct MinkowskiMesh<'a>{ @@ -508,9 +508,9 @@ impl MeshQuery for MinkowskiMesh<'_>{ } fn face_nd(&self,face_id:MinkowskiFace)->(Planar64Vec3,Planar64){ match face_id{ - MinkowskiFace::FaceVert(f0,v1)=>{ - let (n,d)=self.mesh0.face_nd(f0); - (n,d+n.dot(self.mesh1.vert(v1))) + MinkowskiFace::VertFace(v0,f1)=>{ + let (n,d)=self.mesh1.face_nd(f1); + (-n,d-n.dot(self.mesh0.vert(v0))) }, MinkowskiFace::EdgeEdge(e0,e1)=>{ let edge0_n=self.mesh0.edge_n(e0); @@ -523,9 +523,9 @@ impl MeshQuery for MinkowskiMesh<'_>{ let sign=e0d.signum_i64(); (n*(sign*2),(e0d-e1d)*sign) }, - MinkowskiFace::VertFace(v0,f1)=>{ - let (n,d)=self.mesh1.face_nd(f1); - (-n,d-n.dot(self.mesh0.vert(v0))) + MinkowskiFace::FaceVert(f0,v1)=>{ + let (n,d)=self.mesh0.face_nd(f0); + (n,d+n.dot(self.mesh1.vert(v1))) }, } } @@ -538,33 +538,28 @@ impl MeshQuery for MinkowskiMesh<'_>{ } fn face_edges(&self,face_id:MinkowskiFace)->Cow>{ match face_id{ - MinkowskiFace::FaceVert(f0,v1)=>{ - let face0_n=self.mesh0.face_nd(f0).0; - Cow::Owned(self.mesh0.face_edges(f0).iter().map(|&(edge_id0,edge_face_id0)|{ - //compare v1 edges - //candidate edges have negative dot with edge_face_id0 normal - //choose the edge with the smallest edgedir dot with f0 normal - //MinkowskiFace::EdgeEdge(edge_id0,edge_id1) - //if there is no candidate edges - //MinkowskiFace::FaceVert(edge_face_id0,v1) - (MinkowskiEdge::EdgeVert(edge_id0,v1),{ + MinkowskiFace::VertFace(v0,f1)=>{ + let face1_n=self.mesh1.face_nd(f1).0; + Cow::Owned(self.mesh1.face_edges(f1).iter().map(|&(edge_id1,edge_face_id1)|{ + //same as above + (MinkowskiEdge::VertEdge(v0,edge_id1),{ let mut best_edge=None; let mut best_d=Planar64::MAX; - let edge_face0_n=self.mesh0.face_nd(edge_face_id0).0; - let v1e=self.mesh1.vert_directed_edges(v1); - for &directed_edge_id1 in v1e.iter(){ - let edge1_n=self.mesh1.directed_edge_n(directed_edge_id1); - if edge_face0_n.dot(edge1_n) for MinkowskiMesh<'_>{ */ todo!() }, - MinkowskiFace::VertFace(v0,f1)=>{ - let face1_n=self.mesh1.face_nd(f1).0; - Cow::Owned(self.mesh1.face_edges(f1).iter().map(|&(edge_id1,edge_face_id1)|{ - //same as above - (MinkowskiEdge::VertEdge(v0,edge_id1),{ + MinkowskiFace::FaceVert(f0,v1)=>{ + let face0_n=self.mesh0.face_nd(f0).0; + Cow::Owned(self.mesh0.face_edges(f0).iter().map(|&(edge_id0,edge_face_id0)|{ + //compare v1 edges + //candidate edges have negative dot with edge_face_id0 normal + //choose the edge with the smallest edgedir dot with f0 normal + //MinkowskiFace::EdgeEdge(edge_id0,edge_id1) + //if there is no candidate edges + //MinkowskiFace::FaceVert(edge_face_id0,v1) + (MinkowskiEdge::EdgeVert(edge_id0,v1),{ let mut best_edge=None; let mut best_d=Planar64::MAX; - let edge_face1_n=self.mesh1.face_nd(edge_face_id1).0; - let v0e=self.mesh0.vert_directed_edges(v0); - for &directed_edge_id0 in v0e.iter(){ - let edge0_n=self.mesh0.directed_edge_n(directed_edge_id0); - if edge_face1_n.dot(edge0_n)