|
1
|
|
|
import math |
|
2
|
|
|
|
|
3
|
|
|
from gem import vector |
|
4
|
|
|
from gem import matrix |
|
5
|
|
|
|
|
6
|
|
|
class Circle(object): |
|
7
|
|
|
def __init__(self, center, radius): |
|
8
|
|
|
self.center = center |
|
9
|
|
|
self.radius = radius |
|
10
|
|
|
|
|
11
|
|
|
def getFurthestPoint(self, direction): |
|
12
|
|
|
if (direction is not vector.Vector(3)): |
|
13
|
|
|
direction.i_normalize() |
|
14
|
|
|
return self.center + direction + self.radius |
|
15
|
|
|
|
|
16
|
|
|
class Sphere(object): |
|
17
|
|
|
def __init__(self, center, radius): |
|
18
|
|
|
self.center = center |
|
19
|
|
|
self.radius = radius |
|
20
|
|
|
|
|
21
|
|
|
def getFurthestPoint(self, direction): |
|
22
|
|
|
if (direction is not vector.Vector(3)): |
|
23
|
|
|
direction.i_normalize() |
|
24
|
|
|
return self.center + direction + self.radius |
|
25
|
|
|
|
|
26
|
|
|
class Triangle(object): |
|
27
|
|
|
def __init__(self, center, b, h, rotation): |
|
28
|
|
|
self.center = center |
|
29
|
|
|
self.thirdDims = vector.Vector(3, data=[b / 3.0, h / 3.0, 0.0]) |
|
30
|
|
|
self.rotation = rotation |
|
31
|
|
|
|
|
32
|
|
|
# TO BE IMPLEMENTED |
|
33
|
|
|
# Sides of the triangle |
|
34
|
|
|
self.A = 0 |
|
35
|
|
|
self.B = 0 |
|
36
|
|
|
self.C = 0 |
|
37
|
|
|
|
|
38
|
|
|
# Reference Real Time Collision Detection by Christer Ericson |
|
39
|
|
|
# Calculate the 2D area of a given triangle |
|
40
|
|
|
def triArea2D(self, x1, y1, x2, y2, x3, y3): |
|
41
|
|
|
return (x1 - x2)*(y2 - y3) - (x2 - x3) * (y1 - y2) |
|
42
|
|
|
|
|
43
|
|
|
# Compute barycentric coordinates (u, v, w) for |
|
44
|
|
|
# Point P with respect to triangle (a, b, c) |
|
45
|
|
|
def barycentric(self, a, b, c, p): |
|
46
|
|
|
#Unnormalized triangle normal |
|
47
|
|
|
m = (b - a).cross(c - a) |
|
48
|
|
|
|
|
49
|
|
|
#Nominators and one-over-denominators for u and v ratios |
|
50
|
|
|
nu = 0 |
|
51
|
|
|
nv = 0 |
|
52
|
|
|
ood = 0 |
|
53
|
|
|
|
|
54
|
|
|
# Absolute components for determining projection plane |
|
55
|
|
|
x = math.abs(m[0]) |
|
56
|
|
|
y = math.abs(m[1]) |
|
57
|
|
|
z = math.abs(m[2]) |
|
58
|
|
|
|
|
59
|
|
|
if x >= y and x >= z: |
|
60
|
|
|
# x is largest, project to the yz plane |
|
61
|
|
|
nu = self.triArea2D(p[1], p[2], b[1], b[2], c[1], c[2]) # Area of PBC in yz plane |
|
62
|
|
|
nv = self.triArea2D(p[1], p[2], c[1], c[2], a[1], a[2]) # Area of PCA in yz plane |
|
63
|
|
|
ood = 1.0 / m[0] # 1 / (2 * area of ABC in yz plane) |
|
64
|
|
|
elif y >= x and y >= z: |
|
65
|
|
|
# y is largest, project to the xz plane |
|
66
|
|
|
nu = self.triArea2D(p[0], p[2], b[0], b[2], c[0], c[2]) # Area of PBC in xz plane |
|
67
|
|
|
nv = self.triArea2D(p[0], p[2], c[0], c[2], a[0], a[2]) # Area of PCA in xz plane |
|
68
|
|
|
ood = 1.0 / -m[1] # 1 / (2 * area of ABC in xz plane) |
|
69
|
|
|
else: |
|
70
|
|
|
# z is largest, project to the xy plane |
|
71
|
|
|
nu = self.triArea2D(p[0], p[1], b[0], b[1], c[0], c[1]) # Area of PBC in xy plane |
|
72
|
|
|
nv = self.triArea2D(p[0], p[1], c[0], c[1], a[0], a[1]) # Area of PCA in xy plane |
|
73
|
|
|
ood = 1.0 / m[2] |
|
74
|
|
|
|
|
75
|
|
|
# Barycentric coordinates |
|
76
|
|
|
u = nu * ood |
|
77
|
|
|
v = nv * ood |
|
78
|
|
|
w = 1.0 - u - v |
|
79
|
|
|
|
|
80
|
|
|
return vector.Vector(3, data=[u, v, w]) |
|
81
|
|
|
|
|
82
|
|
|
# Test if point p is contained in triangle (a, b, c) |
|
83
|
|
|
def testPointTriangle(self, a, b, c, p): |
|
84
|
|
|
bV = self.barycentric(a, b, c, p) |
|
85
|
|
|
return (bV[1] >= 0.0) and (bV[2] >= 0.0) and ((bV[1] + bV[2]) <= 1.0) |
|
86
|
|
|
|
|
87
|
|
|
def getFurthestPoint(self, direction): |
|
88
|
|
|
midBase = vector.Vector(3).right() * self.thirdDims.vector[0] |
|
89
|
|
|
midHeight = vector.Vector(3).up() * self.thirdDims.vector[1] |
|
90
|
|
|
|
|
91
|
|
|
direction4 = vector.Vector(4, data=[direction.vector[0], direction.vector[1], direction.vector[2], 1.0]) |
|
92
|
|
|
|
|
93
|
|
|
vertices = [] |
|
94
|
|
|
vertices.append( self.center.vector[1], midHeight) |
|
95
|
|
|
vertices.append( midBase, -midHeight) |
|
96
|
|
|
vertices.append(-midBase, -midHeight) |
|
97
|
|
|
|
|
98
|
|
|
translation = matrix.Matrix(4).translate(vector.Vector(4, data=[self.center.vector[0], self.center.vector[1], self.center.vector[2], 1.0])) |
|
99
|
|
|
world = self.rotation * translation |
|
100
|
|
|
|
|
101
|
|
|
furtherPoint = world * vector.Vector(4, data=[vertices[0].vector[0], vertices[0].vector[1], vertices[0].vector[2], 1.0]) |
|
102
|
|
|
|
|
103
|
|
|
maxDot = furtherPoint.dot(direction4) |
|
104
|
|
|
for i in range(1, 3, 1): |
|
105
|
|
|
vertex = world * vector.vector(4, data=[vertices[i].vector[0], vertices[i].vector[1], vertices[i].vector[2], 1.0]) |
|
106
|
|
|
dot = vertex.dot(direction4) |
|
107
|
|
|
if(dot > maxDot): |
|
108
|
|
|
maxDot = dot |
|
109
|
|
|
furtherPoint = vertex |
|
110
|
|
|
return vector.Vector(3, data=[furtherPoint.vector[0], furtherPoint.vector[1], furtherPoint.vector[2]]) |
|
111
|
|
|
|
|
112
|
|
|
class Rectangle(object): |
|
113
|
|
|
def __init__(self, center, w, h, rotation): |
|
114
|
|
|
self.center = center |
|
115
|
|
|
self.halfDims = vector.Vector(3, data=[w / 2.0, h / 2.0, 0.0]) |
|
116
|
|
|
self.rotation = rotation |
|
117
|
|
|
|
|
118
|
|
|
def getFurthestPoint(self, direction): |
|
119
|
|
|
halfWidth = vector.Vector(3).right() * self.halfDims.vector[0] |
|
120
|
|
|
halfHeight = vector.Vector(3).up() * self.halfDims.vector[1] |
|
121
|
|
|
|
|
122
|
|
|
direction4 = vector.Vector(4, data=[direction.vector[0], direction.vector[1], direction.vector[2], 1.0]) |
|
123
|
|
|
|
|
124
|
|
|
vertices = [] |
|
125
|
|
|
vertices.append( halfWidth + halfHeight) |
|
126
|
|
|
vertices.append(-halfWidth + halfHeight) |
|
127
|
|
|
vertices.append(-halfWidth - halfHeight) |
|
128
|
|
|
vertices.append( halfWidth - halfHeight) |
|
129
|
|
|
|
|
130
|
|
|
translation = matrix.Matrix(4).translate(vector.Vector(4, data=[self.center.vector[0], self.center.vector[1], self.center.vector[2], 1.0])) |
|
131
|
|
|
world = self.rotation * translation |
|
132
|
|
|
|
|
133
|
|
|
furtherPoint = world * vector.Vector(4, data=[vertices[0].vector[0], vertices[0].vector[1], vertices[0].vector[2], 1.0]) |
|
134
|
|
|
|
|
135
|
|
|
maxDot = furtherPoint.dot(direction4) |
|
136
|
|
|
for i in range(1, 4, 1): |
|
137
|
|
|
vertex = world * vector.Vector(4, data=[vertices[i].vector[0], vertices[i].vector[1], vertices[i].vector[2], 1.0]) |
|
138
|
|
|
dot = vertex.dot(direction4) |
|
139
|
|
|
if(dot > maxDot): |
|
140
|
|
|
maxDot = dot |
|
141
|
|
|
furtherPoint = vertex |
|
142
|
|
|
return vector.Vector(3, data=[furtherPoint.vector[0], furtherPoint.vector[1], furtherPoint.vector[2]]) |
|
143
|
|
|
|
|
144
|
|
|
class Box(object): |
|
145
|
|
|
def __init__(self, center, w, h ,d , rotation): |
|
146
|
|
|
self.center = center |
|
147
|
|
|
self.halfDims = vector.Vector(3, data=[w / 2.0, h / 2.0, d / 2.0]) |
|
148
|
|
|
self.rotation = rotation |
|
149
|
|
|
|
|
150
|
|
|
def getFurthestPoint(self, direction): |
|
151
|
|
|
halfWidth = vector.Vector(3).right() * self.halfDims.vector[0] |
|
152
|
|
|
halfHeight = vector.Vector(3).up() * self.halfDims.vector[1] |
|
153
|
|
|
halfDepth = vector.Vector(3).back() * self.halfDims.vector[2] |
|
154
|
|
|
|
|
155
|
|
|
direction4 = vector.Vector(4, data=[direction.vector[0], direction.vector[1], direction.vector[2], 1.0]) |
|
156
|
|
|
|
|
157
|
|
|
vertices = [] |
|
158
|
|
|
vertices.append(halfWidth + halfHeight + halfDepth) |
|
159
|
|
|
vertices.append(-halfWidth + halfHeight + halfDepth) |
|
160
|
|
|
vertices.append(halfWidth - halfHeight + halfDepth) |
|
161
|
|
|
vertices.append(halfWidth + halfHeight - halfDepth) |
|
162
|
|
|
vertices.append(-halfWidth - halfHeight + halfDepth) |
|
163
|
|
|
vertices.append(halfWidth - halfHeight - halfDepth) |
|
164
|
|
|
vertices.append(-halfWidth + halfHeight - halfDepth) |
|
165
|
|
|
vertices.append(-halfWidth - halfHeight - halfDepth) |
|
166
|
|
|
|
|
167
|
|
|
translation = matrix.Matrix(4).translate(vector.Vector(4, data=[self.center.vector[0], self.center.vector[1], self.center.vector[2], 1.0])) |
|
168
|
|
|
world = self.rotation * translation |
|
169
|
|
|
|
|
170
|
|
|
furtherPoint = world * vector.Vector(4, data=[vertices[0].vector[0], vertices[0].vector[1], vertices[0].vector[2], 1.0]) |
|
171
|
|
|
|
|
172
|
|
|
maxDot = furtherPoint.dot(direction4) |
|
173
|
|
|
for i in range(1, 8, 1): |
|
174
|
|
|
vertex = world * vector.Vector(4, data=[vertices[i].vector[0], vertices[i].vector[1], vertices[i].vector[2], 1.0]) |
|
175
|
|
|
dot = vertex.dot(direction4) |
|
176
|
|
|
if(dot > maxDot): |
|
177
|
|
|
maxDot = dot |
|
178
|
|
|
furtherPoint = vertex |
|
179
|
|
|
return vector.Vector(3, data=[furtherPoint.vector[0], furtherPoint.vector[1], furtherPoint.vector[2]]) |
|
180
|
|
|
|
explosiveduck /
ed2d
