2023-11-30 09:51:17 +00:00
use std ::borrow ::{ Borrow , Cow } ;
2024-02-05 04:59:33 +00:00
use strafesnet_common ::model ;
2024-01-30 00:19:41 +00:00
use strafesnet_common ::zeroes ;
use strafesnet_common ::integer ::{ self , Planar64 , Planar64Vec3 } ;
2023-11-30 09:51:17 +00:00
pub trait UndirectedEdge {
type DirectedEdge :Copy + DirectedEdge ;
fn as_directed ( & self , parity :bool ) ->Self ::DirectedEdge ;
}
pub trait DirectedEdge {
type UndirectedEdge :Copy + 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 ( ) )
}
}
2024-02-07 03:04:23 +00:00
2024-02-07 04:29:36 +00:00
#[ 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 ) ;
2023-11-30 09:51:17 +00:00
/// DirectedEdgeId refers to an EdgeId when undirected.
2024-02-07 04:29:36 +00:00
#[ 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 ) ;
2024-02-07 03:04:23 +00:00
2024-02-07 04:29:36 +00:00
impl UndirectedEdge for SubmeshEdgeId {
type DirectedEdge = SubmeshDirectedEdgeId ;
fn as_directed ( & self , parity :bool ) ->SubmeshDirectedEdgeId {
SubmeshDirectedEdgeId ( self . 0 | ( ( parity as u32 ) < < ( u32 ::BITS - 1 ) ) )
2024-02-07 03:04:23 +00:00
}
}
2024-02-07 04:29:36 +00:00
impl DirectedEdge for SubmeshDirectedEdgeId {
type UndirectedEdge = SubmeshEdgeId ;
fn as_undirected ( & self ) ->SubmeshEdgeId {
SubmeshEdgeId ( self . 0 & ! ( 1 < < ( u32 ::BITS - 1 ) ) )
2023-11-30 09:51:17 +00:00
}
fn parity ( & self ) ->bool {
2024-02-07 03:04:23 +00:00
self . 0 & ( 1 < < ( u32 ::BITS - 1 ) ) ! = 0
2023-11-30 09:51:17 +00:00
}
}
//Vertex <-> Edge <-> Face -> Collide
pub enum FEV < F , E :DirectedEdge , V > {
Face ( F ) ,
Edge ( E ::UndirectedEdge ) ,
Vert ( V ) ,
}
//use Unit32 #[repr(C)] for map files
struct Face {
normal :Planar64Vec3 ,
dot :Planar64 ,
}
struct Vert ( Planar64Vec3 ) ;
pub trait MeshQuery < FACE :Clone , EDGE :Clone + DirectedEdge , VERT :Clone > {
fn edge_n ( & self , edge_id :EDGE ::UndirectedEdge ) ->Planar64Vec3 {
let verts = self . edge_verts ( edge_id ) ;
self . vert ( verts [ 1 ] . clone ( ) ) - self . vert ( verts [ 0 ] . clone ( ) )
}
fn directed_edge_n ( & self , directed_edge_id :EDGE ) ->Planar64Vec3 {
let verts = self . edge_verts ( directed_edge_id . as_undirected ( ) ) ;
( self . vert ( verts [ 1 ] . clone ( ) ) - self . vert ( verts [ 0 ] . clone ( ) ) ) * ( ( directed_edge_id . parity ( ) as i64 ) * 2 - 1 )
}
fn vert ( & self , vert_id :VERT ) ->Planar64Vec3 ;
fn face_nd ( & self , face_id :FACE ) ->( Planar64Vec3 , Planar64 ) ;
fn face_edges ( & self , face_id :FACE ) ->Cow < Vec < EDGE > > ;
fn edge_faces ( & self , edge_id :EDGE ::UndirectedEdge ) ->Cow < [ FACE ; 2 ] > ;
fn edge_verts ( & self , edge_id :EDGE ::UndirectedEdge ) ->Cow < [ VERT ; 2 ] > ;
fn vert_edges ( & self , vert_id :VERT ) ->Cow < Vec < EDGE > > ;
fn vert_faces ( & self , vert_id :VERT ) ->Cow < Vec < FACE > > ;
}
struct FaceRefs {
2024-02-07 04:29:36 +00:00
edges :Vec < SubmeshDirectedEdgeId > ,
2023-11-30 09:51:17 +00:00
//verts:Vec<VertId>,
}
struct EdgeRefs {
2024-02-07 04:29:36 +00:00
faces :[ SubmeshFaceId ; 2 ] , //left, right
verts :[ SubmeshVertId ; 2 ] , //bottom, top
2023-11-30 09:51:17 +00:00
}
struct VertRefs {
2024-02-07 04:29:36 +00:00
faces :Vec < SubmeshFaceId > ,
edges :Vec < SubmeshDirectedEdgeId > ,
2023-11-30 09:51:17 +00:00
}
2024-02-07 01:30:02 +00:00
struct PhysicsMeshData {
2024-02-07 03:04:23 +00:00
//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.
2024-02-07 04:29:36 +00:00
faces :Vec < Face > , //MeshFaceId indexes this list
verts :Vec < Vert > , //MeshVertId indexes this list
2023-11-30 09:51:17 +00:00
}
2024-02-07 01:30:02 +00:00
struct PhysicsMeshTopology {
//mapping of local ids to PhysicsMeshData ids
2024-02-07 04:29:36 +00:00
faces :Vec < MeshFaceId > , //SubmeshFaceId indexes this list
verts :Vec < MeshVertId > , //SubmeshVertId indexes this list
2024-02-07 01:30:02 +00:00
//all ids here are local to this object
face_topology :Vec < FaceRefs > ,
edge_topology :Vec < EdgeRefs > ,
vert_topology :Vec < VertRefs > ,
}
2024-02-07 04:29:36 +00:00
#[ derive(id::Id) ]
pub struct PhysicsMeshId ( u32 ) ;
#[ derive(Debug,Clone,Copy,Hash,id::Id,Eq,PartialEq) ]
pub struct PhysicsSubmeshId ( u32 ) ;
2024-02-07 01:30:02 +00:00
pub struct PhysicsMesh {
2024-02-07 04:29:36 +00:00
data :PhysicsMeshData ,
2024-02-07 03:04:23 +00:00
//index 0 is the complete mesh.
//index 1-2+ is convex submeshes.
//Most objects in roblox maps are already convex, so the list length is 1
//as soon as the mesh is divided into 2 submeshes, the list length jumps to 3.
//length 2 is unnecessary since the complete mesh would be a duplicate of the only submesh, but would still function properly
submeshes :Vec < PhysicsMeshTopology > ,
2024-02-07 01:30:02 +00:00
}
2024-02-06 03:49:36 +00:00
impl PhysicsMesh {
pub fn unit_cube ( ) ->Self {
2024-02-07 01:30:02 +00:00
//go go gadget debug print mesh
2024-02-07 04:29:36 +00:00
let data = PhysicsMeshData {
2024-02-07 03:04:23 +00:00
faces :vec ! [
Face { normal :Planar64Vec3 ::raw ( 4294967296 , 0 , 0 ) , dot :Planar64 ::raw ( 4294967296 ) } ,
Face { normal :Planar64Vec3 ::raw ( 0 , 4294967296 , 0 ) , dot :Planar64 ::raw ( 4294967296 ) } ,
Face { normal :Planar64Vec3 ::raw ( 0 , 0 , 4294967296 ) , dot :Planar64 ::raw ( 4294967296 ) } ,
Face { normal :Planar64Vec3 ::raw ( - 4294967296 , 0 , 0 ) , dot :Planar64 ::raw ( 4294967296 ) } ,
Face { normal :Planar64Vec3 ::raw ( 0 , - 4294967296 , 0 ) , dot :Planar64 ::raw ( 4294967296 ) } ,
Face { normal :Planar64Vec3 ::raw ( 0 , 0 , - 4294967296 ) , dot :Planar64 ::raw ( 4294967296 ) }
] ,
verts :vec ! [
Vert ( Planar64Vec3 ::raw ( 4294967296 , - 4294967296 , - 4294967296 ) ) ,
Vert ( Planar64Vec3 ::raw ( 4294967296 , 4294967296 , - 4294967296 ) ) ,
Vert ( Planar64Vec3 ::raw ( 4294967296 , 4294967296 , 4294967296 ) ) ,
Vert ( Planar64Vec3 ::raw ( 4294967296 , - 4294967296 , 4294967296 ) ) ,
Vert ( Planar64Vec3 ::raw ( - 4294967296 , 4294967296 , - 4294967296 ) ) ,
Vert ( Planar64Vec3 ::raw ( - 4294967296 , 4294967296 , 4294967296 ) ) ,
Vert ( Planar64Vec3 ::raw ( - 4294967296 , - 4294967296 , 4294967296 ) ) ,
Vert ( Planar64Vec3 ::raw ( - 4294967296 , - 4294967296 , - 4294967296 ) )
]
} ;
let mesh_topology = PhysicsMeshTopology {
2024-02-07 04:29:36 +00:00
faces :( 0 .. data . faces . len ( ) as u32 ) . map ( MeshFaceId ::new ) . collect ( ) ,
verts :( 0 .. data . verts . len ( ) as u32 ) . map ( MeshVertId ::new ) . collect ( ) ,
2024-02-07 03:04:23 +00:00
face_topology :vec ! [
2024-02-07 04:29:36 +00:00
FaceRefs { edges :vec ! [ SubmeshDirectedEdgeId ( 9223372036854775808 ) , SubmeshDirectedEdgeId ( 9223372036854775809 ) , SubmeshDirectedEdgeId ( 9223372036854775810 ) , SubmeshDirectedEdgeId ( 3 ) ] } ,
FaceRefs { edges :vec ! [ SubmeshDirectedEdgeId ( 9223372036854775812 ) , SubmeshDirectedEdgeId ( 9223372036854775813 ) , SubmeshDirectedEdgeId ( 6 ) , SubmeshDirectedEdgeId ( 1 ) ] } ,
FaceRefs { edges :vec ! [ SubmeshDirectedEdgeId ( 7 ) , SubmeshDirectedEdgeId ( 2 ) , SubmeshDirectedEdgeId ( 9223372036854775814 ) , SubmeshDirectedEdgeId ( 9223372036854775816 ) ] } ,
FaceRefs { edges :vec ! [ SubmeshDirectedEdgeId ( 8 ) , SubmeshDirectedEdgeId ( 5 ) , SubmeshDirectedEdgeId ( 9223372036854775817 ) , SubmeshDirectedEdgeId ( 10 ) ] } ,
FaceRefs { edges :vec ! [ SubmeshDirectedEdgeId ( 9223372036854775815 ) , SubmeshDirectedEdgeId ( 9223372036854775818 ) , SubmeshDirectedEdgeId ( 11 ) , SubmeshDirectedEdgeId ( 9223372036854775811 ) ] } ,
FaceRefs { edges :vec ! [ SubmeshDirectedEdgeId ( 4 ) , SubmeshDirectedEdgeId ( 0 ) , SubmeshDirectedEdgeId ( 9223372036854775819 ) , SubmeshDirectedEdgeId ( 9 ) ] }
2024-02-07 03:04:23 +00:00
] ,
edge_topology :vec ! [
2024-02-07 04:29:36 +00:00
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 ) ] }
2024-02-07 03:04:23 +00:00
] ,
vert_topology :vec ! [
2024-02-07 04:29:36 +00:00
VertRefs { faces :vec ! [ SubmeshFaceId ( 0 ) , SubmeshFaceId ( 4 ) , SubmeshFaceId ( 5 ) ] , edges :vec ! [ SubmeshDirectedEdgeId ( 9223372036854775811 ) , SubmeshDirectedEdgeId ( 9223372036854775819 ) , SubmeshDirectedEdgeId ( 9223372036854775808 ) ] } ,
VertRefs { faces :vec ! [ SubmeshFaceId ( 0 ) , SubmeshFaceId ( 5 ) , SubmeshFaceId ( 1 ) ] , edges :vec ! [ SubmeshDirectedEdgeId ( 9223372036854775812 ) , SubmeshDirectedEdgeId ( 0 ) , SubmeshDirectedEdgeId ( 9223372036854775809 ) ] } ,
VertRefs { faces :vec ! [ SubmeshFaceId ( 0 ) , SubmeshFaceId ( 2 ) , SubmeshFaceId ( 1 ) ] , edges :vec ! [ SubmeshDirectedEdgeId ( 1 ) , SubmeshDirectedEdgeId ( 9223372036854775810 ) , SubmeshDirectedEdgeId ( 9223372036854775814 ) ] } ,
VertRefs { faces :vec ! [ SubmeshFaceId ( 0 ) , SubmeshFaceId ( 2 ) , SubmeshFaceId ( 4 ) ] , edges :vec ! [ SubmeshDirectedEdgeId ( 2 ) , SubmeshDirectedEdgeId ( 3 ) , SubmeshDirectedEdgeId ( 9223372036854775815 ) ] } ,
VertRefs { faces :vec ! [ SubmeshFaceId ( 3 ) , SubmeshFaceId ( 5 ) , SubmeshFaceId ( 1 ) ] , edges :vec ! [ SubmeshDirectedEdgeId ( 4 ) , SubmeshDirectedEdgeId ( 9223372036854775817 ) , SubmeshDirectedEdgeId ( 9223372036854775813 ) ] } ,
VertRefs { faces :vec ! [ SubmeshFaceId ( 2 ) , SubmeshFaceId ( 3 ) , SubmeshFaceId ( 1 ) ] , edges :vec ! [ SubmeshDirectedEdgeId ( 5 ) , SubmeshDirectedEdgeId ( 6 ) , SubmeshDirectedEdgeId ( 9223372036854775816 ) ] } ,
VertRefs { faces :vec ! [ SubmeshFaceId ( 2 ) , SubmeshFaceId ( 3 ) , SubmeshFaceId ( 4 ) ] , edges :vec ! [ SubmeshDirectedEdgeId ( 7 ) , SubmeshDirectedEdgeId ( 8 ) , SubmeshDirectedEdgeId ( 9223372036854775818 ) ] } ,
VertRefs { faces :vec ! [ SubmeshFaceId ( 4 ) , SubmeshFaceId ( 3 ) , SubmeshFaceId ( 5 ) ] , edges :vec ! [ SubmeshDirectedEdgeId ( 10 ) , SubmeshDirectedEdgeId ( 11 ) , SubmeshDirectedEdgeId ( 9 ) ] }
2024-02-07 03:04:23 +00:00
]
} ;
2024-02-07 01:30:02 +00:00
Self {
2024-02-07 04:29:36 +00:00
data ,
2024-02-07 03:04:23 +00:00
submeshes :vec ! [ mesh_topology ] ,
2024-02-07 01:30:02 +00:00
}
2024-02-06 03:49:36 +00:00
}
pub fn unit_cylinder ( ) ->Self {
Self ::unit_cube ( )
}
2024-02-07 03:04:23 +00:00
pub fn mesh_data ( & self ) ->& PhysicsMeshData {
2024-02-07 04:29:36 +00:00
& self . data
2024-02-07 03:04:23 +00:00
}
pub fn complete_mesh ( & self ) ->& PhysicsMeshTopology {
& self . submeshes [ 0 ]
}
2024-02-07 04:29:36 +00:00
pub fn submeshes ( & self ) ->& [ PhysicsMeshTopology ] {
2024-02-07 03:04:23 +00:00
if self . submeshes . len ( ) = = 1 {
//the complete mesh is already a convex mesh
& self . submeshes [ 0 .. 0 ]
} else {
& self . submeshes [ 1 .. ]
}
}
2024-02-07 04:29:36 +00:00
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 ,
} )
}
2024-02-06 03:49:36 +00:00
}
2023-11-30 09:51:17 +00:00
2024-02-07 03:04:23 +00:00
//mesh builder code
2023-11-30 09:51:17 +00:00
#[ derive(Default,Clone) ]
struct VertRefGuy {
2024-02-07 04:29:36 +00:00
edges :std ::collections ::HashSet < SubmeshDirectedEdgeId > ,
faces :std ::collections ::HashSet < SubmeshFaceId > ,
2023-11-30 09:51:17 +00:00
}
#[ derive(Clone,Hash,Eq,PartialEq) ]
2024-02-07 04:29:36 +00:00
struct EdgeRefVerts ( [ SubmeshVertId ; 2 ] ) ;
2023-11-30 09:51:17 +00:00
impl EdgeRefVerts {
2024-02-07 04:29:36 +00:00
fn new ( v0 :SubmeshVertId , v1 :SubmeshVertId ) ->( Self , bool ) {
2023-11-30 09:51:17 +00:00
( if v0 . 0 < v1 . 0 {
Self ( [ v0 , v1 ] )
} else {
Self ( [ v1 , v0 ] )
} , v0 . 0 < v1 . 0 )
}
}
2024-02-07 04:29:36 +00:00
struct EdgeRefFaces ( [ SubmeshFaceId ; 2 ] ) ;
2023-11-30 09:51:17 +00:00
impl EdgeRefFaces {
fn new ( ) ->Self {
2024-02-07 04:29:36 +00:00
Self ( [ SubmeshFaceId ( 0 ) ; 2 ] )
2023-11-30 09:51:17 +00:00
}
2024-02-07 04:29:36 +00:00
fn push ( & mut self , i :usize , face_id :SubmeshFaceId ) {
2023-11-30 09:51:17 +00:00
self . 0 [ i ] = face_id ;
}
}
2024-02-07 04:29:36 +00:00
struct FaceRefEdges ( Vec < SubmeshDirectedEdgeId > ) ;
2023-11-30 09:51:17 +00:00
#[ derive(Default) ]
struct EdgePool {
edge_guys :Vec < ( EdgeRefVerts , EdgeRefFaces ) > ,
edge_id_from_guy :std ::collections ::HashMap < EdgeRefVerts , usize > ,
}
impl EdgePool {
2024-02-07 04:29:36 +00:00
fn push ( & mut self , edge_ref_verts :EdgeRefVerts ) ->( & mut EdgeRefFaces , SubmeshEdgeId ) {
2023-11-30 09:51:17 +00:00
let edge_id = if let Some ( & edge_id ) = self . edge_id_from_guy . get ( & edge_ref_verts ) {
edge_id
} else {
let edge_id = self . edge_guys . len ( ) ;
self . edge_guys . push ( ( edge_ref_verts . clone ( ) , EdgeRefFaces ::new ( ) ) ) ;
self . edge_id_from_guy . insert ( edge_ref_verts , edge_id ) ;
edge_id
} ;
2024-02-07 04:29:36 +00:00
( & mut unsafe { self . edge_guys . get_unchecked_mut ( edge_id ) } . 1 , SubmeshEdgeId ::new ( edge_id as u32 ) )
2023-11-30 09:51:17 +00:00
}
}
2024-02-05 04:59:33 +00:00
impl From < & model ::IndexedModel > for PhysicsMesh {
fn from ( indexed_model :& model ::IndexedModel ) ->Self {
2023-11-30 09:51:17 +00:00
assert! ( indexed_model . unique_pos . len ( ) ! = 0 , " Mesh cannot have 0 vertices " ) ;
let verts = indexed_model . unique_pos . iter ( ) . map ( | v | Vert ( v . clone ( ) ) ) . collect ( ) ;
let mut vert_ref_guys = vec! [ VertRefGuy ::default ( ) ; indexed_model . unique_pos . len ( ) ] ;
let mut edge_pool = EdgePool ::default ( ) ;
let mut face_i = 0 ;
let mut faces = Vec ::new ( ) ;
let mut face_ref_guys = Vec ::new ( ) ;
2024-02-05 23:25:44 +00:00
for group in & indexed_model . polygon_groups { for poly_vertices in group . polys ( ) {
2024-02-07 04:29:36 +00:00
let face_id = SubmeshFaceId ::new ( face_i ) ;
2023-11-30 09:51:17 +00:00
//one face per poly
let mut normal = Planar64Vec3 ::ZERO ;
2024-02-05 23:25:44 +00:00
let len = poly_vertices . len ( ) ;
let face_edges = poly_vertices . iter ( ) . enumerate ( ) . map ( | ( i , & vert_id ) | {
let vert0_id = indexed_model . unique_vertices [ vert_id . get ( ) as usize ] . pos . get ( ) as usize ;
let vert1_id = indexed_model . unique_vertices [ poly_vertices [ ( i + 1 ) % len ] . get ( ) as usize ] . pos . get ( ) as usize ;
2023-11-30 09:51:17 +00:00
//https://www.khronos.org/opengl/wiki/Calculating_a_Surface_Normal (Newell's Method)
let v0 = indexed_model . unique_pos [ vert0_id ] ;
let v1 = indexed_model . unique_pos [ vert1_id ] ;
normal + = Planar64Vec3 ::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
2024-02-07 04:29:36 +00:00
let ( edge_ref_verts , is_sorted ) = EdgeRefVerts ::new ( SubmeshVertId ::new ( vert0_id as u32 ) , SubmeshVertId ::new ( vert1_id as u32 ) ) ;
2023-11-30 09:51:17 +00:00
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 , face_id ) ;
//index edges & face into vertices
{
let vert_ref_guy = unsafe { vert_ref_guys . get_unchecked_mut ( vert0_id ) } ;
vert_ref_guy . edges . insert ( edge_id . as_directed ( is_sorted ) ) ;
vert_ref_guy . faces . insert ( face_id ) ;
unsafe { vert_ref_guys . get_unchecked_mut ( vert1_id ) } . edges . insert ( edge_id . as_directed ( ! is_sorted ) ) ;
}
//return directed_edge_id
edge_id . as_directed ( is_sorted )
} ) . collect ( ) ;
//choose precision loss randomly idk
normal = normal / len as i64 ;
let mut dot = Planar64 ::ZERO ;
2024-02-05 23:25:44 +00:00
for & v in poly_vertices {
dot + = normal . dot ( indexed_model . unique_pos [ indexed_model . unique_vertices [ v . get ( ) as usize ] . pos . get ( ) as usize ] ) ;
2023-11-30 09:51:17 +00:00
}
faces . push ( Face { normal , dot :dot / len as i64 } ) ;
face_ref_guys . push ( FaceRefEdges ( face_edges ) ) ;
face_i + = 1 ;
} }
//conceivably faces, edges, and vertices exist now
Self {
faces ,
verts ,
face_topology :face_ref_guys . into_iter ( ) . map ( | face_ref_guy | {
FaceRefs { edges :face_ref_guy . 0 }
} ) . collect ( ) ,
edge_topology :edge_pool . edge_guys . into_iter ( ) . map ( | ( edge_ref_verts , edge_ref_faces ) |
EdgeRefs { faces :edge_ref_faces . 0 , verts :edge_ref_verts . 0 }
) . collect ( ) ,
vert_topology :vert_ref_guys . into_iter ( ) . map ( | vert_ref_guy |
VertRefs {
edges :vert_ref_guy . edges . into_iter ( ) . collect ( ) ,
faces :vert_ref_guy . faces . into_iter ( ) . collect ( ) ,
}
) . collect ( ) ,
}
}
}
2024-02-07 04:29:36 +00:00
struct PhysicsMeshView < ' a > {
data :& ' a PhysicsMeshData ,
topology :& ' a PhysicsMeshTopology ,
}
2024-02-07 03:04:23 +00:00
impl PhysicsMeshView < '_ > {
2023-11-30 09:51:17 +00:00
pub fn verts < ' a > ( & ' a self ) ->impl Iterator < Item = Planar64Vec3 > + ' a {
2024-02-07 04:29:36 +00:00
self . data . verts . iter ( ) . map ( | Vert ( pos ) | * pos )
2023-11-30 09:51:17 +00:00
}
}
2024-02-07 04:29:36 +00:00
impl MeshQuery < SubmeshFaceId , SubmeshDirectedEdgeId , SubmeshVertId > for PhysicsMeshView < '_ > {
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 )
2023-11-30 09:51:17 +00:00
}
//ideally I never calculate the vertex position, but I have to for the graphical meshes...
2024-02-07 04:29:36 +00:00
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
2023-11-30 09:51:17 +00:00
}
2024-02-07 04:29:36 +00:00
fn face_edges ( & self , face_id :SubmeshFaceId ) ->Cow < Vec < SubmeshDirectedEdgeId > > {
Cow ::Borrowed ( & self . topology . face_topology [ face_id . get ( ) as usize ] . edges )
2023-11-30 09:51:17 +00:00
}
2024-02-07 04:29:36 +00:00
fn edge_faces ( & self , edge_id :SubmeshEdgeId ) ->Cow < [ SubmeshFaceId ; 2 ] > {
Cow ::Borrowed ( & self . topology . edge_topology [ edge_id . get ( ) as usize ] . faces )
2023-11-30 09:51:17 +00:00
}
2024-02-07 04:29:36 +00:00
fn edge_verts ( & self , edge_id :SubmeshEdgeId ) ->Cow < [ SubmeshVertId ; 2 ] > {
Cow ::Borrowed ( & self . topology . edge_topology [ edge_id . get ( ) as usize ] . verts )
2023-11-30 09:51:17 +00:00
}
2024-02-07 04:29:36 +00:00
fn vert_edges ( & self , vert_id :SubmeshVertId ) ->Cow < Vec < SubmeshDirectedEdgeId > > {
Cow ::Borrowed ( & self . topology . vert_topology [ vert_id . get ( ) as usize ] . edges )
2023-11-30 09:51:17 +00:00
}
2024-02-07 04:29:36 +00:00
fn vert_faces ( & self , vert_id :SubmeshVertId ) ->Cow < Vec < SubmeshFaceId > > {
Cow ::Borrowed ( & self . topology . vert_topology [ vert_id . get ( ) as usize ] . faces )
2023-11-30 09:51:17 +00:00
}
}
2024-02-07 04:29:36 +00:00
pub struct PhysicsMeshTransform < ' a > {
vertex :& ' a integer ::Planar64Affine3 ,
normal :& ' a integer ::Planar64Mat3 ,
det :Planar64 ,
2024-02-07 03:04:23 +00:00
}
2024-02-07 04:29:36 +00:00
impl PhysicsMeshTransform < '_ > {
pub fn new < ' a > (
vertex :& ' a integer ::Planar64Affine3 ,
normal :& ' a integer ::Planar64Mat3 ,
det :Planar64
) ->PhysicsMeshTransform < ' a > {
PhysicsMeshTransform {
vertex ,
normal ,
det ,
}
}
2023-11-30 09:51:17 +00:00
}
2024-02-07 03:04:23 +00:00
pub struct TransformedMesh < ' a > {
2024-02-07 04:29:36 +00:00
view :PhysicsMeshView < ' a > ,
2024-02-07 03:04:23 +00:00
transform :PhysicsMeshTransform < ' a > ,
}
2023-11-30 09:51:17 +00:00
impl TransformedMesh < '_ > {
pub fn new < ' a > (
2024-02-07 03:04:23 +00:00
mesh_data :& ' a PhysicsMeshData ,
topology :& ' a PhysicsMeshTopology ,
2024-02-07 04:29:36 +00:00
vertex :& ' a integer ::Planar64Affine3 ,
normal :& ' a integer ::Planar64Mat3 ,
det :Planar64 ,
2023-11-30 09:51:17 +00:00
) ->TransformedMesh < ' a > {
TransformedMesh {
2024-02-07 04:29:36 +00:00
view :PhysicsMeshView {
data : mesh_data ,
topology ,
} ,
transform :PhysicsMeshTransform {
vertex ,
normal ,
det ,
}
2023-11-30 09:51:17 +00:00
}
}
2024-02-07 04:29:36 +00:00
fn farthest_vert ( & self , dir :Planar64Vec3 ) ->SubmeshVertId {
2023-11-30 09:51:17 +00:00
let mut best_dot = Planar64 ::MIN ;
2024-02-07 04:29:36 +00:00
let mut best_vert = SubmeshVertId ( 0 ) ;
//this happens to be well-defined. there are no virtual virtices
for ( i , vert_id ) in self . view . topology . verts . iter ( ) . enumerate ( ) {
let vert = self . view . data . verts [ vert_id . get ( ) as usize ] ;
let p = self . transform . vertex . transform_point3 ( vert . 0 ) ;
2023-11-30 09:51:17 +00:00
let d = dir . dot ( p ) ;
if best_dot < d {
best_dot = d ;
2024-02-07 04:29:36 +00:00
best_vert = SubmeshVertId ::new ( i as u32 ) ;
2023-11-30 09:51:17 +00:00
}
}
best_vert
}
}
2024-02-07 04:29:36 +00:00
impl MeshQuery < SubmeshFaceId , SubmeshDirectedEdgeId , SubmeshVertId > for TransformedMesh < '_ > {
fn face_nd ( & self , face_id :SubmeshFaceId ) ->( Planar64Vec3 , Planar64 ) {
let ( n , d ) = self . view . face_nd ( face_id ) ;
let transformed_n = * self . transform . normal * n ;
let transformed_d = d + transformed_n . dot ( self . transform . vertex . translation ) / self . transform . det ;
( transformed_n / self . transform . det , transformed_d )
2023-11-30 09:51:17 +00:00
}
2024-02-07 04:29:36 +00:00
fn vert ( & self , vert_id :SubmeshVertId ) ->Planar64Vec3 {
self . transform . vertex . transform_point3 ( self . view . vert ( vert_id ) )
2023-11-30 09:51:17 +00:00
}
#[ inline ]
2024-02-07 04:29:36 +00:00
fn face_edges ( & self , face_id :SubmeshFaceId ) ->Cow < Vec < SubmeshDirectedEdgeId > > {
self . view . face_edges ( face_id )
2023-11-30 09:51:17 +00:00
}
#[ inline ]
2024-02-07 04:29:36 +00:00
fn edge_faces ( & self , edge_id :SubmeshEdgeId ) ->Cow < [ SubmeshFaceId ; 2 ] > {
self . view . edge_faces ( edge_id )
2023-11-30 09:51:17 +00:00
}
#[ inline ]
2024-02-07 04:29:36 +00:00
fn edge_verts ( & self , edge_id :SubmeshEdgeId ) ->Cow < [ SubmeshVertId ; 2 ] > {
self . view . edge_verts ( edge_id )
2023-11-30 09:51:17 +00:00
}
#[ inline ]
2024-02-07 04:29:36 +00:00
fn vert_edges ( & self , vert_id :SubmeshVertId ) ->Cow < Vec < SubmeshDirectedEdgeId > > {
self . view . vert_edges ( vert_id )
2023-11-30 09:51:17 +00:00
}
#[ inline ]
2024-02-07 04:29:36 +00:00
fn vert_faces ( & self , vert_id :SubmeshVertId ) ->Cow < Vec < SubmeshFaceId > > {
self . view . vert_faces ( vert_id )
2023-11-30 09:51:17 +00:00
}
}
//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) ]
pub enum MinkowskiVert {
2024-02-07 04:29:36 +00:00
VertVert ( SubmeshVertId , SubmeshVertId ) ,
2023-11-30 09:51:17 +00:00
}
#[ derive(Clone,Copy) ]
pub enum MinkowskiEdge {
2024-02-07 04:29:36 +00:00
VertEdge ( SubmeshVertId , SubmeshEdgeId ) ,
EdgeVert ( SubmeshEdgeId , SubmeshVertId ) ,
2023-11-30 09:51:17 +00:00
//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) ]
pub enum MinkowskiDirectedEdge {
2024-02-07 04:29:36 +00:00
VertEdge ( SubmeshVertId , SubmeshDirectedEdgeId ) ,
EdgeVert ( SubmeshDirectedEdgeId , SubmeshVertId ) ,
2023-11-30 09:51:17 +00:00
//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(Debug,Clone,Copy,Hash,Eq,PartialEq) ]
pub enum MinkowskiFace {
2024-02-07 04:29:36 +00:00
VertFace ( SubmeshVertId , SubmeshFaceId ) ,
EdgeEdge ( SubmeshEdgeId , SubmeshEdgeId , bool ) ,
FaceVert ( SubmeshFaceId , SubmeshVertId ) ,
2023-11-30 09:51:17 +00:00
//EdgeFace
//FaceEdge
//FaceFace
}
pub struct MinkowskiMesh < ' a > {
2024-02-07 03:04:23 +00:00
mesh0 :TransformedMesh < ' a > ,
mesh1 :TransformedMesh < ' a > ,
2023-11-30 09:51:17 +00:00
}
//infinity fev algorithm state transition
enum Transition {
Done , //found closest vert, no edges are better
Vert ( MinkowskiVert ) , //transition to vert
}
enum EV {
Vert ( MinkowskiVert ) ,
Edge ( MinkowskiEdge ) ,
}
impl MinkowskiMesh < '_ > {
2024-02-07 03:04:23 +00:00
pub fn minkowski_sum < ' a > ( mesh0 :TransformedMesh < ' a > , mesh1 :TransformedMesh < ' a > ) ->MinkowskiMesh < ' a > {
2023-11-30 09:51:17 +00:00
MinkowskiMesh {
mesh0 ,
mesh1 ,
}
}
fn farthest_vert ( & self , dir :Planar64Vec3 ) ->MinkowskiVert {
MinkowskiVert ::VertVert ( self . mesh0 . farthest_vert ( dir ) , self . mesh1 . farthest_vert ( - dir ) )
}
fn next_transition_vert ( & self , vert_id :MinkowskiVert , best_distance_squared :& mut Planar64 , infinity_dir :Planar64Vec3 , point :Planar64Vec3 ) ->Transition {
let mut best_transition = Transition ::Done ;
for & directed_edge_id in self . vert_edges ( vert_id ) . iter ( ) {
let edge_n = self . directed_edge_n ( directed_edge_id ) ;
//is boundary uncrossable by a crawl from infinity
2023-12-02 10:09:48 +00:00
let edge_verts = self . edge_verts ( directed_edge_id . as_undirected ( ) ) ;
//select opposite vertex
let test_vert_id = edge_verts [ directed_edge_id . parity ( ) as usize ] ;
//test if it's closer
let diff = point - self . vert ( test_vert_id ) ;
2024-01-30 00:19:41 +00:00
if zeroes ::zeroes1 ( edge_n . dot ( diff ) , edge_n . dot ( infinity_dir ) ) . len ( ) = = 0 {
2023-11-30 09:51:17 +00:00
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 Planar64 , infinity_dir :Planar64Vec3 , 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 ) . iter ( ) {
let edge_n = self . directed_edge_n ( directed_edge_id ) ;
//is boundary uncrossable by a crawl from infinity
2023-12-02 10:09:48 +00:00
//check if time of collision is outside Time::MIN..Time::MAX
let d = edge_n . dot ( diff ) ;
2024-01-30 00:19:41 +00:00
if zeroes ::zeroes1 ( d , edge_n . dot ( infinity_dir ) ) . len ( ) = = 0 {
2023-11-30 09:51:17 +00:00
//test the edge
let edge_nn = edge_n . dot ( edge_n ) ;
if Planar64 ::ZERO < = d & & d < = edge_nn {
let distance_squared = {
let c = diff . cross ( edge_n ) ;
c . dot ( c ) / edge_nn
} ;
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 , infinity_dir :Planar64Vec3 , point :Planar64Vec3 ) ->EV {
let mut best_distance_squared = {
let diff = point - self . vert ( vert_id ) ;
diff . dot ( diff )
} ;
loop {
match self . next_transition_vert ( vert_id , & mut best_distance_squared , infinity_dir , point ) {
Transition ::Done = > return self . final_ev ( vert_id , & mut best_distance_squared , infinity_dir , 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 infinity_fev ( & self , infinity_dir :Planar64Vec3 , point :Planar64Vec3 ) ->FEV ::< MinkowskiFace , MinkowskiDirectedEdge , MinkowskiVert > {
//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 ( self . farthest_vert ( infinity_dir ) , infinity_dir , point ) {
//if a vert is returned, it is the closest point to the infinity point
EV ::Vert ( vert_id ) = > FEV ::< MinkowskiFace , MinkowskiDirectedEdge , MinkowskiVert > ::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 ) ;
2023-12-02 10:09:48 +00:00
// point is multiplied by two because vert_sum sums two vertices.
let delta_pos = point * 2 - {
2023-11-30 09:51:17 +00:00
let & [ v0 , v1 ] = self . edge_verts ( edge_id ) . borrow ( ) ;
self . vert ( v0 ) + self . vert ( v1 )
} ;
for ( i , & face_id ) in self . edge_faces ( edge_id ) . 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 ) ;
2023-12-02 10:09:48 +00:00
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
2024-01-30 00:19:41 +00:00
if ( boundary_d ) < = Planar64 ::ZERO & & zeroes ::zeroes1 ( boundary_d , boundary_n . dot ( infinity_dir ) * 2 ) . len ( ) = = 0 {
2023-11-30 09:51:17 +00:00
//both faces cannot pass this condition, return early if one does.
return FEV ::< MinkowskiFace , MinkowskiDirectedEdge , MinkowskiVert > ::Face ( face_id ) ;
}
}
FEV ::< MinkowskiFace , MinkowskiDirectedEdge , MinkowskiVert > ::Edge ( edge_id )
} ,
}
}
fn closest_fev_not_inside ( & self , mut infinity_body :crate ::physics ::Body ) ->Option < FEV ::< MinkowskiFace , MinkowskiDirectedEdge , MinkowskiVert > > {
infinity_body . infinity_dir ( ) . map_or ( None , | dir | {
let infinity_fev = self . infinity_fev ( - dir , infinity_body . position ) ;
//a line is simpler to solve than a parabola
infinity_body . velocity = dir ;
infinity_body . acceleration = Planar64Vec3 ::ZERO ;
//crawl in from negative infinity along a tangent line to get the closest fev
2024-01-30 00:19:41 +00:00
match crate ::face_crawler ::crawl_fev ( infinity_fev , self , & infinity_body , integer ::Time ::MIN , infinity_body . time ) {
2023-11-30 09:51:17 +00:00
crate ::face_crawler ::CrawlResult ::Miss ( fev ) = > Some ( fev ) ,
crate ::face_crawler ::CrawlResult ::Hit ( _ , _ ) = > None ,
}
} )
}
2024-01-30 00:19:41 +00:00
pub fn predict_collision_in ( & self , relative_body :& crate ::physics ::Body , time_limit :integer ::Time ) ->Option < ( MinkowskiFace , integer ::Time ) > {
2023-11-30 09:51:17 +00:00
self . closest_fev_not_inside ( relative_body . clone ( ) ) . map_or ( None , | fev | {
//continue forwards along the body parabola
match crate ::face_crawler ::crawl_fev ( fev , self , relative_body , relative_body . time , time_limit ) {
crate ::face_crawler ::CrawlResult ::Miss ( _ ) = > None ,
crate ::face_crawler ::CrawlResult ::Hit ( face , time ) = > Some ( ( face , time ) ) ,
}
} )
}
2024-01-30 00:19:41 +00:00
pub fn predict_collision_out ( & self , relative_body :& crate ::physics ::Body , time_limit :integer ::Time ) ->Option < ( MinkowskiFace , integer ::Time ) > {
2023-11-30 09:51:17 +00:00
//create an extrapolated body at time_limit
let infinity_body = crate ::physics ::Body ::new (
relative_body . extrapolated_position ( time_limit ) ,
- relative_body . extrapolated_velocity ( time_limit ) ,
relative_body . acceleration ,
- time_limit ,
) ;
self . closest_fev_not_inside ( infinity_body ) . map_or ( None , | fev | {
//continue backwards along the body parabola
match crate ::face_crawler ::crawl_fev ( fev , self , & - relative_body . clone ( ) , - time_limit , - relative_body . time ) {
crate ::face_crawler ::CrawlResult ::Miss ( _ ) = > None ,
crate ::face_crawler ::CrawlResult ::Hit ( face , time ) = > Some ( ( face , - time ) ) , //no need to test -time<time_limit because of the first step
}
} )
}
2024-01-30 00:19:41 +00:00
pub fn predict_collision_face_out ( & self , relative_body :& crate ::physics ::Body , time_limit :integer ::Time , contact_face_id :MinkowskiFace ) ->Option < ( MinkowskiEdge , integer ::Time ) > {
2023-11-30 09:51:17 +00:00
//no algorithm needed, there is only one state and two cases (Edge,None)
//determine when it passes an edge ("sliding off" case)
let mut best_time = time_limit ;
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 ) . iter ( ) {
let edge_n = self . directed_edge_n ( directed_edge_id ) ;
//f x e points in
let n = face_n . cross ( edge_n ) ;
let verts = self . edge_verts ( directed_edge_id . as_undirected ( ) ) ;
let d = n . dot ( self . vert ( verts [ 0 ] ) + self . vert ( verts [ 1 ] ) ) ;
//WARNING! d outside of *2
2024-01-30 00:19:41 +00:00
for t in zeroes ::zeroes2 ( ( n . dot ( relative_body . position ) ) * 2 - d , n . dot ( relative_body . velocity ) * 2 , n . dot ( relative_body . acceleration ) ) {
let t = relative_body . time + integer ::Time ::from ( t ) ;
2023-11-30 09:51:17 +00:00
if relative_body . time < t & & t < best_time & & n . dot ( relative_body . extrapolated_velocity ( t ) ) < Planar64 ::ZERO {
best_time = t ;
best_edge = Some ( directed_edge_id ) ;
break ;
}
}
}
best_edge . map ( | e | ( e . as_undirected ( ) , best_time ) )
}
}
impl MeshQuery < MinkowskiFace , MinkowskiDirectedEdge , MinkowskiVert > for MinkowskiMesh < '_ > {
fn face_nd ( & self , face_id :MinkowskiFace ) ->( Planar64Vec3 , Planar64 ) {
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 ) . borrow ( ) ;
let & [ e1v0 , e1v1 ] = self . mesh1 . edge_verts ( e1 ) . borrow ( ) ;
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 ) , ( e0d - e1d ) * ( parity as i64 * 2 - 1 ) )
} ,
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 face_edges ( & self , face_id :MinkowskiFace ) ->Cow < Vec < MinkowskiDirectedEdge > > {
match face_id {
MinkowskiFace ::VertFace ( v0 , f1 ) = > {
Cow ::Owned ( self . mesh1 . face_edges ( f1 ) . iter ( ) . map ( | & edge_id1 | {
MinkowskiDirectedEdge ::VertEdge ( v0 , edge_id1 . reverse ( ) )
} ) . collect ( ) )
} ,
MinkowskiFace ::EdgeEdge ( e0 , e1 , parity ) = > {
let e0v = self . mesh0 . edge_verts ( e0 ) ;
let e1v = self . mesh1 . edge_verts ( e1 ) ;
//could sort this if ordered edges are needed
//probably just need to reverse this list according to parity
Cow ::Owned ( vec! [
MinkowskiDirectedEdge ::VertEdge ( e0v [ 0 ] , e1 . as_directed ( parity ) ) ,
MinkowskiDirectedEdge ::EdgeVert ( e0 . as_directed ( ! parity ) , e1v [ 0 ] ) ,
MinkowskiDirectedEdge ::VertEdge ( e0v [ 1 ] , e1 . as_directed ( ! parity ) ) ,
MinkowskiDirectedEdge ::EdgeVert ( e0 . as_directed ( parity ) , e1v [ 1 ] ) ,
] )
} ,
MinkowskiFace ::FaceVert ( f0 , v1 ) = > {
Cow ::Owned ( self . mesh0 . face_edges ( f0 ) . iter ( ) . map ( | & edge_id0 | {
MinkowskiDirectedEdge ::EdgeVert ( edge_id0 , v1 )
} ) . collect ( ) )
} ,
}
}
fn edge_faces ( & self , edge_id :MinkowskiEdge ) ->Cow < [ 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 ) . borrow ( ) ;
Cow ::Owned ( [ ( e1f1 , false ) , ( e1f0 , true ) ] . map ( | ( edge_face_id1 , face_parity ) | {
let mut best_edge = None ;
let mut best_d = Planar64 ::ZERO ;
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 . iter ( ) {
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 < Planar64 ::ZERO {
let edge0_nn = edge0_n . dot ( edge0_n ) ;
//divide by zero???
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 ) . borrow ( ) ;
Cow ::Owned ( [ ( e0f0 , true ) , ( e0f1 , false ) ] . map ( | ( edge_face_id0 , face_parity ) | {
let mut best_edge = None ;
let mut best_d = Planar64 ::ZERO ;
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 . iter ( ) {
let edge1_n = self . mesh1 . directed_edge_n ( directed_edge_id1 ) ;
let d = edge_face0_n . dot ( edge1_n ) ;
if d < Planar64 ::ZERO {
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 ) ->Cow < [ MinkowskiVert ; 2 ] > {
match edge_id {
MinkowskiEdge ::VertEdge ( v0 , e1 ) = > {
Cow ::Owned ( self . mesh1 . edge_verts ( e1 ) . map ( | vert_id1 | {
MinkowskiVert ::VertVert ( v0 , vert_id1 )
} ) )
} ,
MinkowskiEdge ::EdgeVert ( e0 , v1 ) = > {
Cow ::Owned ( self . mesh0 . edge_verts ( e0 ) . map ( | vert_id0 | {
MinkowskiVert ::VertVert ( vert_id0 , v1 )
} ) )
} ,
}
}
fn vert_edges ( & self , vert_id :MinkowskiVert ) ->Cow < Vec < 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 = self . mesh0 . vert_faces ( v0 ) ;
let v1f = self . mesh1 . vert_faces ( v1 ) ;
let v0f_n :Vec < Planar64Vec3 > = v0f . iter ( ) . map ( | & face_id | self . mesh0 . face_nd ( face_id ) . 0 ) . collect ( ) ;
let v1f_n :Vec < Planar64Vec3 > = v1f . iter ( ) . map ( | & face_id | self . mesh1 . face_nd ( face_id ) . 0 ) . collect ( ) ;
let the_len = v0f . len ( ) + v1f . len ( ) ;
for & directed_edge_id in self . mesh0 . vert_edges ( v0 ) . iter ( ) {
let n = self . mesh0 . directed_edge_n ( directed_edge_id ) ;
let nn = n . dot ( n ) ;
//make a set of faces
let mut face_normals = Vec ::with_capacity ( the_len ) ;
//add mesh0 faces as-is
face_normals . clone_from ( & v0f_n ) ;
for face_n in & v1f_n {
//add reflected mesh1 faces
face_normals . push ( * face_n - n * ( face_n . dot ( n ) * 2 / nn ) ) ;
}
if is_empty_volume ( face_normals ) {
edges . push ( MinkowskiDirectedEdge ::EdgeVert ( directed_edge_id , v1 ) ) ;
}
}
for & directed_edge_id in self . mesh1 . vert_edges ( v1 ) . iter ( ) {
let n = self . mesh1 . directed_edge_n ( directed_edge_id ) ;
let nn = n . dot ( n ) ;
let mut face_normals = Vec ::with_capacity ( the_len ) ;
face_normals . clone_from ( & v1f_n ) ;
for face_n in & v0f_n {
face_normals . push ( * face_n - n * ( face_n . dot ( n ) * 2 / nn ) ) ;
}
if is_empty_volume ( face_normals ) {
edges . push ( MinkowskiDirectedEdge ::VertEdge ( v0 , directed_edge_id ) ) ;
}
}
Cow ::Owned ( edges )
} ,
}
}
fn vert_faces ( & self , _vert_id :MinkowskiVert ) ->Cow < Vec < MinkowskiFace > > {
unimplemented! ( )
}
}
fn is_empty_volume ( normals :Vec < Planar64Vec3 > ) ->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 ] ) ;
if let Some ( comp ) = & d_comp {
if * comp * d < Planar64 ::ZERO {
return true ;
}
} else {
d_comp = Some ( d ) ;
}
}
}
}
}
return false ;
}
#[ test ]
fn test_is_empty_volume ( ) {
assert! ( ! is_empty_volume ( [ Planar64Vec3 ::X , Planar64Vec3 ::Y , Planar64Vec3 ::Z ] . to_vec ( ) ) ) ;
assert! ( is_empty_volume ( [ Planar64Vec3 ::X , Planar64Vec3 ::Y , Planar64Vec3 ::Z , Planar64Vec3 ::NEG_X ] . to_vec ( ) ) ) ;
}
#[ test ]
fn build_me_a_cube ( ) {
2024-02-05 06:37:09 +00:00
let mesh = PhysicsMesh ::unit_cube ( ) ;
2023-11-30 09:51:17 +00:00
//println!("mesh={:?}",mesh);
}