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

partially implement md generic

Browse Source
This commit is contained in:
Rhys Lloyd
2025-12-10 14:52:14 -08:00
parent 3a7aaa20f8
commit ac4c84f562
1 changed files with 67 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,20 +2,22 @@ use strafesnet_common::integer::vec3;
use strafesnet_common::integer::vec3::Vector3;
use strafesnet_common::integer::{Fixed,Planar64,Planar64Vec3};
use crate::model::{DirectedEdge,FEV,MeshQuery,MinkowskiMesh,MinkowskiVert};
use crate::model::{DirectedEdge,FEV,MeshQuery};
// TODO: remove mesh invert
use crate::model::{MinkowskiMesh,MinkowskiVert};
// This algorithm is based on Lua code
// written by Trey Reynolds in 2021
type Simplex<const N:usize>=[MinkowskiVert;N];
type Simplex<const N:usize,Vert>=[Vert;N];
#[derive(Clone,Copy)]
enum Simplex1_3{
Simplex1(Simplex<1>),
Simplex2(Simplex<2>),
Simplex3(Simplex<3>),
enum Simplex1_3<Vert>{
Simplex1(Simplex<1,Vert>),
Simplex2(Simplex<2,Vert>),
Simplex3(Simplex<3,Vert>),
}
impl Simplex1_3{
fn push_front(self,v:MinkowskiVert)->Simplex2_4{
impl<Vert> Simplex1_3<Vert>{
fn push_front(self,v:Vert)->Simplex2_4<Vert>{
match self{
Simplex1_3::Simplex1([v0])=>Simplex2_4::Simplex2([v,v0]),
Simplex1_3::Simplex2([v0,v1])=>Simplex2_4::Simplex3([v,v0,v1]),
@@ -23,10 +25,11 @@ impl Simplex1_3{
}
}
}
enum Simplex2_4{
Simplex2(Simplex<2>),
Simplex3(Simplex<3>),
Simplex4(Simplex<4>),
#[derive(Clone,Copy)]
enum Simplex2_4<Vert>{
Simplex2(Simplex<2,Vert>),
Simplex3(Simplex<3,Vert>),
Simplex4(Simplex<4,Vert>),
}
/*
@@ -42,23 +45,23 @@ local function absDet(r, u, v, w)
end
end
*/
impl Simplex2_4{
fn det_is_zero(&self,mesh:&MinkowskiMesh)->bool{
impl<Vert> Simplex2_4<Vert>{
fn det_is_zero<M:MeshQuery<Vert=Vert>>(self,mesh:&M)->bool{
match self{
&Self::Simplex4([p0,p1,p2,p3])=>{
Self::Simplex4([p0,p1,p2,p3])=>{
let p0=mesh.vert(p0);
let p1=mesh.vert(p1);
let p2=mesh.vert(p2);
let p3=mesh.vert(p3);
(p1-p0).cross(p2-p0).dot(p3-p0)==Fixed::ZERO
},
&Self::Simplex3([p0,p1,p2])=>{
Self::Simplex3([p0,p1,p2])=>{
let p0=mesh.vert(p0);
let p1=mesh.vert(p1);
let p2=mesh.vert(p2);
(p1-p0).cross(p2-p0)==vec3::zero()
},
&Self::Simplex2([p0,p1])=>{
Self::Simplex2([p0,p1])=>{
let p0=mesh.vert(p0);
let p1=mesh.vert(p1);
p1-p0==vec3::zero()
@@ -97,11 +100,11 @@ const fn choose_any_direction()->Planar64Vec3{
vec3::X
}
fn reduce1(
[v0]:Simplex<1>,
mesh:&MinkowskiMesh,
fn reduce1<M:MeshQuery>(
[v0]:Simplex<1,M::Vert>,
mesh:&M,
point:Planar64Vec3,
)->Reduced{
)->Reduced<M::Vert>{
// --debug.profilebegin("reduceSimplex0")
// local a = a1 - a0
let p0=mesh.vert(v0);
@@ -126,11 +129,11 @@ fn reduce1(
}
// local function reduceSimplex1(a0, a1, b0, b1)
fn reduce2(
[v0,v1]:Simplex<2>,
mesh:&MinkowskiMesh,
fn reduce2<M:MeshQuery>(
[v0,v1]:Simplex<2,M::Vert>,
mesh:&M,
point:Planar64Vec3,
)->Reduced{
)->Reduced<M::Vert>{
// --debug.profilebegin("reduceSimplex1")
// local a = a1 - a0
// local b = b1 - b0
@@ -183,11 +186,11 @@ fn reduce2(
}
// local function reduceSimplex2(a0, a1, b0, b1, c0, c1)
fn reduce3(
[v0,mut v1,v2]:Simplex<3>,
mesh:&MinkowskiMesh,
fn reduce3<M:MeshQuery>(
[v0,mut v1,v2]:Simplex<3,M::Vert>,
mesh:&M,
point:Planar64Vec3,
)->Reduced{
)->Reduced<M::Vert>{
// --debug.profilebegin("reduceSimplex2")
// local a = a1 - a0
// local b = b1 - b0
@@ -292,11 +295,11 @@ fn reduce3(
}
// local function reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1)
fn reduce4(
[v0,mut v1,mut v2,v3]:Simplex<4>,
mesh:&MinkowskiMesh,
fn reduce4<M:MeshQuery>(
[v0,mut v1,mut v2,v3]:Simplex<4,M::Vert>,
mesh:&M,
point:Planar64Vec3,
)->Reduce{
)->Reduce<M::Vert>{
// --debug.profilebegin("reduceSimplex3")
// local a = a1 - a0
// local b = b1 - b0
@@ -481,18 +484,18 @@ fn reduce4(
})
}
struct Reduced{
struct Reduced<Vert>{
dir:Planar64Vec3,
simplex:Simplex1_3,
simplex:Simplex1_3<Vert>,
}
enum Reduce{
Escape(Simplex<4>),
Reduced(Reduced),
enum Reduce<Vert>{
Escape(Simplex<4,Vert>),
Reduced(Reduced<Vert>),
}
impl Simplex2_4{
fn reduce(self,mesh:&MinkowskiMesh,point:Planar64Vec3)->Reduce{
impl<Vert> Simplex2_4<Vert>{
fn reduce<M:MeshQuery<Vert=Vert>>(self,mesh:&M,point:Planar64Vec3)->Reduce<Vert>{
match self{
Self::Simplex2(simplex)=>Reduce::Reduced(reduce2(simplex,mesh,point)),
Self::Simplex3(simplex)=>Reduce::Reduced(reduce3(simplex,mesh,point)),
@@ -501,10 +504,10 @@ impl Simplex2_4{
}
}
pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{
pub fn contains_point(mesh:&MinkowskiMesh<'_>,point:Planar64Vec3)->bool{
const ENABLE_FAST_FAIL:bool=true;
// TODO: remove mesh negation
minimum_difference::<ENABLE_FAST_FAIL,_>(&-mesh,point,
minimum_difference::<ENABLE_FAST_FAIL,_,_>(&-mesh,point,
// on_exact
|is_intersecting,_simplex|{
is_intersecting
@@ -521,16 +524,16 @@ pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{
//infinity fev algorithm state transition
#[derive(Debug)]
enum Transition{
enum Transition<Vert>{
Done,//found closest vert, no edges are better
Vert(MinkowskiVert),//transition to vert
Vert(Vert),//transition to vert
}
enum EV{
Vert(MinkowskiVert),
Edge(crate::model::MinkowskiEdge),
enum EV<M:MeshQuery>{
Vert(M::Vert),
Edge(<M::Edge as DirectedEdge>::UndirectedEdge),
}
impl From<EV> for FEV<MinkowskiMesh<'_>>{
fn from(value:EV)->Self{
impl<M:MeshQuery> From<EV<M>> for FEV<M>{
fn from(value:EV<M>)->Self{
match value{
EV::Vert(minkowski_vert)=>FEV::Vert(minkowski_vert),
EV::Edge(minkowski_edge)=>FEV::Edge(minkowski_edge),
@@ -549,7 +552,7 @@ struct ThickPlane{
epsilon:Fixed<3,96>,
}
impl ThickPlane{
fn new(mesh:&MinkowskiMesh,[v0,v1,v2]:Simplex<3>)->Self{
fn new<M:MeshQuery>(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);
@@ -573,7 +576,7 @@ struct ThickLine{
epsilon:Fixed<4,128>,
}
impl ThickLine{
fn new(mesh:&MinkowskiMesh,[v0,v1]:Simplex<2>)->Self{
fn new<M:MeshQuery>(mesh:&M,[v0,v1]:Simplex<2,M::Vert>)->Self{
let p0=mesh.vert(v0);
let p1=mesh.vert(v1);
let point=p0;
@@ -590,14 +593,14 @@ impl Contains for ThickLine{
}
}
struct EVFinder<'a,C>{
mesh:&'a MinkowskiMesh<'a>,
struct EVFinder<'a,M,C>{
mesh:&'a M,
constraint:C,
best_distance_squared:Fixed<2,64>,
}
impl<C:Contains> EVFinder<'_,C>{
fn next_transition_vert(&mut self,vert_id:MinkowskiVert,point:Planar64Vec3)->Transition{
impl<M:MeshQuery,C:Contains> EVFinder<'_,M,C>{
fn next_transition_vert(&mut self,vert_id:M::Vert,point:Planar64Vec3)->Transition<M::Vert>{
let mut best_transition=Transition::Done;
for &directed_edge_id in self.mesh.vert_edges(vert_id).as_ref(){
//test if this edge's opposite vertex closer
@@ -615,7 +618,7 @@ impl<C:Contains> EVFinder<'_,C>{
}
best_transition
}
fn final_ev(&mut self,vert_id:MinkowskiVert,point:Planar64Vec3)->EV{
fn final_ev(&mut self,vert_id:M::Vert,point:Planar64Vec3)->EV<M>{
let mut best_transition=EV::Vert(vert_id);
let vert_pos=self.mesh.vert(vert_id);
let diff=point-vert_pos;
@@ -643,7 +646,7 @@ impl<C:Contains> EVFinder<'_,C>{
}
best_transition
}
fn crawl_boundaries(&mut self,mut vert_id:MinkowskiVert,point:Planar64Vec3)->EV{
fn crawl_boundaries(&mut self,mut vert_id:M::Vert,point:Planar64Vec3)->EV<M>{
loop{
match self.next_transition_vert(vert_id,point){
Transition::Done=>return self.final_ev(vert_id,point),
@@ -653,7 +656,7 @@ impl<C:Contains> EVFinder<'_,C>{
}
}
/// This function drops a vertex down to an edge or a face if the path from infinity did not cross any vertex-edge boundaries but the point is supposed to have already crossed a boundary down from a vertex
fn crawl_to_closest_ev(mesh:&MinkowskiMesh,simplex:Simplex<2>,point:Planar64Vec3)->EV{
fn crawl_to_closest_ev<M:MeshQuery>(mesh:&M,simplex:Simplex<2,M::Vert>,point:Planar64Vec3)->EV<M>{
// 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.
@@ -671,7 +674,7 @@ fn crawl_to_closest_ev(mesh:&MinkowskiMesh,simplex:Simplex<2>,point:Planar64Vec3
}
/// This function drops a vertex down to an edge or a face if the path from infinity did not cross any vertex-edge boundaries but the point is supposed to have already crossed a boundary down from a vertex
fn crawl_to_closest_fev<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3>,point:Planar64Vec3)->FEV::<MinkowskiMesh<'a>>{
fn crawl_to_closest_fev<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3,MinkowskiVert>,point:Planar64Vec3)->FEV::<MinkowskiMesh<'a>>{
// naively start at the closest vertex
// the closest vertex is not necessarily the one with the fewest boundary hops
// but it doesn't matter, we will get there regardless.
@@ -721,7 +724,7 @@ fn crawl_to_closest_fev<'a>(mesh:&MinkowskiMesh<'a>,simplex:Simplex<3>,point:Pla
pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option<FEV<MinkowskiMesh<'a>>>{
const ENABLE_FAST_FAIL:bool=false;
// TODO: remove mesh negation
minimum_difference::<ENABLE_FAST_FAIL,_>(&-mesh,point,
minimum_difference::<ENABLE_FAST_FAIL,_,_>(&-mesh,point,
// on_exact
|is_intersecting,simplex|{
if is_intersecting{
@@ -771,11 +774,11 @@ pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->O
// queryQ, radiusQ,
// exitRadius, testIntersection
// )
fn minimum_difference<const ENABLE_FAST_FAIL:bool,T>(
mesh:&MinkowskiMesh,
fn minimum_difference<const ENABLE_FAST_FAIL:bool,T,M:MeshQuery>(
mesh:&M,
point:Planar64Vec3,
on_exact:impl FnOnce(bool,Simplex1_3)->T,
on_escape:impl FnOnce(Simplex<4>)->T,
on_exact:impl FnOnce(bool,Simplex1_3<M::Vert>)->T,
on_escape:impl FnOnce(Simplex<4,M::Vert>)->T,
on_fast_fail:impl FnOnce()->T,
)->T{
// local initialAxis = queryQ() - queryP()