library-mirrors/HC
1
0
Fork 0
mirror of https://github.com/vrld/HC.git synced 2024-12-18 16:14:20 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Fix commit adfe9a9a breaking EPA.

Browse Source
This commit is contained in:
Matthias Richter 2013-09-03 17:13:27 +02:00
parent adfe9a9a5d
commit 97ae2d8928
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gjk.lua
View File

@ -26,6 +26,7 @@ THE SOFTWARE.
local _PACKAGE = (...):match("^(.+)%.[^%.]+") local _PACKAGE = (...):match("^(.+)%.[^%.]+")
local vector = require(_PACKAGE .. '.vector-light') local vector = require(_PACKAGE .. '.vector-light')
local huge, abs = math.huge, math.abs
local function support(shape_a, shape_b, dx, dy) local function support(shape_a, shape_b, dx, dy)
local x,y = shape_a:support(dx,dy) local x,y = shape_a:support(dx,dy)
@ -34,7 +35,7 @@ end
-- returns closest edge to the origin -- returns closest edge to the origin
local function closest_edge(simplex) local function closest_edge(simplex)
local e = {dist = math.huge} local e = {dist = huge}
local i = #simplex-1 local i = #simplex-1
for k = 1,#simplex-1,2 do for k = 1,#simplex-1,2 do
@ -65,14 +66,14 @@ local function EPA(shape_a, shape_b, simplex)
end end
-- the expanding polytype algorithm -- the expanding polytype algorithm
local last_diff_dist = math.huge local last_diff_dist = huge
while true do while true do
local e = closest_edge(simplex) local e = closest_edge(simplex)
local px,py = support(shape_a, shape_b, e.nx, e.ny) local px,py = support(shape_a, shape_b, e.nx, e.ny)
local d = vector.dot(px,py, e.nx, e.ny) local d = vector.dot(px,py, e.nx, e.ny)
local diff_dist = d - e.dist local diff_dist = d - e.dist
if diff_dist < 1e-6 or last_diff_dist - diff_dist < 1e-10 then if diff_dist < 1e-6 or abs(last_diff_dist - diff_dist) < 1e-10 then
return -d*e.nx, -d*e.ny return -d*e.nx, -d*e.ny
end end
last_diff_dist = diff_dist last_diff_dist = diff_dist
@ -88,9 +89,11 @@ end
-- : : in direction of the origin. -- : : in direction of the origin.
local function do_line(simplex) local function do_line(simplex)
local bx,by, ax,ay = unpack(simplex) local bx,by, ax,ay = unpack(simplex)
local abx,aby = bx-ax, by-ay local abx,aby = bx-ax, by-ay
local dx,dy = vector.perpendicular(abx,aby) local dx,dy = vector.perpendicular(abx,aby)
if vector.dot(dx,dy, -ax,-ay) < 0 then if vector.dot(dx,dy, -ax,-ay) < 0 then
dx,dy = -dx,-dy dx,dy = -dx,-dy
end end
@ -163,6 +166,7 @@ local function GJK(shape_a, shape_b)
if vector.dot(ax,ay, dx,dy) <= 0 then if vector.dot(ax,ay, dx,dy) <= 0 then
return false return false
end end
simplex[n+1], simplex[n+2] = ax,ay simplex[n+1], simplex[n+2] = ax,ay
simplex, dx, dy = do_line(simplex, dx, dy) simplex, dx, dy = do_line(simplex, dx, dy)
n = 4 n = 4