Mercurial > games > semicongine

comparison semiconginev2/old/collision.nim @ 1218:56781cc0fc7c compiletime-tests

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