Mercurial > games > semicongine

comparison semiconginev2/contrib/algorithms/collision.nim @ 1227:4d97cfc4888b

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
merge
author sam <sam@basx.dev>
date Wed, 17 Jul 2024 23:45:43 +0700
parents c8e3037aca66
children
comparison
equal deleted inserted replaced
1170:2addc5f6804f 1227:4d97cfc4888b
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)