diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index 7476129d..0e867f0d 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -131,12 +131,370 @@ fn reduce1( (direction,Simplex1_3::Simplex1([v0])) } +// local function reduceSimplex1(a0, a1, b0, b1) +fn reduce2( + [v0,v1]:Simplex<2>, + mesh:&MinkowskiMesh, + 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:direction.narrow_1().unwrap(), + 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>, + mesh:&MinkowskiMesh, + 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:direction.narrow_1().unwrap(), + 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:uv.narrow_1().unwrap(), + simplex:Simplex1_3::Simplex2([v0,v1]), + }; + } + + // return direction, a0, a1, b0, b1 + return Reduced{ + dir:direction.narrow_1().unwrap(), + 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:uv.narrow_1().unwrap(), + 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>, + mesh:&MinkowskiMesh, + 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:uv.narrow_1().unwrap(), + simplex:Simplex1_3::Simplex3([v0,v1,v2]), + }); + }else{ + return Reduce::Reduced(Reduced{ + dir:-uv.narrow_1().unwrap(), + 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:uv.narrow_1().unwrap(), + simplex:Simplex1_3::Simplex2([v0,v1]), + }); + }else{ + return Reduce::Reduced(Reduced{ + dir:-uv.narrow_1().unwrap(), + simplex:Simplex1_3::Simplex2([v0,v1]), + }); + } + } + + // return direction, a0, a1, b0, b1 + return Reduce::Reduced(Reduced{ + dir:direction.narrow_1().unwrap(), + 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:uv.narrow_1().unwrap(), + simplex:Simplex1_3::Simplex1([v0]), + }); + }else{ + return Reduce::Reduced(Reduced{ + dir:-uv.narrow_1().unwrap(), + 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>), - Reduced{ - dir:Planar64Vec3, - simplex:Simplex1_3, - } + Reduced(Reduced), } fn reduce( @@ -145,347 +503,9 @@ fn reduce( point:Planar64Vec3, )->Reduce{ match simplex{ - // local function reduceSimplex1(a0, a1, b0, b1) - Simplex2_4::Simplex2([v0,v1])=>{ - // --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 Reduce::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 Reduce::Reduced{ - dir:direction.narrow_1().unwrap(), - 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 - Reduce::Reduced{ - dir, - simplex:Simplex1_3::Simplex1([v0]), - } - }, - // local function reduceSimplex2(a0, a1, b0, b1, c0, c1) - Simplex2_4::Simplex3([v0,mut v1,v2])=>{ - // --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 Reduce::Reduced{ - dir:direction.narrow_1().unwrap(), - 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 Reduce::Reduced{ - dir:uv.narrow_1().unwrap(), - simplex:Simplex1_3::Simplex2([v0,v1]), - }; - } - - // return direction, a0, a1, b0, b1 - return Reduce::Reduced{ - dir:direction.narrow_1().unwrap(), - simplex:Simplex1_3::Simplex2([v0,v1]), - }; - } - - // local direction = p - let dir=p; - // if direction.magnitude == 0 then - if dir==vec3::zero(){ - // direction = uv - return Reduce::Reduced{ - dir:uv.narrow_1().unwrap(), - simplex:Simplex1_3::Simplex1([v0]), - }; - } - // return direction, a0, a0 - Reduce::Reduced{ - dir, - simplex:Simplex1_3::Simplex1([v0]), - } - }, - // local function reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1) - Simplex2_4::Simplex4([v0,mut v1,mut v2,v3])=>{ - // --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{ - dir:uv.narrow_1().unwrap(), - simplex:Simplex1_3::Simplex3([v0,v1,v2]), - }; - }else{ - return Reduce::Reduced{ - dir:-uv.narrow_1().unwrap(), - 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{ - dir:uv.narrow_1().unwrap(), - simplex:Simplex1_3::Simplex2([v0,v1]), - }; - }else{ - return Reduce::Reduced{ - dir:-uv.narrow_1().unwrap(), - simplex:Simplex1_3::Simplex2([v0,v1]), - }; - } - } - - // return direction, a0, a1, b0, b1 - return Reduce::Reduced{ - dir:direction.narrow_1().unwrap(), - 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{ - dir:uv.narrow_1().unwrap(), - simplex:Simplex1_3::Simplex1([v0]), - }; - }else{ - return Reduce::Reduced{ - dir:-uv.narrow_1().unwrap(), - simplex:Simplex1_3::Simplex1([v0]), - }; - } - } - - // return direction, a0, a1 - Reduce::Reduced{ - dir, - simplex:Simplex1_3::Simplex1([v0]), - } - }, + Simplex2_4::Simplex2(simplex)=>Reduce::Reduced(reduce2(simplex,mesh,point)), + Simplex2_4::Simplex3(simplex)=>Reduce::Reduced(reduce3(simplex,mesh,point)), + Simplex2_4::Simplex4(simplex)=>reduce4(simplex,mesh,point), } } @@ -622,9 +642,9 @@ fn minimum_difference( // Topology information is computed if needed. return on_escape(simplex); }, - Reduce::Reduced{dir,simplex}=>{ - direction=dir; - simplex_small=simplex; + Reduce::Reduced(reduced)=>{ + direction=reduced.dir; + simplex_small=reduced.simplex; }, } }