diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index 6f63d4ca..538bb0e5 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -27,16 +27,6 @@ enum Simplex2_4{ Simplex3(Simplex<3>), Simplex4(Simplex<4>), } -impl core::ops::Index for Simplex2_4{ - type Output=MinkowskiVert; - fn index(&self,index:usize)->&Self::Output{ - match self{ - Simplex2_4::Simplex2(s)=>&s[index], - Simplex2_4::Simplex3(s)=>&s[index], - Simplex2_4::Simplex4(s)=>&s[index], - } - } -} /* local function absDet(r, u, v, w) @@ -555,12 +545,12 @@ pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{ const ENABLE_FAST_FAIL:bool=true; minimum_difference::(mesh,point, // on_exact - |simplex,direction|{ + |last_pos,direction|{ // local norm = direction.unit // local dist = a:Dot(norm) // local hits = -dist < radiusP + radiusQ // return hits - (mesh.vert(simplex[1])+point).dot(direction).is_positive() + (last_pos+point).dot(direction).is_positive() }, // on_escape |_simplex|{ @@ -575,7 +565,7 @@ pub fn closest_fev(mesh:&MinkowskiMesh,point:Planar64Vec3)->Topology{ const ENABLE_FAST_FAIL:bool=false; minimum_difference::(mesh,point, // on_exact - |simplex,_direction|Topology{simplex}, + |_last_pos,_direction|unimplemented!(), // on_escape |simplex|{ // local norm, dist, u0, u1, v0, v1, w0, w1 = expand(queryP, queryQ, a0, a1, b0, b1, c0, c1, d0, d1, 1e-5) @@ -595,7 +585,7 @@ pub fn closest_fev(mesh:&MinkowskiMesh,point:Planar64Vec3)->Topology{ fn minimum_difference( mesh:&MinkowskiMesh, point:Planar64Vec3, - on_exact:impl FnOnce(Simplex2_4,Planar64Vec3)->T, + on_exact:impl FnOnce(Planar64Vec3,Planar64Vec3)->T, on_escape:impl FnOnce(Simplex<4>)->T, on_fast_fail:impl FnOnce()->T, )->T{ @@ -608,8 +598,10 @@ fn minimum_difference( if direction==vec3::zero(){ direction=choose_any_direction(); } - let new_point=mesh.farthest_vert(direction); - let Reduced{dir:mut direction,simplex:mut simplex_small}=reduce1([new_point],mesh,point); + let last_point=mesh.farthest_vert(direction); + // 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 @@ -630,10 +622,10 @@ fn minimum_difference( // if // direction:Dot(next_point - a) <= 0 or // absDet(next_point, a, b, c) < 1e-6 - if !direction.dot(next_pos-mesh.vert(simplex_big[1])).is_positive() + 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(simplex_big,direction); + return on_exact(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) @@ -649,5 +641,8 @@ fn minimum_difference( simplex_small=reduced.simplex; }, } + + // next loop this will be a + last_pos=next_pos; } }