diff --git a/engine/physics/src/face_crawler.rs b/engine/physics/src/face_crawler.rs index a012fd17..44cfb914 100644 --- a/engine/physics/src/face_crawler.rs +++ b/engine/physics/src/face_crawler.rs @@ -21,12 +21,6 @@ impl CrawlResult{ CrawlResult::Hit(face,time)=>Some((face,time)), } } - pub fn miss(self)->Option>{ - match self{ - CrawlResult::Miss(fev)=>Some(fev), - CrawlResult::Hit(_,_)=>None, - } - } } // TODO: move predict_collision_face_out algorithm in here or something diff --git a/engine/physics/src/lib.rs b/engine/physics/src/lib.rs index 9b7db872..56e4a458 100644 --- a/engine/physics/src/lib.rs +++ b/engine/physics/src/lib.rs @@ -1,7 +1,8 @@ mod body; -mod push_solve; mod face_crawler; mod model; +mod push_solve; +mod minimum_difference; pub mod physics; diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs new file mode 100644 index 00000000..a01a37f3 --- /dev/null +++ b/engine/physics/src/minimum_difference.rs @@ -0,0 +1,901 @@ +use strafesnet_common::integer::vec3; +use strafesnet_common::integer::vec3::Vector3; +use strafesnet_common::integer::{Fixed,Planar64,Planar64Vec3}; + +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=[Vert;N]; +#[derive(Clone,Copy)] +enum Simplex1_3{ + Simplex1(Simplex<1,Vert>), + Simplex2(Simplex<2,Vert>), + Simplex3(Simplex<3,Vert>), +} +impl Simplex1_3{ + fn push_front(self,v:Vert)->Simplex2_4{ + match self{ + Simplex1_3::Simplex1([v0])=>Simplex2_4::Simplex2([v,v0]), + Simplex1_3::Simplex2([v0,v1])=>Simplex2_4::Simplex3([v,v0,v1]), + Simplex1_3::Simplex3([v0,v1,v2])=>Simplex2_4::Simplex4([v,v0,v1,v2]), + } + } +} +#[derive(Clone,Copy)] +enum Simplex2_4{ + Simplex2(Simplex<2,Vert>), + Simplex3(Simplex<3,Vert>), + Simplex4(Simplex<4,Vert>), +} + +/* +local function absDet(r, u, v, w) + if w then + return math.abs((u - r):Cross(v - r):Dot(w - r)) + elseif v then + return (u - r):Cross(v - r).magnitude + elseif u then + return (u - r).magnitude + else + return 1 + end +end +*/ +impl Simplex2_4{ + fn det_is_zero>(self,mesh:&M)->bool{ + match self{ + 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])=>{ + 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])=>{ + let p0=mesh.vert(p0); + let p1=mesh.vert(p1); + p1-p0==vec3::zero() + } + } + } +} + +/* +local function choosePerpendicularDirection(d) + local x, y, z = d.x, d.y, d.z + local best = math.min(x*x, y*y, z*z) + if x*x == best then + return Vector3.new(y*y + z*z, -x*y, -x*z) + elseif y*y == best then + return Vector3.new(-x*y, x*x + z*z, -y*z) + else + return Vector3.new(-x*z, -y*z, x*x + y*y) + end +end +*/ +fn choose_perpendicular_direction(d:Planar64Vec3)->Planar64Vec3{ + let x=d.x.abs(); + let y=d.y.abs(); + let z=d.z.abs(); + if xPlanar64Vec3{ + vec3::X +} + +fn narrow_dir2(dir:Vector3>)->Planar64Vec3{ + if dir==vec3::zero(){ + return dir.narrow_1().unwrap(); + } + let x=dir.x.as_bits().unsigned_abs().bits(); + let y=dir.y.as_bits().unsigned_abs().bits(); + let z=dir.z.as_bits().unsigned_abs().bits(); + let big=x.max(y).max(z); + const MAX_BITS:u32=64+31; + if MAX_BITS>(big-MAX_BITS) + }else{ + dir + }.narrow_1().unwrap() +} +fn narrow_dir3(dir:Vector3>)->Planar64Vec3{ + if dir==vec3::zero(){ + return dir.narrow_1().unwrap(); + } + let x=dir.x.as_bits().unsigned_abs().bits(); + let y=dir.y.as_bits().unsigned_abs().bits(); + let z=dir.z.as_bits().unsigned_abs().bits(); + let big=x.max(y).max(z); + const MAX_BITS:u32=96+31; + if MAX_BITS>(big-MAX_BITS) + }else{ + dir + }.narrow_1().unwrap() +} + +fn reduce1( + [v0]:Simplex<1,M::Vert>, + mesh:&M, + point:Planar64Vec3, +)->Reduced{ + // --debug.profilebegin("reduceSimplex0") + // local a = a1 - a0 + let p0=mesh.vert(v0); + + // local p = -a + let p=-(p0+point); + + // local direction = p + let mut dir=p; + + // if direction.magnitude == 0 then + // direction = chooseAnyDirection() + if dir==vec3::zero(){ + dir=choose_any_direction(); + } + + // return direction, a0, a1 + Reduced{ + dir, + simplex:Simplex1_3::Simplex1([v0]), + } +} + +// local function reduceSimplex1(a0, a1, b0, b1) +fn reduce2( + [v0,v1]:Simplex<2,M::Vert>, + mesh:&M, + point:Planar64Vec3, +)->Reduced{ + // --debug.profilebegin("reduceSimplex1") + // local a = a1 - a0 + // local b = b1 - b0 + let p0=mesh.vert(v0); + let p1=mesh.vert(v1); + + // local p = -a + // local u = b - a + let p=-(p0+point); + let u=p1-p0; + + // -- modify to take into account the radiuses + // local p_u = p:Dot(u) + let p_u=p.dot(u); + + // if p_u >= 0 then + if !p_u.is_negative(){ + // local direction = u:Cross(p):Cross(u) + let direction=u.cross(p).cross(u); + + // if direction.magnitude == 0 then + if direction==vec3::zero(){ + return Reduced{ + dir:choose_perpendicular_direction(u), + simplex:Simplex1_3::Simplex2([v0,v1]), + }; + } + + // -- modify the direction to take into account a0R and b0R + // return direction, a0, a1, b0, b1 + return Reduced{ + dir:narrow_dir3(direction), + simplex:Simplex1_3::Simplex2([v0,v1]), + }; + } + + // local direction = p + let mut dir=p; + + // if direction.magnitude == 0 then + if dir==vec3::zero(){ + dir=choose_perpendicular_direction(u); + } + + // return direction, a0, a1 + Reduced{ + dir, + simplex:Simplex1_3::Simplex1([v0]), + } +} + +// local function reduceSimplex2(a0, a1, b0, b1, c0, c1) +fn reduce3( + [v0,mut v1,v2]:Simplex<3,M::Vert>, + mesh:&M, + point:Planar64Vec3, +)->Reduced{ + // --debug.profilebegin("reduceSimplex2") + // local a = a1 - a0 + // local b = b1 - b0 + // local c = c1 - c0 + let p0=mesh.vert(v0); + let p1=mesh.vert(v1); + let p2=mesh.vert(v2); + + // local p = -a + // local u = b - a + // local v = c - a + let p=-(p0+point); + let mut u=p1-p0; + let v=p2-p0; + + // local uv = u:Cross(v) + // local up = u:Cross(p) + // local pv = p:Cross(v) + // local uv_up = uv:Dot(up) + // local uv_pv = uv:Dot(pv) + let mut uv=u.cross(v); + let mut up=u.cross(p); + let pv=p.cross(v); + let uv_up=uv.dot(up); + let uv_pv=uv.dot(pv); + + // if uv_up >= 0 and uv_pv >= 0 then + if !uv_up.is_negative()&&!uv_pv.is_negative(){ + // local uvp = uv:Dot(p) + let uvp=uv.dot(p); + + // local direction = uvp < 0 and -uv or uv + let direction=if uvp.is_negative(){ + -uv + }else{ + uv + }; + + // return direction, a0, a1, b0, b1, c0, c1 + return Reduced{ + dir:narrow_dir2(direction), + simplex:Simplex1_3::Simplex3([v0,v1,v2]), + }; + } + + // local u_u = u:Dot(u) + // local v_v = v:Dot(v) + // local uDist = uv_up/(u_u*v.magnitude) + // local vDist = uv_pv/(v_v*u.magnitude) + // local minDist2 = math.min(uDist, vDist) + let u_dist=uv_up*v.length(); + let v_dist=uv_pv*u.length(); + + // if vDist == minDist2 then + if v_dist= 0 then + if !p_u.is_negative(){ + // local direction = up:Cross(u) + let direction=up.cross(u); + // if direction.magnitude == 0 then + if direction==vec3::zero(){ + // direction = uv + return Reduced{ + dir:narrow_dir2(uv), + simplex:Simplex1_3::Simplex2([v0,v1]), + }; + } + + // return direction, a0, a1, b0, b1 + return Reduced{ + dir:narrow_dir3(direction), + simplex:Simplex1_3::Simplex2([v0,v1]), + }; + } + + // local direction = p + let dir=p; + // if direction.magnitude == 0 then + if dir==vec3::zero(){ + // direction = uv + return Reduced{ + dir:narrow_dir2(uv), + simplex:Simplex1_3::Simplex1([v0]), + }; + } + // return direction, a0, a0 + Reduced{ + dir, + simplex:Simplex1_3::Simplex1([v0]), + } +} + +// local function reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1) +fn reduce4( + [v0,mut v1,mut v2,v3]:Simplex<4,M::Vert>, + mesh:&M, + point:Planar64Vec3, +)->Reduce{ + // --debug.profilebegin("reduceSimplex3") + // local a = a1 - a0 + // local b = b1 - b0 + // local c = c1 - c0 + // local d = d1 - d0 + let p0=mesh.vert(v0); + let p1=mesh.vert(v1); + let p2=mesh.vert(v2); + let p3=mesh.vert(v3); + + // local p = -a + // local u = b - a + // local v = c - a + // local w = d - a + let p=-(p0+point); + let mut u=p1-p0; + let mut v=p2-p0; + let w=p3-p0; + + // local uv = u:Cross(v) + // local vw = v:Cross(w) + // local wu = w:Cross(u) + // local uvw = uv:Dot(w) + // local pvw = vw:Dot(p) + // local upw = wu:Dot(p) + // local uvp = uv:Dot(p) + let mut uv=u.cross(v); + let vw=v.cross(w); + let wu=w.cross(u); + let uv_w=uv.dot(w); + let pv_w=vw.dot(p); + let up_w=wu.dot(p); + let uv_p=uv.dot(p); + + // if pvw/uvw >= 0 and upw/uvw >= 0 and uvp/uvw >= 0 then + if !pv_w.div_sign(uv_w).is_negative() + &&!up_w.div_sign(uv_w).is_negative() + &&!uv_p.div_sign(uv_w).is_negative(){ + // origin is contained, this is a positive detection + // local direction = Vector3.new(0, 0, 0) + // return direction, a0, a1, b0, b1, c0, c1, d0, d1 + return Reduce::Escape([v0,v1,v2,v3]); + } + + // local uvwSign = uvw < 0 and -1 or uvw > 0 and 1 or 0 + // local uvDist = uvp*uvwSign/uv.magnitude + // local vwDist = pvw*uvwSign/vw.magnitude + // local wuDist = upw*uvwSign/wu.magnitude + // local minDist3 = math.min(uvDist, vwDist, wuDist) + let uv_dist=uv_p.mul_sign(uv_w); + let vw_dist=pv_w.mul_sign(uv_w); + let wu_dist=up_w.mul_sign(uv_w); + let wu_len=wu.length(); + let uv_len=uv.length(); + let vw_len=vw.length(); + + if vw_dist*wu_len= 0 and uv_pv >= 0 then + if !uv_up.is_negative()&&!uv_pv.is_negative(){ + // local direction = uvw < 0 and uv or -uv + // return direction, a0, a1, b0, b1, c0, c1 + if uv_w.is_negative(){ + return Reduce::Reduced(Reduced{ + dir:narrow_dir2(uv), + simplex:Simplex1_3::Simplex3([v0,v1,v2]), + }); + }else{ + return Reduce::Reduced(Reduced{ + dir:narrow_dir2(-uv), + simplex:Simplex1_3::Simplex3([v0,v1,v2]), + }); + } + } + + // local u_u = u:Dot(u) + // local v_v = v:Dot(v) + // local uDist = uv_up/(u_u*v.magnitude) + // local vDist = uv_pv/(v_v*u.magnitude) + // local minDist2 = math.min(uDist, vDist) + let u_dist=uv_up*v.length(); + let v_dist=uv_pv*u.length(); + + // if vDist == minDist2 then + if v_dist= 0 then + if !p_u.is_negative(){ + // local direction = up:Cross(u) + let direction=up.cross(u); + // if direction.magnitude == 0 then + if direction==vec3::zero(){ + // direction = uvw < 0 and uv or -uv + // return direction, a0, a1, b0, b1 + if uv_w.is_negative(){ + return Reduce::Reduced(Reduced{ + dir:narrow_dir2(uv), + simplex:Simplex1_3::Simplex2([v0,v1]), + }); + }else{ + return Reduce::Reduced(Reduced{ + dir:narrow_dir2(-uv), + simplex:Simplex1_3::Simplex2([v0,v1]), + }); + } + } + + // return direction, a0, a1, b0, b1 + return Reduce::Reduced(Reduced{ + dir:narrow_dir3(direction), + simplex:Simplex1_3::Simplex2([v0,v1]), + }); + } + + // local direction = p + let dir=p; + // if direction.magnitude == 0 then + if dir==vec3::zero(){ + // direction = uvw < 0 and uv or -uv + if uv_w.is_negative(){ + return Reduce::Reduced(Reduced{ + dir:narrow_dir2(uv), + simplex:Simplex1_3::Simplex1([v0]), + }); + }else{ + return Reduce::Reduced(Reduced{ + dir:narrow_dir2(-uv), + simplex:Simplex1_3::Simplex1([v0]), + }); + } + } + + // return direction, a0, a1 + Reduce::Reduced(Reduced{ + dir, + simplex:Simplex1_3::Simplex1([v0]), + }) +} + +struct Reduced{ + dir:Planar64Vec3, + simplex:Simplex1_3, +} + +enum Reduce{ + Escape(Simplex<4,Vert>), + Reduced(Reduced), +} + +impl Simplex2_4{ + fn reduce>(self,mesh:&M,point:Planar64Vec3)->Reduce{ + match self{ + Self::Simplex2(simplex)=>Reduce::Reduced(reduce2(simplex,mesh,point)), + Self::Simplex3(simplex)=>Reduce::Reduced(reduce3(simplex,mesh,point)), + Self::Simplex4(simplex)=>reduce4(simplex,mesh,point), + } + } +} + +//infinity fev algorithm state transition +#[derive(Debug)] +enum Transition{ + Done,//found closest vert, no edges are better + Vert(Vert),//transition to vert +} +enum EV{ + Vert(M::Vert), + Edge(::UndirectedEdge), +} +impl From> for FEV{ + fn from(value:EV)->Self{ + match value{ + EV::Vert(minkowski_vert)=>FEV::Vert(minkowski_vert), + EV::Edge(minkowski_edge)=>FEV::Edge(minkowski_edge), + } + } +} + +trait Contains{ + fn contains(&self,point:Planar64Vec3)->bool; +} + +// convenience type to check if a point is within some threshold of a plane. +struct ThickPlane{ + point:Planar64Vec3, + normal:Vector3>, + epsilon:Fixed<3,96>, +} +impl ThickPlane{ + fn new(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); + let point=p0; + let normal=(p1-p0).cross(p2-p0); + // Allow ~ 2*sqrt(3) units of thickness on the plane + // This is to account for the variance of two voxels across the longest diagonal + let epsilon=(normal.length()*(Planar64::EPSILON*3)).wrap_3(); + Self{point,normal,epsilon} + } +} +impl Contains for ThickPlane{ + fn contains(&self,point:Planar64Vec3)->bool{ + (point-self.point).dot(self.normal).abs()<=self.epsilon + } +} + +struct ThickLine{ + point:Planar64Vec3, + dir:Planar64Vec3, + epsilon:Fixed<4,128>, +} +impl ThickLine{ + fn new(mesh:&M,[v0,v1]:Simplex<2,M::Vert>)->Self{ + let p0=mesh.vert(v0); + let p1=mesh.vert(v1); + let point=p0; + let dir=p1-p0; + // Allow ~ 2*sqrt(3) units of thickness on the plane + // This is to account for the variance of two voxels across the longest diagonal + let epsilon=(dir.length_squared()*(Planar64::EPSILON*3)).widen_4(); + Self{point,dir,epsilon} + } +} +impl Contains for ThickLine{ + fn contains(&self,point:Planar64Vec3)->bool{ + (point-self.point).cross(self.dir).length_squared()<=self.epsilon + } +} + +struct EVFinder<'a,M,C>{ + mesh:&'a M, + constraint:C, + best_distance_squared:Fixed<2,64>, +} + +impl EVFinder<'_,M,C>{ + fn next_transition_vert(&mut self,vert_id:M::Vert,point:Planar64Vec3)->Transition{ + 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 + let edge_verts=self.mesh.edge_verts(directed_edge_id.as_undirected()); + //select opposite vertex + let test_vert_id=edge_verts.as_ref()[directed_edge_id.parity() as usize]; + let test_pos=self.mesh.vert(test_vert_id); + let diff=point-test_pos; + let distance_squared=diff.dot(diff); + // ensure test_vert_id is coplanar to simplex + if distance_squaredEV{ + let mut best_transition=EV::Vert(vert_id); + let vert_pos=self.mesh.vert(vert_id); + let diff=point-vert_pos; + for &directed_edge_id in self.mesh.vert_edges(vert_id).as_ref(){ + //test if this edge is closer + let edge_verts=self.mesh.edge_verts(directed_edge_id.as_undirected()); + let test_vert_id=edge_verts.as_ref()[directed_edge_id.parity() as usize]; + let test_pos=self.mesh.vert(test_vert_id); + let edge_n=test_pos-vert_pos; + let d=edge_n.dot(diff); + //test the edge + let edge_nn=edge_n.dot(edge_n); + // ensure edge contains closest point and directed_edge_id is coplanar to simplex + if !d.is_negative()&&d<=edge_nn&&self.constraint.contains(test_pos){ + let distance_squared={ + let c=diff.cross(edge_n); + //wrap for speed + (c.dot(c)/edge_nn).divide().wrap_2() + }; + if distance_squared<=self.best_distance_squared{ + best_transition=EV::Edge(directed_edge_id.as_undirected()); + self.best_distance_squared=distance_squared; + } + } + } + best_transition + } + fn crawl_boundaries(&mut self,mut vert_id:M::Vert,point:Planar64Vec3)->EV{ + loop{ + match self.next_transition_vert(vert_id,point){ + Transition::Done=>return self.final_ev(vert_id,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 crawl_to_closest_ev(mesh:&M,simplex:Simplex<2,M::Vert>,point:Planar64Vec3)->EV{ + // 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. + let (vert_id,best_distance_squared)=simplex.into_iter().map(|vert_id|{ + let diff=point-mesh.vert(vert_id); + (vert_id,diff.dot(diff)) + }).min_by_key(|&(_,d)|d).unwrap(); + + let constraint=ThickLine::new(mesh,simplex); + let mut finder=EVFinder{constraint,mesh,best_distance_squared}; + //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 + finder.crawl_boundaries(vert_id,point) +} + +/// 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,MinkowskiVert>,point:Planar64Vec3)->FEV::>{ + // 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. + let (vert_id,best_distance_squared)=simplex.into_iter().map(|vert_id|{ + let diff=point-mesh.vert(vert_id); + (vert_id,diff.dot(diff)) + }).min_by_key(|&(_,d)|d).unwrap(); + + let constraint=ThickPlane::new(mesh,simplex); + let mut finder=EVFinder{constraint,mesh,best_distance_squared}; + //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 finder.crawl_boundaries(vert_id,point){ + //if a vert is returned, it is the closest point to the infinity point + EV::Vert(vert_id)=>FEV::Vert(vert_id), + EV::Edge(edge_id)=>{ + //cross to face if we are on the wrong side + let edge_n=mesh.edge_n(edge_id); + // point is multiplied by two because vert_sum sums two vertices. + let delta_pos=point*2-{ + let &[v0,v1]=mesh.edge_verts(edge_id).as_ref(); + mesh.vert(v0)+mesh.vert(v1) + }; + for (i,&face_id) in mesh.edge_faces(edge_id).as_ref().iter().enumerate(){ + //test if this face is closer + let (face_n,d)=mesh.face_nd(face_id); + //if test point is behind face, the face is invalid + // TODO: find out why I thought of this backwards + if !(face_n.dot(point)-d).is_positive(){ + continue; + } + //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); + let boundary_d=boundary_n.dot(delta_pos); + //is test point behind edge, i.e. contained in the face + if !boundary_d.is_positive(){ + //both faces cannot pass this condition, return early if one does. + return FEV::Face(face_id); + } + } + FEV::Edge(edge_id) + }, + } +} + +pub fn closest_fev_not_inside<'a>(mesh:&MinkowskiMesh<'a>,point:Planar64Vec3)->Option>>{ + const ENABLE_FAST_FAIL:bool=false; + // TODO: remove mesh negation + minimum_difference::(&-mesh,point, + // on_exact + |is_intersecting,simplex|{ + if is_intersecting{ + return None; + } + // Convert simplex to FEV + // Vertices must be inverted since the mesh is inverted + Some(match simplex{ + Simplex1_3::Simplex1([v0])=>FEV::Vert(-v0), + Simplex1_3::Simplex2([v0,v1])=>{ + // invert + let (v0,v1)=(-v0,-v1); + crawl_to_closest_ev(mesh,[v0,v1],point).into() + }, + Simplex1_3::Simplex3([v0,v1,v2])=>{ + // invert + let (v0,v1,v2)=(-v0,-v1,-v2); + // Shimmy to the side until you find a face that contains the closest point + // it's ALWAYS representable as a face, but this algorithm may + // return E or V in edge cases but I don't think that will break the face crawler + crawl_to_closest_fev(mesh,[v0,v1,v2],point) + }, + }) + }, + // on_escape + |_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); + None + }, + // fast_fail value is irrelevant and will never be returned! + ||unreachable!() + ) +} + +pub fn contains_point(mesh:&MinkowskiMesh<'_>,point:Planar64Vec3)->bool{ + const ENABLE_FAST_FAIL:bool=true; + // TODO: remove mesh negation + minimum_difference::(&-mesh,point, + // on_exact + |is_intersecting,_simplex|{ + is_intersecting + }, + // on_escape + |_simplex|{ + // intersection is guaranteed at this point + true + }, + // fast_fail value + ||false + ) +} + +// local function minimumDifference( +// queryP, radiusP, +// queryQ, radiusQ, +// exitRadius, testIntersection +// ) +fn minimum_difference( + mesh:&M, + point:Planar64Vec3, + on_exact:impl FnOnce(bool,Simplex1_3)->T, + on_escape:impl FnOnce(Simplex<4,M::Vert>)->T, + on_fast_fail:impl FnOnce()->T, +)->T{ + // local initialAxis = queryQ() - queryP() + // local new_point_p = queryP(initialAxis) + // local new_point_q = queryQ(-initialAxis) + // local direction, a0, a1, b0, b1, c0, c1, d0, d1 + let mut initial_axis=mesh.hint_point()+point; + // degenerate case + if initial_axis==vec3::zero(){ + initial_axis=choose_any_direction(); + } + let last_point=mesh.farthest_vert(-initial_axis); + // this represents the 'a' value in the commented code + let mut last_pos=mesh.vert(last_point); + let Reduced{dir:mut direction,simplex:mut simplex_small}=reduce1([last_point],mesh,point); + + // exitRadius = testIntersection and 0 or exitRadius or 1/0 + // for _ = 1, 100 do + loop{ + // new_point_p = queryP(-direction) + // new_point_q = queryQ(direction) + // local next_point = new_point_q - new_point_p + let next_point=mesh.farthest_vert(direction); + let next_pos=mesh.vert(next_point); + + // if -direction:Dot(next_point) > (exitRadius + radiusP + radiusQ)*direction.magnitude then + if ENABLE_FAST_FAIL&&direction.dot(next_pos+point).is_negative(){ + return on_fast_fail(); + } + + let simplex_big=simplex_small.push_front(next_point); + + // if + // direction:Dot(next_point - a) <= 0 or + // absDet(next_point, a, b, c) < 1e-6 + if !direction.dot(next_pos-last_pos).is_positive() + ||simplex_big.det_is_zero(mesh){ + // Found enough information to compute the exact closest point. + // local norm = direction.unit + // local dist = a:Dot(norm) + // local hits = -dist < radiusP + radiusQ + let is_intersecting=(last_pos+point).dot(direction).is_positive(); + return on_exact(is_intersecting,simplex_small); + } + + // direction, a0, a1, b0, b1, c0, c1, d0, d1 = reduceSimplex(new_point_p, new_point_q, a0, a1, b0, b1, c0, c1) + match simplex_big.reduce(mesh,point){ + // if a and b and c and d then + Reduce::Escape(simplex)=>{ + // Enough information to conclude that the meshes are intersecting. + // Topology information is computed if needed. + return on_escape(simplex); + }, + Reduce::Reduced(reduced)=>{ + direction=reduced.dir; + simplex_small=reduced.simplex; + }, + } + + // next loop this will be a + last_pos=next_pos; + } +} + +#[cfg(test)] +mod test{ + use super::*; + use crate::model::{PhysicsMesh,PhysicsMeshView}; + + fn mesh_contains_point(mesh:PhysicsMeshView<'_>,point:Planar64Vec3)->bool{ + const ENABLE_FAST_FAIL:bool=true; + // TODO: remove mesh negation + minimum_difference::(&mesh,point, + // on_exact + |is_intersecting,_simplex|{ + is_intersecting + }, + // on_escape + |_simplex|{ + // intersection is guaranteed at this point + true + }, + // fast_fail value + ||false + ) + } + + #[test] + fn test_cube_points(){ + let mesh=PhysicsMesh::unit_cube(); + let mesh_view=mesh.complete_mesh_view(); + for x in -2..=2{ + for y in -2..=2{ + for z in -2..=2{ + let point=vec3::int(x,y,z)>>1; + assert!(mesh_contains_point(mesh_view,point),"Mesh did not contain point {point}"); + } + } + } + } +} diff --git a/engine/physics/src/model.rs b/engine/physics/src/model.rs index 0665bf31..3c24a221 100644 --- a/engine/physics/src/model.rs +++ b/engine/physics/src/model.rs @@ -91,8 +91,8 @@ pub trait MeshQuery{ (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]>; @@ -439,7 +439,7 @@ impl TryFrom<&model::Mesh> for PhysicsMesh{ } } -#[derive(Debug)] +#[derive(Debug,Clone,Copy)] pub struct PhysicsMeshView<'a>{ data:&'a PhysicsMeshData, topology:&'a PhysicsMeshTopology, @@ -458,6 +458,18 @@ impl MeshQuery for PhysicsMeshView<'_>{ // 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; @@ -496,7 +508,7 @@ impl PhysicsMeshTransform{ } } -#[derive(Debug)] +#[derive(Debug,Clone,Copy)] pub struct TransformedMesh<'a>{ view:PhysicsMeshView<'a>, transform:&'a PhysicsMeshTransform, @@ -514,18 +526,6 @@ impl TransformedMesh<'_>{ pub fn verts<'a>(&'a self)->impl Iterator>>+'a{ self.view.data.verts.iter().map(|&Vert(pos)|self.transform.vertex.transform_point3(pos)) } - 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 - ) - } } impl MeshQuery for TransformedMesh<'_>{ type Face=SubmeshFaceId; @@ -546,6 +546,18 @@ impl MeshQuery for TransformedMesh<'_>{ 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) @@ -572,10 +584,19 @@ impl MeshQuery for TransformedMesh<'_>{ //(face,vertex) //(edge,edge) //(vertex,face) -#[derive(Clone,Copy,Debug)] +#[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)] pub enum MinkowskiEdge{ VertEdge(SubmeshVertId,SubmeshEdgeId), @@ -612,7 +633,7 @@ impl DirectedEdge for MinkowskiDirectedEdge{ } } } -#[derive(Clone,Copy,Debug,Hash,Eq,PartialEq)] +#[derive(Clone,Copy,Debug,Hash)] pub enum MinkowskiFace{ VertFace(SubmeshVertId,SubmeshFaceId), EdgeEdge(SubmeshEdgeId,SubmeshEdgeId,bool), @@ -628,23 +649,20 @@ pub struct MinkowskiMesh<'a>{ mesh1:TransformedMesh<'a>, } -//infinity fev algorithm state transition -#[derive(Debug)] -enum Transition{ - Done,//found closest vert, no edges are better - Vert(MinkowskiVert),//transition to vert -} -enum EV{ - Vert(MinkowskiVert), - Edge(MinkowskiEdge), -} - pub type GigaTime=Ratio,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{ @@ -652,140 +670,27 @@ impl MinkowskiMesh<'_>{ 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 Fixed<2,64>,infinity_dir:Planar64Vec3,point:Planar64Vec3)->Transition{ - let mut best_transition=Transition::Done; - for &directed_edge_id in self.vert_edges(vert_id).as_ref(){ - let edge_n=self.directed_edge_n(directed_edge_id); - //is boundary uncrossable by a crawl from infinity - let edge_verts=self.edge_verts(directed_edge_id.as_undirected()); - //select opposite vertex - let test_vert_id=edge_verts.as_ref()[directed_edge_id.parity() as usize]; - //test if it's closer - let diff=point-self.vert(test_vert_id); - if edge_n.dot(infinity_dir).is_zero(){ - 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 Fixed<2,64>,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).as_ref(){ - let edge_n=self.directed_edge_n(directed_edge_id); - //is boundary uncrossable by a crawl from infinity - //check if time of collision is outside Time::MIN..Time::MAX - if edge_n.dot(infinity_dir).is_zero(){ - let d=edge_n.dot(diff); - //test the edge - let edge_nn=edge_n.dot(edge_n); - if !d.is_negative()&&d<=edge_nn{ - let distance_squared={ - let c=diff.cross(edge_n); - //wrap for speed - (c.dot(c)/edge_nn).divide().wrap_2() - }; - 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::>{ - //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::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); - // point is multiplied by two because vert_sum sums two vertices. - let delta_pos=point*2-{ - let &[v0,v1]=self.edge_verts(edge_id).as_ref(); - self.vert(v0)+self.vert(v1) - }; - for (i,&face_id) in self.edge_faces(edge_id).as_ref().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); - 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 - if !boundary_d.is_positive()&&boundary_n.dot(infinity_dir).is_zero(){ - //both faces cannot pass this condition, return early if one does. - return FEV::Face(face_id); - } - } - FEV::Edge(edge_id) - }, - } - } - // TODO: fundamentally improve this algorithm. - // All it needs to do is find the closest point on the mesh - // and return the FEV which the point resides on. - // - // What it actually does is use the above functions to trace a ray in from infinity, - // crawling the closest point along the mesh surface until the ray reaches - // the starting point to discover the final FEV. - // - // The actual collision prediction probably does a single test - // and then immediately returns with 0 FEV transitions on average, - // because of the strict time_limit constraint. - // - // Most of the calculation time is just calculating the starting point - // for the "actual" crawling algorithm below (predict_collision_{in|out}). - fn closest_fev_not_inside(&self,mut infinity_body:Body,start_time:Bound<&Time>)->Option>>{ - infinity_body.infinity_dir().and_then(|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=vec3::zero(); - //crawl in from negative infinity along a tangent line to get the closest fev - infinity_fev.crawl(self,&infinity_body,Bound::Unbounded,start_time).miss() - }) - } pub fn predict_collision_in(&self,relative_body:&Body,range:impl RangeBounds