aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/contrib/BreakableJoints/ode.cpp')
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/ode.cpp1404
1 files changed, 0 insertions, 1404 deletions
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp b/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp
deleted file mode 100644
index 7137960..0000000
--- a/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp
+++ /dev/null
@@ -1,1404 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 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#ifdef _MSC_VER
24#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
25#endif
26
27// this source file is mostly concerned with the data structures, not the
28// numerics.
29
30#include "objects.h"
31#include <ode/ode.h>
32#include "joint.h"
33#include <ode/odemath.h>
34#include <ode/matrix.h>
35#include "step.h"
36#include <ode/memory.h>
37#include <ode/error.h>
38
39// misc defines
40#define ALLOCA dALLOCA16
41
42//****************************************************************************
43// utility
44
45static inline void initObject (dObject *obj, dxWorld *w)
46{
47 obj->world = w;
48 obj->next = 0;
49 obj->tome = 0;
50 obj->userdata = 0;
51 obj->tag = 0;
52}
53
54
55// add an object `obj' to the list who's head pointer is pointed to by `first'.
56
57static inline void addObjectToList (dObject *obj, dObject **first)
58{
59 obj->next = *first;
60 obj->tome = first;
61 if (*first) (*first)->tome = &obj->next;
62 (*first) = obj;
63}
64
65
66// remove the object from the linked list
67
68static inline void removeObjectFromList (dObject *obj)
69{
70 if (obj->next) obj->next->tome = obj->tome;
71 *(obj->tome) = obj->next;
72 // safeguard
73 obj->next = 0;
74 obj->tome = 0;
75}
76
77
78// remove the joint from neighbour lists of all connected bodies
79
80static void removeJointReferencesFromAttachedBodies (dxJoint *j)
81{
82 for (int i=0; i<2; i++) {
83 dxBody *body = j->node[i].body;
84 if (body) {
85 dxJointNode *n = body->firstjoint;
86 dxJointNode *last = 0;
87 while (n) {
88 if (n->joint == j) {
89 if (last) last->next = n->next;
90 else body->firstjoint = n->next;
91 break;
92 }
93 last = n;
94 n = n->next;
95 }
96 }
97 }
98 j->node[0].body = 0;
99 j->node[0].next = 0;
100 j->node[1].body = 0;
101 j->node[1].next = 0;
102}
103
104//****************************************************************************
105// island processing
106
107// this groups all joints and bodies in a world into islands. all objects
108// in an island are reachable by going through connected bodies and joints.
109// each island can be simulated separately.
110// note that joints that are not attached to anything will not be included
111// in any island, an so they do not affect the simulation.
112//
113// this function starts new island from unvisited bodies. however, it will
114// never start a new islands from a disabled body. thus islands of disabled
115// bodies will not be included in the simulation. disabled bodies are
116// re-enabled if they are found to be part of an active island.
117
118static void processIslands (dxWorld *world, dReal stepsize)
119{
120 dxBody *b,*bb,**body;
121 dxJoint *j,**joint;
122
123 // nothing to do if no bodies
124 if (world->nb <= 0) return;
125
126 // make arrays for body and joint lists (for a single island) to go into
127 body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*));
128 joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*));
129 int bcount = 0; // number of bodies in `body'
130 int jcount = 0; // number of joints in `joint'
131
132 // set all body/joint tags to 0
133 for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
134 for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
135
136 // allocate a stack of unvisited bodies in the island. the maximum size of
137 // the stack can be the lesser of the number of bodies or joints, because
138 // new bodies are only ever added to the stack by going through untagged
139 // joints. all the bodies in the stack must be tagged!
140 int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
141 dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*));
142
143 for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
144 // get bb = the next enabled, untagged body, and tag it
145 if (bb->tag || (bb->flags & dxBodyDisabled)) continue;
146 bb->tag = 1;
147
148 // tag all bodies and joints starting from bb.
149 int stacksize = 0;
150 b = bb;
151 body[0] = bb;
152 bcount = 1;
153 jcount = 0;
154 goto quickstart;
155 while (stacksize > 0) {
156 b = stack[--stacksize]; // pop body off stack
157 body[bcount++] = b; // put body on body list
158 quickstart:
159
160 // traverse and tag all body's joints, add untagged connected bodies
161 // to stack
162 for (dxJointNode *n=b->firstjoint; n; n=n->next) {
163 if (!n->joint->tag) {
164 n->joint->tag = 1;
165 joint[jcount++] = n->joint;
166 if (n->body && !n->body->tag) {
167 n->body->tag = 1;
168 stack[stacksize++] = n->body;
169 }
170 }
171 }
172 dIASSERT(stacksize <= world->nb);
173 dIASSERT(stacksize <= world->nj);
174 }
175
176 // now do something with body and joint lists
177 dInternalStepIsland (world,body,bcount,joint,jcount,stepsize);
178
179 // what we've just done may have altered the body/joint tag values.
180 // we must make sure that these tags are nonzero.
181 // also make sure all bodies are in the enabled state.
182 int i;
183 for (i=0; i<bcount; i++) {
184 body[i]->tag = 1;
185 body[i]->flags &= ~dxBodyDisabled;
186 }
187 for (i=0; i<jcount; i++) joint[i]->tag = 1;
188 }
189
190 // if debugging, check that all objects (except for disabled bodies,
191 // unconnected joints, and joints that are connected to disabled bodies)
192 // were tagged.
193# ifndef dNODEBUG
194 for (b=world->firstbody; b; b=(dxBody*)b->next) {
195 if (b->flags & dxBodyDisabled) {
196 if (b->tag) dDebug (0,"disabled body tagged");
197 }
198 else {
199 if (!b->tag) dDebug (0,"enabled body not tagged");
200 }
201 }
202 for (j=world->firstjoint; j; j=(dxJoint*)j->next) {
203 if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) ||
204 (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) {
205 if (!j->tag) dDebug (0,"attached enabled joint not tagged");
206 }
207 else {
208 if (j->tag) dDebug (0,"unattached or disabled joint tagged");
209 }
210 }
211# endif
212 /******************** breakable joint contribution ***********************/
213 dxJoint* nextJ;
214 if (!world->firstjoint)
215 nextJ = 0;
216 else
217 nextJ = (dxJoint*)world->firstjoint->next;
218 for (j=world->firstjoint; j; j=nextJ) {
219 nextJ = (dxJoint*)j->next;
220 // check if joint is breakable and broken
221 if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
222 // detach (break) the joint
223 dJointAttach (j, 0, 0);
224 // call the callback function if it is set
225 if (j->breakInfo->callback) j->breakInfo->callback (j);
226 // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
227 if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
228 }
229 }
230 /*************************************************************************/
231}
232
233//****************************************************************************
234// debugging
235
236// see if an object list loops on itself (if so, it's bad).
237
238static int listHasLoops (dObject *first)
239{
240 if (first==0 || first->next==0) return 0;
241 dObject *a=first,*b=first->next;
242 int skip=0;
243 while (b) {
244 if (a==b) return 1;
245 b = b->next;
246 if (skip) a = a->next;
247 skip ^= 1;
248 }
249 return 0;
250}
251
252
253// check the validity of the world data structures
254
255static void checkWorld (dxWorld *w)
256{
257 dxBody *b;
258 dxJoint *j;
259
260 // check there are no loops
261 if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops");
262 if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops");
263
264 // check lists are well formed (check `tome' pointers)
265 for (b=w->firstbody; b; b=(dxBody*)b->next) {
266 if (b->next && b->next->tome != &b->next)
267 dDebug (0,"bad tome pointer in body list");
268 }
269 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
270 if (j->next && j->next->tome != &j->next)
271 dDebug (0,"bad tome pointer in joint list");
272 }
273
274 // check counts
275 int n = 0;
276 for (b=w->firstbody; b; b=(dxBody*)b->next) n++;
277 if (w->nb != n) dDebug (0,"body count incorrect");
278 n = 0;
279 for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++;
280 if (w->nj != n) dDebug (0,"joint count incorrect");
281
282 // set all tag values to a known value
283 static int count = 0;
284 count++;
285 for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count;
286 for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count;
287
288 // check all body/joint world pointers are ok
289 for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w)
290 dDebug (0,"bad world pointer in body list");
291 for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w)
292 dDebug (0,"bad world pointer in joint list");
293
294 /*
295 // check for half-connected joints - actually now these are valid
296 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
297 if (j->node[0].body || j->node[1].body) {
298 if (!(j->node[0].body && j->node[1].body))
299 dDebug (0,"half connected joint found");
300 }
301 }
302 */
303
304 // check that every joint node appears in the joint lists of both bodies it
305 // attaches
306 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
307 for (int i=0; i<2; i++) {
308 if (j->node[i].body) {
309 int ok = 0;
310 for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) {
311 if (n->joint == j) ok = 1;
312 }
313 if (ok==0) dDebug (0,"joint not in joint list of attached body");
314 }
315 }
316 }
317
318 // check all body joint lists (correct body ptrs)
319 for (b=w->firstbody; b; b=(dxBody*)b->next) {
320 for (dxJointNode *n=b->firstjoint; n; n=n->next) {
321 if (&n->joint->node[0] == n) {
322 if (n->joint->node[1].body != b)
323 dDebug (0,"bad body pointer in joint node of body list (1)");
324 }
325 else {
326 if (n->joint->node[0].body != b)
327 dDebug (0,"bad body pointer in joint node of body list (2)");
328 }
329 if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body");
330 }
331 }
332
333 // check all body pointers in joints, check they are distinct
334 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
335 if (j->node[0].body && (j->node[0].body == j->node[1].body))
336 dDebug (0,"non-distinct body pointers in joint");
337 if ((j->node[0].body && j->node[0].body->tag != count) ||
338 (j->node[1].body && j->node[1].body->tag != count))
339 dDebug (0,"bad body pointer in joint");
340 }
341}
342
343
344void dWorldCheck (dxWorld *w)
345{
346 checkWorld (w);
347}
348
349//****************************************************************************
350// body
351
352dxBody *dBodyCreate (dxWorld *w)
353{
354 dAASSERT (w);
355 dxBody *b = new dxBody;
356 initObject (b,w);
357 b->firstjoint = 0;
358 b->flags = 0;
359 b->geom = 0;
360 dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0);
361 dSetZero (b->invI,4*3);
362 b->invI[0] = 1;
363 b->invI[5] = 1;
364 b->invI[10] = 1;
365 b->invMass = 1;
366 dSetZero (b->pos,4);
367 dSetZero (b->q,4);
368 b->q[0] = 1;
369 dRSetIdentity (b->R);
370 dSetZero (b->lvel,4);
371 dSetZero (b->avel,4);
372 dSetZero (b->facc,4);
373 dSetZero (b->tacc,4);
374 dSetZero (b->finite_rot_axis,4);
375 addObjectToList (b,(dObject **) &w->firstbody);
376 w->nb++;
377 return b;
378}
379
380
381void dBodyDestroy (dxBody *b)
382{
383 dAASSERT (b);
384
385 // all geoms that link to this body must be notified that the body is about
386 // to disappear. note that the call to dGeomSetBody(geom,0) will result in
387 // dGeomGetBodyNext() returning 0 for the body, so we must get the next body
388 // before setting the body to 0.
389 dxGeom *next_geom = 0;
390 for (dxGeom *geom = b->geom; geom; geom = next_geom) {
391 next_geom = dGeomGetBodyNext (geom);
392 dGeomSetBody (geom,0);
393 }
394
395 // detach all neighbouring joints, then delete this body.
396 dxJointNode *n = b->firstjoint;
397 while (n) {
398 // sneaky trick to speed up removal of joint references (black magic)
399 n->joint->node[(n == n->joint->node)].body = 0;
400
401 dxJointNode *next = n->next;
402 n->next = 0;
403 removeJointReferencesFromAttachedBodies (n->joint);
404 n = next;
405 }
406 removeObjectFromList (b);
407 b->world->nb--;
408 delete b;
409}
410
411
412void dBodySetData (dBodyID b, void *data)
413{
414 dAASSERT (b);
415 b->userdata = data;
416}
417
418
419void *dBodyGetData (dBodyID b)
420{
421 dAASSERT (b);
422 return b->userdata;
423}
424
425
426void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)
427{
428 dAASSERT (b);
429 b->pos[0] = x;
430 b->pos[1] = y;
431 b->pos[2] = z;
432
433 // notify all attached geoms that this body has moved
434 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
435 dGeomMoved (geom);
436}
437
438
439void dBodySetRotation (dBodyID b, const dMatrix3 R)
440{
441 dAASSERT (b && R);
442 dQuaternion q;
443 dRtoQ (R,q);
444 dNormalize4 (q);
445 b->q[0] = q[0];
446 b->q[1] = q[1];
447 b->q[2] = q[2];
448 b->q[3] = q[3];
449 dQtoR (b->q,b->R);
450
451 // notify all attached geoms that this body has moved
452 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
453 dGeomMoved (geom);
454}
455
456
457void dBodySetQuaternion (dBodyID b, const dQuaternion q)
458{
459 dAASSERT (b && q);
460 b->q[0] = q[0];
461 b->q[1] = q[1];
462 b->q[2] = q[2];
463 b->q[3] = q[3];
464 dNormalize4 (b->q);
465 dQtoR (b->q,b->R);
466
467 // notify all attached geoms that this body has moved
468 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
469 dGeomMoved (geom);
470}
471
472
473void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z)
474{
475 dAASSERT (b);
476 b->lvel[0] = x;
477 b->lvel[1] = y;
478 b->lvel[2] = z;
479}
480
481
482void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)
483{
484 dAASSERT (b);
485 b->avel[0] = x;
486 b->avel[1] = y;
487 b->avel[2] = z;
488}
489
490
491const dReal * dBodyGetPosition (dBodyID b)
492{
493 dAASSERT (b);
494 return b->pos;
495}
496
497
498const dReal * dBodyGetRotation (dBodyID b)
499{
500 dAASSERT (b);
501 return b->R;
502}
503
504
505const dReal * dBodyGetQuaternion (dBodyID b)
506{
507 dAASSERT (b);
508 return b->q;
509}
510
511
512const dReal * dBodyGetLinearVel (dBodyID b)
513{
514 dAASSERT (b);
515 return b->lvel;
516}
517
518
519const dReal * dBodyGetAngularVel (dBodyID b)
520{
521 dAASSERT (b);
522 return b->avel;
523}
524
525
526void dBodySetMass (dBodyID b, const dMass *mass)
527{
528 dAASSERT (b && mass);
529 memcpy (&b->mass,mass,sizeof(dMass));
530 if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) {
531 dDEBUGMSG ("inertia must be positive definite");
532 dRSetIdentity (b->invI);
533 }
534 b->invMass = dRecip(b->mass.mass);
535}
536
537
538void dBodyGetMass (dBodyID b, dMass *mass)
539{
540 dAASSERT (b && mass);
541 memcpy (mass,&b->mass,sizeof(dMass));
542}
543
544
545void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)
546{
547 dAASSERT (b);
548 b->facc[0] += fx;
549 b->facc[1] += fy;
550 b->facc[2] += fz;
551}
552
553
554void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
555{
556 dAASSERT (b);
557 b->tacc[0] += fx;
558 b->tacc[1] += fy;
559 b->tacc[2] += fz;
560}
561
562
563void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)
564{
565 dAASSERT (b);
566 dVector3 t1,t2;
567 t1[0] = fx;
568 t1[1] = fy;
569 t1[2] = fz;
570 t1[3] = 0;
571 dMULTIPLY0_331 (t2,b->R,t1);
572 b->facc[0] += t2[0];
573 b->facc[1] += t2[1];
574 b->facc[2] += t2[2];
575}
576
577
578void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
579{
580 dAASSERT (b);
581 dVector3 t1,t2;
582 t1[0] = fx;
583 t1[1] = fy;
584 t1[2] = fz;
585 t1[3] = 0;
586 dMULTIPLY0_331 (t2,b->R,t1);
587 b->tacc[0] += t2[0];
588 b->tacc[1] += t2[1];
589 b->tacc[2] += t2[2];
590}
591
592
593void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
594 dReal px, dReal py, dReal pz)
595{
596 dAASSERT (b);
597 b->facc[0] += fx;
598 b->facc[1] += fy;
599 b->facc[2] += fz;
600 dVector3 f,q;
601 f[0] = fx;
602 f[1] = fy;
603 f[2] = fz;
604 q[0] = px - b->pos[0];
605 q[1] = py - b->pos[1];
606 q[2] = pz - b->pos[2];
607 dCROSS (b->tacc,+=,q,f);
608}
609
610
611void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
612 dReal px, dReal py, dReal pz)
613{
614 dAASSERT (b);
615 dVector3 prel,f,p;
616 f[0] = fx;
617 f[1] = fy;
618 f[2] = fz;
619 f[3] = 0;
620 prel[0] = px;
621 prel[1] = py;
622 prel[2] = pz;
623 prel[3] = 0;
624 dMULTIPLY0_331 (p,b->R,prel);
625 b->facc[0] += f[0];
626 b->facc[1] += f[1];
627 b->facc[2] += f[2];
628 dCROSS (b->tacc,+=,p,f);
629}
630
631
632void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
633 dReal px, dReal py, dReal pz)
634{
635 dAASSERT (b);
636 dVector3 frel,f;
637 frel[0] = fx;
638 frel[1] = fy;
639 frel[2] = fz;
640 frel[3] = 0;
641 dMULTIPLY0_331 (f,b->R,frel);
642 b->facc[0] += f[0];
643 b->facc[1] += f[1];
644 b->facc[2] += f[2];
645 dVector3 q;
646 q[0] = px - b->pos[0];
647 q[1] = py - b->pos[1];
648 q[2] = pz - b->pos[2];
649 dCROSS (b->tacc,+=,q,f);
650}
651
652
653void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
654 dReal px, dReal py, dReal pz)
655{
656 dAASSERT (b);
657 dVector3 frel,prel,f,p;
658 frel[0] = fx;
659 frel[1] = fy;
660 frel[2] = fz;
661 frel[3] = 0;
662 prel[0] = px;
663 prel[1] = py;
664 prel[2] = pz;
665 prel[3] = 0;
666 dMULTIPLY0_331 (f,b->R,frel);
667 dMULTIPLY0_331 (p,b->R,prel);
668 b->facc[0] += f[0];
669 b->facc[1] += f[1];
670 b->facc[2] += f[2];
671 dCROSS (b->tacc,+=,p,f);
672}
673
674
675const dReal * dBodyGetForce (dBodyID b)
676{
677 dAASSERT (b);
678 return b->facc;
679}
680
681
682const dReal * dBodyGetTorque (dBodyID b)
683{
684 dAASSERT (b);
685 return b->tacc;
686}
687
688
689void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)
690{
691 dAASSERT (b);
692 b->facc[0] = x;
693 b->facc[1] = y;
694 b->facc[2] = z;
695}
696
697
698void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
699{
700 dAASSERT (b);
701 b->tacc[0] = x;
702 b->tacc[1] = y;
703 b->tacc[2] = z;
704}
705
706
707void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
708 dVector3 result)
709{
710 dAASSERT (b);
711 dVector3 prel,p;
712 prel[0] = px;
713 prel[1] = py;
714 prel[2] = pz;
715 prel[3] = 0;
716 dMULTIPLY0_331 (p,b->R,prel);
717 result[0] = p[0] + b->pos[0];
718 result[1] = p[1] + b->pos[1];
719 result[2] = p[2] + b->pos[2];
720}
721
722
723void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
724 dVector3 result)
725{
726 dAASSERT (b);
727 dVector3 prel,p;
728 prel[0] = px;
729 prel[1] = py;
730 prel[2] = pz;
731 prel[3] = 0;
732 dMULTIPLY0_331 (p,b->R,prel);
733 result[0] = b->lvel[0];
734 result[1] = b->lvel[1];
735 result[2] = b->lvel[2];
736 dCROSS (result,+=,b->avel,p);
737}
738
739
740void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
741 dVector3 result)
742{
743 dAASSERT (b);
744 dVector3 p;
745 p[0] = px - b->pos[0];
746 p[1] = py - b->pos[1];
747 p[2] = pz - b->pos[2];
748 p[3] = 0;
749 result[0] = b->lvel[0];
750 result[1] = b->lvel[1];
751 result[2] = b->lvel[2];
752 dCROSS (result,+=,b->avel,p);
753}
754
755
756void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
757 dVector3 result)
758{
759 dAASSERT (b);
760 dVector3 prel;
761 prel[0] = px - b->pos[0];
762 prel[1] = py - b->pos[1];
763 prel[2] = pz - b->pos[2];
764 prel[3] = 0;
765 dMULTIPLY1_331 (result,b->R,prel);
766}
767
768
769void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
770 dVector3 result)
771{
772 dAASSERT (b);
773 dVector3 p;
774 p[0] = px;
775 p[1] = py;
776 p[2] = pz;
777 p[3] = 0;
778 dMULTIPLY0_331 (result,b->R,p);
779}
780
781
782void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
783 dVector3 result)
784{
785 dAASSERT (b);
786 dVector3 p;
787 p[0] = px;
788 p[1] = py;
789 p[2] = pz;
790 p[3] = 0;
791 dMULTIPLY1_331 (result,b->R,p);
792}
793
794
795void dBodySetFiniteRotationMode (dBodyID b, int mode)
796{
797 dAASSERT (b);
798 b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis);
799 if (mode) {
800 b->flags |= dxBodyFlagFiniteRotation;
801 if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 ||
802 b->finite_rot_axis[2] != 0) {
803 b->flags |= dxBodyFlagFiniteRotationAxis;
804 }
805 }
806}
807
808
809void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)
810{
811 dAASSERT (b);
812 b->finite_rot_axis[0] = x;
813 b->finite_rot_axis[1] = y;
814 b->finite_rot_axis[2] = z;
815 if (x != 0 || y != 0 || z != 0) {
816 dNormalize3 (b->finite_rot_axis);
817 b->flags |= dxBodyFlagFiniteRotationAxis;
818 }
819 else {
820 b->flags &= ~dxBodyFlagFiniteRotationAxis;
821 }
822}
823
824
825int dBodyGetFiniteRotationMode (dBodyID b)
826{
827 dAASSERT (b);
828 return ((b->flags & dxBodyFlagFiniteRotation) != 0);
829}
830
831
832void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)
833{
834 dAASSERT (b);
835 result[0] = b->finite_rot_axis[0];
836 result[1] = b->finite_rot_axis[1];
837 result[2] = b->finite_rot_axis[2];
838}
839
840
841int dBodyGetNumJoints (dBodyID b)
842{
843 dAASSERT (b);
844 int count=0;
845 for (dxJointNode *n=b->firstjoint; n; n=n->next, count++);
846 return count;
847}
848
849
850dJointID dBodyGetJoint (dBodyID b, int index)
851{
852 dAASSERT (b);
853 int i=0;
854 for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) {
855 if (i == index) return n->joint;
856 }
857 return 0;
858}
859
860
861void dBodyEnable (dBodyID b)
862{
863 dAASSERT (b);
864 b->flags &= ~dxBodyDisabled;
865}
866
867
868void dBodyDisable (dBodyID b)
869{
870 dAASSERT (b);
871 b->flags |= dxBodyDisabled;
872}
873
874
875int dBodyIsEnabled (dBodyID b)
876{
877 dAASSERT (b);
878 return ((b->flags & dxBodyDisabled) == 0);
879}
880
881
882void dBodySetGravityMode (dBodyID b, int mode)
883{
884 dAASSERT (b);
885 if (mode) b->flags &= ~dxBodyNoGravity;
886 else b->flags |= dxBodyNoGravity;
887}
888
889
890int dBodyGetGravityMode (dBodyID b)
891{
892 dAASSERT (b);
893 return ((b->flags & dxBodyNoGravity) == 0);
894}
895
896//****************************************************************************
897// joints
898
899static void dJointInit (dxWorld *w, dxJoint *j)
900{
901 dIASSERT (w && j);
902 initObject (j,w);
903 j->vtable = 0;
904 j->flags = 0;
905 j->node[0].joint = j;
906 j->node[0].body = 0;
907 j->node[0].next = 0;
908 j->node[1].joint = j;
909 j->node[1].body = 0;
910 j->node[1].next = 0;
911 addObjectToList (j,(dObject **) &w->firstjoint);
912 w->nj++;
913}
914
915
916static dxJoint *createJoint (dWorldID w, dJointGroupID group,
917 dxJoint::Vtable *vtable)
918{
919 dIASSERT (w && vtable);
920 dxJoint *j;
921 if (group) {
922 j = (dxJoint*) group->stack.alloc (vtable->size);
923 group->num++;
924 }
925 else j = (dxJoint*) dAlloc (vtable->size);
926 dJointInit (w,j);
927 j->vtable = vtable;
928 if (group) j->flags |= dJOINT_INGROUP;
929 if (vtable->init) vtable->init (j);
930 j->feedback = 0;
931 /******************** breakable joint contribution ***********************/
932 j->breakInfo = 0;
933 /*************************************************************************/
934 return j;
935}
936
937
938dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)
939{
940 dAASSERT (w);
941 return createJoint (w,group,&__dball_vtable);
942}
943
944
945dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)
946{
947 dAASSERT (w);
948 return createJoint (w,group,&__dhinge_vtable);
949}
950
951
952dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)
953{
954 dAASSERT (w);
955 return createJoint (w,group,&__dslider_vtable);
956}
957
958
959dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
960 const dContact *c)
961{
962 dAASSERT (w && c);
963 dxJointContact *j = (dxJointContact *)
964 createJoint (w,group,&__dcontact_vtable);
965 j->contact = *c;
966 return j;
967}
968
969
970dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)
971{
972 dAASSERT (w);
973 return createJoint (w,group,&__dhinge2_vtable);
974}
975
976
977dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
978{
979 dAASSERT (w);
980 return createJoint (w,group,&__duniversal_vtable);
981}
982
983
984dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
985{
986 dAASSERT (w);
987 return createJoint (w,group,&__dfixed_vtable);
988}
989
990
991dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)
992{
993 dAASSERT (w);
994 return createJoint (w,group,&__dnull_vtable);
995}
996
997
998dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)
999{
1000 dAASSERT (w);
1001 return createJoint (w,group,&__damotor_vtable);
1002}
1003
1004
1005void dJointDestroy (dxJoint *j)
1006{
1007 dAASSERT (j);
1008 if (j->flags & dJOINT_INGROUP) return;
1009 removeJointReferencesFromAttachedBodies (j);
1010 removeObjectFromList (j);
1011 /******************** breakable joint contribution ***********************/
1012 if (j->breakInfo) delete j->breakInfo;
1013 /*************************************************************************/
1014 j->world->nj--;
1015 dFree (j,j->vtable->size);
1016}
1017
1018
1019dJointGroupID dJointGroupCreate (int max_size)
1020{
1021 // not any more ... dUASSERT (max_size > 0,"max size must be > 0");
1022 dxJointGroup *group = new dxJointGroup;
1023 group->num = 0;
1024 return group;
1025}
1026
1027
1028void dJointGroupDestroy (dJointGroupID group)
1029{
1030 dAASSERT (group);
1031 dJointGroupEmpty (group);
1032 delete group;
1033}
1034
1035
1036void dJointGroupEmpty (dJointGroupID group)
1037{
1038 // the joints in this group are detached starting from the most recently
1039 // added (at the top of the stack). this helps ensure that the various
1040 // linked lists are not traversed too much, as the joints will hopefully
1041 // be at the start of those lists.
1042 // if any group joints have their world pointer set to 0, their world was
1043 // previously destroyed. no special handling is required for these joints.
1044
1045 dAASSERT (group);
1046 int i;
1047 dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*));
1048 dxJoint *j = (dxJoint*) group->stack.rewind();
1049 for (i=0; i < group->num; i++) {
1050 jlist[i] = j;
1051 j = (dxJoint*) (group->stack.next (j->vtable->size));
1052 }
1053 for (i=group->num-1; i >= 0; i--) {
1054 if (jlist[i]->world) {
1055 removeJointReferencesFromAttachedBodies (jlist[i]);
1056 removeObjectFromList (jlist[i]);
1057 jlist[i]->world->nj--;
1058 }
1059 }
1060 group->num = 0;
1061 group->stack.freeAll();
1062}
1063
1064
1065void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
1066{
1067 // check arguments
1068 dUASSERT (joint,"bad joint argument");
1069 dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2");
1070 dxWorld *world = joint->world;
1071 dUASSERT ( (!body1 || body1->world == world) &&
1072 (!body2 || body2->world == world),
1073 "joint and bodies must be in same world");
1074
1075 // check if the joint can not be attached to just one body
1076 dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
1077 ((body1 != 0) ^ (body2 != 0))),
1078 "joint can not be attached to just one body");
1079
1080 // remove any existing body attachments
1081 if (joint->node[0].body || joint->node[1].body) {
1082 removeJointReferencesFromAttachedBodies (joint);
1083 }
1084
1085 // if a body is zero, make sure that it is body2, so 0 --> node[1].body
1086 if (body1==0) {
1087 body1 = body2;
1088 body2 = 0;
1089 joint->flags |= dJOINT_REVERSE;
1090 }
1091 else {
1092 joint->flags &= (~dJOINT_REVERSE);
1093 }
1094
1095 // attach to new bodies
1096 joint->node[0].body = body1;
1097 joint->node[1].body = body2;
1098 if (body1) {
1099 joint->node[1].next = body1->firstjoint;
1100 body1->firstjoint = &joint->node[1];
1101 }
1102 else joint->node[1].next = 0;
1103 if (body2) {
1104 joint->node[0].next = body2->firstjoint;
1105 body2->firstjoint = &joint->node[0];
1106 }
1107 else {
1108 joint->node[0].next = 0;
1109 }
1110}
1111
1112
1113void dJointSetData (dxJoint *joint, void *data)
1114{
1115 dAASSERT (joint);
1116 joint->userdata = data;
1117}
1118
1119
1120void *dJointGetData (dxJoint *joint)
1121{
1122 dAASSERT (joint);
1123 return joint->userdata;
1124}
1125
1126
1127int dJointGetType (dxJoint *joint)
1128{
1129 dAASSERT (joint);
1130 return joint->vtable->typenum;
1131}
1132
1133
1134dBodyID dJointGetBody (dxJoint *joint, int index)
1135{
1136 dAASSERT (joint);
1137 if (index >= 0 && index < 2) return joint->node[index].body;
1138 else return 0;
1139}
1140
1141
1142void dJointSetFeedback (dxJoint *joint, dJointFeedback *f)
1143{
1144 dAASSERT (joint);
1145 joint->feedback = f;
1146}
1147
1148
1149dJointFeedback *dJointGetFeedback (dxJoint *joint)
1150{
1151 dAASSERT (joint);
1152 return joint->feedback;
1153}
1154
1155
1156int dAreConnected (dBodyID b1, dBodyID b2)
1157{
1158 dAASSERT (b1 && b2);
1159 // look through b1's neighbour list for b2
1160 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1161 if (n->body == b2) return 1;
1162 }
1163 return 0;
1164}
1165
1166
1167int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type)
1168{
1169 dAASSERT (b1 && b2);
1170 // look through b1's neighbour list for b2
1171 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1172 if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1;
1173 }
1174 return 0;
1175}
1176
1177//****************************************************************************
1178// world
1179
1180dxWorld * dWorldCreate()
1181{
1182 dxWorld *w = new dxWorld;
1183 w->firstbody = 0;
1184 w->firstjoint = 0;
1185 w->nb = 0;
1186 w->nj = 0;
1187 dSetZero (w->gravity,4);
1188 w->global_erp = REAL(0.2);
1189#if defined(dSINGLE)
1190 w->global_cfm = 1e-5f;
1191#elif defined(dDOUBLE)
1192 w->global_cfm = 1e-10;
1193#else
1194 #error dSINGLE or dDOUBLE must be defined
1195#endif
1196 return w;
1197}
1198
1199
1200void dWorldDestroy (dxWorld *w)
1201{
1202 // delete all bodies and joints
1203 dAASSERT (w);
1204 dxBody *nextb, *b = w->firstbody;
1205 while (b) {
1206 nextb = (dxBody*) b->next;
1207 delete b;
1208 b = nextb;
1209 }
1210 dxJoint *nextj, *j = w->firstjoint;
1211 while (j) {
1212 nextj = (dxJoint*)j->next;
1213 if (j->flags & dJOINT_INGROUP) {
1214 // the joint is part of a group, so "deactivate" it instead
1215 j->world = 0;
1216 j->node[0].body = 0;
1217 j->node[0].next = 0;
1218 j->node[1].body = 0;
1219 j->node[1].next = 0;
1220 dMessage (0,"warning: destroying world containing grouped joints");
1221 }
1222 else {
1223 dFree (j,j->vtable->size);
1224 }
1225 j = nextj;
1226 }
1227 delete w;
1228}
1229
1230
1231void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
1232{
1233 dAASSERT (w);
1234 w->gravity[0] = x;
1235 w->gravity[1] = y;
1236 w->gravity[2] = z;
1237}
1238
1239
1240void dWorldGetGravity (dWorldID w, dVector3 g)
1241{
1242 dAASSERT (w);
1243 g[0] = w->gravity[0];
1244 g[1] = w->gravity[1];
1245 g[2] = w->gravity[2];
1246}
1247
1248
1249void dWorldSetERP (dWorldID w, dReal erp)
1250{
1251 dAASSERT (w);
1252 w->global_erp = erp;
1253}
1254
1255
1256dReal dWorldGetERP (dWorldID w)
1257{
1258 dAASSERT (w);
1259 return w->global_erp;
1260}
1261
1262
1263void dWorldSetCFM (dWorldID w, dReal cfm)
1264{
1265 dAASSERT (w);
1266 w->global_cfm = cfm;
1267}
1268
1269
1270dReal dWorldGetCFM (dWorldID w)
1271{
1272 dAASSERT (w);
1273 return w->global_cfm;
1274}
1275
1276
1277void dWorldStep (dWorldID w, dReal stepsize)
1278{
1279 dUASSERT (w,"bad world argument");
1280 dUASSERT (stepsize > 0,"stepsize must be > 0");
1281 processIslands (w,stepsize);
1282}
1283
1284
1285void dWorldImpulseToForce (dWorldID w, dReal stepsize,
1286 dReal ix, dReal iy, dReal iz,
1287 dVector3 force)
1288{
1289 dAASSERT (w);
1290 stepsize = dRecip(stepsize);
1291 force[0] = stepsize * ix;
1292 force[1] = stepsize * iy;
1293 force[2] = stepsize * iz;
1294 // @@@ force[3] = 0;
1295}
1296
1297//****************************************************************************
1298// testing
1299
1300#define NUM 100
1301
1302#define DO(x)
1303
1304
1305extern "C" void dTestDataStructures()
1306{
1307 int i;
1308 DO(printf ("testDynamicsStuff()\n"));
1309
1310 dBodyID body [NUM];
1311 int nb = 0;
1312 dJointID joint [NUM];
1313 int nj = 0;
1314
1315 for (i=0; i<NUM; i++) body[i] = 0;
1316 for (i=0; i<NUM; i++) joint[i] = 0;
1317
1318 DO(printf ("creating world\n"));
1319 dWorldID w = dWorldCreate();
1320 checkWorld (w);
1321
1322 for (;;) {
1323 if (nb < NUM && dRandReal() > 0.5) {
1324 DO(printf ("creating body\n"));
1325 body[nb] = dBodyCreate (w);
1326 DO(printf ("\t--> %p\n",body[nb]));
1327 nb++;
1328 checkWorld (w);
1329 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1330 }
1331 if (nj < NUM && nb > 2 && dRandReal() > 0.5) {
1332 dBodyID b1 = body [dRand() % nb];
1333 dBodyID b2 = body [dRand() % nb];
1334 if (b1 != b2) {
1335 DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
1336 joint[nj] = dJointCreateBall (w,0);
1337 DO(printf ("\t-->%p\n",joint[nj]));
1338 checkWorld (w);
1339 dJointAttach (joint[nj],b1,b2);
1340 nj++;
1341 checkWorld (w);
1342 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1343 }
1344 }
1345 if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
1346 dBodyID b1 = body [dRand() % nb];
1347 dBodyID b2 = body [dRand() % nb];
1348 if (b1 != b2) {
1349 int k = dRand() % nj;
1350 DO(printf ("reattaching joint %p\n",joint[k]));
1351 dJointAttach (joint[k],b1,b2);
1352 checkWorld (w);
1353 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1354 }
1355 }
1356 if (nb > 0 && dRandReal() > 0.5) {
1357 int k = dRand() % nb;
1358 DO(printf ("destroying body %p\n",body[k]));
1359 dBodyDestroy (body[k]);
1360 checkWorld (w);
1361 for (; k < (NUM-1); k++) body[k] = body[k+1];
1362 nb--;
1363 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1364 }
1365 if (nj > 0 && dRandReal() > 0.5) {
1366 int k = dRand() % nj;
1367 DO(printf ("destroying joint %p\n",joint[k]));
1368 dJointDestroy (joint[k]);
1369 checkWorld (w);
1370 for (; k < (NUM-1); k++) joint[k] = joint[k+1];
1371 nj--;
1372 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1373 }
1374 }
1375
1376 /*
1377 printf ("creating world\n");
1378 dWorldID w = dWorldCreate();
1379 checkWorld (w);
1380 printf ("creating body\n");
1381 dBodyID b1 = dBodyCreate (w);
1382 checkWorld (w);
1383 printf ("creating body\n");
1384 dBodyID b2 = dBodyCreate (w);
1385 checkWorld (w);
1386 printf ("creating joint\n");
1387 dJointID j = dJointCreateBall (w);
1388 checkWorld (w);
1389 printf ("attaching joint\n");
1390 dJointAttach (j,b1,b2);
1391 checkWorld (w);
1392 printf ("destroying joint\n");
1393 dJointDestroy (j);
1394 checkWorld (w);
1395 printf ("destroying body\n");
1396 dBodyDestroy (b1);
1397 checkWorld (w);
1398 printf ("destroying body\n");
1399 dBodyDestroy (b2);
1400 checkWorld (w);
1401 printf ("destroying world\n");
1402 dWorldDestroy (w);
1403 */
1404}
generated by cgit v1.1 at 2024-11-09 22:36:53 +0000