blob: 71ccee31ac1b692a3e73aeab735981a3842b5d78 (
plain) (
blame)
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
|
package sims.collision.narrowphase
package gjk
import sims.collision._
import sims.math._
import scala.collection.mutable.ListBuffer
class GJK[A <: Collidable: ClassManifest] extends narrowphase.NarrowPhaseDetector[A] {
def penetration(pair: (A, A)): Option[Penetration] = {
val ms = new MinkowskiSum(pair)
val s = ms.support(Vector2D.i)
val simplex = new ListBuffer[Vector2D]
simplex prepend s
var direction = -s
while (true) {
val a = ms.support(direction)
if ((a dot direction) < 0) return None
simplex prepend a
val newDirection = checkSimplex(simplex, direction)
if (newDirection == null) return Some(EPA.penetration(simplex, ms))
else direction = newDirection
}
throw new IllegalArgumentException("Something went wrong, should not reach here.")
}
/** Checks whether the given simplex contains the origin. If it does, `null` is returned.
* Otherwise a new search direction is returned and the simplex is updated. */
private def checkSimplex(simplex: ListBuffer[Vector2D], direction: Vector2D): Vector2D = {
if (simplex.length == 2) { //simplex == 2
val a = simplex(0)
val b = simplex(1)
val ab = b - a
val ao = -a
if (ao directionOf ab) {
ab cross ao cross ab
} else {
simplex.remove(1)
ao
}
} // end simplex == 2
else if (simplex.length == 3) { //simplex == 3
val a = simplex(0)
val b = simplex(1)
val c = simplex(2)
val ab = b - a
val ac = c - a
val ao = -a
val winding = ab cross ac
if (ao directionOf (ab cross winding)) {
if (ao directionOf ab) {
simplex.remove(2)
ab cross ao cross ab
} else if (ao directionOf ac) {
simplex.remove(1)
ac cross ao cross ac
} else {
simplex.remove(2)
simplex.remove(1)
ao
}
} else {
if (ao directionOf (winding cross ac)) {
if (ao directionOf ac) {
simplex.remove(1)
ac cross ao cross ac
} else {
simplex.remove(2)
simplex.remove(1)
ao
}
} else {
null
}
}
} //end simplex == 3
else throw new IllegalArgumentException("Invalid simplex size.")
}
def collision(pair: (A, A)): Option[Collision[A]] = {
val p = penetration(pair)
if (p.isEmpty) return None
val manif = CS.getCollisionPoints(pair, p.get.normal)
Some(new Collision[A] {
val item1 = pair._1
val item2 = pair._2
val normal = manif.normal
val overlap = p.get.overlap
val points = manif.points
})
}
}
object GJK {
}
|
