wip paste fns
This commit is contained in:
@@ -9,6 +9,266 @@ use MinkowskiVert::VertVert as PQ;
|
||||
const SIMPLEX_TETRAHEDRON:usize=4;
|
||||
type Simplex=arrayvec::ArrayVec<MinkowskiVert,SIMPLEX_TETRAHEDRON>;
|
||||
|
||||
/*
|
||||
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,
|
||||
|
||||
Reference in New Issue
Block a user