|
1226
|
1 const MAX_COLLISON_DETECTION_ITERATIONS = 20
|
|
|
2 const MAX_COLLISON_POINT_CALCULATION_ITERATIONS = 20
|
|
|
3
|
|
|
4 type
|
|
|
5 ColliderType* = enum
|
|
|
6 Box, Sphere, Points
|
|
|
7 Collider* = object
|
|
|
8 transform*: Mat4 = Unit4F32
|
|
|
9 case theType*: ColliderType
|
|
|
10 of Box: discard
|
|
|
11 of Sphere: radius*: float32
|
|
|
12 of Points: points*: seq[Vec3f]
|
|
|
13
|
|
|
14 func between(value, b1, b2: float32): bool =
|
|
|
15 min(b1, b2) <= value and value <= max(b1, b2)
|
|
|
16
|
|
|
17 func Contains*(collider: Collider, x: Vec3f): bool =
|
|
|
18 # from https://math.stackexchange.com/questions/1472049/check-if-a-point-is-inside-a-rectangular-shaped-area-3d
|
|
|
19 case collider.theType:
|
|
|
20 of Box:
|
|
|
21 let
|
|
|
22 P1 = collider.transform * NewVec3f(0, 0, 0) # origin
|
|
|
23 P2 = collider.transform * Z
|
|
|
24 P4 = collider.transform * X
|
|
|
25 P5 = collider.transform * Y
|
|
|
26 u = (P1 - P4).Cross(P1 - P5)
|
|
|
27 v = (P1 - P2).Cross(P1 - P5)
|
|
|
28 w = (P1 - P2).Cross(P1 - P4)
|
|
|
29 uP1 = u.Dot(P1)
|
|
|
30 uP2 = u.Dot(P2)
|
|
|
31 vP1 = v.Dot(P1)
|
|
|
32 vP4 = v.Dot(P4)
|
|
|
33 wP1 = w.Dot(P1)
|
|
|
34 wP5 = w.Dot(P5)
|
|
|
35 ux = u.Dot(x)
|
|
|
36 vx = v.Dot(x)
|
|
|
37 wx = w.Dot(x)
|
|
|
38 ux.between(uP1, uP2) and vx.between(vP1, vP4) and wx.between(wP1, wP5)
|
|
|
39 of Sphere:
|
|
|
40 (collider.transform * x).Length < (collider.transform * NewVec3f()).Length
|
|
|
41 of Points:
|
|
|
42 raise newException(Exception, "Points are not supported yet for 'contains'")
|
|
|
43
|
|
|
44 # implementation of GJK, based on https://blog.winter.dev/2020/gjk-algorithm/
|
|
|
45
|
|
|
46 # most generic implementation of findFurthestPoint
|
|
|
47 # add other implementations of findFurthestPoint for other kind of geometry or optimization
|
|
|
48 # (will be selected depening on type of the first parameter)
|
|
|
49 func findFurthestPoint(points: openArray[Vec3f], direction: Vec3f): Vec3f =
|
|
|
50 var maxDist = low(float32)
|
|
|
51 for p in points:
|
|
|
52 let dist = direction.Dot(p)
|
|
|
53 if dist > maxDist:
|
|
|
54 maxDist = dist
|
|
|
55 result = p
|
|
|
56
|
|
|
57 func findFurthestPoint(transform: Mat4, direction: Vec3f): Vec3f =
|
|
|
58 return findFurthestPoint(
|
|
|
59 [
|
|
|
60 transform * NewVec3f(0, 0, 0),
|
|
|
61 transform * X,
|
|
|
62 transform * Y,
|
|
|
63 transform * Z,
|
|
|
64 transform * (X + Y),
|
|
|
65 transform * (X + Z),
|
|
|
66 transform * (Y + Z),
|
|
|
67 transform * (X + Y + Z),
|
|
|
68 ],
|
|
|
69 direction
|
|
|
70 )
|
|
|
71 func findFurthestPoint(collider: Collider, direction: Vec3f): Vec3f =
|
|
|
72 case collider.theType
|
|
|
73 of Sphere:
|
|
|
74 let directionNormalizedToSphere = ((direction / direction.Length) * collider.radius)
|
|
|
75 collider.transform * directionNormalizedToSphere
|
|
|
76 of Box:
|
|
|
77 findFurthestPoint(collider.transform, direction)
|
|
|
78 of Points:
|
|
|
79 findFurthestPoint(collider.points, direction)
|
|
|
80
|
|
|
81 func supportPoint(a, b: Collider, direction: Vec3f): Vec3f =
|
|
|
82 a.findFurthestPoint(direction) - b.findFurthestPoint(-direction)
|
|
|
83
|
|
|
84 func sameDirection(direction: Vec3f, ao: Vec3f): bool =
|
|
|
85 direction.Dot(ao) > 0
|
|
|
86
|
|
|
87 func line(simplex: var seq[Vec3f], direction: var Vec3f): bool =
|
|
|
88 let
|
|
|
89 a = simplex[0]
|
|
|
90 b = simplex[1]
|
|
|
91 ab = b - a
|
|
|
92 ao = - a
|
|
|
93
|
|
|
94 if sameDirection(ab, ao):
|
|
|
95 direction = Cross(Cross(ab, ao), ab)
|
|
|
96 else:
|
|
|
97 simplex = @[a]
|
|
|
98 direction = ao
|
|
|
99
|
|
|
100 return false
|
|
|
101
|
|
|
102 func triangle(simplex: var seq[Vec3f], direction: var Vec3f, twoDimensional = false): bool =
|
|
|
103 let
|
|
|
104 a = simplex[0]
|
|
|
105 b = simplex[1]
|
|
|
106 c = simplex[2]
|
|
|
107 ab = b - a
|
|
|
108 ac = c - a
|
|
|
109 ao = - a
|
|
|
110 abc = ab.Cross(ac)
|
|
|
111
|
|
|
112 if sameDirection(abc.Cross(ac), ao):
|
|
|
113 if sameDirection(ac, ao):
|
|
|
114 simplex = @[a, c]
|
|
|
115 direction = ac.Cross(ao).Cross(ac)
|
|
|
116 else:
|
|
|
117 simplex = @[a, b]
|
|
|
118 return line(simplex, direction)
|
|
|
119 else:
|
|
|
120 if (sameDirection(ab.Cross(abc), ao)):
|
|
|
121 simplex = @[a, b]
|
|
|
122 return line(simplex, direction)
|
|
|
123 else:
|
|
|
124 if twoDimensional:
|
|
|
125 return true
|
|
|
126 if (sameDirection(abc, ao)):
|
|
|
127 direction = abc
|
|
|
128 else:
|
|
|
129 simplex = @[a, c, b]
|
|
|
130 direction = -abc
|
|
|
131
|
|
|
132 return false
|
|
|
133
|
|
|
134 func tetrahedron(simplex: var seq[Vec3f], direction: var Vec3f): bool =
|
|
|
135 let
|
|
|
136 a = simplex[0]
|
|
|
137 b = simplex[1]
|
|
|
138 c = simplex[2]
|
|
|
139 d = simplex[3]
|
|
|
140 ab = b - a
|
|
|
141 ac = c - a
|
|
|
142 ad = d - a
|
|
|
143 ao = - a
|
|
|
144 abc = ab.Cross(ac)
|
|
|
145 acd = ac.Cross(ad)
|
|
|
146 adb = ad.Cross(ab)
|
|
|
147
|
|
|
148 if sameDirection(abc, ao):
|
|
|
149 simplex = @[a, b, c]
|
|
|
150 return triangle(simplex, direction)
|
|
|
151 if sameDirection(acd, ao):
|
|
|
152 simplex = @[a, c, d]
|
|
|
153 return triangle(simplex, direction)
|
|
|
154 if sameDirection(adb, ao):
|
|
|
155 simplex = @[a, d, b]
|
|
|
156 return triangle(simplex, direction)
|
|
|
157
|
|
|
158 return true
|
|
|
159
|
|
|
160 func getFaceNormals(polytope: seq[Vec3f], faces: seq[int]): (seq[Vec4f], int) =
|
|
|
161 var
|
|
|
162 normals: seq[Vec4f]
|
|
|
163 minTriangle = 0
|
|
|
164 minDistance = high(float32)
|
|
|
165
|
|
|
166 for i in countup(0, faces.len - 1, 3):
|
|
|
167 let
|
|
|
168 a = polytope[faces[i + 0]]
|
|
|
169 b = polytope[faces[i + 1]]
|
|
|
170 c = polytope[faces[i + 2]]
|
|
|
171
|
|
|
172 var normal = (b - a).Cross(c - a).Normalized()
|
|
|
173 var distance = normal.Dot(a)
|
|
|
174
|
|
|
175 if distance < 0:
|
|
|
176 normal = normal * -1'f32
|
|
|
177 distance = distance * -1'f32
|
|
|
178
|
|
|
179 normals.add normal.ToVec4(distance)
|
|
|
180
|
|
|
181 if distance < minDistance:
|
|
|
182 minTriangle = i div 3
|
|
|
183 minDistance = distance
|
|
|
184
|
|
|
185 return (normals, minTriangle)
|
|
|
186
|
|
|
187 func addIfUniqueEdge(edges: var seq[(int, int)], faces: seq[int], a: int, b: int) =
|
|
|
188 let reverse = edges.find((faces[b], faces[a]))
|
|
|
189 if (reverse >= 0):
|
|
|
190 edges.delete(reverse)
|
|
|
191 else:
|
|
|
192 edges.add (faces[a], faces[b])
|
|
|
193
|
|
|
194 func nextSimplex(simplex: var seq[Vec3f], direction: var Vec3f, twoDimensional = false): bool =
|
|
|
195 case simplex.len
|
|
|
196 of 2: simplex.line(direction)
|
|
|
197 of 3: simplex.triangle(direction, twoDimensional)
|
|
|
198 of 4: simplex.tetrahedron(direction)
|
|
|
199 else: raise newException(Exception, "Error in simplex")
|
|
|
200
|
|
|
201 func collisionPoint3D(simplex: var seq[Vec3f], a, b: Collider): tuple[normal: Vec3f, penetrationDepth: float32] =
|
|
|
202 var
|
|
|
203 polytope = simplex
|
|
|
204 faces = @[
|
|
|
205 0, 1, 2,
|
|
|
206 0, 3, 1,
|
|
|
207 0, 2, 3,
|
|
|
208 1, 3, 2
|
|
|
209 ]
|
|
|
210 (normals, minFace) = getFaceNormals(polytope, faces)
|
|
|
211 minNormal: Vec3f
|
|
|
212 minDistance = high(float32)
|
|
|
213 iterCount = 0
|
|
|
214
|
|
|
215 while minDistance == high(float32) and iterCount < MAX_COLLISON_POINT_CALCULATION_ITERATIONS:
|
|
|
216 minNormal = normals[minFace].xyz
|
|
|
217 minDistance = normals[minFace].w
|
|
|
218 var
|
|
|
219 support = supportPoint(a, b, minNormal)
|
|
|
220 sDistance = minNormal.Dot(support)
|
|
|
221
|
|
|
222 if abs(sDistance - minDistance) > 0.001'f32:
|
|
|
223 minDistance = high(float32)
|
|
|
224 var uniqueEdges: seq[(int, int)]
|
|
|
225 var i = 0
|
|
|
226 while i < normals.len:
|
|
|
227 if sameDirection(normals[i], support):
|
|
|
228 var f = i * 3
|
|
|
229
|
|
|
230 addIfUniqueEdge(uniqueEdges, faces, f + 0, f + 1)
|
|
|
231 addIfUniqueEdge(uniqueEdges, faces, f + 1, f + 2)
|
|
|
232 addIfUniqueEdge(uniqueEdges, faces, f + 2, f + 0)
|
|
|
233
|
|
|
234 faces[f + 2] = faces.pop()
|
|
|
235 faces[f + 1] = faces.pop()
|
|
|
236 faces[f + 0] = faces.pop()
|
|
|
237
|
|
|
238 normals[i] = normals.pop()
|
|
|
239
|
|
|
240 dec i
|
|
|
241 inc i
|
|
|
242
|
|
|
243 var newFaces: seq[int]
|
|
|
244 for (edgeIndex1, edgeIndex2) in uniqueEdges:
|
|
|
245 newFaces.add edgeIndex1
|
|
|
246 newFaces.add edgeIndex2
|
|
|
247 newFaces.add polytope.len
|
|
|
248
|
|
|
249 polytope.add support
|
|
|
250
|
|
|
251 var (newNormals, newMinFace) = getFaceNormals(polytope, newFaces)
|
|
|
252 if newNormals.len == 0:
|
|
|
253 break
|
|
|
254
|
|
|
255 var oldMinDistance = high(float32)
|
|
|
256 for j in 0 ..< normals.len:
|
|
|
257 if normals[j].w < oldMinDistance:
|
|
|
258 oldMinDistance = normals[j].w
|
|
|
259 minFace = j
|
|
|
260
|
|
|
261 if (newNormals[newMinFace].w < oldMinDistance):
|
|
|
262 minFace = newMinFace + normals.len
|
|
|
263
|
|
|
264 for f in newFaces:
|
|
|
265 faces.add f
|
|
|
266 for n in newNormals:
|
|
|
267 normals.add n
|
|
|
268 inc iterCount
|
|
|
269
|
|
|
270 result = (normal: minNormal, penetrationDepth: minDistance + 0.001'f32)
|
|
|
271
|
|
|
272
|
|
|
273 func collisionPoint2D(polytopeIn: seq[Vec3f], a, b: Collider): tuple[normal: Vec3f, penetrationDepth: float32] =
|
|
|
274 var
|
|
|
275 polytope = polytopeIn
|
|
|
276 minIndex = 0
|
|
|
277 minDistance = high(float32)
|
|
|
278 iterCount = 0
|
|
|
279 minNormal: Vec2f
|
|
|
280
|
|
|
281 while minDistance == high(float32) and iterCount < MAX_COLLISON_POINT_CALCULATION_ITERATIONS:
|
|
|
282 for i in 0 ..< polytope.len:
|
|
|
283 let
|
|
|
284 j = (i + 1) mod polytope.len
|
|
|
285 vertexI = polytope[i]
|
|
|
286 vertexJ = polytope[j]
|
|
|
287 ij = vertexJ - vertexI
|
|
|
288 var
|
|
|
289 normal = NewVec2f(ij.y, -ij.x).Normalized()
|
|
|
290 distance = normal.Dot(vertexI)
|
|
|
291
|
|
|
292 if (distance < 0):
|
|
|
293 distance *= -1'f32
|
|
|
294 normal = normal * -1'f32
|
|
|
295
|
|
|
296 if distance < minDistance:
|
|
|
297 minDistance = distance
|
|
|
298 minNormal = normal
|
|
|
299 minIndex = j
|
|
|
300
|
|
|
301 let
|
|
|
302 support = supportPoint(a, b, minNormal.ToVec3)
|
|
|
303 sDistance = minNormal.Dot(support)
|
|
|
304
|
|
|
305 if(abs(sDistance - minDistance) > 0.001):
|
|
|
306 minDistance = high(float32)
|
|
|
307 polytope.insert(support, minIndex)
|
|
|
308 inc iterCount
|
|
|
309
|
|
|
310 result = (normal: NewVec3f(minNormal.x, minNormal.y), penetrationDepth: minDistance + 0.001'f32)
|
|
|
311
|
|
|
312 func Intersects*(a, b: Collider, as2D = false): bool =
|
|
|
313 var
|
|
|
314 support = supportPoint(a, b, NewVec3f(0.8153, -0.4239, if as2D: 0.0 else: 0.5786)) # just random initial vector
|
|
|
315 simplex = newSeq[Vec3f]()
|
|
|
316 direction = -support
|
|
|
317 n = 0
|
|
|
318 simplex.insert(support, 0)
|
|
|
319 while n < MAX_COLLISON_DETECTION_ITERATIONS:
|
|
|
320 support = supportPoint(a, b, direction)
|
|
|
321 if support.Dot(direction) <= 0:
|
|
|
322 return false
|
|
|
323 simplex.insert(support, 0)
|
|
|
324 if nextSimplex(simplex, direction, twoDimensional = as2D):
|
|
|
325 return true
|
|
|
326 # prevent numeric instability
|
|
|
327 if direction == NewVec3f(0, 0, 0):
|
|
|
328 direction[0] = 0.0001
|
|
|
329 inc n
|
|
|
330
|
|
|
331 func Collision*(a, b: Collider, as2D = false): tuple[hasCollision: bool, normal: Vec3f, penetrationDepth: float32] =
|
|
|
332 var
|
|
|
333 support = supportPoint(a, b, NewVec3f(0.8153, -0.4239, if as2D: 0.0 else: 0.5786)) # just random initial vector
|
|
|
334 simplex = newSeq[Vec3f]()
|
|
|
335 direction = -support
|
|
|
336 n = 0
|
|
|
337 simplex.insert(support, 0)
|
|
|
338 while n < MAX_COLLISON_DETECTION_ITERATIONS:
|
|
|
339 support = supportPoint(a, b, direction)
|
|
|
340 if support.Dot(direction) <= 0:
|
|
|
341 return result
|
|
|
342 simplex.insert(support, 0)
|
|
|
343 if nextSimplex(simplex, direction, twoDimensional = as2D):
|
|
|
344 let (normal, depth) = if as2D: collisionPoint2D(simplex, a, b) else: collisionPoint3D(simplex, a, b)
|
|
|
345 return (true, normal, depth)
|
|
|
346 # prevent numeric instability
|
|
|
347 if direction == NewVec3f(0, 0, 0):
|
|
|
348 direction[0] = 0.0001
|
|
|
349 inc n
|
|
|
350
|
|
|
351 func CalculateCollider*(points: openArray[Vec3f], theType: ColliderType): Collider =
|
|
|
352 var
|
|
|
353 minX = high(float32)
|
|
|
354 maxX = low(float32)
|
|
|
355 minY = high(float32)
|
|
|
356 maxY = low(float32)
|
|
|
357 minZ = high(float32)
|
|
|
358 maxZ = low(float32)
|
|
|
359 center: Vec3f
|
|
|
360
|
|
|
361 for p in points:
|
|
|
362 minX = min(minX, p.x)
|
|
|
363 maxX = max(maxX, p.x)
|
|
|
364 minY = min(minY, p.y)
|
|
|
365 maxY = max(maxY, p.y)
|
|
|
366 minZ = min(minZ, p.z)
|
|
|
367 maxZ = max(maxz, p.z)
|
|
|
368 center = center + p
|
|
|
369 center = center / float32(points.len)
|
|
|
370
|
|
|
371 let
|
|
|
372 scaleX = (maxX - minX)
|
|
|
373 scaleY = (maxY - minY)
|
|
|
374 scaleZ = (maxZ - minZ)
|
|
|
375
|
|
|
376 if theType == Points:
|
|
|
377 result = Collider(theType: Points, points: @points)
|
|
|
378 else:
|
|
|
379 result = Collider(theType: theType, transform: Translate(minX, minY, minZ) * Scale(scaleX, scaleY, scaleZ))
|
|
|
380
|
|
|
381 if theType == Sphere:
|
|
|
382 result.transform = Translate(center)
|
|
|
383 for p in points:
|
|
|
384 result.radius = max(result.radius, (p - center).Length)
|
