|
1 /************************************************************************* |
|
2 * * |
|
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * |
|
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org * |
|
5 * * |
|
6 * This library is free software; you can redistribute it and/or * |
|
7 * modify it under the terms of EITHER: * |
|
8 * (1) The GNU Lesser General Public License as published by the Free * |
|
9 * Software Foundation; either version 2.1 of the License, or (at * |
|
10 * your option) any later version. The text of the GNU Lesser * |
|
11 * General Public License is included with this library in the * |
|
12 * file LICENSE.TXT. * |
|
13 * (2) The BSD-style license that is included with this library in * |
|
14 * the file LICENSE-BSD.TXT. * |
|
15 * * |
|
16 * This library is distributed in the hope that it will be useful, * |
|
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
|
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
|
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
|
20 * * |
|
21 *************************************************************************/ |
|
22 |
|
23 /* |
|
24 |
|
25 core collision functions and data structures, plus part of the public API |
|
26 for geometry objects |
|
27 |
|
28 */ |
|
29 #ifndef __WINSCW__ |
|
30 #include <e32std.h> |
|
31 #endif // !__WINSCW__ |
|
32 |
|
33 #include <ode/common.h> |
|
34 #include <ode/matrix.h> |
|
35 #include <ode/rotation.h> |
|
36 #include <ode/objects.h> |
|
37 #include <ode/odemath.h> |
|
38 #include "collision_kernel.h" |
|
39 #include "collision_util.h" |
|
40 #include "collision_std.h" |
|
41 #include "collision_transform.h" |
|
42 #include <ode/ode.h> |
|
43 |
|
44 //**************************************************************************** |
|
45 // helper functions for dCollide()ing a space with another geom |
|
46 |
|
47 // this struct records the parameters passed to dCollideSpaceGeom() |
|
48 |
|
49 // Allocate and free posr - we cache a single posr to avoid thrashing |
|
50 dxPosR* dAllocPosr() |
|
51 { |
|
52 dxPosR* retPosR; |
|
53 if (GetGlobalData()->s_cachedPosR) |
|
54 { |
|
55 retPosR = GetGlobalData()->s_cachedPosR; |
|
56 GetGlobalData()->s_cachedPosR = 0; |
|
57 } |
|
58 else |
|
59 { |
|
60 retPosR = (dxPosR*) dAlloc (sizeof(dxPosR)); |
|
61 } |
|
62 return retPosR; |
|
63 } |
|
64 |
|
65 void dFreePosr(dxPosR* oldPosR) |
|
66 { |
|
67 if (oldPosR) |
|
68 { |
|
69 if (GetGlobalData()->s_cachedPosR) |
|
70 { |
|
71 dFree(GetGlobalData()->s_cachedPosR, sizeof(dxPosR)); |
|
72 } |
|
73 GetGlobalData()->s_cachedPosR = oldPosR; |
|
74 } |
|
75 } |
|
76 |
|
77 void dClearPosrCache(void) |
|
78 { |
|
79 if (GetGlobalData()->s_cachedPosR) |
|
80 { |
|
81 dFree(GetGlobalData()->s_cachedPosR, sizeof(dxPosR)); |
|
82 GetGlobalData()->s_cachedPosR = 0; |
|
83 } |
|
84 } |
|
85 |
|
86 struct SpaceGeomColliderData { |
|
87 int flags; // space left in contacts array |
|
88 dContactGeom *contact; |
|
89 int skip; |
|
90 }; |
|
91 |
|
92 |
|
93 static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2) |
|
94 { |
|
95 SpaceGeomColliderData *d = (SpaceGeomColliderData*) data; |
|
96 if (d->flags & NUMC_MASK) { |
|
97 int n = dCollide (o1,o2,d->flags,d->contact,d->skip); |
|
98 d->contact = CONTACT (d->contact,d->skip*n); |
|
99 d->flags -= n; |
|
100 } |
|
101 } |
|
102 |
|
103 |
|
104 static int dCollideSpaceGeom (dxGeom *o1, dxGeom *o2, int flags, |
|
105 dContactGeom *contact, int skip) |
|
106 { |
|
107 SpaceGeomColliderData data; |
|
108 data.flags = flags; |
|
109 data.contact = contact; |
|
110 data.skip = skip; |
|
111 dSpaceCollide2 (o1,o2,&data,&space_geom_collider); |
|
112 return (flags & NUMC_MASK) - (data.flags & NUMC_MASK); |
|
113 } |
|
114 |
|
115 //**************************************************************************** |
|
116 // dispatcher for the N^2 collider functions |
|
117 |
|
118 |
|
119 // setCollider() will refuse to write over a collider entry once it has |
|
120 // been written. |
|
121 |
|
122 static void setCollider (int i, int j, dColliderFn *fn) |
|
123 { |
|
124 if (GetGlobalData()->colliders[i][j].fn == 0) { |
|
125 GetGlobalData()->colliders[i][j].fn = fn; |
|
126 GetGlobalData()->colliders[i][j].reverse = 0; |
|
127 } |
|
128 if (GetGlobalData()->colliders[j][i].fn == 0) { |
|
129 GetGlobalData()->colliders[j][i].fn = fn; |
|
130 GetGlobalData()->colliders[j][i].reverse = 1; |
|
131 } |
|
132 } |
|
133 |
|
134 |
|
135 static void setAllColliders (int i, dColliderFn *fn) |
|
136 { |
|
137 for (int j=0; j<dGeomNumClasses; j++) setCollider (i,j,fn); |
|
138 } |
|
139 |
|
140 |
|
141 static void initColliders() |
|
142 { |
|
143 int i,j; |
|
144 |
|
145 if (GetGlobalData()->colliders_initialized) return; |
|
146 GetGlobalData()->colliders_initialized = 1; |
|
147 |
|
148 memset (GetGlobalData()->colliders,0,sizeof(GetGlobalData()->colliders)); |
|
149 |
|
150 // setup space colliders |
|
151 for (i=dFirstSpaceClass; i <= dLastSpaceClass; i++) { |
|
152 for (j=0; j < dGeomNumClasses; j++) { |
|
153 setCollider (i,j,&dCollideSpaceGeom); |
|
154 } |
|
155 } |
|
156 |
|
157 setCollider (dSphereClass,dSphereClass,&dCollideSphereSphere); |
|
158 setCollider (dSphereClass,dBoxClass,&dCollideSphereBox); |
|
159 setCollider (dSphereClass,dPlaneClass,&dCollideSpherePlane); |
|
160 setCollider (dBoxClass,dBoxClass,&dCollideBoxBox); |
|
161 setCollider (dBoxClass,dPlaneClass,&dCollideBoxPlane); |
|
162 setCollider (dCapsuleClass,dSphereClass,&dCollideCapsuleSphere); |
|
163 setCollider (dCapsuleClass,dBoxClass,&dCollideCapsuleBox); |
|
164 setCollider (dCapsuleClass,dCapsuleClass,&dCollideCapsuleCapsule); |
|
165 setCollider (dCapsuleClass,dPlaneClass,&dCollideCapsulePlane); |
|
166 setCollider (dRayClass,dSphereClass,&dCollideRaySphere); |
|
167 setCollider (dRayClass,dBoxClass,&dCollideRayBox); |
|
168 setCollider (dRayClass,dCapsuleClass,&dCollideRayCapsule); |
|
169 setCollider (dRayClass,dPlaneClass,&dCollideRayPlane); |
|
170 setCollider (dRayClass,dCylinderClass,&dCollideRayCylinder); |
|
171 setCollider (dCylinderClass,dBoxClass,&dCollideCylinderBox); |
|
172 setCollider (dCylinderClass,dSphereClass,&dCollideCylinderSphere); |
|
173 setCollider (dCylinderClass,dPlaneClass,&dCollideCylinderPlane); |
|
174 //setCollider (dCylinderClass,dCylinderClass,&dCollideCylinderCylinder); |
|
175 |
|
176 //--> Convex Collision |
|
177 setCollider (dConvexClass,dPlaneClass,&dCollideConvexPlane); |
|
178 setCollider (dSphereClass,dConvexClass,&dCollideSphereConvex); |
|
179 setCollider (dConvexClass,dBoxClass,&dCollideConvexBox); |
|
180 setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsule); |
|
181 setCollider (dConvexClass,dConvexClass,&dCollideConvexConvex); |
|
182 setCollider (dRayClass,dConvexClass,&dCollideRayConvex); |
|
183 //<-- Convex Collision |
|
184 |
|
185 //--> dHeightfield Collision |
|
186 setCollider (dHeightfieldClass,dRayClass,&dCollideHeightfield); |
|
187 setCollider (dHeightfieldClass,dSphereClass,&dCollideHeightfield); |
|
188 setCollider (dHeightfieldClass,dBoxClass,&dCollideHeightfield); |
|
189 setCollider (dHeightfieldClass,dCapsuleClass,&dCollideHeightfield); |
|
190 setCollider (dHeightfieldClass,dCylinderClass,&dCollideHeightfield); |
|
191 setCollider (dHeightfieldClass,dConvexClass,&dCollideHeightfield); |
|
192 #if dTRIMESH_ENABLED |
|
193 setCollider (dHeightfieldClass,dTriMeshClass,&dCollideHeightfield); |
|
194 #endif |
|
195 //<-- dHeightfield Collision |
|
196 |
|
197 setAllColliders (dGeomTransformClass,&dCollideTransform); |
|
198 } |
|
199 |
|
200 |
|
201 EXPORT_C int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, |
|
202 int skip) |
|
203 { |
|
204 |
|
205 // no contacts if both geoms are the same |
|
206 if (o1 == o2) return 0; |
|
207 |
|
208 // no contacts if both geoms on the same body, and the body is not 0 |
|
209 if (o1->body == o2->body && o1->body) return 0; |
|
210 |
|
211 o1->recomputePosr(); |
|
212 o2->recomputePosr(); |
|
213 |
|
214 dColliderEntry *ce = &GetGlobalData()->colliders[o1->type][o2->type]; |
|
215 int count = 0; |
|
216 if (ce->fn) { |
|
217 if (ce->reverse) { |
|
218 count = (*ce->fn) (o2,o1,flags,contact,skip); |
|
219 for (int i=0; i<count; i++) { |
|
220 dContactGeom *c = CONTACT(contact,skip*i); |
|
221 c->normal[0] = -c->normal[0]; |
|
222 c->normal[1] = -c->normal[1]; |
|
223 c->normal[2] = -c->normal[2]; |
|
224 dxGeom *tmp = c->g1; |
|
225 c->g1 = c->g2; |
|
226 c->g2 = tmp; |
|
227 int tmpint = c->side1; |
|
228 c->side1 = c->side2; |
|
229 c->side2 = tmpint; |
|
230 } |
|
231 } |
|
232 else { |
|
233 count = (*ce->fn) (o1,o2,flags,contact,skip); |
|
234 } |
|
235 } |
|
236 return count; |
|
237 } |
|
238 |
|
239 //**************************************************************************** |
|
240 // dxGeom |
|
241 |
|
242 dxGeom::dxGeom (dSpaceID _space, int is_placeable) |
|
243 { |
|
244 initColliders(); |
|
245 |
|
246 // setup body vars. invalid type of -1 must be changed by the constructor. |
|
247 type = -1; |
|
248 gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED; |
|
249 if (is_placeable) gflags |= GEOM_PLACEABLE; |
|
250 data = 0; |
|
251 body = 0; |
|
252 body_next = 0; |
|
253 if (is_placeable) { |
|
254 final_posr = dAllocPosr(); |
|
255 dSetZero (final_posr->pos,4); |
|
256 dRSetIdentity (final_posr->R); |
|
257 } |
|
258 else { |
|
259 final_posr = 0; |
|
260 } |
|
261 offset_posr = 0; |
|
262 |
|
263 // setup space vars |
|
264 next = 0; |
|
265 tome = 0; |
|
266 parent_space = 0; |
|
267 dSetZero (aabb,6); |
|
268 category_bits = ~0; |
|
269 collide_bits = ~0; |
|
270 |
|
271 // put this geom in a space if required |
|
272 if (_space) dSpaceAdd (_space,this); |
|
273 } |
|
274 |
|
275 |
|
276 dxGeom::~dxGeom() |
|
277 { |
|
278 if (parent_space) dSpaceRemove (parent_space,this); |
|
279 if ((gflags & GEOM_PLACEABLE) && (!body || (body && offset_posr))) |
|
280 dFreePosr(final_posr); |
|
281 if (offset_posr) dFreePosr(offset_posr); |
|
282 bodyRemove(); |
|
283 } |
|
284 |
|
285 |
|
286 int dxGeom::AABBTest (dxGeom */*o*/, dReal /*aabb*/[6]) |
|
287 { |
|
288 return 1; |
|
289 } |
|
290 |
|
291 |
|
292 void dxGeom::bodyRemove() |
|
293 { |
|
294 if (body) { |
|
295 // delete this geom from body list |
|
296 dxGeom **last = &body->geom, *g = body->geom; |
|
297 while (g) { |
|
298 if (g == this) { |
|
299 *last = g->body_next; |
|
300 break; |
|
301 } |
|
302 last = &g->body_next; |
|
303 g = g->body_next; |
|
304 } |
|
305 body = 0; |
|
306 body_next = 0; |
|
307 } |
|
308 } |
|
309 |
|
310 inline void myswap(dReal& a, dReal& b) { dReal t=b; b=a; a=t; } |
|
311 |
|
312 |
|
313 inline void matrixInvert(const dMatrix3& inMat, dMatrix3& outMat) |
|
314 { |
|
315 memcpy(outMat, inMat, sizeof(dMatrix3)); |
|
316 // swap _12 and _21 |
|
317 myswap(outMat[0 + 4*1], outMat[1 + 4*0]); |
|
318 // swap _31 and _13 |
|
319 myswap(outMat[2 + 4*0], outMat[0 + 4*2]); |
|
320 // swap _23 and _32 |
|
321 myswap(outMat[1 + 4*2], outMat[2 + 4*1]); |
|
322 } |
|
323 |
|
324 void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr) |
|
325 { |
|
326 dMatrix3 inv_offset; |
|
327 matrixInvert(offset_posr.R, inv_offset); |
|
328 |
|
329 dMULTIPLY0_333(body_posr.R, final_posr.R, inv_offset); |
|
330 dVector3 world_offset; |
|
331 dMULTIPLY0_331(world_offset, body_posr.R, offset_posr.pos); |
|
332 body_posr.pos[0] = final_posr.pos[0] - world_offset[0]; |
|
333 body_posr.pos[1] = final_posr.pos[1] - world_offset[1]; |
|
334 body_posr.pos[2] = final_posr.pos[2] - world_offset[2]; |
|
335 } |
|
336 |
|
337 void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr) |
|
338 { |
|
339 dMatrix3 inv_body; |
|
340 matrixInvert(body_posr.R, inv_body); |
|
341 |
|
342 dMULTIPLY0_333(offset_posr.R, inv_body, world_posr.R); |
|
343 dVector3 world_offset; |
|
344 world_offset[0] = world_posr.pos[0] - body_posr.pos[0]; |
|
345 world_offset[1] = world_posr.pos[1] - body_posr.pos[1]; |
|
346 world_offset[2] = world_posr.pos[2] - body_posr.pos[2]; |
|
347 dMULTIPLY0_331(offset_posr.pos, inv_body, world_offset); |
|
348 } |
|
349 |
|
350 void dxGeom::computePosr() |
|
351 { |
|
352 // should only be recalced if we need to - ie offset from a body |
|
353 |
|
354 dMULTIPLY0_331 (final_posr->pos,body->posr.R,offset_posr->pos); |
|
355 final_posr->pos[0] += body->posr.pos[0]; |
|
356 final_posr->pos[1] += body->posr.pos[1]; |
|
357 final_posr->pos[2] += body->posr.pos[2]; |
|
358 dMULTIPLY0_333 (final_posr->R,body->posr.R,offset_posr->R); |
|
359 } |
|
360 |
|
361 //**************************************************************************** |
|
362 // misc |
|
363 |
|
364 dxGeom *dGeomGetBodyNext (dxGeom *geom) |
|
365 { |
|
366 return geom->body_next; |
|
367 } |
|
368 |
|
369 //**************************************************************************** |
|
370 // public API for geometry objects |
|
371 |
|
372 |
|
373 |
|
374 EXPORT_C void dGeomDestroy (dxGeom *g) |
|
375 { |
|
376 delete g; |
|
377 } |
|
378 |
|
379 |
|
380 EXPORT_C void dGeomSetData (dxGeom *g, void *data) |
|
381 { |
|
382 g->data = data; |
|
383 } |
|
384 |
|
385 |
|
386 EXPORT_C void *dGeomGetData (dxGeom *g) |
|
387 { |
|
388 return g->data; |
|
389 } |
|
390 |
|
391 |
|
392 EXPORT_C void dGeomSetBody (dxGeom *g, dxBody *b) |
|
393 { |
|
394 |
|
395 if (b) { |
|
396 if (!g->body) dFreePosr(g->final_posr); |
|
397 if (g->body != b) { |
|
398 if (g->offset_posr) { |
|
399 dFreePosr(g->offset_posr); |
|
400 g->offset_posr = 0; |
|
401 } |
|
402 g->final_posr = &b->posr; |
|
403 g->bodyRemove(); |
|
404 g->bodyAdd (b); |
|
405 } |
|
406 dGeomMoved (g); |
|
407 } |
|
408 else { |
|
409 if (g->body) { |
|
410 if (g->offset_posr) |
|
411 { |
|
412 // if we're offset, we already have our own final position, make sure its updated |
|
413 g->recomputePosr(); |
|
414 dFreePosr(g->offset_posr); |
|
415 g->offset_posr = 0; |
|
416 } |
|
417 else |
|
418 { |
|
419 g->final_posr = dAllocPosr(); |
|
420 memcpy (g->final_posr->pos,g->body->posr.pos,sizeof(dVector3)); |
|
421 memcpy (g->final_posr->R,g->body->posr.R,sizeof(dMatrix3)); |
|
422 } |
|
423 g->bodyRemove(); |
|
424 } |
|
425 // dGeomMoved() should not be called if the body is being set to 0, as the |
|
426 // new position of the geom is set to the old position of the body, so the |
|
427 // effective position of the geom remains unchanged. |
|
428 } |
|
429 } |
|
430 |
|
431 |
|
432 EXPORT_C dBodyID dGeomGetBody (dxGeom *g) |
|
433 { |
|
434 return g->body; |
|
435 } |
|
436 |
|
437 |
|
438 EXPORT_C void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z) |
|
439 { |
|
440 if (g->offset_posr) { |
|
441 // move body such that body+offset = position |
|
442 dVector3 world_offset; |
|
443 dMULTIPLY0_331(world_offset, g->body->posr.R, g->offset_posr->pos); |
|
444 dBodySetPosition(g->body, |
|
445 x - world_offset[0], |
|
446 y - world_offset[1], |
|
447 z - world_offset[2]); |
|
448 } |
|
449 else if (g->body) { |
|
450 // this will call dGeomMoved (g), so we don't have to |
|
451 dBodySetPosition (g->body,x,y,z); |
|
452 } |
|
453 else { |
|
454 g->final_posr->pos[0] = x; |
|
455 g->final_posr->pos[1] = y; |
|
456 g->final_posr->pos[2] = z; |
|
457 dGeomMoved (g); |
|
458 } |
|
459 } |
|
460 |
|
461 |
|
462 EXPORT_C void dGeomSetRotation (dxGeom *g, const dMatrix3 R) |
|
463 { |
|
464 |
|
465 if (g->offset_posr) { |
|
466 g->recomputePosr(); |
|
467 // move body such that body+offset = rotation |
|
468 dxPosR new_final_posr; |
|
469 dxPosR new_body_posr; |
|
470 memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); |
|
471 memcpy(new_final_posr.R, R, sizeof(dMatrix3)); |
|
472 getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr); |
|
473 dBodySetRotation(g->body, new_body_posr.R); |
|
474 dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]); |
|
475 } |
|
476 else if (g->body) { |
|
477 // this will call dGeomMoved (g), so we don't have to |
|
478 dBodySetRotation (g->body,R); |
|
479 } |
|
480 else { |
|
481 memcpy (g->final_posr->R,R,sizeof(dMatrix3)); |
|
482 dGeomMoved (g); |
|
483 } |
|
484 } |
|
485 |
|
486 |
|
487 EXPORT_C void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat) |
|
488 { |
|
489 if (g->offset_posr) { |
|
490 g->recomputePosr(); |
|
491 // move body such that body+offset = rotation |
|
492 dxPosR new_final_posr; |
|
493 dxPosR new_body_posr; |
|
494 dQtoR (quat, new_final_posr.R); |
|
495 memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); |
|
496 |
|
497 getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr); |
|
498 dBodySetRotation(g->body, new_body_posr.R); |
|
499 dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]); |
|
500 } |
|
501 if (g->body) { |
|
502 // this will call dGeomMoved (g), so we don't have to |
|
503 dBodySetQuaternion (g->body,quat); |
|
504 } |
|
505 else { |
|
506 dQtoR (quat, g->final_posr->R); |
|
507 dGeomMoved (g); |
|
508 } |
|
509 } |
|
510 |
|
511 |
|
512 EXPORT_C const dReal * dGeomGetPosition (dxGeom *g) |
|
513 { |
|
514 g->recomputePosr(); |
|
515 return g->final_posr->pos; |
|
516 } |
|
517 |
|
518 |
|
519 EXPORT_C void dGeomCopyPosition(dxGeom *g, dVector3 pos) |
|
520 { |
|
521 g->recomputePosr(); |
|
522 const dReal* src = g->final_posr->pos; |
|
523 pos[0] = src[0]; |
|
524 pos[1] = src[1]; |
|
525 pos[2] = src[2]; |
|
526 } |
|
527 |
|
528 |
|
529 EXPORT_C const dReal * dGeomGetRotation (dxGeom *g) |
|
530 { |
|
531 g->recomputePosr(); |
|
532 return g->final_posr->R; |
|
533 } |
|
534 |
|
535 |
|
536 EXPORT_C void dGeomCopyRotation(dxGeom *g, dMatrix3 R) |
|
537 { |
|
538 g->recomputePosr(); |
|
539 const dReal* src = g->final_posr->R; |
|
540 R[0] = src[0]; |
|
541 R[1] = src[1]; |
|
542 R[2] = src[2]; |
|
543 R[4] = src[4]; |
|
544 R[5] = src[5]; |
|
545 R[6] = src[6]; |
|
546 R[8] = src[8]; |
|
547 R[9] = src[9]; |
|
548 R[10] = src[10]; |
|
549 } |
|
550 |
|
551 |
|
552 EXPORT_C void dGeomGetQuaternion (dxGeom *g, dQuaternion quat) |
|
553 { |
|
554 if (g->body && !g->offset_posr) { |
|
555 const dReal * body_quat = dBodyGetQuaternion (g->body); |
|
556 quat[0] = body_quat[0]; |
|
557 quat[1] = body_quat[1]; |
|
558 quat[2] = body_quat[2]; |
|
559 quat[3] = body_quat[3]; |
|
560 } |
|
561 else { |
|
562 g->recomputePosr(); |
|
563 dRtoQ (g->final_posr->R, quat); |
|
564 } |
|
565 } |
|
566 |
|
567 |
|
568 EXPORT_C void dGeomGetAABB (dxGeom *g, dReal aabb[6]) |
|
569 { |
|
570 g->recomputeAABB(); |
|
571 memcpy (aabb,g->aabb,6 * sizeof(dReal)); |
|
572 } |
|
573 |
|
574 |
|
575 EXPORT_C int dGeomIsSpace (dxGeom *g) |
|
576 { |
|
577 |
|
578 return IS_SPACE(g); |
|
579 } |
|
580 |
|
581 |
|
582 EXPORT_C dSpaceID dGeomGetSpace (dxGeom *g) |
|
583 { |
|
584 |
|
585 return g->parent_space; |
|
586 } |
|
587 |
|
588 |
|
589 EXPORT_C int dGeomGetClass (dxGeom *g) |
|
590 { |
|
591 |
|
592 return g->type; |
|
593 } |
|
594 |
|
595 |
|
596 EXPORT_C void dGeomSetCategoryBits (dxGeom *g, unsigned long bits) |
|
597 { |
|
598 |
|
599 g->category_bits = bits; |
|
600 } |
|
601 |
|
602 |
|
603 EXPORT_C void dGeomSetCollideBits (dxGeom *g, unsigned long bits) |
|
604 { |
|
605 |
|
606 g->collide_bits = bits; |
|
607 } |
|
608 |
|
609 |
|
610 EXPORT_C unsigned long dGeomGetCategoryBits (dxGeom *g) |
|
611 { |
|
612 |
|
613 return g->category_bits; |
|
614 } |
|
615 |
|
616 |
|
617 EXPORT_C unsigned long dGeomGetCollideBits (dxGeom *g) |
|
618 { |
|
619 |
|
620 return g->collide_bits; |
|
621 } |
|
622 |
|
623 |
|
624 EXPORT_C void dGeomEnable (dxGeom *g) |
|
625 { |
|
626 |
|
627 g->gflags |= GEOM_ENABLED; |
|
628 } |
|
629 |
|
630 EXPORT_C void dGeomDisable (dxGeom *g) |
|
631 { |
|
632 |
|
633 g->gflags &= ~GEOM_ENABLED; |
|
634 } |
|
635 |
|
636 EXPORT_C int dGeomIsEnabled (dxGeom *g) |
|
637 { |
|
638 |
|
639 return (g->gflags & GEOM_ENABLED) != 0; |
|
640 } |
|
641 |
|
642 |
|
643 //**************************************************************************** |
|
644 // C interface that lets the user make new classes. this interface is a lot |
|
645 // more cumbersome than C++ subclassing, which is what is used internally |
|
646 // in ODE. this API is mainly to support legacy code. |
|
647 |
|
648 struct dxUserGeom : public dxGeom { |
|
649 void *user_data; |
|
650 |
|
651 dxUserGeom (int class_num); |
|
652 ~dxUserGeom(); |
|
653 void computeAABB(); |
|
654 int AABBTest (dxGeom *o, dReal aabb[6]); |
|
655 }; |
|
656 |
|
657 |
|
658 dxUserGeom::dxUserGeom (int class_num) : dxGeom (0,1) |
|
659 { |
|
660 type = class_num; |
|
661 int size = GetGlobalData()->user_classes[type-dFirstUserClass].bytes; |
|
662 user_data = dAlloc (size); |
|
663 memset (user_data,0,size); |
|
664 } |
|
665 |
|
666 |
|
667 dxUserGeom::~dxUserGeom() |
|
668 { |
|
669 dGeomClass *c = &GetGlobalData()->user_classes[type-dFirstUserClass]; |
|
670 if (c->dtor) c->dtor (this); |
|
671 dFree (user_data,c->bytes); |
|
672 } |
|
673 |
|
674 |
|
675 void dxUserGeom::computeAABB() |
|
676 { |
|
677 GetGlobalData()->user_classes[type-dFirstUserClass].aabb (this,aabb); |
|
678 } |
|
679 |
|
680 |
|
681 int dxUserGeom::AABBTest (dxGeom *o, dReal aabb[6]) |
|
682 { |
|
683 dGeomClass *c = &GetGlobalData()->user_classes[type-dFirstUserClass]; |
|
684 if (c->aabb_test) return c->aabb_test (this,o,aabb); |
|
685 else return 1; |
|
686 } |
|
687 |
|
688 |
|
689 static int dCollideUserGeomWithGeom (dxGeom *o1, dxGeom *o2, int flags, |
|
690 dContactGeom *contact, int skip) |
|
691 { |
|
692 // this generic collider function is called the first time that a user class |
|
693 // tries to collide against something. it will find out the correct collider |
|
694 // function and then set the colliders array so that the correct function is |
|
695 // called directly the next time around. |
|
696 |
|
697 int t1 = o1->type; // note that o1 is a user geom |
|
698 int t2 = o2->type; // o2 *may* be a user geom |
|
699 |
|
700 // find the collider function to use. if o1 does not know how to collide with |
|
701 // o2, then o2 might know how to collide with o1 (provided that it is a user |
|
702 // geom). |
|
703 dColliderFn *fn = GetGlobalData()->user_classes[t1-dFirstUserClass].collider (t2); |
|
704 int reverse = 0; |
|
705 if (!fn && t2 >= dFirstUserClass && t2 <= dLastUserClass) { |
|
706 fn = GetGlobalData()->user_classes[t2-dFirstUserClass].collider (t1); |
|
707 reverse = 1; |
|
708 } |
|
709 |
|
710 // set the colliders array so that the correct function is called directly |
|
711 // the next time around. note that fn can be 0 here if no collider was found, |
|
712 // which means that dCollide() will always return 0 for this case. |
|
713 GetGlobalData()->colliders[t1][t2].fn = fn; |
|
714 GetGlobalData()->colliders[t1][t2].reverse = reverse; |
|
715 GetGlobalData()->colliders[t2][t1].fn = fn; |
|
716 GetGlobalData()->colliders[t2][t1].reverse = !reverse; |
|
717 |
|
718 // now call the collider function indirectly through dCollide(), so that |
|
719 // contact reversing is properly handled. |
|
720 return dCollide (o1,o2,flags,contact,skip); |
|
721 } |
|
722 |
|
723 |
|
724 EXPORT_C int dCreateGeomClass (const dGeomClass *c) |
|
725 { |
|
726 |
|
727 |
|
728 if (GetGlobalData()->num_user_classes >= dMaxUserClasses) { |
|
729 |
|
730 } |
|
731 GetGlobalData()->user_classes[GetGlobalData()->num_user_classes] = *c; |
|
732 int class_number = GetGlobalData()->num_user_classes + dFirstUserClass; |
|
733 initColliders(); |
|
734 setAllColliders (class_number,&dCollideUserGeomWithGeom); |
|
735 |
|
736 GetGlobalData()->num_user_classes++; |
|
737 return class_number; |
|
738 } |
|
739 |
|
740 |
|
741 EXPORT_C void * dGeomGetClassData (dxGeom *g) |
|
742 { |
|
743 |
|
744 dxUserGeom *user = (dxUserGeom*) g; |
|
745 return user->user_data; |
|
746 } |
|
747 |
|
748 |
|
749 EXPORT_C dGeomID dCreateGeom (int classnum) |
|
750 { |
|
751 |
|
752 return new dxUserGeom (classnum); |
|
753 } |
|
754 |
|
755 |
|
756 |
|
757 /* ************************************************************************ */ |
|
758 /* geom offset from body */ |
|
759 |
|
760 void dGeomCreateOffset (dxGeom *g) |
|
761 { |
|
762 |
|
763 if (g->offset_posr) |
|
764 { |
|
765 return; // already created |
|
766 } |
|
767 |
|
768 |
|
769 g->final_posr = dAllocPosr(); |
|
770 g->offset_posr = dAllocPosr(); |
|
771 dSetZero (g->offset_posr->pos,4); |
|
772 dRSetIdentity (g->offset_posr->R); |
|
773 |
|
774 g->gflags |= GEOM_POSR_BAD; |
|
775 } |
|
776 |
|
777 EXPORT_C void dGeomSetOffsetPosition (dxGeom *g, dReal x, dReal y, dReal z) |
|
778 { |
|
779 |
|
780 |
|
781 if (!g->offset_posr) |
|
782 { |
|
783 dGeomCreateOffset(g); |
|
784 } |
|
785 g->offset_posr->pos[0] = x; |
|
786 g->offset_posr->pos[1] = y; |
|
787 g->offset_posr->pos[2] = z; |
|
788 dGeomMoved (g); |
|
789 } |
|
790 |
|
791 EXPORT_C void dGeomSetOffsetRotation (dxGeom *g, const dMatrix3 R) |
|
792 { |
|
793 |
|
794 if (!g->offset_posr) |
|
795 { |
|
796 dGeomCreateOffset (g); |
|
797 } |
|
798 memcpy (g->offset_posr->R,R,sizeof(dMatrix3)); |
|
799 dGeomMoved (g); |
|
800 } |
|
801 |
|
802 EXPORT_C void dGeomSetOffsetQuaternion (dxGeom *g, const dQuaternion quat) |
|
803 { |
|
804 |
|
805 if (!g->offset_posr) |
|
806 { |
|
807 dGeomCreateOffset (g); |
|
808 } |
|
809 dQtoR (quat, g->offset_posr->R); |
|
810 dGeomMoved (g); |
|
811 } |
|
812 |
|
813 EXPORT_C void dGeomSetOffsetWorldPosition (dxGeom *g, dReal x, dReal y, dReal z) |
|
814 { |
|
815 |
|
816 if (!g->offset_posr) |
|
817 { |
|
818 dGeomCreateOffset(g); |
|
819 } |
|
820 dBodyGetPosRelPoint(g->body, x, y, z, g->offset_posr->pos); |
|
821 dGeomMoved (g); |
|
822 } |
|
823 |
|
824 EXPORT_C void dGeomSetOffsetWorldRotation (dxGeom *g, const dMatrix3 R) |
|
825 { |
|
826 |
|
827 if (!g->offset_posr) |
|
828 { |
|
829 dGeomCreateOffset (g); |
|
830 } |
|
831 g->recomputePosr(); |
|
832 |
|
833 dxPosR new_final_posr; |
|
834 memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); |
|
835 memcpy(new_final_posr.R, R, sizeof(dMatrix3)); |
|
836 |
|
837 getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr); |
|
838 dGeomMoved (g); |
|
839 } |
|
840 |
|
841 EXPORT_C void dGeomSetOffsetWorldQuaternion (dxGeom *g, const dQuaternion quat) |
|
842 { |
|
843 |
|
844 if (!g->offset_posr) |
|
845 { |
|
846 dGeomCreateOffset (g); |
|
847 } |
|
848 |
|
849 g->recomputePosr(); |
|
850 |
|
851 dxPosR new_final_posr; |
|
852 memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3)); |
|
853 dQtoR (quat, new_final_posr.R); |
|
854 |
|
855 getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr); |
|
856 dGeomMoved (g); |
|
857 } |
|
858 |
|
859 EXPORT_C void dGeomClearOffset(dxGeom *g) |
|
860 { |
|
861 |
|
862 if (g->offset_posr) |
|
863 { |
|
864 // no longer need an offset posr |
|
865 dFreePosr(g->offset_posr); |
|
866 g->offset_posr = 0; |
|
867 // the geom will now share the position of the body |
|
868 dFreePosr(g->final_posr); |
|
869 g->final_posr = &g->body->posr; |
|
870 // geom has moved |
|
871 g->gflags &= ~GEOM_POSR_BAD; |
|
872 dGeomMoved (g); |
|
873 } |
|
874 } |
|
875 |
|
876 EXPORT_C int dGeomIsOffset(dxGeom *g) |
|
877 { |
|
878 |
|
879 return ((0 != g->offset_posr) ? 1 : 0); |
|
880 } |
|
881 |
|
882 static const dVector3 OFFSET_POSITION_ZERO = { REAL(0.0f), REAL(0.0f), REAL(0.0f), REAL(0.0f) }; |
|
883 |
|
884 EXPORT_C const dReal * dGeomGetOffsetPosition (dxGeom *g) |
|
885 { |
|
886 |
|
887 if (g->offset_posr) |
|
888 { |
|
889 return g->offset_posr->pos; |
|
890 } |
|
891 return OFFSET_POSITION_ZERO; |
|
892 } |
|
893 |
|
894 EXPORT_C void dGeomCopyOffsetPosition (dxGeom *g, dVector3 pos) |
|
895 { |
|
896 |
|
897 if (g->offset_posr) |
|
898 { |
|
899 const dReal* src = g->offset_posr->pos; |
|
900 pos[0] = src[0]; |
|
901 pos[1] = src[1]; |
|
902 pos[2] = src[2]; |
|
903 } |
|
904 else |
|
905 { |
|
906 pos[0] = 0; |
|
907 pos[1] = 0; |
|
908 pos[2] = 0; |
|
909 } |
|
910 } |
|
911 |
|
912 static const dMatrix3 OFFSET_ROTATION_ZERO = |
|
913 { |
|
914 REAL(1.0f), REAL(0.0f), REAL(0.0f), REAL(0.0f), |
|
915 REAL(0.0f), REAL(1.0f), REAL(0.0f), REAL(0.0f), |
|
916 REAL(0.0f), REAL(0.0f), REAL(1.0f), REAL(0.0f), |
|
917 }; |
|
918 |
|
919 EXPORT_C const dReal * dGeomGetOffsetRotation (dxGeom *g) |
|
920 { |
|
921 |
|
922 if (g->offset_posr) |
|
923 { |
|
924 return g->offset_posr->R; |
|
925 } |
|
926 return OFFSET_ROTATION_ZERO; |
|
927 } |
|
928 |
|
929 EXPORT_C void dGeomCopyOffsetRotation (dxGeom *g, dMatrix3 R) |
|
930 { |
|
931 |
|
932 if (g->offset_posr) |
|
933 { |
|
934 const dReal* src = g->final_posr->R; |
|
935 R[0] = src[0]; |
|
936 R[1] = src[1]; |
|
937 R[2] = src[2]; |
|
938 R[4] = src[4]; |
|
939 R[5] = src[5]; |
|
940 R[6] = src[6]; |
|
941 R[8] = src[8]; |
|
942 R[9] = src[9]; |
|
943 R[10] = src[10]; |
|
944 } |
|
945 else |
|
946 { |
|
947 R[0] = OFFSET_ROTATION_ZERO[0]; |
|
948 R[1] = OFFSET_ROTATION_ZERO[1]; |
|
949 R[2] = OFFSET_ROTATION_ZERO[2]; |
|
950 R[4] = OFFSET_ROTATION_ZERO[4]; |
|
951 R[5] = OFFSET_ROTATION_ZERO[5]; |
|
952 R[6] = OFFSET_ROTATION_ZERO[6]; |
|
953 R[8] = OFFSET_ROTATION_ZERO[8]; |
|
954 R[9] = OFFSET_ROTATION_ZERO[9]; |
|
955 R[10] = OFFSET_ROTATION_ZERO[10]; |
|
956 } |
|
957 } |
|
958 |
|
959 EXPORT_C void dGeomGetOffsetQuaternion (dxGeom *g, dQuaternion result) |
|
960 { |
|
961 if (g->offset_posr) |
|
962 { |
|
963 dRtoQ (g->offset_posr->R, result); |
|
964 } |
|
965 else |
|
966 { |
|
967 dSetZero (result,4); |
|
968 result[0] = REAL(1.0); |
|
969 } |
|
970 } |
|
971 |
|
972 //**************************************************************************** |
|
973 // initialization and shutdown routines - allocate and initialize data, |
|
974 // cleanup before exiting |
|
975 |
|
976 extern void opcode_collider_cleanup(); |
|
977 |
|
978 EXPORT_C void dInitODE() |
|
979 { |
|
980 #ifndef __WINSCW__ |
|
981 Mem::FillZ( GetGlobalData(), sizeof( TOdeStaticData ) ); |
|
982 GetGlobalData()->autoEnableDepth = 2; |
|
983 #endif // !__WINSCW__ |
|
984 } |
|
985 |
|
986 EXPORT_C void dCloseODE() |
|
987 { |
|
988 GetGlobalData()->colliders_initialized = 0; |
|
989 GetGlobalData()->num_user_classes = 0; |
|
990 dClearPosrCache(); |
|
991 } |