955 lines
36 KiB
Rust
955 lines
36 KiB
Rust
use std::collections::{HashSet,HashMap};
|
|
use core::ops::{Bound,RangeBounds};
|
|
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;
|
|
|
|
type Body=crate::body::Body<strafesnet_common::physics::TimeInner>;
|
|
|
|
struct AsRefHelper<T>(T);
|
|
impl<T> AsRef<T> for AsRefHelper<T>{
|
|
fn as_ref(&self)->&T{
|
|
&self.0
|
|
}
|
|
}
|
|
|
|
pub trait UndirectedEdge{
|
|
type DirectedEdge:Copy+DirectedEdge;
|
|
fn as_directed(&self,parity:bool)->Self::DirectedEdge;
|
|
}
|
|
pub trait DirectedEdge{
|
|
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)-><<Self as DirectedEdge>::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)]
|
|
pub struct MeshFaceId(u32);
|
|
|
|
#[derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq)]
|
|
pub struct SubmeshVertId(u32);
|
|
#[derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq)]
|
|
pub struct SubmeshEdgeId(u32);
|
|
/// DirectedEdgeId refers to an EdgeId when undirected.
|
|
#[derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq)]
|
|
pub struct SubmeshDirectedEdgeId(u32);
|
|
#[derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq)]
|
|
pub struct SubmeshFaceId(u32);
|
|
|
|
impl UndirectedEdge for SubmeshEdgeId{
|
|
type DirectedEdge=SubmeshDirectedEdgeId;
|
|
fn as_directed(&self,parity:bool)->SubmeshDirectedEdgeId{
|
|
SubmeshDirectedEdgeId(self.0|((parity as u32)<<(u32::BITS-1)))
|
|
}
|
|
}
|
|
impl DirectedEdge for SubmeshDirectedEdgeId{
|
|
type UndirectedEdge=SubmeshEdgeId;
|
|
fn as_undirected(&self)->SubmeshEdgeId{
|
|
SubmeshEdgeId(self.0&!(1<<(u32::BITS-1)))
|
|
}
|
|
fn parity(&self)->bool{
|
|
self.0&(1<<(u32::BITS-1))!=0
|
|
}
|
|
}
|
|
|
|
//Vertex <-> Edge <-> Face -> Collide
|
|
#[derive(Debug)]
|
|
pub enum FEV<M:MeshQuery>{
|
|
Face(M::Face),
|
|
Edge(<M::Edge as DirectedEdge>::UndirectedEdge),
|
|
Vert(M::Vert),
|
|
}
|
|
|
|
//use Unit32 #[repr(C)] for map files
|
|
#[derive(Clone,Copy,Debug,Hash,Eq,PartialEq)]
|
|
struct Face{
|
|
normal:Planar64Vec3,
|
|
dot:Planar64,
|
|
}
|
|
#[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:<Self::Edge as DirectedEdge>::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.
|
|
#[expect(dead_code)]
|
|
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:<Self::Edge as DirectedEdge>::UndirectedEdge)->impl AsRef<[Self::Face;2]>;
|
|
fn edge_verts(&self,edge_id:<Self::Edge as DirectedEdge>::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
|
|
// clockwise when looking towards the face normal, i.e. right hand rule.
|
|
edges:Vec<SubmeshDirectedEdgeId>,
|
|
//verts are redundant, use edge[i].verts[0]
|
|
//verts:Vec<VertId>,
|
|
}
|
|
#[derive(Debug)]
|
|
struct EdgeRefs{
|
|
faces:[SubmeshFaceId;2],//left, right
|
|
verts:[SubmeshVertId;2],//start, end
|
|
}
|
|
#[derive(Debug)]
|
|
struct VertRefs{
|
|
faces:Vec<SubmeshFaceId>,
|
|
// edges are always directed away from the vert
|
|
edges:Vec<SubmeshDirectedEdgeId>,
|
|
}
|
|
#[derive(Debug)]
|
|
pub struct PhysicsMeshData{
|
|
//this contains all real and virtual faces used in both the complete mesh and convex submeshes
|
|
//faces are sorted such that all faces that belong to the complete mesh appear first, and then
|
|
//all remaining faces are virtual to operate internal logic of the face crawler
|
|
//and cannot be part of a physics collision
|
|
//virtual faces are only used in convex submeshes.
|
|
faces:Vec<Face>,//MeshFaceId indexes this list
|
|
verts:Vec<Vert>,//MeshVertId indexes this list
|
|
}
|
|
#[derive(Debug)]
|
|
pub struct PhysicsMeshTopology{
|
|
//mapping of local ids to PhysicsMeshData ids
|
|
faces:Vec<MeshFaceId>,//SubmeshFaceId indexes this list
|
|
verts:Vec<MeshVertId>,//SubmeshVertId indexes this list
|
|
//all ids here are local to this object
|
|
face_topology:Vec<FaceRefs>,
|
|
edge_topology:Vec<EdgeRefs>,
|
|
vert_topology:Vec<VertRefs>,
|
|
}
|
|
#[derive(Clone,Copy,Hash,id::Id,Eq,PartialEq)]
|
|
pub struct PhysicsMeshId(u32);
|
|
impl Into<MeshId> for PhysicsMeshId{
|
|
fn into(self)->MeshId{
|
|
MeshId::new(self.0)
|
|
}
|
|
}
|
|
impl From<MeshId> for PhysicsMeshId{
|
|
fn from(value:MeshId)->Self{
|
|
Self::new(value.get())
|
|
}
|
|
}
|
|
#[derive(Debug,Default,Clone,Copy,Hash,id::Id,Eq,PartialEq)]
|
|
pub struct PhysicsSubmeshId(u32);
|
|
pub struct PhysicsMesh{
|
|
data:PhysicsMeshData,
|
|
// The complete mesh is unused at this time.
|
|
// complete_mesh:PhysicsMeshTopology,
|
|
// Submeshes are guaranteed to be convex and may contain
|
|
// "virtual" faces which are not part of the complete mesh.
|
|
// Physics calculations should never resolve to hitting
|
|
// a virtual face.
|
|
submeshes:Vec<PhysicsMeshTopology>,
|
|
}
|
|
impl PhysicsMesh{
|
|
pub fn unit_cube()->Self{
|
|
//go go gadget debug print mesh
|
|
let data=PhysicsMeshData{
|
|
faces:vec![
|
|
Face{normal:vec3::raw_xyz( 4294967296, 0, 0),dot:Planar64::raw(4294967296)},
|
|
Face{normal:vec3::raw_xyz( 0, 4294967296, 0),dot:Planar64::raw(4294967296)},
|
|
Face{normal:vec3::raw_xyz( 0, 0, 4294967296),dot:Planar64::raw(4294967296)},
|
|
Face{normal:vec3::raw_xyz(-4294967296, 0, 0),dot:Planar64::raw(4294967296)},
|
|
Face{normal:vec3::raw_xyz( 0,-4294967296, 0),dot:Planar64::raw(4294967296)},
|
|
Face{normal:vec3::raw_xyz( 0, 0,-4294967296),dot:Planar64::raw(4294967296)}
|
|
],
|
|
verts:vec![
|
|
Vert(vec3::raw_xyz( 4294967296,-4294967296,-4294967296)),
|
|
Vert(vec3::raw_xyz( 4294967296, 4294967296,-4294967296)),
|
|
Vert(vec3::raw_xyz( 4294967296, 4294967296, 4294967296)),
|
|
Vert(vec3::raw_xyz( 4294967296,-4294967296, 4294967296)),
|
|
Vert(vec3::raw_xyz(-4294967296, 4294967296,-4294967296)),
|
|
Vert(vec3::raw_xyz(-4294967296, 4294967296, 4294967296)),
|
|
Vert(vec3::raw_xyz(-4294967296,-4294967296, 4294967296)),
|
|
Vert(vec3::raw_xyz(-4294967296,-4294967296,-4294967296))
|
|
]
|
|
};
|
|
let mesh_topology=PhysicsMeshTopology{
|
|
faces:(0..data.faces.len() as u32).map(MeshFaceId::new).collect(),
|
|
verts:(0..data.verts.len() as u32).map(MeshVertId::new).collect(),
|
|
face_topology:vec![
|
|
FaceRefs{edges:vec![SubmeshDirectedEdgeId((9223372036854775808u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775809u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775810u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId(3)]},
|
|
FaceRefs{edges:vec![SubmeshDirectedEdgeId((9223372036854775812u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775813u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId(6),SubmeshDirectedEdgeId(1)]},
|
|
FaceRefs{edges:vec![SubmeshDirectedEdgeId(7),SubmeshDirectedEdgeId(2),SubmeshDirectedEdgeId((9223372036854775814u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775816u64-(1<<63)+(1<<31)) as u32)]},
|
|
FaceRefs{edges:vec![SubmeshDirectedEdgeId(8),SubmeshDirectedEdgeId(5),SubmeshDirectedEdgeId((9223372036854775817u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId(10)]},
|
|
FaceRefs{edges:vec![SubmeshDirectedEdgeId((9223372036854775815u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775818u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId(11),SubmeshDirectedEdgeId((9223372036854775811u64-(1<<63)+(1<<31)) as u32)]},
|
|
FaceRefs{edges:vec![SubmeshDirectedEdgeId(4),SubmeshDirectedEdgeId(0),SubmeshDirectedEdgeId((9223372036854775819u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId(9)]}
|
|
],
|
|
edge_topology:vec![
|
|
EdgeRefs{faces:[SubmeshFaceId(0),SubmeshFaceId(5)],verts:[SubmeshVertId(0),SubmeshVertId(1)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(0),SubmeshFaceId(1)],verts:[SubmeshVertId(1),SubmeshVertId(2)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(0),SubmeshFaceId(2)],verts:[SubmeshVertId(2),SubmeshVertId(3)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(4),SubmeshFaceId(0)],verts:[SubmeshVertId(0),SubmeshVertId(3)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(1),SubmeshFaceId(5)],verts:[SubmeshVertId(1),SubmeshVertId(4)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(1),SubmeshFaceId(3)],verts:[SubmeshVertId(4),SubmeshVertId(5)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(2),SubmeshFaceId(1)],verts:[SubmeshVertId(2),SubmeshVertId(5)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(4),SubmeshFaceId(2)],verts:[SubmeshVertId(3),SubmeshVertId(6)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(2),SubmeshFaceId(3)],verts:[SubmeshVertId(5),SubmeshVertId(6)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(3),SubmeshFaceId(5)],verts:[SubmeshVertId(4),SubmeshVertId(7)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(4),SubmeshFaceId(3)],verts:[SubmeshVertId(6),SubmeshVertId(7)]},
|
|
EdgeRefs{faces:[SubmeshFaceId(5),SubmeshFaceId(4)],verts:[SubmeshVertId(0),SubmeshVertId(7)]}
|
|
],
|
|
vert_topology:vec![
|
|
VertRefs{faces:vec![SubmeshFaceId(0),SubmeshFaceId(4),SubmeshFaceId(5)],edges:vec![SubmeshDirectedEdgeId((9223372036854775811u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775819u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775808u64-(1<<63)+(1<<31)) as u32)]},
|
|
VertRefs{faces:vec![SubmeshFaceId(0),SubmeshFaceId(5),SubmeshFaceId(1)],edges:vec![SubmeshDirectedEdgeId((9223372036854775812u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId(0),SubmeshDirectedEdgeId((9223372036854775809u64-(1<<63)+(1<<31)) as u32)]},
|
|
VertRefs{faces:vec![SubmeshFaceId(0),SubmeshFaceId(2),SubmeshFaceId(1)],edges:vec![SubmeshDirectedEdgeId(1),SubmeshDirectedEdgeId((9223372036854775810u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775814u64-(1<<63)+(1<<31)) as u32)]},
|
|
VertRefs{faces:vec![SubmeshFaceId(0),SubmeshFaceId(2),SubmeshFaceId(4)],edges:vec![SubmeshDirectedEdgeId(2),SubmeshDirectedEdgeId(3),SubmeshDirectedEdgeId((9223372036854775815u64-(1<<63)+(1<<31)) as u32)]},
|
|
VertRefs{faces:vec![SubmeshFaceId(3),SubmeshFaceId(5),SubmeshFaceId(1)],edges:vec![SubmeshDirectedEdgeId(4),SubmeshDirectedEdgeId((9223372036854775817u64-(1<<63)+(1<<31)) as u32),SubmeshDirectedEdgeId((9223372036854775813u64-(1<<63)+(1<<31)) as u32)]},
|
|
VertRefs{faces:vec![SubmeshFaceId(2),SubmeshFaceId(3),SubmeshFaceId(1)],edges:vec![SubmeshDirectedEdgeId(5),SubmeshDirectedEdgeId(6),SubmeshDirectedEdgeId((9223372036854775816u64-(1<<63)+(1<<31)) as u32)]},
|
|
VertRefs{faces:vec![SubmeshFaceId(2),SubmeshFaceId(3),SubmeshFaceId(4)],edges:vec![SubmeshDirectedEdgeId(7),SubmeshDirectedEdgeId(8),SubmeshDirectedEdgeId((9223372036854775818u64-(1<<63)+(1<<31)) as u32)]},
|
|
VertRefs{faces:vec![SubmeshFaceId(4),SubmeshFaceId(3),SubmeshFaceId(5)],edges:vec![SubmeshDirectedEdgeId(10),SubmeshDirectedEdgeId(11),SubmeshDirectedEdgeId(9)]}
|
|
]
|
|
};
|
|
Self{
|
|
data,
|
|
// complete_mesh:mesh_topology.clone(),
|
|
submeshes:vec![mesh_topology],
|
|
}
|
|
}
|
|
pub fn unit_cylinder()->Self{
|
|
Self::unit_cube()
|
|
}
|
|
#[inline]
|
|
pub fn complete_mesh(&self)->&PhysicsMeshTopology{
|
|
// If there is exactly one submesh, then the complete mesh is identical to it.
|
|
if self.submeshes.len()==1{
|
|
self.submeshes.first().unwrap()
|
|
}else{
|
|
panic!("PhysicsMesh complete mesh is not known");
|
|
}
|
|
}
|
|
#[inline]
|
|
pub fn complete_mesh_view(&self)->PhysicsMeshView<'_>{
|
|
PhysicsMeshView{
|
|
data:&self.data,
|
|
topology:self.complete_mesh(),
|
|
}
|
|
}
|
|
#[inline]
|
|
pub fn submeshes(&self)->&[PhysicsMeshTopology]{
|
|
&self.submeshes
|
|
}
|
|
#[inline]
|
|
pub fn submesh_view(&self,submesh_id:PhysicsSubmeshId)->PhysicsMeshView<'_>{
|
|
PhysicsMeshView{
|
|
data:&self.data,
|
|
topology:&self.submeshes()[submesh_id.get() as usize],
|
|
}
|
|
}
|
|
pub fn submesh_views(&self)->impl Iterator<Item=PhysicsMeshView<'_>>{
|
|
self.submeshes().iter().map(|topology|PhysicsMeshView{
|
|
data:&self.data,
|
|
topology,
|
|
})
|
|
}
|
|
}
|
|
|
|
//mesh builder code
|
|
#[derive(Default,Clone)]
|
|
struct VertRefGuy{
|
|
edges:HashSet<SubmeshDirectedEdgeId>,
|
|
faces:HashSet<SubmeshFaceId>,
|
|
}
|
|
#[derive(Clone,Hash,Eq,PartialEq)]
|
|
struct EdgeRefVerts([SubmeshVertId;2]);
|
|
impl EdgeRefVerts{
|
|
const fn new(v0:SubmeshVertId,v1:SubmeshVertId)->(Self,bool){
|
|
(if v0.0<v1.0{
|
|
Self([v0,v1])
|
|
}else{
|
|
Self([v1,v0])
|
|
},v0.0<v1.0)
|
|
}
|
|
}
|
|
struct EdgeRefFaces([SubmeshFaceId;2]);
|
|
impl EdgeRefFaces{
|
|
const fn new()->Self{
|
|
Self([SubmeshFaceId(0);2])
|
|
}
|
|
fn push(&mut self,i:usize,face_id:SubmeshFaceId){
|
|
self.0[i]=face_id;
|
|
}
|
|
}
|
|
struct FaceRefEdges(Vec<SubmeshDirectedEdgeId>);
|
|
#[derive(Default)]
|
|
struct EdgePool{
|
|
edge_guys:Vec<(EdgeRefVerts,EdgeRefFaces)>,
|
|
edge_id_from_guy:HashMap<EdgeRefVerts,SubmeshEdgeId>,
|
|
}
|
|
impl EdgePool{
|
|
fn push(&mut self,edge_ref_verts:EdgeRefVerts)->(&mut EdgeRefFaces,SubmeshEdgeId){
|
|
let edge_id=*self.edge_id_from_guy.entry(edge_ref_verts.clone()).or_insert_with(||{
|
|
let edge_id=SubmeshEdgeId::new(self.edge_guys.len() as u32);
|
|
self.edge_guys.push((edge_ref_verts,EdgeRefFaces::new()));
|
|
edge_id
|
|
});
|
|
(&mut self.edge_guys[edge_id.get() as usize].1,edge_id)
|
|
}
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub enum PhysicsMeshError{
|
|
ZeroVertices,
|
|
NoPhysicsGroups,
|
|
}
|
|
impl std::fmt::Display for PhysicsMeshError{
|
|
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
|
write!(f,"{self:?}")
|
|
}
|
|
}
|
|
impl std::error::Error for PhysicsMeshError{}
|
|
|
|
impl TryFrom<&model::Mesh> for PhysicsMesh{
|
|
type Error=PhysicsMeshError;
|
|
fn try_from(mesh:&model::Mesh)->Result<Self,PhysicsMeshError>{
|
|
if mesh.unique_pos.len()==0{
|
|
return Err(PhysicsMeshError::ZeroVertices);
|
|
}
|
|
// An empty physics mesh is a waste of resources
|
|
if mesh.physics_groups.len()==0{
|
|
return Err(PhysicsMeshError::NoPhysicsGroups);
|
|
}
|
|
let verts=mesh.unique_pos.iter().copied().map(Vert).collect();
|
|
//TODO: fix submeshes
|
|
//flat map mesh.physics_groups[$1].groups.polys()[$2] as face_id
|
|
//lower face_id points to upper face_id
|
|
//the same face is not allowed to be in multiple polygon groups
|
|
// because SubmeshFaceId -> CompleteMeshFaceId -> SubmeshFaceId is ambiguous
|
|
// when multiple SubmeshFaceId point to one MeshFaceId
|
|
let mut faces=Vec::new();
|
|
let mut face_id_from_face=HashMap::new();
|
|
let mesh_topologies:Vec<PhysicsMeshTopology>=mesh.physics_groups.iter().map(|physics_group|{
|
|
//construct submesh
|
|
let mut submesh_faces=Vec::new();//these contain a map from submeshId->meshId
|
|
let mut submesh_verts=Vec::new();
|
|
let mut submesh_vert_id_from_mesh_vert_id=HashMap::<MeshVertId,SubmeshVertId>::new();
|
|
//lazy closure
|
|
let mut get_submesh_vert_id=|vert_id:MeshVertId|*submesh_vert_id_from_mesh_vert_id.entry(vert_id).or_insert_with(||{
|
|
let submesh_vert_id=SubmeshVertId::new(submesh_verts.len() as u32);
|
|
submesh_verts.push(vert_id);
|
|
submesh_vert_id
|
|
});
|
|
let mut edge_pool=EdgePool::default();
|
|
let mut vert_ref_guys=vec![VertRefGuy::default();mesh.unique_pos.len()];
|
|
let mut face_ref_guys=Vec::new();
|
|
for polygon_group_id in &physics_group.groups{
|
|
let polygon_group=&mesh.polygon_groups[polygon_group_id.get() as usize];
|
|
for poly_vertices in polygon_group.polys(){
|
|
let submesh_face_id=SubmeshFaceId::new(submesh_faces.len() as u32);
|
|
//one face per poly
|
|
let mut normal=Vector3::new([Fixed::ZERO,Fixed::ZERO,Fixed::ZERO]);
|
|
let len=poly_vertices.len();
|
|
let face_edges=poly_vertices.into_iter().enumerate().map(|(i,vert_id)|{
|
|
let vert0_id=MeshVertId::new(mesh.unique_vertices[vert_id.get() as usize].pos.get() as u32);
|
|
let vert1_id=MeshVertId::new(mesh.unique_vertices[poly_vertices[(i+1)%len].get() as usize].pos.get() as u32);
|
|
//index submesh verts
|
|
let submesh_vert0_id=get_submesh_vert_id(vert0_id);
|
|
let submesh_vert1_id=get_submesh_vert_id(vert1_id);
|
|
//https://www.khronos.org/opengl/wiki/Calculating_a_Surface_Normal (Newell's Method)
|
|
let v0=mesh.unique_pos[vert0_id.get() as usize];
|
|
let v1=mesh.unique_pos[vert1_id.get() as usize];
|
|
normal+=Vector3::new([
|
|
(v0.y-v1.y)*(v0.z+v1.z),
|
|
(v0.z-v1.z)*(v0.x+v1.x),
|
|
(v0.x-v1.x)*(v0.y+v1.y),
|
|
]);
|
|
//get/create edge and push face into it
|
|
let (edge_ref_verts,is_sorted)=EdgeRefVerts::new(submesh_vert0_id,submesh_vert1_id);
|
|
let (edge_ref_faces,edge_id)=edge_pool.push(edge_ref_verts);
|
|
//polygon vertices as assumed to be listed clockwise
|
|
//populate the edge face on the left or right depending on how the edge vertices got sorted
|
|
edge_ref_faces.push(!is_sorted as usize,submesh_face_id);
|
|
//index edges & face into vertices
|
|
{
|
|
let vert_ref_guy=&mut vert_ref_guys[submesh_vert0_id.get() as usize];
|
|
vert_ref_guy.edges.insert(edge_id.as_directed(is_sorted));
|
|
vert_ref_guy.faces.insert(submesh_face_id);
|
|
vert_ref_guys[submesh_vert1_id.get() as usize].edges.insert(edge_id.as_directed(!is_sorted));
|
|
}
|
|
//return directed_edge_id
|
|
edge_id.as_directed(is_sorted)
|
|
}).collect();
|
|
let mut dot=Fixed::ZERO;
|
|
// find the average dot
|
|
for &v in poly_vertices{
|
|
dot+=normal.dot(mesh.unique_pos[mesh.unique_vertices[v.get() as usize].pos.get() as usize]);
|
|
}
|
|
//assume face hash is stable, and there are no flush faces...
|
|
let face=Face{
|
|
normal:(normal/len as i64).divide().narrow_1().unwrap(),
|
|
dot:(dot/(len*len) as i64).narrow_1().unwrap(),
|
|
};
|
|
let face_id=*face_id_from_face.entry(face).or_insert_with(||{
|
|
let face_id=MeshFaceId::new(faces.len() as u32);
|
|
faces.push(face);
|
|
face_id
|
|
});
|
|
submesh_faces.push(face_id);
|
|
face_ref_guys.push(FaceRefEdges(face_edges));
|
|
}
|
|
}
|
|
PhysicsMeshTopology{
|
|
faces:submesh_faces,
|
|
verts:submesh_verts,
|
|
face_topology:face_ref_guys.into_iter().map(|FaceRefEdges(edges)|{
|
|
FaceRefs{edges}
|
|
}).collect(),
|
|
edge_topology:edge_pool.edge_guys.into_iter().map(|(EdgeRefVerts(verts),EdgeRefFaces(faces))|
|
|
EdgeRefs{faces,verts}
|
|
).collect(),
|
|
vert_topology:vert_ref_guys.into_iter().map(|VertRefGuy{edges,faces}|
|
|
VertRefs{
|
|
edges:edges.into_iter().collect(),
|
|
faces:faces.into_iter().collect(),
|
|
}
|
|
).collect(),
|
|
}
|
|
}).collect();
|
|
Ok(Self{
|
|
data:PhysicsMeshData{
|
|
faces,
|
|
verts,
|
|
},
|
|
// complete_mesh:None,
|
|
submeshes:mesh_topologies,
|
|
})
|
|
}
|
|
}
|
|
|
|
#[derive(Debug,Clone,Copy)]
|
|
pub struct PhysicsMeshView<'a>{
|
|
data:&'a PhysicsMeshData,
|
|
topology:&'a PhysicsMeshTopology,
|
|
}
|
|
impl MeshQuery for PhysicsMeshView<'_>{
|
|
type Face=SubmeshFaceId;
|
|
type Edge=SubmeshDirectedEdgeId;
|
|
type Vert=SubmeshVertId;
|
|
type Normal=Planar64Vec3;
|
|
type Offset=Planar64;
|
|
fn face_nd(&self,face_id:SubmeshFaceId)->(Planar64Vec3,Planar64){
|
|
let face_idx=self.topology.faces[face_id.get() as usize].get() as usize;
|
|
(self.data.faces[face_idx].normal,self.data.faces[face_idx].dot)
|
|
}
|
|
fn hint_point(&self)->Planar64Vec3{
|
|
// invariant: meshes always encompass the origin
|
|
vec3::zero()
|
|
}
|
|
fn farthest_vert(&self,dir:Planar64Vec3)->SubmeshVertId{
|
|
//this happens to be well-defined. there are no virtual virtices
|
|
SubmeshVertId::new(
|
|
self.topology.verts.iter()
|
|
.enumerate()
|
|
.max_by_key(|&(_,&vert_id)|
|
|
dir.dot(self.data.verts[vert_id.get() as usize].0)
|
|
)
|
|
//assume there is more than zero vertices.
|
|
.unwrap().0 as u32
|
|
)
|
|
}
|
|
//ideally I never calculate the vertex position, but I have to for the graphical meshes...
|
|
fn vert(&self,vert_id:SubmeshVertId)->Planar64Vec3{
|
|
let vert_idx=self.topology.verts[vert_id.get() as usize].get() as usize;
|
|
self.data.verts[vert_idx].0
|
|
}
|
|
fn face_edges(&self,face_id:SubmeshFaceId)->impl AsRef<[SubmeshDirectedEdgeId]>{
|
|
self.topology.face_topology[face_id.get() as usize].edges.as_slice()
|
|
}
|
|
fn edge_faces(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshFaceId;2]>{
|
|
AsRefHelper(self.topology.edge_topology[edge_id.get() as usize].faces)
|
|
}
|
|
fn edge_verts(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshVertId;2]>{
|
|
AsRefHelper(self.topology.edge_topology[edge_id.get() as usize].verts)
|
|
}
|
|
fn vert_edges(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshDirectedEdgeId]>{
|
|
self.topology.vert_topology[vert_id.get() as usize].edges.as_slice()
|
|
}
|
|
fn vert_faces(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshFaceId]>{
|
|
self.topology.vert_topology[vert_id.get() as usize].faces.as_slice()
|
|
}
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub struct PhysicsMeshTransform{
|
|
pub vertex:integer::Planar64Affine3,
|
|
pub normal:integer::mat3::Matrix3<Fixed<2,64>>,
|
|
pub det:Fixed<3,96>,
|
|
}
|
|
impl PhysicsMeshTransform{
|
|
pub fn new(transform:integer::Planar64Affine3)->Self{
|
|
Self{
|
|
normal:transform.matrix3.adjugate().transpose(),
|
|
det:transform.matrix3.det(),
|
|
vertex:transform,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Debug,Clone,Copy)]
|
|
pub struct TransformedMesh<'a>{
|
|
view:PhysicsMeshView<'a>,
|
|
transform:&'a PhysicsMeshTransform,
|
|
}
|
|
impl TransformedMesh<'_>{
|
|
pub const fn new<'a>(
|
|
view:PhysicsMeshView<'a>,
|
|
transform:&'a PhysicsMeshTransform,
|
|
)->TransformedMesh<'a>{
|
|
TransformedMesh{
|
|
view,
|
|
transform,
|
|
}
|
|
}
|
|
pub fn verts<'a>(&'a self)->impl Iterator<Item=Vector3<Fixed<2,64>>>+'a{
|
|
self.view.data.verts.iter().map(|&Vert(pos)|self.transform.vertex.transform_point3(pos))
|
|
}
|
|
pub fn faces(&self)->impl Iterator<Item=SubmeshFaceId>{
|
|
(0..self.view.topology.faces.len() as u32).map(SubmeshFaceId::new)
|
|
}
|
|
}
|
|
impl MeshQuery for TransformedMesh<'_>{
|
|
type Face=SubmeshFaceId;
|
|
type Edge=SubmeshDirectedEdgeId;
|
|
type Vert=SubmeshVertId;
|
|
type Normal=Vector3<Fixed<3,96>>;
|
|
type Offset=Fixed<4,128>;
|
|
fn face_nd(&self,face_id:SubmeshFaceId)->(Self::Normal,Self::Offset){
|
|
let (n,d)=self.view.face_nd(face_id);
|
|
let transformed_n=self.transform.normal*n;
|
|
let transformed_d=d*self.transform.det+transformed_n.dot(self.transform.vertex.translation);
|
|
(transformed_n,transformed_d)
|
|
}
|
|
fn vert(&self,vert_id:SubmeshVertId)->Planar64Vec3{
|
|
// wrap for speed
|
|
self.transform.vertex.transform_point3(self.view.vert(vert_id)).wrap_1()
|
|
}
|
|
fn hint_point(&self)->Planar64Vec3{
|
|
self.transform.vertex.translation
|
|
}
|
|
fn farthest_vert(&self,dir:Planar64Vec3)->SubmeshVertId{
|
|
//this happens to be well-defined. there are no virtual virtices
|
|
SubmeshVertId::new(
|
|
self.view.topology.verts.iter()
|
|
.enumerate()
|
|
.max_by_key(|&(_,&vert_id)|
|
|
dir.dot(self.transform.vertex.transform_point3(self.view.data.verts[vert_id.get() as usize].0))
|
|
)
|
|
//assume there is more than zero vertices.
|
|
.unwrap().0 as u32
|
|
)
|
|
}
|
|
#[inline]
|
|
fn face_edges(&self,face_id:SubmeshFaceId)->impl AsRef<[SubmeshDirectedEdgeId]>{
|
|
self.view.face_edges(face_id)
|
|
}
|
|
#[inline]
|
|
fn edge_faces(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshFaceId;2]>{
|
|
self.view.edge_faces(edge_id)
|
|
}
|
|
#[inline]
|
|
fn edge_verts(&self,edge_id:SubmeshEdgeId)->impl AsRef<[SubmeshVertId;2]>{
|
|
self.view.edge_verts(edge_id)
|
|
}
|
|
#[inline]
|
|
fn vert_edges(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshDirectedEdgeId]>{
|
|
self.view.vert_edges(vert_id)
|
|
}
|
|
#[inline]
|
|
fn vert_faces(&self,vert_id:SubmeshVertId)->impl AsRef<[SubmeshFaceId]>{
|
|
self.view.vert_faces(vert_id)
|
|
}
|
|
}
|
|
|
|
//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)
|
|
//(vertex,face)
|
|
#[derive(Clone,Copy,Debug,Eq,PartialEq)]
|
|
pub enum MinkowskiVert{
|
|
VertVert(SubmeshVertId,SubmeshVertId),
|
|
}
|
|
// TODO: remove this
|
|
impl core::ops::Neg for MinkowskiVert{
|
|
type Output=Self;
|
|
fn neg(self)->Self::Output{
|
|
match self{
|
|
MinkowskiVert::VertVert(v0,v1)=>MinkowskiVert::VertVert(v1,v0),
|
|
}
|
|
}
|
|
}
|
|
#[derive(Clone,Copy,Debug,Eq,PartialEq)]
|
|
pub enum MinkowskiEdge{
|
|
VertEdge(SubmeshVertId,SubmeshEdgeId),
|
|
EdgeVert(SubmeshEdgeId,SubmeshVertId),
|
|
//EdgeEdge when edges are parallel
|
|
}
|
|
impl UndirectedEdge for MinkowskiEdge{
|
|
type DirectedEdge=MinkowskiDirectedEdge;
|
|
fn as_directed(&self,parity:bool)->Self::DirectedEdge{
|
|
match self{
|
|
MinkowskiEdge::VertEdge(v0,e1)=>MinkowskiDirectedEdge::VertEdge(*v0,e1.as_directed(parity)),
|
|
MinkowskiEdge::EdgeVert(e0,v1)=>MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),*v1),
|
|
}
|
|
}
|
|
}
|
|
#[derive(Clone,Copy,Debug,Eq,PartialEq)]
|
|
pub enum MinkowskiDirectedEdge{
|
|
VertEdge(SubmeshVertId,SubmeshDirectedEdgeId),
|
|
EdgeVert(SubmeshDirectedEdgeId,SubmeshVertId),
|
|
//EdgeEdge when edges are parallel
|
|
}
|
|
impl DirectedEdge for MinkowskiDirectedEdge{
|
|
type UndirectedEdge=MinkowskiEdge;
|
|
fn as_undirected(&self)->Self::UndirectedEdge{
|
|
match self{
|
|
MinkowskiDirectedEdge::VertEdge(v0,e1)=>MinkowskiEdge::VertEdge(*v0,e1.as_undirected()),
|
|
MinkowskiDirectedEdge::EdgeVert(e0,v1)=>MinkowskiEdge::EdgeVert(e0.as_undirected(),*v1),
|
|
}
|
|
}
|
|
fn parity(&self)->bool{
|
|
match self{
|
|
MinkowskiDirectedEdge::VertEdge(_,e)
|
|
|MinkowskiDirectedEdge::EdgeVert(e,_)=>e.parity(),
|
|
}
|
|
}
|
|
}
|
|
#[derive(Clone,Copy,Debug,Hash)]
|
|
pub enum MinkowskiFace{
|
|
VertFace(SubmeshVertId,SubmeshFaceId),
|
|
EdgeEdge(SubmeshEdgeId,SubmeshEdgeId,bool),
|
|
FaceVert(SubmeshFaceId,SubmeshVertId),
|
|
//EdgeFace
|
|
//FaceEdge
|
|
//FaceFace
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub struct MinkowskiMesh<'a>{
|
|
mesh0:TransformedMesh<'a>,
|
|
mesh1:TransformedMesh<'a>,
|
|
}
|
|
|
|
pub type GigaTime=Ratio<Fixed<4,128>,Fixed<4,128>>;
|
|
pub fn into_giga_time(time:Time,relative_to:Time)->GigaTime{
|
|
let r=(time-relative_to).to_ratio();
|
|
Ratio::new(r.num.widen_4(),r.den.widen_4())
|
|
}
|
|
|
|
// TODO: remove this
|
|
impl<'a> core::ops::Neg for &MinkowskiMesh<'a>{
|
|
type Output=MinkowskiMesh<'a>;
|
|
fn neg(self)->Self::Output{
|
|
MinkowskiMesh::minkowski_sum(self.mesh1,self.mesh0)
|
|
}
|
|
}
|
|
|
|
impl MinkowskiMesh<'_>{
|
|
pub fn minkowski_sum<'a>(mesh0:TransformedMesh<'a>,mesh1:TransformedMesh<'a>)->MinkowskiMesh<'a>{
|
|
MinkowskiMesh{
|
|
mesh0,
|
|
mesh1,
|
|
}
|
|
}
|
|
pub fn predict_collision_in(&self,relative_body:&Body,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
|
|
let fev=crate::minimum_difference::closest_fev_not_inside(self,relative_body.position)?;
|
|
//continue forwards along the body parabola
|
|
fev.crawl(self,relative_body,range.start_bound(),range.end_bound()).hit()
|
|
}
|
|
pub fn predict_collision_out(&self,relative_body:&Body,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
|
|
let (lower_bound,upper_bound)=(range.start_bound(),range.end_bound());
|
|
// TODO: handle unbounded collision using infinity fev
|
|
let time=match upper_bound{
|
|
Bound::Included(&time)=>time,
|
|
Bound::Excluded(&time)=>time,
|
|
Bound::Unbounded=>unimplemented!("unbounded collision out"),
|
|
};
|
|
let fev=crate::minimum_difference::closest_fev_not_inside(self,relative_body.extrapolated_position(time))?;
|
|
// swap and negate bounds to do a time inversion
|
|
let (lower_bound,upper_bound)=(upper_bound.map(|&t|-t),lower_bound.map(|&t|-t));
|
|
let infinity_body=-relative_body;
|
|
//continue backwards along the body parabola
|
|
fev.crawl(self,&infinity_body,lower_bound.as_ref(),upper_bound.as_ref()).hit()
|
|
//no need to test -time<time_limit because of the first step
|
|
.map(|(face,time)|(face,-time))
|
|
}
|
|
pub fn predict_collision_face_out(&self,relative_body:&Body,range:impl RangeBounds<Time>,contact_face_id:MinkowskiFace)->Option<(MinkowskiDirectedEdge,GigaTime)>{
|
|
// TODO: make better
|
|
use crate::face_crawler::{low,upp};
|
|
//no algorithm needed, there is only one state and two cases (Edge,None)
|
|
//determine when it passes an edge ("sliding off" case)
|
|
let start_time=range.start_bound().map(|&t|(t-relative_body.time).to_ratio());
|
|
let mut best_time=range.end_bound().map(|&t|into_giga_time(t,relative_body.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(){
|
|
let edge_n=self.directed_edge_n(directed_edge_id);
|
|
//f x e points in
|
|
let n=face_n.cross(edge_n);
|
|
let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
|
|
let d=n.dot(self.vert(v0)+self.vert(v1));
|
|
//WARNING! d outside of *2
|
|
//WARNING: truncated precision
|
|
//wrap for speed
|
|
for dt in Fixed::<4,128>::zeroes2(((n.dot(relative_body.position))*2-d).wrap_4(),n.dot(relative_body.velocity).wrap_4()*2,n.dot(relative_body.acceleration).wrap_4()){
|
|
if low(&start_time,&dt)&&upp(&dt,&best_time)&&n.dot(relative_body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
|
|
best_time=Bound::Included(dt);
|
|
best_edge=Some((directed_edge_id,dt));
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
best_edge
|
|
}
|
|
pub fn contains_point(&self,point:Planar64Vec3)->bool{
|
|
crate::minimum_difference::contains_point(self,point)
|
|
}
|
|
}
|
|
impl MeshQuery for MinkowskiMesh<'_>{
|
|
type Face=MinkowskiFace;
|
|
type Edge=MinkowskiDirectedEdge;
|
|
type Vert=MinkowskiVert;
|
|
type Normal=Vector3<Fixed<3,96>>;
|
|
type Offset=Fixed<4,128>;
|
|
// TODO: relative d
|
|
fn face_nd(&self,face_id:MinkowskiFace)->(Self::Normal,Self::Offset){
|
|
match face_id{
|
|
MinkowskiFace::VertFace(v0,f1)=>{
|
|
let (n,d)=self.mesh1.face_nd(f1);
|
|
(-n,d-n.dot(self.mesh0.vert(v0)))
|
|
},
|
|
MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
|
|
let edge0_n=self.mesh0.edge_n(e0);
|
|
let edge1_n=self.mesh1.edge_n(e1);
|
|
let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).as_ref();
|
|
let &[e1v0,e1v1]=self.mesh1.edge_verts(e1).as_ref();
|
|
let n=edge0_n.cross(edge1_n);
|
|
let e0d=n.dot(self.mesh0.vert(e0v0)+self.mesh0.vert(e0v1));
|
|
let e1d=n.dot(self.mesh1.vert(e1v0)+self.mesh1.vert(e1v1));
|
|
((n*(parity as i64*4-2)).widen_3(),((e0d-e1d)*(parity as i64*2-1)).widen_4())
|
|
},
|
|
MinkowskiFace::FaceVert(f0,v1)=>{
|
|
let (n,d)=self.mesh0.face_nd(f0);
|
|
(n,d-n.dot(self.mesh1.vert(v1)))
|
|
},
|
|
}
|
|
}
|
|
fn vert(&self,vert_id:MinkowskiVert)->Planar64Vec3{
|
|
match vert_id{
|
|
MinkowskiVert::VertVert(v0,v1)=>{
|
|
self.mesh0.vert(v0)-self.mesh1.vert(v1)
|
|
},
|
|
}
|
|
}
|
|
fn hint_point(&self)->Planar64Vec3{
|
|
self.mesh0.transform.vertex.translation-self.mesh1.transform.vertex.translation
|
|
}
|
|
fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{
|
|
MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir))
|
|
}
|
|
fn face_edges(&self,face_id:MinkowskiFace)->impl AsRef<[MinkowskiDirectedEdge]>{
|
|
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()
|
|
},
|
|
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),
|
|
]
|
|
},
|
|
MinkowskiFace::FaceVert(f0,v1)=>{
|
|
self.mesh0.face_edges(f0).as_ref().iter().map(|&edge_id0|
|
|
MinkowskiDirectedEdge::EdgeVert(edge_id0,v1)
|
|
).collect()
|
|
},
|
|
}
|
|
}
|
|
fn edge_faces(&self,edge_id:MinkowskiEdge)->impl AsRef<[MinkowskiFace;2]>{
|
|
match edge_id{
|
|
MinkowskiEdge::VertEdge(v0,e1)=>{
|
|
//faces are listed backwards from the minkowski mesh
|
|
let v0e=self.mesh0.vert_edges(v0);
|
|
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>,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(){
|
|
let edge0_n=self.mesh0.directed_edge_n(directed_edge_id0);
|
|
//must be behind other face.
|
|
let d=edge_face1_n.dot(edge0_n);
|
|
if d.is_negative(){
|
|
let edge0_nn=edge0_n.dot(edge0_n);
|
|
// Assume not every number is huge
|
|
// TODO: revisit this
|
|
let dd=(d*d)/(edge_face1_nn*edge0_nn);
|
|
if best_d<dd{
|
|
best_d=dd;
|
|
best_edge=Some(directed_edge_id0);
|
|
}
|
|
}
|
|
}
|
|
best_edge.map_or(
|
|
MinkowskiFace::VertFace(v0,edge_face_id1),
|
|
|directed_edge_id0|MinkowskiFace::EdgeEdge(directed_edge_id0.as_undirected(),e1,directed_edge_id0.parity()^face_parity)
|
|
)
|
|
}))
|
|
},
|
|
MinkowskiEdge::EdgeVert(e0,v1)=>{
|
|
//tracking index with an external variable because .enumerate() is not available
|
|
let v1e=self.mesh1.vert_edges(v1);
|
|
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>,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(){
|
|
let edge1_n=self.mesh1.directed_edge_n(directed_edge_id1);
|
|
let d=edge_face0_n.dot(edge1_n);
|
|
if d.is_negative(){
|
|
let edge1_nn=edge1_n.dot(edge1_n);
|
|
let dd=(d*d)/(edge_face0_nn*edge1_nn);
|
|
if best_d<dd{
|
|
best_d=dd;
|
|
best_edge=Some(directed_edge_id1);
|
|
}
|
|
}
|
|
}
|
|
best_edge.map_or(
|
|
MinkowskiFace::FaceVert(edge_face_id0,v1),
|
|
|directed_edge_id1|MinkowskiFace::EdgeEdge(e0,directed_edge_id1.as_undirected(),directed_edge_id1.parity()^face_parity)
|
|
)
|
|
}))
|
|
},
|
|
}
|
|
}
|
|
fn edge_verts(&self,edge_id:MinkowskiEdge)->impl AsRef<[MinkowskiVert;2]>{
|
|
AsRefHelper(match edge_id{
|
|
MinkowskiEdge::VertEdge(v0,e1)=>self.mesh1.edge_verts(e1).as_ref().map(|vert_id1|
|
|
MinkowskiVert::VertVert(v0,vert_id1)
|
|
),
|
|
MinkowskiEdge::EdgeVert(e0,v1)=>self.mesh0.edge_verts(e0).as_ref().map(|vert_id0|
|
|
MinkowskiVert::VertVert(vert_id0,v1)
|
|
),
|
|
})
|
|
}
|
|
fn vert_edges(&self,vert_id:MinkowskiVert)->impl AsRef<[MinkowskiDirectedEdge]>{
|
|
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_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(){
|
|
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
|
|
// drop faces beyond v0f_n
|
|
face_normals.truncate(v0f.len());
|
|
// make a set of faces from mesh0's perspective
|
|
for face_n in &v1f_n{
|
|
//add reflected mesh1 faces
|
|
//wrap for speed
|
|
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));
|
|
}
|
|
}
|
|
face_normals.clone_from(&v1f_n);
|
|
for &directed_edge_id in self.mesh1.vert_edges(v1).as_ref(){
|
|
let n=self.mesh1.directed_edge_n(directed_edge_id);
|
|
let nn=n.dot(n);
|
|
// drop faces beyond v1f_n
|
|
face_normals.truncate(v1f.len());
|
|
// make a set of faces from mesh1's perspective
|
|
for face_n in &v0f_n{
|
|
//wrap for speed
|
|
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));
|
|
}
|
|
}
|
|
edges
|
|
},
|
|
}
|
|
}
|
|
fn vert_faces(&self,_vert_id:MinkowskiVert)->impl AsRef<[MinkowskiFace]>{
|
|
unimplemented!();
|
|
#[expect(unreachable_code)]
|
|
Vec::new()
|
|
}
|
|
}
|
|
|
|
fn is_empty_volume(normals:&[Vector3<Fixed<3,96>>])->bool{
|
|
let len=normals.len();
|
|
for i in 0..len-1{
|
|
for j in i+1..len{
|
|
let n=normals[i].cross(normals[j]);
|
|
let mut d_comp=None;
|
|
for k in 0..len{
|
|
if k!=i&&k!=j{
|
|
let d=n.dot(normals[k]).is_negative();
|
|
if let &Some(comp)=&d_comp{
|
|
// This is testing if d_comp*d < 0
|
|
if comp^d{
|
|
return true;
|
|
}
|
|
}else{
|
|
d_comp=Some(d);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
#[test]
|
|
fn test_is_empty_volume(){
|
|
assert!(!is_empty_volume(&[vec3::X.widen_3(),vec3::Y.widen_3(),vec3::Z.widen_3()]));
|
|
assert!(is_empty_volume(&[vec3::X.widen_3(),vec3::Y.widen_3(),vec3::Z.widen_3(),vec3::NEG_X.widen_3()]));
|
|
}
|