2
# Copyright (C) 2004 Jean-Baptiste LAMY -- jiba@tuxfamily.org
4
# This program is free software; you can redistribute it and/or modify
5
# it under the terms of the GNU General Public License as published by
6
# the Free Software Foundation; either version 2 of the License, or
7
# (at your option) any later version.
9
# This program is distributed in the hope that it will be useful,
10
# but WITHOUT ANY WARRANTY; without even the implied warranty of
11
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12
# GNU General Public License for more details.
14
# You should have received a copy of the GNU General Public License
15
# along with this program; if not, write to the Free Software
16
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18
# A collider for a Land
20
cdef class _Land(GeomObject):
21
"""Landscape/terrain collider for Soya. This is based on Benoit Chaperot's
22
contributed terrain collider for ODE, with some changes to take advantage
23
of precomputed normals in Soya's landscape engine.
27
We loop through each "cell," or square of vertices in the heightfield,
28
that is underneath the axis-aligned bounding box (AABB) of the geom we're
29
testing for collisions with. For rendering, each cell is divided into
30
two triangles. We use ray collision to check each edge of one of the
31
triangles, because the other triangle shares edges with adjacent cells
32
and we don't want to check edges twice. We make a ray for each direction
33
along the edge to get two collision points so we can take the midpoint
34
of the two collision points if there is a collision. Right now the normal
35
is halfway between the normals computed by Soya for the vertices making
36
the edge being tested. This could be weighted, but we don't bother right
37
now and it seems to work fine.
39
Next, we test the plane of each triangle. We use ODE's own plane collision
40
routines to give us a set of collision points on the plane. We then test
41
to see if any of the collision points are within the triangle we're testing
42
by first seeing if the point is within the cell (a simple range check),
43
then testing if it's in the correct isoceles right triangle, which means
44
just looking at the sum of the x and z deltas from the upper left vertex
45
of the cell. If it's less than the length of one leg, it's in the upper
46
triangle. If it's greater, it's in the lower. We keep the normal from any
47
plane contact points we keep.
49
Ray collision is not currently implemented, but could easily be handled
50
using Soya's own raypicking routines. Likewise plane collision, which
51
might be able to use the view frustum culling routines. I don't currently
52
plan to implement these because I have ambitions about doing all collision
53
detection within Soya rather than using ODE's collision detection.
56
cdef _soya._Land _land
58
cdef void _get_aabb(self, dReal aabb[6]):
59
"""Get the axis-aligned bounding box.
60
This is slow and it's fortunate that lands aren't currently considered
61
placeable so it only gets called once. If we want lands to be
62
placeable, we need to cache the information and invalidate it
63
if the land is moved. This may require going back to subclassing
64
_soya._Land, or coming up with a hook mechanism for getting notified
65
when a coordsyst is updated."""
68
cdef float min_x, max_x, min_y, max_y, min_z, max_z
70
cdef GLfloat m[19], P[3]
74
# Calculate the true AABB
75
# Make sure it's not getting called a lot
76
print "Calculating AABB for land (slow)"
78
# Get the matrix for transforming points to world coordinates
79
_soya.multiply_matrix(m, land._root_matrix(), self.world._inverted_root_matrix())
81
# Set up the minima and maxima
82
_soya.point_by_matrix_copy(P, land._vertices[0].coord, m)
90
for i from 1 <= i < land._nb_vertex_width * land._nb_vertex_depth:
91
# Transform each vertex to world coordinates
92
_soya.point_by_matrix_copy(P, land._vertices[i].coord, m)
118
cdef int _collide_cell(self, int x, int z, dGeomID o1,
119
dGeomID o2, int max_contacts, int flags,
120
dContactGeom *contact, int skip):
121
"""Test for any collisions within a single cell of the heightfield
124
cdef _soya.LandVertex *vA, *vB, *vC, *vD
125
cdef GLfloat A[3], B[3], C[3], D[3], NB[3], NC[3], ND[3]
126
cdef GLfloat AB[3], AC[3], BC[3], BD[3], CD[3]
127
cdef GLfloat plane[4]
128
cdef int num_contacts, numPlaneContacts
129
cdef dContactGeom ContactA[3], ContactB[3], *pContact
130
cdef dContactGeom planeContact[10]
131
cdef GLfloat m[19] # Matrix to convert to world coordinates
132
cdef _soya._Land land
138
_soya.multiply_matrix(m, land._root_matrix(), self.world._inverted_root_matrix())
140
# Get the four vertices for this cell
141
# Pointer arithmetic is evil.
142
vA = land._vertices + (x + z * land._nb_vertex_width)
144
vC = vA + land._nb_vertex_width
145
vD = vB + land._nb_vertex_width
147
# Hmmm, already have normals calculated for both
148
# vertices *and* triangles. This could be good.
150
# Transform each vertex to the world's coordsys
151
_soya.point_by_matrix_copy(A, vA.coord, m)
152
_soya.point_by_matrix_copy(B, vB.coord, m)
153
_soya.point_by_matrix_copy(C, vC.coord, m)
154
_soya.point_by_matrix_copy(D, vD.coord, m)
156
# Transform the normals to the world's coordsys
157
_soya.point_by_matrix_copy(NB, vB.normal, m)
158
_soya.point_by_matrix_copy(NC, vC.normal, m)
159
_soya.point_by_matrix_copy(ND, vD.normal, m)
161
# Renormalize the normals. This is only needed if we allow the land
162
# to be scaled by Land.scale(fx, fy, fz)
163
_soya.vector_normalize(NB)
164
_soya.vector_normalize(NC)
165
_soya.vector_normalize(ND)
168
# Make all the vectors we need
169
_soya.vector_from_points(AB, A, B)
170
_soya.vector_normalize(AB)
171
_soya.vector_from_points(AC, A, C)
172
_soya.vector_normalize(AC)
173
_soya.vector_from_points(BC, B, C)
174
_soya.vector_normalize(BC)
175
_soya.vector_from_points(BD, B, D)
176
_soya.vector_normalize(BD)
177
_soya.vector_from_points(CD, C, D)
178
_soya.vector_normalize(CD)
180
# Ray collision to test edges of one triangle
181
# Don't need to test the other because adjacent cells will
182
num_contacts = num_contacts + collide_edge(B, C, BC, NB,
183
NC, o1, o2, max_contacts - num_contacts, flags,
184
<dContactGeom*>(<char*>contact + (num_contacts * skip)))
186
if num_contacts == max_contacts:
189
num_contacts = num_contacts + collide_edge(B, D, BD, NB,
190
ND, o1, o2, max_contacts - num_contacts, flags,
191
<dContactGeom*>(<char*>contact + (num_contacts * skip)))
193
if num_contacts == max_contacts:
196
num_contacts = num_contacts + collide_edge(C, D, CD, NC,
197
ND, o1, o2, max_contacts - num_contacts, flags,
198
<dContactGeom*>(<char*>contact + (num_contacts * skip)))
200
if num_contacts == max_contacts:
203
# Now do planes for each of the two triangle planes
204
# XXX If the plane test fails, we could skip the ray tests.
205
_soya.vector_cross_product(plane, AC, AB)
206
#_soya.vector_normalize(plane)
207
plane[3] = plane[0] * A[0] + plane[1] * A[1] + plane[2] * A[2]
208
dGeomPlaneSetParams(_land_plane, plane[0], plane[1], plane[2], plane[3])
210
numPlaneContacts = dCollide(o2, _land_plane, flags, planeContact, sizeof(dContactGeom))
212
#print "a", numPlaneContacts
214
for i from 0 <= i < numPlaneContacts:
215
# Figure out if the point is in the triangle.
216
# Only need to test x and z coord because we already know it's
217
# in the plane and the plane is not vertical
219
# First, check if it's in the cell
220
if planeContact[i].pos[0] < A[0] or planeContact[i].pos[0] > D[0]:
223
if planeContact[i].pos[2] < A[2] or planeContact[i].pos[2] > D[2]:
226
# Now, check if it's in the correct triangle in the cell.
227
# This is made easier since we only support isoceles right triangles
228
if (planeContact[i].pos[0] - A[0] + planeContact[i].pos[2] - A[2]) > (D[0] - A[0]):
231
# It's in the triangle, so add it to our list of contacts
232
pContact = <dContactGeom*>((<char*>contact) + (num_contacts * skip))
233
#print <long>contact, <long>pContact, num_contacts, skip
234
pContact.pos[0] = planeContact[i].pos[0]
235
pContact.pos[1] = planeContact[i].pos[1]
236
pContact.pos[2] = planeContact[i].pos[2]
237
pContact.normal[0] = -planeContact[i].normal[0]
238
pContact.normal[1] = -planeContact[i].normal[1]
239
pContact.normal[2] = -planeContact[i].normal[2]
240
pContact.depth = planeContact[i].depth
244
##print "a", pContact.pos[0], pContact.pos[1], pContact.pos[2], pContact.normal[0], pContact.normal[1], pContact.normal[2], pContact.depth
245
num_contacts = num_contacts + 1
247
if num_contacts == max_contacts:
250
# Try the plane for the other triangle
251
_soya.vector_cross_product(plane, BD, CD)
252
_soya.vector_normalize(plane)
253
plane[3] = plane[0] * D[0] + plane[1] * D[1] + plane[2] * D[2]
254
dGeomPlaneSetParams(_land_plane, plane[0], plane[1], plane[2], plane[3])
256
numPlaneContacts = dCollide(o2, _land_plane, flags, planeContact, sizeof(dContactGeom))
258
#print "b", numPlaneContacts
260
for i from 0 <= i < numPlaneContacts:
261
# Figure out if the point is in the triangle.
262
# Only need to test x and z coord because we already know it's
263
# in the plane and the plane is not vertical
265
# First, check if it's in the cell
266
if planeContact[i].pos[0] < A[0] or planeContact[i].pos[0] > D[0]:
269
if planeContact[i].pos[2] < A[2] or planeContact[i].pos[2] > D[2]:
272
# Now, check if it's in the correct triangle in the cell.
273
# This is made easier since we only support isoceles right triangles
274
if (planeContact[i].pos[0] - A[0] + planeContact[i].pos[2] - A[2]) < (D[0] - A[0]):
277
# It's in the triangle, so add it to our list of contacts
278
pContact = <dContactGeom*>((<char*>contact) + (num_contacts * skip))
279
#print <long>contact, <long>pContact, num_contacts, skip
280
pContact.pos[0] = planeContact[i].pos[0]
281
pContact.pos[1] = planeContact[i].pos[1]
282
pContact.pos[2] = planeContact[i].pos[2]
283
pContact.normal[0] = -planeContact[i].normal[0]
284
pContact.normal[1] = -planeContact[i].normal[1]
285
pContact.normal[2] = -planeContact[i].normal[2]
286
pContact.depth = planeContact[i].depth
290
#print "b", pContact.pos[0], pContact.pos[1], pContact.pos[2], pContact.normal[0], pContact.normal[1], pContact.normal[2], pContact.depth
292
num_contacts = num_contacts + 1
294
if num_contacts == max_contacts:
300
cdef int _collide(self, dGeomID o1, dGeomID o2, int flags,
301
dContactGeom *contact, int skip):
302
cdef int num_contacts, max_contacts, x, z, i, j, k
303
cdef GLfloat min_x, max_x, min_z, max_z
304
cdef dReal aabb[6], depth
305
cdef dContactGeom *pContact
306
cdef dVector3 lengths
308
cdef _soya.LandVertex *vA, *vB, *vC, *vD
309
cdef GLfloat m[19] # Matrix to convert to land's coordinate system
310
cdef GLfloat BC, BD, CD
311
cdef GLfloat plane[4]
312
cdef _soya._Land land
313
cdef GLfloat P[3] # Point for converting AABB corners
317
# Get the maximum number of contacts from the flags
318
max_contacts = (flags & 0xffff) or 1
320
# Only 10 contacts allowed for called collision functions
321
flags = (flags & 0xffff0000) | 10
323
#R = dGeomGetRotation(o2)
325
# Get the AAAB of the geom
326
dGeomGetAABB(o2, aabb)
328
# Convert the AABB to the land's coordinate system. XXX this is slower
329
# than if we could assume the lands' axes were aligned with the
330
# world's axes, but we can't.
333
_soya.multiply_matrix(m, self.world._root_matrix(), land._inverted_root_matrix())
339
_soya.point_by_matrix(P, m)
341
# Only care about x and z axes
347
# Do the other seven corners
348
for i, j, k in ((1, 2, 4), (0, 3, 4), (1, 3, 4), (0, 2, 5), (1, 2, 5), (0, 3, 5), (1, 3, 5)):
353
_soya.point_by_matrix(P, m)
365
# Test all cells that are under the AABB
366
for z from <int>floor(max(0, min_z)) <= z < <int>ceil(min(land._nb_vertex_depth, max_z)):
367
for x from <int>floor(max(0, min_x)) <= x < <int>ceil(min(land._nb_vertex_width, max_x)):
368
num_contacts = num_contacts + self._collide_cell(x, z, o1,
369
o2, max_contacts - num_contacts, flags,
370
<dContactGeom*>((<char*>contact) + (num_contacts*skip)),
375
def __new__(self, _soya._Land land, SpaceBase space=None):
377
self.gid = dCreateGeom(dLandClass)
379
if space is not None:
380
dSpaceAdd(space.sid, self.gid)
382
def __init__(self, _soya._Land land, SpaceBase space=None):
383
GeomObject.__init__(self, land._parent, space)
385
def __dealloc__(self):
386
print "dealloc", self
393
"""Land can't have a body, so return environment"""
397
cdef dGeomID _land_plane # Reusable plane geom
398
cdef dGeomID _land_ray # Reusable ray geom
400
_land_plane = dCreatePlane(NULL, 0.0, 1.0, 0.0, 0.0)
401
_land_ray = dCreateRay(NULL, 1.0)
403
cdef void LandGetAABB(dGeomID geom, dReal aabb[6]):
405
land = <_Land>dGeomGetData(geom)
408
cdef int LandCollide(dGeomID o1, dGeomID o2, int flags,
409
dContactGeom *contact, int skip):
411
land = <_Land>dGeomGetData(o1)
412
return land._collide(o1, o2, flags, contact, skip)
415
cdef dColliderFn * LandGetColliderFn(int gclass):
417
if gclass in (dSphereClass, dBoxClass, dCCylinderClass):
423
cdef int LandAABBTest(dGeomID o1, dGeomID o2, dReal aabb2[6]):
427
cdef dGeomClass dLandGeomClass
429
dLandGeomClass.bytes = 0
430
dLandGeomClass.collider = LandGetColliderFn
431
dLandGeomClass.aabb = LandGetAABB
432
dLandGeomClass.aabb_test = NULL # Need to write this function
433
dLandGeomClass.dtor = NULL
437
dLandClass = dCreateGeomClass(&dLandGeomClass)
added
removed
ode/land.pyx
