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