Mercurial > games > semicongine

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