diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index a04cbf8b..e947f354 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -2,12 +2,13 @@ use strafesnet_common::integer::vec3; use strafesnet_common::integer::vec3::Vector3; use strafesnet_common::integer::{Fixed,Planar64Vec3}; -use crate::model::{MeshQuery,MinkowskiMesh,MinkowskiVert}; +use crate::model::{DirectedEdge,FEV,MeshQuery,MinkowskiMesh,MinkowskiVert}; // This algorithm is based on Lua code // written by Trey Reynolds in 2021 type Simplex=[MinkowskiVert;N]; +#[derive(Clone,Copy)] enum Simplex1_3{ Simplex1(Simplex<1>), Simplex2(Simplex<2>), @@ -546,7 +547,7 @@ pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{ // TODO: remove mesh negation minimum_difference::(&-mesh,point, // on_exact - |last_pos,direction|{ + |_simplex,last_pos,direction|{ // local norm = direction.unit // local dist = a:Dot(norm) // local hits = -dist < radiusP + radiusQ @@ -562,17 +563,61 @@ pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{ ||false ) } -pub fn closest_fev(mesh:&MinkowskiMesh,point:Planar64Vec3)->Topology{ +#[derive(Debug)] +pub struct OhNoes; +pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option>,OhNoes>>{ const ENABLE_FAST_FAIL:bool=false; // TODO: remove mesh negation minimum_difference::(&-mesh,point, // on_exact - |_last_pos,_direction|unimplemented!(), + |simplex,_last_pos,_direction|{ + // Convert simplex to FEV + // Vertices must be inverted since the mesh is inverted + Some(match simplex{ + Simplex1_3::Simplex1([v0])=>Ok(FEV::Vert(-v0)), + Simplex1_3::Simplex2([v0,v1])=>{ + // invert + let (v0,v1)=(-v0,-v1); + // dumbest stupidest brute force search + let v0e=mesh.vert_edges(v0); + for &v0e in v0e.as_ref(){ + // check opposite vertex to see if it is v1 + if mesh.edge_verts(v0e.as_undirected()).as_ref()[v0e.parity() as usize]==v1{ + return Some(Ok(FEV::Edge(v0e.as_undirected()))); + } + } + Err(OhNoes) + }, + Simplex1_3::Simplex3([v0,v1,v2])=>{ + // invert + let (v0,v1,v2)=(-v0,-v1,-v2); + // dumbest stupidest brute force search + let v0e=mesh.vert_edges(v0); + for &v0e in v0e.as_ref(){ + // check opposite vertex to see if it is v1 + if mesh.edge_verts(v0e.as_undirected()).as_ref()[v0e.parity() as usize]==v1{ + // check if a vertex of the face is v2 + let ef=mesh.edge_faces(v0e.as_undirected()); + for &ef in ef.as_ref(){ + let fe=mesh.face_edges(ef); + for e in fe.as_ref(){ + if mesh.edge_verts(e.as_undirected()).as_ref()[e.parity() as usize]==v2{ + return Some(Ok(FEV::Face(ef))); + } + } + } + } + } + Err(OhNoes) + }, + }) + }, // on_escape - |simplex|{ + |_simplex|{ + // intersection is guaranteed at this point // local norm, dist, u0, u1, v0, v1, w0, w1 = expand(queryP, queryQ, a0, a1, b0, b1, c0, c1, d0, d1, 1e-5) - let simplex=refine_to_exact(mesh,simplex); - Topology{simplex} + // let simplex=refine_to_exact(mesh,simplex); + None }, // fast_fail value is irrelevant and will never be returned! ||unreachable!() @@ -587,7 +632,7 @@ pub fn closest_fev(mesh:&MinkowskiMesh,point:Planar64Vec3)->Topology{ fn minimum_difference( mesh:&MinkowskiMesh, point:Planar64Vec3, - on_exact:impl FnOnce(Planar64Vec3,Planar64Vec3)->T, + on_exact:impl FnOnce(Simplex1_3,Planar64Vec3,Planar64Vec3)->T, on_escape:impl FnOnce(Simplex<4>)->T, on_fast_fail:impl FnOnce()->T, )->T{ @@ -627,7 +672,7 @@ fn minimum_difference( if !direction.dot(next_pos-last_pos).is_positive() ||simplex_big.det_is_zero(mesh){ // Found enough information to compute the exact closest point. - return on_exact(last_pos,direction); + return on_exact(simplex_small,last_pos,direction); } // direction, a0, a1, b0, b1, c0, c1, d0, d1 = reduceSimplex(new_point_p, new_point_q, a0, a1, b0, b1, c0, c1) diff --git a/engine/physics/src/model.rs b/engine/physics/src/model.rs index a184dd45..2f443c44 100644 --- a/engine/physics/src/model.rs +++ b/engine/physics/src/model.rs @@ -571,7 +571,16 @@ impl MeshQuery for TransformedMesh<'_>{ pub enum MinkowskiVert{ VertVert(SubmeshVertId,SubmeshVertId), } -#[derive(Clone,Copy,Debug)] +// 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),