forked from g3n/engine
-
Notifications
You must be signed in to change notification settings - Fork 0
/
convexhull.go
465 lines (380 loc) · 15.1 KB
/
convexhull.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
// Copyright 2016 The G3N Authors. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.
package shape
import (
"github.com/g3n/engine/geometry"
"github.com/g3n/engine/math32"
"github.com/g3n/engine/experimental/collision"
)
// ConvexHull is a convex triangle-based geometry used for collision detection and contact resolution.
type ConvexHull struct {
geometry.Geometry
// Cached geometry properties
faces [][3]math32.Vector3
faceNormals []math32.Vector3
worldFaceNormals []math32.Vector3
uniqueEdges []math32.Vector3
worldUniqueEdges []math32.Vector3
}
func NewConvexHull(geom *geometry.Geometry) *ConvexHull {
ch := new(ConvexHull)
// // TODO check if geometry is convex, panic if not
//if !geom.IsConvex() {
// panic("geometry needs to be convex")
//}
// // TODO future: create function to break up geometry into convex shapes and add all shapes to body
ch.Geometry = *geom
// Perform single-time computations
ch.computeFaceNormalsAndUniqueEdges()
return ch
}
// Compute and store face normals and unique edges
func (ch *ConvexHull) computeFaceNormalsAndUniqueEdges() {
ch.Geometry.ReadFaces(func(vA, vB, vC math32.Vector3) bool {
// Store face vertices
var face [3]math32.Vector3
face[0] = vA
face[1] = vB
face[2] = vC
ch.faces = append(ch.faces, face)
// Compute edges
edge1 := math32.NewVec3().SubVectors(&vB, &vA)
edge2 := math32.NewVec3().SubVectors(&vC, &vB)
edge3 := math32.NewVec3().SubVectors(&vA, &vC)
// Compute and store face normal in b.faceNormals
faceNormal := math32.NewVec3().CrossVectors(edge2, edge1)
if faceNormal.Length() > 0 {
faceNormal.Normalize().Negate()
}
ch.faceNormals = append(ch.faceNormals, *faceNormal)
// Compare unique edges recorded so far with the three new face edges and store the unique ones
tol := float32(1e-6)
for p := 0; p < len(ch.uniqueEdges); p++ {
ue := ch.uniqueEdges[p]
if !ue.AlmostEquals(edge1, tol) {
ch.uniqueEdges = append(ch.uniqueEdges, *edge1)
}
if !ue.AlmostEquals(edge2, tol) {
ch.uniqueEdges = append(ch.uniqueEdges, *edge1)
}
if !ue.AlmostEquals(edge3, tol) {
ch.uniqueEdges = append(ch.uniqueEdges, *edge1)
}
}
return false
})
// Allocate space for worldFaceNormals and worldUniqueEdges
ch.worldFaceNormals = make([]math32.Vector3, len(ch.faceNormals))
ch.worldUniqueEdges = make([]math32.Vector3, len(ch.uniqueEdges))
}
// ComputeWorldFaceNormalsAndUniqueEdges
func (ch *ConvexHull) ComputeWorldFaceNormalsAndUniqueEdges(quat *math32.Quaternion) {
// Re-compute world face normals from local face normals
for i := 0; i < len(ch.faceNormals); i++ {
ch.worldFaceNormals[i] = ch.faceNormals[i]
ch.worldFaceNormals[i].ApplyQuaternion(quat)
}
// Re-compute world unique edges from local unique edges
for i := 0; i < len(ch.uniqueEdges); i++ {
ch.worldUniqueEdges[i] = ch.uniqueEdges[i]
ch.worldUniqueEdges[i].ApplyQuaternion(quat)
}
}
func (ch *ConvexHull) Faces() [][3]math32.Vector3 {
return ch.faces
}
func (ch *ConvexHull) FaceNormals() []math32.Vector3 {
return ch.faceNormals
}
func (ch *ConvexHull) WorldFaceNormals() []math32.Vector3 {
return ch.worldFaceNormals
}
func (ch *ConvexHull) UniqueEdges() []math32.Vector3 {
return ch.uniqueEdges
}
func (ch *ConvexHull) WorldUniqueEdges() []math32.Vector3 {
return ch.worldUniqueEdges
}
// FindPenetrationAxis finds the penetration axis between two convex bodies.
// The normal points from bodyA to bodyB.
// Returns false if there is no penetration. If there is a penetration - returns true and the penetration axis.
func (ch *ConvexHull) FindPenetrationAxis(chB *ConvexHull, posA, posB *math32.Vector3, quatA, quatB *math32.Quaternion) (bool, math32.Vector3) {
// Keep track of the smaller depth found so far
// Note that the penetration axis is the one that causes
// the smallest penetration depth when the two hulls are squished onto that axis!
// (may seem a bit counter-intuitive)
depthMin := math32.Inf(1)
var penetrationAxis math32.Vector3
var depth float32
// Assume the geometries are penetrating.
// As soon as (and if) we figure out that they are not, then return false.
penetrating := true
worldFaceNormalsA := ch.WorldFaceNormals()
worldFaceNormalsB := ch.WorldFaceNormals()
// Check world normals of body A
for _, worldFaceNormal := range worldFaceNormalsA {
// Check whether the face is colliding with geomB
penetrating, depth = ch.TestPenetrationAxis(chB, &worldFaceNormal, posA, posB, quatA, quatB)
if !penetrating {
return false, penetrationAxis // penetrationAxis doesn't matter since not penetrating
}
if depth < depthMin {
depthMin = depth
penetrationAxis.Copy(&worldFaceNormal)
}
}
// Check world normals of body B
for _, worldFaceNormal := range worldFaceNormalsB {
// Check whether the face is colliding with geomB
penetrating, depth = ch.TestPenetrationAxis(chB, &worldFaceNormal, posA, posB, quatA, quatB)
if !penetrating {
return false, penetrationAxis // penetrationAxis doesn't matter since not penetrating
}
if depth < depthMin {
depthMin = depth
penetrationAxis.Copy(&worldFaceNormal)
}
}
worldUniqueEdgesA := ch.WorldUniqueEdges()
worldUniqueEdgesB := ch.WorldUniqueEdges()
// Check all combinations of unique world edges
for _, worldUniqueEdgeA := range worldUniqueEdgesA {
for _, worldUniqueEdgeB := range worldUniqueEdgesB {
// Cross edges
edgeCross := math32.NewVec3().CrossVectors(&worldUniqueEdgeA, &worldUniqueEdgeB)
// If the edges are not aligned
tol := float32(1e-6)
if edgeCross.Length() > tol { // Cross product is not close to zero
edgeCross.Normalize()
penetrating, depth = ch.TestPenetrationAxis(chB, edgeCross, posA, posB, quatA, quatB)
if !penetrating {
return false, penetrationAxis
}
if depth < depthMin {
depthMin = depth
penetrationAxis.Copy(edgeCross)
}
}
}
}
deltaC := math32.NewVec3().SubVectors(posA, posB)
if deltaC.Dot(&penetrationAxis) > 0.0 {
penetrationAxis.Negate()
}
return true, penetrationAxis
}
// Both hulls are projected onto the axis and the overlap size (penetration depth) is returned if there is one.
// return {number} The overlap depth, or FALSE if no penetration.
func (ch *ConvexHull) TestPenetrationAxis(chB *ConvexHull, worldAxis, posA, posB *math32.Vector3, quatA, quatB *math32.Quaternion) (bool, float32) {
maxA, minA := ch.ProjectOntoWorldAxis(worldAxis, posA, quatA)
maxB, minB := chB.ProjectOntoWorldAxis(worldAxis, posB, quatB)
if maxA < minB || maxB < minA {
return false, 0 // Separated
}
d0 := maxA - minB
d1 := maxB - minA
if d0 < d1 {
return true, d0
} else {
return true, d1
}
}
// ProjectOntoWorldAxis projects the geometry onto the specified world axis.
func (ch *ConvexHull) ProjectOntoWorldAxis(worldAxis, pos *math32.Vector3, quat *math32.Quaternion) (float32, float32) {
// Transform the world axis to local
quatConj := quat.Conjugate()
localAxis := worldAxis.Clone().ApplyQuaternion(quatConj)
// Project onto the local axis
max, min := ch.Geometry.ProjectOntoAxis(localAxis)
// Offset to obtain values relative to world origin
localOrigin := math32.NewVec3().Sub(pos).ApplyQuaternion(quatConj)
add := localOrigin.Dot(localAxis)
min -= add
max -= add
return max, min
}
// =====================================================================
//{array} result The an array of contact point objects, see clipFaceAgainstHull
func (ch *ConvexHull) ClipAgainstHull(chB *ConvexHull, posA, posB *math32.Vector3, quatA, quatB *math32.Quaternion, penAxis *math32.Vector3, minDist, maxDist float32) []collision.Contact {
var contacts []collision.Contact
// Invert penetration axis so it points from b to a
invPenAxis := penAxis.Clone().Negate()
// Find face of B that is closest (i.e. that is most aligned with the penetration axis)
closestFaceBidx := -1
dmax := math32.Inf(-1)
worldFaceNormalsB := chB.WorldFaceNormals()
for i, worldFaceNormal := range worldFaceNormalsB {
// Note - normals must be pointing out of the body so that they align with the penetration axis in the line below
d := worldFaceNormal.Dot(invPenAxis)
if d > dmax {
dmax = d
closestFaceBidx = i
}
}
// If found a closest face (sometimes we don't find one)
if closestFaceBidx >= 0 {
// Copy and transform face vertices to world coordinates
faces := chB.Faces()
worldClosestFaceB := ch.WorldFace(faces[closestFaceBidx], posB, quatB)
// Clip the closest world face of B to only the portion that is inside the hull of A
contacts = ch.clipFaceAgainstHull(posA, penAxis, quatA, worldClosestFaceB, minDist, maxDist)
}
return contacts
}
func (ch *ConvexHull) WorldFace(face [3]math32.Vector3, pos *math32.Vector3, quat *math32.Quaternion) [3]math32.Vector3 {
var result [3]math32.Vector3
result[0] = face[0]
result[1] = face[1]
result[2] = face[2]
result[0].ApplyQuaternion(quat).Add(pos)
result[1].ApplyQuaternion(quat).Add(pos)
result[2].ApplyQuaternion(quat).Add(pos)
return result
}
// Clip a face against a hull.
//@param {Array} worldVertsB1 An array of Vec3 with vertices in the world frame.
//@param Array result Array to store resulting contact points in. Will be objects with properties: point, depth, normal. These are represented in world coordinates.
func (ch *ConvexHull) clipFaceAgainstHull(posA, penAxis *math32.Vector3, quatA *math32.Quaternion, worldClosestFaceB [3]math32.Vector3, minDist, maxDist float32) []collision.Contact {
contacts := make([]collision.Contact, 0)
// Find the face of A with normal closest to the separating axis (i.e. that is most aligned with the penetration axis)
closestFaceAidx := -1
dmax := math32.Inf(-1)
worldFaceNormalsA := ch.WorldFaceNormals()
for i, worldFaceNormal := range worldFaceNormalsA {
// Note - normals must be pointing out of the body so that they align with the penetration axis in the line below
d := worldFaceNormal.Dot(penAxis)
if d > dmax {
dmax = d
closestFaceAidx = i
}
}
if closestFaceAidx < 0 {
// Did not find any closest face...
return contacts
}
//console.log("closest A: ",worldClosestFaceA);
// Get the face and construct connected faces
facesA := ch.Faces()
//worldClosestFaceA := n.WorldFace(facesA[closestFaceAidx], bodyA)
closestFaceA := facesA[closestFaceAidx]
connectedFaces := make([]int, 0) // indexes of the connected faces
for faceIdx := 0; faceIdx < len(facesA); faceIdx++ {
// Skip worldClosestFaceA
if faceIdx == closestFaceAidx {
continue
}
// Test that face has not already been added
for _, cfidx := range connectedFaces {
if cfidx == faceIdx {
continue
}
}
face := facesA[faceIdx]
// Loop through face vertices and see if any of them are also present in the closest face
// If a vertex is shared and this connected face hasn't been recorded yet - record and break inner loop
for pConnFaceVidx := 0; pConnFaceVidx < len(face); pConnFaceVidx++ {
var goToNextFace bool
// Test if face shares a vertex with closetFaceA - add it to connectedFaces if so and break out of both loops
for closFaceVidx := 0; closFaceVidx < len(closestFaceA); closFaceVidx++ {
if closestFaceA[closFaceVidx].Equals(&face[pConnFaceVidx]) {
connectedFaces = append(connectedFaces, faceIdx)
goToNextFace = true
break
}
}
if goToNextFace {
break
}
}
}
worldClosestFaceA := ch.WorldFace(closestFaceA, posA, quatA)
// DEBUGGING
//if n.debugging {
// //log.Error("CONN-FACES: %v", len(connectedFaces))
// for _, fidx := range connectedFaces {
// wFace := n.WorldFace(facesA[fidx], bodyA)
// ShowWorldFace(n.simulation.Scene(), wFace[:], &math32.Color{0.8, 0.8, 0.8})
// }
// //log.Error("worldClosestFaceA: %v", worldClosestFaceA)
// //log.Error("worldClosestFaceB: %v", worldClosestFaceB)
// ShowWorldFace(n.simulation.Scene(), worldClosestFaceA[:], &math32.Color{2, 0, 0})
// ShowWorldFace(n.simulation.Scene(), worldClosestFaceB[:], &math32.Color{0, 2, 0})
//}
clippedFace := make([]math32.Vector3, len(worldClosestFaceB))
for i, v := range worldClosestFaceB {
clippedFace[i] = v
}
// TODO port simplified loop to cannon.js once done and verified
// https://github.com/schteppe/cannon.js/issues/378
// https://github.com/TheRohans/cannon.js/commit/62a1ce47a851b7045e68f7b120b9e4ecb0d91aab#r29106924
// Iterate over connected faces and clip the planes associated with their normals
for _, cfidx := range connectedFaces {
connFace := facesA[cfidx]
connFaceNormal := worldFaceNormalsA[cfidx]
// Choose a vertex in the connected face and use it to find the plane constant
worldFirstVertex := connFace[0].Clone().ApplyQuaternion(quatA).Add(posA)
planeDelta := - worldFirstVertex.Dot(&connFaceNormal)
clippedFace = ch.clipFaceAgainstPlane(clippedFace, connFaceNormal.Clone(), planeDelta)
}
// Plot clipped face
//if n.debugging {
// log.Error("worldClosestFaceBClipped: %v", clippedFace)
// ShowWorldFace(n.simulation.Scene(), clippedFace, &math32.Color{0, 0, 2})
//}
closestFaceAnormal := worldFaceNormalsA[closestFaceAidx]
worldFirstVertex := worldClosestFaceA[0].Clone()//.ApplyQuaternion(quatA).Add(&posA)
planeDelta := -worldFirstVertex.Dot(&closestFaceAnormal)
// For each vertex in the clipped face resolve its depth (relative to the closestFaceA) and create a contact
for _, vertex := range clippedFace {
depth := closestFaceAnormal.Dot(&vertex) + planeDelta
// Cap distance
if depth <= minDist {
depth = minDist
}
if depth <= maxDist {
if depth <= 0 {
contacts = append(contacts, collision.Contact{
Point: vertex,
Normal: closestFaceAnormal,
Depth: depth,
})
}
}
}
return contacts
}
// clipFaceAgainstPlane clips the specified face against the back of the specified plane.
// This is used multiple times when finding the portion of a face of one convex hull that is inside another convex hull.
// planeNormal and planeConstant satisfy the plane equation n*x = p where n is the planeNormal and p is the planeConstant (and x is a point on the plane).
func (ch *ConvexHull) clipFaceAgainstPlane(face []math32.Vector3, planeNormal *math32.Vector3, planeConstant float32) []math32.Vector3 {
// inVertices are the verts making up the face of hullB
clippedFace := make([]math32.Vector3, 0)
// If not a face (if an edge or a vertex) - don't clip it
if len(face) < 2 {
return face
}
firstVertex := face[len(face)-1]
dotFirst := planeNormal.Dot(&firstVertex) + planeConstant
for vi := 0; vi < len(face); vi++ {
lastVertex := face[vi]
dotLast := planeNormal.Dot(&lastVertex) + planeConstant
if dotFirst < 0 { // Inside hull
if dotLast < 0 { // Start < 0, end < 0, so output lastVertex
clippedFace = append(clippedFace, lastVertex)
} else { // Start < 0, end >= 0, so output intersection
newv := firstVertex.Clone().Lerp(&lastVertex, dotFirst / (dotFirst - dotLast))
clippedFace = append(clippedFace, *newv)
}
} else { // Outside hull
if dotLast < 0 { // Start >= 0, end < 0 so output intersection and end
newv := firstVertex.Clone().Lerp(&lastVertex, dotFirst / (dotFirst - dotLast))
clippedFace = append(clippedFace, *newv)
clippedFace = append(clippedFace, lastVertex)
}
}
firstVertex = lastVertex
dotFirst = dotLast
}
return clippedFace
}