diff --git a/engine/physics/src/minimum_difference.rs b/engine/physics/src/minimum_difference.rs index f234bc71..567454db 100644 --- a/engine/physics/src/minimum_difference.rs +++ b/engine/physics/src/minimum_difference.rs @@ -9,6 +9,266 @@ use MinkowskiVert::VertVert as PQ; const SIMPLEX_TETRAHEDRON:usize=4; type Simplex=arrayvec::ArrayVec; +/* +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 +*/ + +/* +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 +*/ + +/* +local function chooseAnyDirection() + return Vector3.new(1, 0, 0) +end +*/ + +/* +local function reduceSimplex0(a0, a1) + --debug.profilebegin("reduceSimplex0") + local a = a1 - a0 + + local p = -a + + local direction = p + if direction.magnitude == 0 then + direction = chooseAnyDirection() + end + return direction, a0, a1 +end +*/ + +/* +local function reduceSimplex1(a0, a1, b0, b1) + --debug.profilebegin("reduceSimplex1") + local a = a1 - a0 + local b = b1 - b0 + + local p = -a + local u = b - a + + -- modify to take into account the radiuses + local p_u = p:Dot(u) + + if p_u >= 0 then + local direction = u:Cross(p):Cross(u) + if direction.magnitude == 0 then + direction = choosePerpendicularDirection(u) + end + -- modify the direction to take into account a0R and b0R + return direction, a0, a1, b0, b1 + end + + local direction = p + if direction.magnitude == 0 then + direction = choosePerpendicularDirection(u) + end + return direction, a0, a1 +end +*/ + +/* +local function reduceSimplex2(a0, a1, b0, b1, c0, c1) + --debug.profilebegin("reduceSimplex2") + local a = a1 - a0 + local b = b1 - b0 + local c = c1 - c0 + + local p = -a + local u = b - a + local v = c - a + + 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) + + --print("simples 2 i j", uv_up, uv_pv) + + if uv_up >= 0 and uv_pv >= 0 then + --print("and we got here??") + local uvp = uv:Dot(p) + local direction = uvp < 0 and -uv or uv + return direction, a0, a1, b0, b1, c0, c1 + end + + --print("no we got here") + + 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) + + if vDist == minDist2 then + u = v + b0 = c0 + b1 = c1 + up = -pv + uv = -uv + u_u = v_v + end + + local p_u = p:Dot(u) + + if p_u >= 0 then + local direction = up:Cross(u) + --print("got here!") + if direction.magnitude == 0 then + --print("the magnitude is 0") + direction = uv + end + return direction, a0, a1, b0, b1 + end + + local direction = p + if direction.magnitude == 0 then + direction = uv + end + return direction, a0, a0 +end +*/ + +/* +-- a is always the new one +local function reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1) + --debug.profilebegin("reduceSimplex3") + local a = a1 - a0 + local b = b1 - b0 + local c = c1 - c0 + local d = d1 - d0 + + local p = -a + local u = b - a + local v = c - a + local w = d - a + + 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) + + if pvw/uvw >= 0 and upw/uvw >= 0 and uvp/uvw >= 0 then + local direction = Vector3.new(0, 0, 0) + return direction, a0, a1, b0, b1, c0, c1, d0, d1 + end + + 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) + + if vwDist == minDist3 then + u, v = v, w + b0, c0 = c0, d0 + b1, c1 = c1, d1 + uv = vw + uvp = pvw + elseif wuDist == minDist3 then + u, v = w, u + b0, c0 = d0, b0 + b1, c1 = d1, b1 + uv = wu + uvp = upw + end + + local up = u:Cross(p) + local pv = p:Cross(v) + local uv_up = uv:Dot(up) + local uv_pv = uv:Dot(pv) + + if uv_up >= 0 and uv_pv >= 0 then + local direction = uvw < 0 and uv or -uv + return direction, a0, a1, b0, b1, c0, c1 + end + + 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) + + if vDist == minDist2 then + u = v + b0 = c0 + b1 = c1 + up = -pv + uv = -uv + u_u = v_v + end + + local p_u = p:Dot(u) + + if p_u >= 0 then + local direction = up:Cross(u) + if direction.magnitude == 0 then + direction = uvw < 0 and uv or -uv + end + return direction, a0, a1, b0, b1 + end + + local direction = p + if direction.magnitude == 0 then + direction = uvw < 0 and uv or -uv + end + return direction, a0, a1 +end +*/ + +/* +local function reduceSimplex(a0, a1, b0, b1, c0, c1, d0, d1) + if d0 then + return reduceSimplex3(a0, a1, b0, b1, c0, c1, d0, d1) + elseif c0 then + return reduceSimplex2(a0, a1, b0, b1, c0, c1) + elseif b0 then + return reduceSimplex1(a0, a1, b0, b1) + elseif a0 then + return reduceSimplex0(a0, a1) + else + error("can a simplex have 0 points?") + end +end +*/ + +fn reduce_simplex( + simplex:Simplex, +)->(Planar64Vec3,Simplex){ + match simplex.as_slice(){ + &[p0,p1,p2,p3]=>reduce_simplex_4(p0,p1,p2,p3), + &[p0,p1,p2]=>reduce_simplex_3(p0,p1,p2), + &[p0,p1]=>reduce_simplex_2(p0,p1), + &[p0]=>reduce_simplex_1(p0), + &[]=>reduce_simplex_0(), + _=>unreachable!(), + } +} + enum Expanded{ Expanded, Unexpanded,