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