├── README.md └── GJK.lua /README.md: -------------------------------------------------------------------------------- 1 | # Lua-GJK 2 | This Lua script is an implementation of the Gilbert-Johnson-Keerthi (GJK) intersection and shortest seperating vector algorithms in 3D. It was created and testing in Roblox using their implementation of the vector dot product, cross product, and magnitude. The distance and intersection functions require you to implement a function for each object you are testing that finds the point furthest on the object in any given vector direction. This function is commonly called the support function. There is no software license. 3 | -------------------------------------------------------------------------------- /GJK.lua: -------------------------------------------------------------------------------- 1 | --Gilbert-Johnson-Keerthi distance and intersection tests 2 | --Tyler R. Hoyer 3 | --11/20/2014 4 | 5 | --May return early if no intersection if found. If it is primed, it will run in amortized constant time (untested). 6 | 7 | --If the distance function is used between colliding objects, the program 8 | --may loop a hundred times without finding a result. If this is the case, 9 | --it will throw an error. The check is omited for speed. If the objects 10 | --might intersect eachother, call the intersection method first. 11 | 12 | --Objects must implement the :getFarthestPoint(dir) function which returns the 13 | --farthest point in a given direction. 14 | 15 | --Used Roblox's Vector3 userdata. Outside implementations will require a implementation of the methods of 16 | --the Vector3's. :Dot, :Cross, .new, and .magnitude must be defined. 17 | 18 | local abs = math.abs 19 | local min = math.min 20 | local huge = math.huge 21 | local origin = Vector3.new() 22 | 23 | local function loopRemoved(data, step) 24 | --We're on the next step 25 | step = step + 1 26 | 27 | --If we have completed the last cycle, stop 28 | if step > #data then 29 | return nil 30 | end 31 | 32 | --To be the combination without the value 33 | local copy = {} 34 | 35 | --Copy the data up to the missing value 36 | for i = 1, step - 1 do 37 | copy[i] = data[i] 38 | end 39 | 40 | --Copy the data on the other side of the missing value 41 | for i = step, #data - 1 do 42 | copy[i] = data[i + 1] 43 | end 44 | 45 | --return the step, combination, and missing value 46 | return step, copy, data[step] 47 | end 48 | 49 | --Finds the vector direction to search for the next point 50 | --in the simplex. 51 | local function getDir(points, to) 52 | --Single point, return vector 53 | if #points == 1 then 54 | return to - points[1] 55 | 56 | --Line, return orthogonal line 57 | elseif #points == 2 then 58 | local v1 = points[2] - points[1] 59 | local v2 = to - points[1] 60 | return v1:Cross(v2):Cross(v1) 61 | 62 | --Triangle, return normal 63 | else 64 | local v1 = points[3] - points[1] 65 | local v2 = points[2] - points[1] 66 | local v3 = to - points[1] 67 | local n = v1:Cross(v2) 68 | return n:Dot(v3) < 0 and -n or n 69 | end 70 | end 71 | 72 | --The function that finds the intersection between two sets 73 | --of points, s1 and s2. s1 and s2 must return the point in 74 | --the set that is furthest in a given direction when called. 75 | --If the start direction sV is specified as the seperation 76 | --vector, the program runs in constant time. (excluding the 77 | --user implemented functions for finding the furthest point). 78 | function intersection(s1, s2, sV) 79 | local points = {} 80 | 81 | -- find point 82 | local function support(dir) 83 | local a = s1(dir) 84 | local b = s2(-dir) 85 | points[#points + 1] = a - b 86 | return dir:Dot(a) < dir:Dot(b) 87 | end 88 | 89 | -- find all points forming a simplex 90 | if support(sV) 91 | or support(getDir(points, origin)) 92 | or support(getDir(points, origin)) 93 | or support(getDir(points, origin)) 94 | then 95 | return false 96 | end 97 | 98 | local step, others, removed = 0 99 | repeat 100 | step, others, removed = loopRemoved(points, step) 101 | local dir = getDir(others, removed) 102 | if others[1]:Dot(dir) > 0 then 103 | points = others 104 | if support(-dir) then 105 | return false 106 | end 107 | step = 0 108 | end 109 | until step == 4 110 | 111 | return true 112 | end 113 | 114 | --Checks if two vectors are equal 115 | local function equals(p1, p2) 116 | return p1.x == p2.x and p1.y == p2.y and p1.z == p2.z 117 | end 118 | 119 | --Gets the mathematical scalar t of the parametrc line defined by 120 | --o + t * v of a point p on the line (the magnitude of the projection). 121 | local function getT(o, v, p) 122 | return (p - o):Dot(v) / v:Dot(v) 123 | end 124 | 125 | --Returns the scalar of the closest point on a line to 126 | --the origin. Note that if the vector is a zero vector then 127 | --it treats it as a point offset instead of a line. 128 | local function lineToOrigin(o, v) 129 | if equals(v, origin) then 130 | return o 131 | end 132 | local t = getT(o, v, origin) 133 | if t < 0 then 134 | t = 0 135 | elseif t > 1 then 136 | t = 1 137 | end 138 | return o + v*t 139 | end 140 | 141 | --Convoluted to deal with cases like points in the same place 142 | local function closestPoint(a, b, c) 143 | --if abc is a line 144 | if c == nil then 145 | --get the scalar of the closest point 146 | local dir = b - a 147 | local t = getT(a, dir, origin) 148 | if t < 0 then t = 0 149 | elseif t > 1 then t = 1 150 | end 151 | --and return the point 152 | return a + dir * t 153 | end 154 | 155 | --Otherwise it is a triangle. 156 | --Define all the lines of the triangle and the normal 157 | local vAB, vBC, vCA = b - a, c - b, a - c 158 | local normal = vAB:Cross(vBC) 159 | 160 | --If two points are in the same place then 161 | if normal.magnitude == 0 then 162 | 163 | --Find the closest line between ab and bc to the origin (it cannot be ac) 164 | local ab = lineToOrigin(a, vAB) 165 | local bc = lineToOrigin(b, vBC) 166 | if ab.magnitude < bc.magnitude then 167 | return ab 168 | else 169 | return bc 170 | end 171 | 172 | --The following statements find the line which is closest to the origin 173 | --by using voroni regions. If it is inside the triangle, it returns the 174 | --normal of the triangle. 175 | elseif a:Dot(a + vAB * getT(a, vAB, c) - c) <= 0 then 176 | return lineToOrigin(a, vAB) 177 | elseif b:Dot(b + vBC * getT(b, vBC, a) - a) <= 0 then 178 | return lineToOrigin(b, vBC) 179 | elseif c:Dot(c + vCA * getT(c, vCA, b) - b) <= 0 then 180 | return lineToOrigin(c, vCA) 181 | else 182 | return -normal * getT(a, normal, origin) 183 | end 184 | end 185 | 186 | --The distance function. Works like the intersect function above. Returns 187 | --the translation vector between the two closest points. 188 | function distance(s1, s2, sV) 189 | local function support (dir) 190 | return s1(dir) - s2(-dir) 191 | end 192 | 193 | --Find the initial three points in the search direction, opposite of the 194 | --search direction, and in the orthoginal direction between those two 195 | --points to the origin. 196 | local a = support(sV) 197 | local b = support(-a) 198 | local c = support(-closestPoint(a, b)) 199 | 200 | --Setup maximum loops 201 | local i = 1 202 | while i < 100 do 203 | i = i + 1 204 | 205 | --Get the closest point on the triangle 206 | local p = closestPoint(a, b, c) 207 | 208 | --If it is the origin, the objects are just touching, 209 | --return a zero vector. 210 | if equals(p, origin) then 211 | return origin 212 | end 213 | 214 | --Search in the direction from the closest point 215 | --to the origin for a point. 216 | local dir = p.unit 217 | local d = support(dir) 218 | local dd = d:Dot(dir) 219 | local dm = math.min( 220 | a:Dot(dir), 221 | b:Dot(dir), 222 | c:Dot(dir) 223 | ) 224 | 225 | --If the new point is farther or equal to the closest 226 | --point on the triangle, then we have found the closest 227 | --point. 228 | if dd >= dm then 229 | --return the point on the minkowski difference as the 230 | --translation vector between the two closest point. 231 | return -p 232 | end 233 | 234 | --Otherwise replace the point on the triangle furthest 235 | --from the origin with the new point 236 | local ma, mb, mc = a:Dot(dir), b:Dot(dir), c:Dot(dir) 237 | if ma > mb then 238 | if ma > mc then 239 | a = d 240 | else 241 | c = d 242 | end 243 | elseif mb > mc then 244 | b = d 245 | else 246 | c = d 247 | end 248 | end 249 | 250 | --Return an error if no point was found in the maximum 251 | --number of iterations 252 | error 'Unable to find distance, are they intersecting?' 253 | end 254 | 255 | return { 256 | intersection = intersection; 257 | distance = distance; 258 | } 259 | --------------------------------------------------------------------------------