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

include relative point

Browse Source
This commit is contained in:
Rhys Lloyd
2025-11-25 09:09:46 -08:00
parent 1b833ef6b3
commit 6621e369f2
1 changed files with 17 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -84,8 +84,9 @@ const fn choose_any_direction()->Planar64Vec3{
} }
fn reduce_simplex( fn reduce_simplex(
mesh:&MinkowskiMesh,
mut simplex:Simplex, mut simplex:Simplex,
mesh:&MinkowskiMesh,
point:Planar64Vec3,
)->(Planar64Vec3,Simplex){ )->(Planar64Vec3,Simplex){
match simplex.len(){ match simplex.len(){
0=>(choose_any_direction(),simplex), 0=>(choose_any_direction(),simplex),
@@ -93,7 +94,7 @@ fn reduce_simplex(
1=>{ 1=>{
// --debug.profilebegin("reduceSimplex0") // --debug.profilebegin("reduceSimplex0")
// local a = a1 - a0 // local a = a1 - a0
let p0=mesh.vert(simplex[0]); let p0=mesh.vert(simplex[0])+point;
// local p = -a // local p = -a
let p=-p0; let p=-p0;
@@ -115,8 +116,8 @@ fn reduce_simplex(
// --debug.profilebegin("reduceSimplex1") // --debug.profilebegin("reduceSimplex1")
// local a = a1 - a0 // local a = a1 - a0
// local b = b1 - b0 // local b = b1 - b0
let p0=mesh.vert(simplex[0]); let p0=mesh.vert(simplex[0])+point;
let p1=mesh.vert(simplex[1]); let p1=mesh.vert(simplex[1])+point;
// local p = -a // local p = -a
// local u = b - a // local u = b - a
@@ -161,9 +162,9 @@ fn reduce_simplex(
// local a = a1 - a0 // local a = a1 - a0
// local b = b1 - b0 // local b = b1 - b0
// local c = c1 - c0 // local c = c1 - c0
let p0=mesh.vert(simplex[0]); let p0=mesh.vert(simplex[0])+point;
let p1=mesh.vert(simplex[1]); let p1=mesh.vert(simplex[1])+point;
let p2=mesh.vert(simplex[2]); let p2=mesh.vert(simplex[2])+point;
// local p = -a // local p = -a
// local u = b - a // local u = b - a
@@ -256,10 +257,10 @@ fn reduce_simplex(
// local b = b1 - b0 // local b = b1 - b0
// local c = c1 - c0 // local c = c1 - c0
// local d = d1 - d0 // local d = d1 - d0
let p0=mesh.vert(simplex[0]); let p0=mesh.vert(simplex[0])+point;
let p1=mesh.vert(simplex[1]); let p1=mesh.vert(simplex[1])+point;
let p2=mesh.vert(simplex[2]); let p2=mesh.vert(simplex[2])+point;
let p3=mesh.vert(simplex[3]); let p3=mesh.vert(simplex[3])+point;
// local p = -a // local p = -a
// local u = b - a // local u = b - a
@@ -470,7 +471,7 @@ pub fn contains_point(mesh:&MinkowskiMesh,point:Planar64Vec3)->bool{
// local dist = a:Dot(norm) // local dist = a:Dot(norm)
// local hits = -dist < radiusP + radiusQ // local hits = -dist < radiusP + radiusQ
// return hits // return hits
mesh.vert(simplex[1]).dot(direction).is_positive() (mesh.vert(simplex[1])+point).dot(direction).is_positive()
}, },
// on_escape // on_escape
|_simplex|{ |_simplex|{
@@ -504,7 +505,7 @@ pub fn closest_fev(mesh:&MinkowskiMesh,point:Planar64Vec3)->Topology{
// ) // )
fn minimum_difference<const ENABLE_FAST_FAIL:bool,T>( fn minimum_difference<const ENABLE_FAST_FAIL:bool,T>(
mesh:&MinkowskiMesh, mesh:&MinkowskiMesh,
rel_pos:Planar64Vec3, point:Planar64Vec3,
on_exact:impl FnOnce(Simplex,Planar64Vec3)->T, on_exact:impl FnOnce(Simplex,Planar64Vec3)->T,
on_escape:impl FnOnce(Simplex)->T, on_escape:impl FnOnce(Simplex)->T,
on_fast_fail:impl FnOnce()->T, on_fast_fail:impl FnOnce()->T,
@@ -513,7 +514,7 @@ fn minimum_difference<const ENABLE_FAST_FAIL:bool,T>(
// local new_point_p = queryP(initialAxis) // local new_point_p = queryP(initialAxis)
// local new_point_q = queryQ(-initialAxis) // local new_point_q = queryQ(-initialAxis)
// local direction, a0, a1, b0, b1, c0, c1, d0, d1 // local direction, a0, a1, b0, b1, c0, c1, d0, d1
let mut direction=mesh.hint_point()+rel_pos; let mut direction=mesh.hint_point()+point;
// degenerate case // degenerate case
if direction==vec3::zero(){ if direction==vec3::zero(){
direction=choose_any_direction(); direction=choose_any_direction();
@@ -537,7 +538,7 @@ fn minimum_difference<const ENABLE_FAST_FAIL:bool,T>(
let next_point=mesh.farthest_vert(direction); let next_point=mesh.farthest_vert(direction);
// if -direction:Dot(next_point) > (exitRadius + radiusP + radiusQ)*direction.magnitude then // if -direction:Dot(next_point) > (exitRadius + radiusP + radiusQ)*direction.magnitude then
if ENABLE_FAST_FAIL&&direction.dot(mesh.vert(next_point)+rel_pos).is_negative(){ if ENABLE_FAST_FAIL&&direction.dot(mesh.vert(next_point)+point).is_negative(){
return on_fast_fail(); return on_fast_fail();
} }
@@ -561,6 +562,6 @@ fn minimum_difference<const ENABLE_FAST_FAIL:bool,T>(
} }
// direction, a0, a1, b0, b1, c0, c1, d0, d1 = reduceSimplex(new_point_p, new_point_q, a0, a1, b0, b1, c0, c1) // direction, a0, a1, b0, b1, c0, c1, d0, d1 = reduceSimplex(new_point_p, new_point_q, a0, a1, b0, b1, c0, c1)
(direction,simplex)=reduce_simplex(mesh,simplex); (direction,simplex)=reduce_simplex(simplex,mesh,point);
} }
} }