mirror of
https://github.com/vrld/HC.git
synced 2024-11-18 12:54:23 +00:00
Collision detection using GJK/EPA instead of SAT.
The Gilbert–Johnson–Keerthi collision detection algorithm is significantly faster than collision detection using the separating axis theorem. GJK can only determine whether two shapes collide, but not the penetration vector. The expanding polytype algorithm can use information from GJK to quickly find the required vector.
This commit is contained in:
Matthias Richter
parent
490b8775a2
commit
ce4b8011da
173
gjk.lua
Normal file
173
gjk.lua
Normal file
@ -0,0 +1,173 @@
|
||||
--[[
|
||||
Copyright (c) 2012 Matthias Richter
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
Except as contained in this notice, the name(s) of the above copyright holders
|
||||
shall not be used in advertising or otherwise to promote the sale, use or
|
||||
other dealings in this Software without prior written authorization.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
THE SOFTWARE.
|
||||
]]--
|
||||
|
||||
local _PACKAGE = (...):match("^(.+)%.[^%.]+")
|
||||
local vector = require(_PACKAGE .. '.vector-light')
|
||||
|
||||
local function support(shape_a, shape_b, dx, dy)
|
||||
local x,y = shape_a:support(dx,dy)
|
||||
return vector.sub(x,y, shape_b:support(-dx, -dy))
|
||||
end
|
||||
|
||||
-- returns closest edge to the origin
|
||||
local function closest_edge(simplex)
|
||||
local e = {dist = math.huge}
|
||||
|
||||
local i = #simplex-1
|
||||
for k = 1,#simplex-1,2 do
|
||||
local ax,ay = simplex[i], simplex[i+1]
|
||||
local bx,by = simplex[k], simplex[k+1]
|
||||
i = k
|
||||
|
||||
local ex,ey = vector.perpendicular(bx-ax, by-ay)
|
||||
local nx,ny = vector.normalize(ex,ey)
|
||||
local d = vector.dot(ax,ay, nx,ny)
|
||||
|
||||
if d < e.dist then
|
||||
e.dist = d
|
||||
e.nx, e.ny = nx, ny
|
||||
e.i = k
|
||||
end
|
||||
end
|
||||
|
||||
return e
|
||||
end
|
||||
|
||||
local function EPA(shape_a, shape_b, simplex)
|
||||
-- make sure simplex is oriented counter clockwise
|
||||
local cx,cy, bx,by, ax,ay = unpack(simplex)
|
||||
if vector.dot(ax-bx,ay-by, cx-bx,cy-by) < 0 then
|
||||
simplex[1],simplex[2] = ax,ay
|
||||
simplex[5],simplex[6] = cx,cy
|
||||
end
|
||||
|
||||
-- the expanding polytype algorithm
|
||||
while true do
|
||||
local e = closest_edge(simplex)
|
||||
local px,py = support(shape_a, shape_b, e.nx, e.ny)
|
||||
local d = vector.dot(px,py, e.nx, e.ny)
|
||||
|
||||
if d - e.dist < 1e-6 then
|
||||
return -d*e.nx, -d*e.ny
|
||||
end
|
||||
|
||||
-- simplex = {..., simplex[e.i-1], px, py, simplex[e.i]
|
||||
table.insert(simplex, e.i, py)
|
||||
table.insert(simplex, e.i, px)
|
||||
end
|
||||
end
|
||||
|
||||
-- : : origin must be in plane between A and B
|
||||
-- B o------o A since A is the furthest point on the MD
|
||||
-- : : in direction of the origin.
|
||||
local function do_line(simplex)
|
||||
local bx,by, ax,ay = unpack(simplex)
|
||||
local abx,aby = bx-ax, by-ay
|
||||
|
||||
local dx,dy = vector.perpendicular(abx,aby)
|
||||
if vector.dot(dx,dy, -ax,-ay) < 0 then
|
||||
dx,dy = -dx,-dy
|
||||
end
|
||||
return simplex, dx,dy
|
||||
end
|
||||
|
||||
-- B .'
|
||||
-- o-._ 1
|
||||
-- | `-. .' The origin can only be in regions 1, 3 or 4:
|
||||
-- | 4 o A 2 A lies on the edge of the MD and we came
|
||||
-- | _.-' '. from left of BC.
|
||||
-- o-' 3
|
||||
-- C '.
|
||||
local function do_triangle(simplex)
|
||||
local cx,cy, bx,by, ax,ay = unpack(simplex)
|
||||
local aox,aoy = -ax,-ay
|
||||
local abx,aby = bx-ax, by-ay
|
||||
local acx,acy = cx-ax, cy-ay
|
||||
|
||||
-- test region 1
|
||||
local dx,dy = vector.perpendicular(abx,aby)
|
||||
if vector.dot(dx,dy, acx,acy) > 0 then
|
||||
dx,dy = -dx,-dy
|
||||
end
|
||||
if vector.dot(dx,dy, aox,aoy) > 0 then
|
||||
-- simplex = {bx,by, ax,ay}
|
||||
simplex[1], simplex[2] = bx,by
|
||||
simplex[3], simplex[4] = ax,ay
|
||||
simplex[5], simplex[6] = nil, nil
|
||||
return simplex, dx,dy
|
||||
end
|
||||
|
||||
-- test region 3
|
||||
dx,dy = vector.perpendicular(acx,acy)
|
||||
if vector.dot(dx,dy, abx,aby) > 0 then
|
||||
dx,dy = -dx,-dy
|
||||
end
|
||||
if vector.dot(dx,dy, aox, aoy) > 0 then
|
||||
-- simplex = {cx,cy, ax,ay}
|
||||
simplex[3], simplex[4] = ax,ay
|
||||
simplex[5], simplex[6] = nil, nil
|
||||
return simplex, dx,dy
|
||||
end
|
||||
|
||||
-- must be in region 4
|
||||
return simplex
|
||||
end
|
||||
|
||||
|
||||
local function GJK(shape_a, shape_b)
|
||||
local ax,ay = support(shape_a, shape_b, 1,0)
|
||||
local simplex = {ax,ay}
|
||||
local n = 2
|
||||
local dx,dy = -ax,-ay
|
||||
|
||||
-- first iteration: line case
|
||||
ax,ay = support(shape_a, shape_b, dx,dy)
|
||||
if vector.dot(ax,ay, dx,dy) <= 0 then
|
||||
return false
|
||||
end
|
||||
simplex[n+1], simplex[n+2] = ax,ay
|
||||
simplex, dx, dy = do_line(simplex, dx, dy)
|
||||
n = 4
|
||||
|
||||
-- all other iterations must be the triangle case
|
||||
while true do
|
||||
ax,ay = support(shape_a, shape_b, dx,dy)
|
||||
|
||||
if vector.dot(ax,ay, dx,dy) <= 0 then
|
||||
return false
|
||||
end
|
||||
|
||||
simplex[n+1], simplex[n+2] = ax,ay
|
||||
simplex, dx, dy = do_triangle(simplex, dx,dy)
|
||||
n = #simplex
|
||||
|
||||
if n == 6 then
|
||||
return true, EPA(shape_a, shape_b, simplex)
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
return GJK
|
||||
91
shapes.lua
91
shapes.lua
@ -24,8 +24,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
THE SOFTWARE.
|
||||
]]--
|
||||
|
||||
local math_abs, math_floor, math_min, math_max = math.abs, math.floor, math.min, math.max
|
||||
local math_sqrt, math_log, math_pi, math_huge = math.sqrt, math.log, math.pi, math.huge
|
||||
local math_min, math_sqrt, math_huge = math.min, math.sqrt, math.huge
|
||||
|
||||
local _PACKAGE = (...):match("^(.+)%.[^%.]+")
|
||||
if not common and common.class then
|
||||
@ -34,38 +33,7 @@ if not common and common.class then
|
||||
end
|
||||
local vector = require(_PACKAGE .. '.vector-light')
|
||||
local Polygon = require(_PACKAGE .. '.polygon')
|
||||
|
||||
local function math_absmin(a,b) return math_abs(a) < math_abs(b) and a or b end
|
||||
local function test_axes(axes, shape_one, shape_two, sx,sy, min_overlap)
|
||||
for _,axis in ipairs(axes) do
|
||||
local l1,r1 = shape_one:projectOn(axis)
|
||||
local l2,r2 = shape_two:projectOn(axis)
|
||||
-- do the intervals overlap?
|
||||
if r1 < l2 or r2 < l1 then return false end
|
||||
|
||||
-- get the smallest absolute overlap
|
||||
local overlap = math_absmin(l2-r1, r2-l1)
|
||||
if math_abs(overlap) < min_overlap then
|
||||
sx,sy = vector.mul(overlap, axis.x, axis.y)
|
||||
min_overlap = math_abs(overlap)
|
||||
end
|
||||
end
|
||||
return true, sx,sy, min_overlap
|
||||
end
|
||||
|
||||
local function SAT(shape_one, axes_one, shape_two, axes_two)
|
||||
local collide, sx,sy, overlap = false, 0,0, math_huge
|
||||
collide, sx,sy, overlap = test_axes(axes_one, shape_one, shape_two, sx,sy, overlap)
|
||||
if not collide then return false end
|
||||
collide, sx,sy, overlap = test_axes(axes_two, shape_one, shape_two, sx,sy, overlap)
|
||||
return collide, sx,sy
|
||||
end
|
||||
|
||||
local function outcircles_intersect(shape_one, shape_two)
|
||||
local x1,y1,r1 = shape_one:outcircle()
|
||||
local x2,y2,r2 = shape_two:outcircle()
|
||||
return vector.len2(x1-x2, y1-y2) <= (r1+r2)*(r1+r2)
|
||||
end
|
||||
local GJK = require(_PACKAGE .. '.gjk') -- actual collision detection
|
||||
|
||||
--
|
||||
-- base class
|
||||
@ -135,35 +103,21 @@ end
|
||||
--
|
||||
-- collision functions
|
||||
--
|
||||
function ConvexPolygonShape:getAxes()
|
||||
local axes = {}
|
||||
local vert = self._polygon.vertices
|
||||
local p,q = vert[#vert], vert[#vert]
|
||||
for i = 1,#vert do
|
||||
p,q = q, vert[i]
|
||||
local x,y = vector.normalize(vector.perpendicular(p.x-q.x, p.y-q.y))
|
||||
axes[#axes+1] = {x = x, y = y}
|
||||
end
|
||||
return axes
|
||||
end
|
||||
|
||||
function ConvexPolygonShape:projectOn(axis)
|
||||
function ConvexPolygonShape:support(dx,dy)
|
||||
local v = self._polygon.vertices
|
||||
local min,max = math_huge,-math_huge
|
||||
local max, vmax = -math_huge
|
||||
for i = 1,#v do
|
||||
local p = vector.dot(v[i].x,v[i].y, axis.x,axis.y) -- = v[i]:projectOn(axis) * axis
|
||||
min = math_min(p, min)
|
||||
max = math_max(p, max)
|
||||
local d = vector.dot(v[i].x,v[i].y, dx,dy)
|
||||
if d > max then
|
||||
max, vmax = d, v[i]
|
||||
end
|
||||
return min, max
|
||||
end
|
||||
return vmax.x, vmax.y
|
||||
end
|
||||
|
||||
function CircleShape:projectOn(axis)
|
||||
-- v:projectOn(a) * a = v * a (see ConvexPolygonShape)
|
||||
-- therefore: (c +- a*r) * a = c*a +- |a|^2 * r
|
||||
local center = vector.dot(self._center.x,self._center.y, axis.x,axis.y)
|
||||
local shift = self._radius * vector.len2(axis.x, axis.y)
|
||||
return center - shift, center + shift
|
||||
function CircleShape:support(dx,dy)
|
||||
return vector.add(self._center.x, self._center.y,
|
||||
vector.mul(self._radius, vector.normalize(dx,dy)))
|
||||
end
|
||||
|
||||
-- collision dispatching:
|
||||
@ -175,8 +129,7 @@ function ConvexPolygonShape:collidesWith(other)
|
||||
end
|
||||
|
||||
-- else: type is POLYGON, use the SAT
|
||||
if not outcircles_intersect(self, other) then return false end
|
||||
return SAT(self, self:getAxes(), other, other:getAxes())
|
||||
return GJK(self, other)
|
||||
end
|
||||
|
||||
function ConcavePolygonShape:collidesWith(other)
|
||||
@ -184,8 +137,6 @@ function ConcavePolygonShape:collidesWith(other)
|
||||
return other:collidesWith(self)
|
||||
end
|
||||
|
||||
if not outcircles_intersect(self, other) then return false end
|
||||
|
||||
-- TODO: better way of doing this. report all the separations?
|
||||
local collide,dx,dy,count = false,0,0,0
|
||||
for _,s in ipairs(self._shapes) do
|
||||
@ -219,21 +170,7 @@ function CircleShape:collidesWith(other)
|
||||
end
|
||||
|
||||
-- else: other._type == POLYGON
|
||||
if not outcircles_intersect(self, other) then return false end
|
||||
-- retrieve closest edge to center
|
||||
local vertices = other._polygon.vertices
|
||||
local closest, dist = vertices[1], vector.len2(self._center.x-vertices[1].x, self._center.y-vertices[1].y)
|
||||
for i = 2,#vertices do
|
||||
local d = vector.len2(self._center.x-vertices[i].x, self._center.y-vertices[i].y)
|
||||
if d < dist then
|
||||
closest, dist = vertices[i], d
|
||||
end
|
||||
end
|
||||
local axis = {x=0,y=1}
|
||||
if dist ~= 0 then
|
||||
axis.x,axis.y = vector.normalize(closest.x-self._center.x, closest.y-self._center.y)
|
||||
end
|
||||
return SAT(self, {axis}, other, other:getAxes())
|
||||
return GJK(self, other)
|
||||
end
|
||||
|
||||
function PointShape:collidesWith(other)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user