aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/ode.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/ode.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/ode.cpp1732
1 files changed, 0 insertions, 1732 deletions
diff --git a/libraries/ode-0.9/ode/src/ode.cpp b/libraries/ode-0.9/ode/src/ode.cpp
deleted file mode 100644
index 46f559a..0000000
--- a/libraries/ode-0.9/ode/src/ode.cpp
+++ /dev/null
@@ -1,1732 +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 "quickstep.h"
37#include "util.h"
38#include <ode/memory.h>
39#include <ode/error.h>
40
41// misc defines
42#define ALLOCA dALLOCA16
43
44//****************************************************************************
45// utility
46
47static inline void initObject (dObject *obj, dxWorld *w)
48{
49 obj->world = w;
50 obj->next = 0;
51 obj->tome = 0;
52 obj->userdata = 0;
53 obj->tag = 0;
54}
55
56
57// add an object `obj' to the list who's head pointer is pointed to by `first'.
58
59static inline void addObjectToList (dObject *obj, dObject **first)
60{
61 obj->next = *first;
62 obj->tome = first;
63 if (*first) (*first)->tome = &obj->next;
64 (*first) = obj;
65}
66
67
68// remove the object from the linked list
69
70static inline void removeObjectFromList (dObject *obj)
71{
72 if (obj->next) obj->next->tome = obj->tome;
73 *(obj->tome) = obj->next;
74 // safeguard
75 obj->next = 0;
76 obj->tome = 0;
77}
78
79
80// remove the joint from neighbour lists of all connected bodies
81
82static void removeJointReferencesFromAttachedBodies (dxJoint *j)
83{
84 for (int i=0; i<2; i++) {
85 dxBody *body = j->node[i].body;
86 if (body) {
87 dxJointNode *n = body->firstjoint;
88 dxJointNode *last = 0;
89 while (n) {
90 if (n->joint == j) {
91 if (last) last->next = n->next;
92 else body->firstjoint = n->next;
93 break;
94 }
95 last = n;
96 n = n->next;
97 }
98 }
99 }
100 j->node[0].body = 0;
101 j->node[0].next = 0;
102 j->node[1].body = 0;
103 j->node[1].next = 0;
104}
105
106//****************************************************************************
107// debugging
108
109// see if an object list loops on itself (if so, it's bad).
110
111static int listHasLoops (dObject *first)
112{
113 if (first==0 || first->next==0) return 0;
114 dObject *a=first,*b=first->next;
115 int skip=0;
116 while (b) {
117 if (a==b) return 1;
118 b = b->next;
119 if (skip) a = a->next;
120 skip ^= 1;
121 }
122 return 0;
123}
124
125
126// check the validity of the world data structures
127
128static void checkWorld (dxWorld *w)
129{
130 dxBody *b;
131 dxJoint *j;
132
133 // check there are no loops
134 if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops");
135 if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops");
136
137 // check lists are well formed (check `tome' pointers)
138 for (b=w->firstbody; b; b=(dxBody*)b->next) {
139 if (b->next && b->next->tome != &b->next)
140 dDebug (0,"bad tome pointer in body list");
141 }
142 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
143 if (j->next && j->next->tome != &j->next)
144 dDebug (0,"bad tome pointer in joint list");
145 }
146
147 // check counts
148 int n = 0;
149 for (b=w->firstbody; b; b=(dxBody*)b->next) n++;
150 if (w->nb != n) dDebug (0,"body count incorrect");
151 n = 0;
152 for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++;
153 if (w->nj != n) dDebug (0,"joint count incorrect");
154
155 // set all tag values to a known value
156 static int count = 0;
157 count++;
158 for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count;
159 for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count;
160
161 // check all body/joint world pointers are ok
162 for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w)
163 dDebug (0,"bad world pointer in body list");
164 for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w)
165 dDebug (0,"bad world pointer in joint list");
166
167 /*
168 // check for half-connected joints - actually now these are valid
169 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
170 if (j->node[0].body || j->node[1].body) {
171 if (!(j->node[0].body && j->node[1].body))
172 dDebug (0,"half connected joint found");
173 }
174 }
175 */
176
177 // check that every joint node appears in the joint lists of both bodies it
178 // attaches
179 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
180 for (int i=0; i<2; i++) {
181 if (j->node[i].body) {
182 int ok = 0;
183 for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) {
184 if (n->joint == j) ok = 1;
185 }
186 if (ok==0) dDebug (0,"joint not in joint list of attached body");
187 }
188 }
189 }
190
191 // check all body joint lists (correct body ptrs)
192 for (b=w->firstbody; b; b=(dxBody*)b->next) {
193 for (dxJointNode *n=b->firstjoint; n; n=n->next) {
194 if (&n->joint->node[0] == n) {
195 if (n->joint->node[1].body != b)
196 dDebug (0,"bad body pointer in joint node of body list (1)");
197 }
198 else {
199 if (n->joint->node[0].body != b)
200 dDebug (0,"bad body pointer in joint node of body list (2)");
201 }
202 if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body");
203 }
204 }
205
206 // check all body pointers in joints, check they are distinct
207 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
208 if (j->node[0].body && (j->node[0].body == j->node[1].body))
209 dDebug (0,"non-distinct body pointers in joint");
210 if ((j->node[0].body && j->node[0].body->tag != count) ||
211 (j->node[1].body && j->node[1].body->tag != count))
212 dDebug (0,"bad body pointer in joint");
213 }
214}
215
216
217void dWorldCheck (dxWorld *w)
218{
219 checkWorld (w);
220}
221
222//****************************************************************************
223// body
224dxWorld* dBodyGetWorld (dxBody* b)
225{
226 dAASSERT (b);
227 return b->world;
228}
229
230dxBody *dBodyCreate (dxWorld *w)
231{
232 dAASSERT (w);
233 dxBody *b = new dxBody;
234 initObject (b,w);
235 b->firstjoint = 0;
236 b->flags = 0;
237 b->geom = 0;
238 b->average_lvel_buffer = 0;
239 b->average_avel_buffer = 0;
240 dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0);
241 dSetZero (b->invI,4*3);
242 b->invI[0] = 1;
243 b->invI[5] = 1;
244 b->invI[10] = 1;
245 b->invMass = 1;
246 dSetZero (b->posr.pos,4);
247 dSetZero (b->q,4);
248 b->q[0] = 1;
249 dRSetIdentity (b->posr.R);
250 dSetZero (b->lvel,4);
251 dSetZero (b->avel,4);
252 dSetZero (b->facc,4);
253 dSetZero (b->tacc,4);
254 dSetZero (b->finite_rot_axis,4);
255 addObjectToList (b,(dObject **) &w->firstbody);
256 w->nb++;
257
258 // set auto-disable parameters
259 b->average_avel_buffer = b->average_lvel_buffer = 0; // no buffer at beginnin
260 dBodySetAutoDisableDefaults (b); // must do this after adding to world
261 b->adis_stepsleft = b->adis.idle_steps;
262 b->adis_timeleft = b->adis.idle_time;
263 b->average_counter = 0;
264 b->average_ready = 0; // average buffer not filled on the beginning
265 dBodySetAutoDisableAverageSamplesCount(b, b->adis.average_samples);
266
267 return b;
268}
269
270
271void dBodyDestroy (dxBody *b)
272{
273 dAASSERT (b);
274
275 // all geoms that link to this body must be notified that the body is about
276 // to disappear. note that the call to dGeomSetBody(geom,0) will result in
277 // dGeomGetBodyNext() returning 0 for the body, so we must get the next body
278 // before setting the body to 0.
279 dxGeom *next_geom = 0;
280 for (dxGeom *geom = b->geom; geom; geom = next_geom) {
281 next_geom = dGeomGetBodyNext (geom);
282 dGeomSetBody (geom,0);
283 }
284
285 // detach all neighbouring joints, then delete this body.
286 dxJointNode *n = b->firstjoint;
287 while (n) {
288 // sneaky trick to speed up removal of joint references (black magic)
289 n->joint->node[(n == n->joint->node)].body = 0;
290
291 dxJointNode *next = n->next;
292 n->next = 0;
293 removeJointReferencesFromAttachedBodies (n->joint);
294 n = next;
295 }
296 removeObjectFromList (b);
297 b->world->nb--;
298
299 // delete the average buffers
300 if(b->average_lvel_buffer)
301 {
302 delete[] (b->average_lvel_buffer);
303 b->average_lvel_buffer = 0;
304 }
305 if(b->average_avel_buffer)
306 {
307 delete[] (b->average_avel_buffer);
308 b->average_avel_buffer = 0;
309 }
310
311 delete b;
312}
313
314
315void dBodySetData (dBodyID b, void *data)
316{
317 dAASSERT (b);
318 b->userdata = data;
319}
320
321
322void *dBodyGetData (dBodyID b)
323{
324 dAASSERT (b);
325 return b->userdata;
326}
327
328
329void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)
330{
331 dAASSERT (b);
332 b->posr.pos[0] = x;
333 b->posr.pos[1] = y;
334 b->posr.pos[2] = z;
335
336 // notify all attached geoms that this body has moved
337 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
338 dGeomMoved (geom);
339}
340
341
342void dBodySetRotation (dBodyID b, const dMatrix3 R)
343{
344 dAASSERT (b && R);
345 dQuaternion q;
346 dRtoQ (R,q);
347 dNormalize4 (q);
348 b->q[0] = q[0];
349 b->q[1] = q[1];
350 b->q[2] = q[2];
351 b->q[3] = q[3];
352 dQtoR (b->q,b->posr.R);
353
354 // notify all attached geoms that this body has moved
355 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
356 dGeomMoved (geom);
357}
358
359
360void dBodySetQuaternion (dBodyID b, const dQuaternion q)
361{
362 dAASSERT (b && q);
363 b->q[0] = q[0];
364 b->q[1] = q[1];
365 b->q[2] = q[2];
366 b->q[3] = q[3];
367 dNormalize4 (b->q);
368 dQtoR (b->q,b->posr.R);
369
370 // notify all attached geoms that this body has moved
371 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
372 dGeomMoved (geom);
373}
374
375
376void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z)
377{
378 dAASSERT (b);
379 b->lvel[0] = x;
380 b->lvel[1] = y;
381 b->lvel[2] = z;
382}
383
384
385void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)
386{
387 dAASSERT (b);
388 b->avel[0] = x;
389 b->avel[1] = y;
390 b->avel[2] = z;
391}
392
393
394const dReal * dBodyGetPosition (dBodyID b)
395{
396 dAASSERT (b);
397 return b->posr.pos;
398}
399
400
401void dBodyCopyPosition (dBodyID b, dVector3 pos)
402{
403 dAASSERT (b);
404 dReal* src = b->posr.pos;
405 pos[0] = src[0];
406 pos[1] = src[1];
407 pos[2] = src[2];
408}
409
410
411const dReal * dBodyGetRotation (dBodyID b)
412{
413 dAASSERT (b);
414 return b->posr.R;
415}
416
417
418void dBodyCopyRotation (dBodyID b, dMatrix3 R)
419{
420 dAASSERT (b);
421 const dReal* src = b->posr.R;
422 R[0] = src[0];
423 R[1] = src[1];
424 R[2] = src[2];
425 R[3] = src[3];
426 R[4] = src[4];
427 R[5] = src[5];
428 R[6] = src[6];
429 R[7] = src[7];
430 R[8] = src[8];
431 R[9] = src[9];
432 R[10] = src[10];
433 R[11] = src[11];
434}
435
436
437const dReal * dBodyGetQuaternion (dBodyID b)
438{
439 dAASSERT (b);
440 return b->q;
441}
442
443
444void dBodyCopyQuaternion (dBodyID b, dQuaternion quat)
445{
446 dAASSERT (b);
447 dReal* src = b->q;
448 quat[0] = src[0];
449 quat[1] = src[1];
450 quat[2] = src[2];
451 quat[3] = src[3];
452}
453
454
455const dReal * dBodyGetLinearVel (dBodyID b)
456{
457 dAASSERT (b);
458 return b->lvel;
459}
460
461
462const dReal * dBodyGetAngularVel (dBodyID b)
463{
464 dAASSERT (b);
465 return b->avel;
466}
467
468
469void dBodySetMass (dBodyID b, const dMass *mass)
470{
471 dAASSERT (b && mass );
472 dIASSERT(dMassCheck(mass));
473
474 // The centre of mass must be at the origin.
475 // Use dMassTranslate( mass, -mass->c[0], -mass->c[1], -mass->c[2] ) to correct it.
476 dUASSERT( fabs( mass->c[0] ) <= dEpsilon &&
477 fabs( mass->c[1] ) <= dEpsilon &&
478 fabs( mass->c[2] ) <= dEpsilon, "The centre of mass must be at the origin." )
479
480 memcpy (&b->mass,mass,sizeof(dMass));
481 if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) {
482 dDEBUGMSG ("inertia must be positive definite!");
483 dRSetIdentity (b->invI);
484 }
485 b->invMass = dRecip(b->mass.mass);
486}
487
488
489void dBodyGetMass (dBodyID b, dMass *mass)
490{
491 dAASSERT (b && mass);
492 memcpy (mass,&b->mass,sizeof(dMass));
493}
494
495
496void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)
497{
498 dAASSERT (b);
499 b->facc[0] += fx;
500 b->facc[1] += fy;
501 b->facc[2] += fz;
502}
503
504
505void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
506{
507 dAASSERT (b);
508 b->tacc[0] += fx;
509 b->tacc[1] += fy;
510 b->tacc[2] += fz;
511}
512
513
514void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)
515{
516 dAASSERT (b);
517 dVector3 t1,t2;
518 t1[0] = fx;
519 t1[1] = fy;
520 t1[2] = fz;
521 t1[3] = 0;
522 dMULTIPLY0_331 (t2,b->posr.R,t1);
523 b->facc[0] += t2[0];
524 b->facc[1] += t2[1];
525 b->facc[2] += t2[2];
526}
527
528
529void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
530{
531 dAASSERT (b);
532 dVector3 t1,t2;
533 t1[0] = fx;
534 t1[1] = fy;
535 t1[2] = fz;
536 t1[3] = 0;
537 dMULTIPLY0_331 (t2,b->posr.R,t1);
538 b->tacc[0] += t2[0];
539 b->tacc[1] += t2[1];
540 b->tacc[2] += t2[2];
541}
542
543
544void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
545 dReal px, dReal py, dReal pz)
546{
547 dAASSERT (b);
548 b->facc[0] += fx;
549 b->facc[1] += fy;
550 b->facc[2] += fz;
551 dVector3 f,q;
552 f[0] = fx;
553 f[1] = fy;
554 f[2] = fz;
555 q[0] = px - b->posr.pos[0];
556 q[1] = py - b->posr.pos[1];
557 q[2] = pz - b->posr.pos[2];
558 dCROSS (b->tacc,+=,q,f);
559}
560
561
562void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
563 dReal px, dReal py, dReal pz)
564{
565 dAASSERT (b);
566 dVector3 prel,f,p;
567 f[0] = fx;
568 f[1] = fy;
569 f[2] = fz;
570 f[3] = 0;
571 prel[0] = px;
572 prel[1] = py;
573 prel[2] = pz;
574 prel[3] = 0;
575 dMULTIPLY0_331 (p,b->posr.R,prel);
576 b->facc[0] += f[0];
577 b->facc[1] += f[1];
578 b->facc[2] += f[2];
579 dCROSS (b->tacc,+=,p,f);
580}
581
582
583void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
584 dReal px, dReal py, dReal pz)
585{
586 dAASSERT (b);
587 dVector3 frel,f;
588 frel[0] = fx;
589 frel[1] = fy;
590 frel[2] = fz;
591 frel[3] = 0;
592 dMULTIPLY0_331 (f,b->posr.R,frel);
593 b->facc[0] += f[0];
594 b->facc[1] += f[1];
595 b->facc[2] += f[2];
596 dVector3 q;
597 q[0] = px - b->posr.pos[0];
598 q[1] = py - b->posr.pos[1];
599 q[2] = pz - b->posr.pos[2];
600 dCROSS (b->tacc,+=,q,f);
601}
602
603
604void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
605 dReal px, dReal py, dReal pz)
606{
607 dAASSERT (b);
608 dVector3 frel,prel,f,p;
609 frel[0] = fx;
610 frel[1] = fy;
611 frel[2] = fz;
612 frel[3] = 0;
613 prel[0] = px;
614 prel[1] = py;
615 prel[2] = pz;
616 prel[3] = 0;
617 dMULTIPLY0_331 (f,b->posr.R,frel);
618 dMULTIPLY0_331 (p,b->posr.R,prel);
619 b->facc[0] += f[0];
620 b->facc[1] += f[1];
621 b->facc[2] += f[2];
622 dCROSS (b->tacc,+=,p,f);
623}
624
625
626const dReal * dBodyGetForce (dBodyID b)
627{
628 dAASSERT (b);
629 return b->facc;
630}
631
632
633const dReal * dBodyGetTorque (dBodyID b)
634{
635 dAASSERT (b);
636 return b->tacc;
637}
638
639
640void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)
641{
642 dAASSERT (b);
643 b->facc[0] = x;
644 b->facc[1] = y;
645 b->facc[2] = z;
646}
647
648
649void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
650{
651 dAASSERT (b);
652 b->tacc[0] = x;
653 b->tacc[1] = y;
654 b->tacc[2] = z;
655}
656
657
658void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
659 dVector3 result)
660{
661 dAASSERT (b);
662 dVector3 prel,p;
663 prel[0] = px;
664 prel[1] = py;
665 prel[2] = pz;
666 prel[3] = 0;
667 dMULTIPLY0_331 (p,b->posr.R,prel);
668 result[0] = p[0] + b->posr.pos[0];
669 result[1] = p[1] + b->posr.pos[1];
670 result[2] = p[2] + b->posr.pos[2];
671}
672
673
674void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
675 dVector3 result)
676{
677 dAASSERT (b);
678 dVector3 prel,p;
679 prel[0] = px;
680 prel[1] = py;
681 prel[2] = pz;
682 prel[3] = 0;
683 dMULTIPLY0_331 (p,b->posr.R,prel);
684 result[0] = b->lvel[0];
685 result[1] = b->lvel[1];
686 result[2] = b->lvel[2];
687 dCROSS (result,+=,b->avel,p);
688}
689
690
691void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
692 dVector3 result)
693{
694 dAASSERT (b);
695 dVector3 p;
696 p[0] = px - b->posr.pos[0];
697 p[1] = py - b->posr.pos[1];
698 p[2] = pz - b->posr.pos[2];
699 p[3] = 0;
700 result[0] = b->lvel[0];
701 result[1] = b->lvel[1];
702 result[2] = b->lvel[2];
703 dCROSS (result,+=,b->avel,p);
704}
705
706
707void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
708 dVector3 result)
709{
710 dAASSERT (b);
711 dVector3 prel;
712 prel[0] = px - b->posr.pos[0];
713 prel[1] = py - b->posr.pos[1];
714 prel[2] = pz - b->posr.pos[2];
715 prel[3] = 0;
716 dMULTIPLY1_331 (result,b->posr.R,prel);
717}
718
719
720void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
721 dVector3 result)
722{
723 dAASSERT (b);
724 dVector3 p;
725 p[0] = px;
726 p[1] = py;
727 p[2] = pz;
728 p[3] = 0;
729 dMULTIPLY0_331 (result,b->posr.R,p);
730}
731
732
733void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
734 dVector3 result)
735{
736 dAASSERT (b);
737 dVector3 p;
738 p[0] = px;
739 p[1] = py;
740 p[2] = pz;
741 p[3] = 0;
742 dMULTIPLY1_331 (result,b->posr.R,p);
743}
744
745
746void dBodySetFiniteRotationMode (dBodyID b, int mode)
747{
748 dAASSERT (b);
749 b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis);
750 if (mode) {
751 b->flags |= dxBodyFlagFiniteRotation;
752 if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 ||
753 b->finite_rot_axis[2] != 0) {
754 b->flags |= dxBodyFlagFiniteRotationAxis;
755 }
756 }
757}
758
759
760void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)
761{
762 dAASSERT (b);
763 b->finite_rot_axis[0] = x;
764 b->finite_rot_axis[1] = y;
765 b->finite_rot_axis[2] = z;
766 if (x != 0 || y != 0 || z != 0) {
767 dNormalize3 (b->finite_rot_axis);
768 b->flags |= dxBodyFlagFiniteRotationAxis;
769 }
770 else {
771 b->flags &= ~dxBodyFlagFiniteRotationAxis;
772 }
773}
774
775
776int dBodyGetFiniteRotationMode (dBodyID b)
777{
778 dAASSERT (b);
779 return ((b->flags & dxBodyFlagFiniteRotation) != 0);
780}
781
782
783void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)
784{
785 dAASSERT (b);
786 result[0] = b->finite_rot_axis[0];
787 result[1] = b->finite_rot_axis[1];
788 result[2] = b->finite_rot_axis[2];
789}
790
791
792int dBodyGetNumJoints (dBodyID b)
793{
794 dAASSERT (b);
795 int count=0;
796 for (dxJointNode *n=b->firstjoint; n; n=n->next, count++);
797 return count;
798}
799
800
801dJointID dBodyGetJoint (dBodyID b, int index)
802{
803 dAASSERT (b);
804 int i=0;
805 for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) {
806 if (i == index) return n->joint;
807 }
808 return 0;
809}
810
811
812void dBodyEnable (dBodyID b)
813{
814 dAASSERT (b);
815 b->flags &= ~dxBodyDisabled;
816 b->adis_stepsleft = b->adis.idle_steps;
817 b->adis_timeleft = b->adis.idle_time;
818 // no code for average-processing needed here
819}
820
821
822void dBodyDisable (dBodyID b)
823{
824 dAASSERT (b);
825 b->flags |= dxBodyDisabled;
826}
827
828
829int dBodyIsEnabled (dBodyID b)
830{
831 dAASSERT (b);
832 return ((b->flags & dxBodyDisabled) == 0);
833}
834
835
836void dBodySetGravityMode (dBodyID b, int mode)
837{
838 dAASSERT (b);
839 if (mode) b->flags &= ~dxBodyNoGravity;
840 else b->flags |= dxBodyNoGravity;
841}
842
843
844int dBodyGetGravityMode (dBodyID b)
845{
846 dAASSERT (b);
847 return ((b->flags & dxBodyNoGravity) == 0);
848}
849
850
851// body auto-disable functions
852
853dReal dBodyGetAutoDisableLinearThreshold (dBodyID b)
854{
855 dAASSERT(b);
856 return dSqrt (b->adis.linear_average_threshold);
857}
858
859
860void dBodySetAutoDisableLinearThreshold (dBodyID b, dReal linear_average_threshold)
861{
862 dAASSERT(b);
863 b->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
864}
865
866
867dReal dBodyGetAutoDisableAngularThreshold (dBodyID b)
868{
869 dAASSERT(b);
870 return dSqrt (b->adis.angular_average_threshold);
871}
872
873
874void dBodySetAutoDisableAngularThreshold (dBodyID b, dReal angular_average_threshold)
875{
876 dAASSERT(b);
877 b->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
878}
879
880
881int dBodyGetAutoDisableAverageSamplesCount (dBodyID b)
882{
883 dAASSERT(b);
884 return b->adis.average_samples;
885}
886
887
888void dBodySetAutoDisableAverageSamplesCount (dBodyID b, unsigned int average_samples_count)
889{
890 dAASSERT(b);
891 b->adis.average_samples = average_samples_count;
892 // update the average buffers
893 if(b->average_lvel_buffer)
894 {
895 delete[] b->average_lvel_buffer;
896 b->average_lvel_buffer = 0;
897 }
898 if(b->average_avel_buffer)
899 {
900 delete[] b->average_avel_buffer;
901 b->average_avel_buffer = 0;
902 }
903 if(b->adis.average_samples > 0)
904 {
905 b->average_lvel_buffer = new dVector3[b->adis.average_samples];
906 b->average_avel_buffer = new dVector3[b->adis.average_samples];
907 }
908 else
909 {
910 b->average_lvel_buffer = 0;
911 b->average_avel_buffer = 0;
912 }
913 // new buffer is empty
914 b->average_counter = 0;
915 b->average_ready = 0;
916}
917
918
919int dBodyGetAutoDisableSteps (dBodyID b)
920{
921 dAASSERT(b);
922 return b->adis.idle_steps;
923}
924
925
926void dBodySetAutoDisableSteps (dBodyID b, int steps)
927{
928 dAASSERT(b);
929 b->adis.idle_steps = steps;
930}
931
932
933dReal dBodyGetAutoDisableTime (dBodyID b)
934{
935 dAASSERT(b);
936 return b->adis.idle_time;
937}
938
939
940void dBodySetAutoDisableTime (dBodyID b, dReal time)
941{
942 dAASSERT(b);
943 b->adis.idle_time = time;
944}
945
946
947int dBodyGetAutoDisableFlag (dBodyID b)
948{
949 dAASSERT(b);
950 return ((b->flags & dxBodyAutoDisable) != 0);
951}
952
953
954void dBodySetAutoDisableFlag (dBodyID b, int do_auto_disable)
955{
956 dAASSERT(b);
957 if (!do_auto_disable)
958 {
959 b->flags &= ~dxBodyAutoDisable;
960 // (mg) we should also reset the IsDisabled state to correspond to the DoDisabling flag
961 b->flags &= ~dxBodyDisabled;
962 b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world);
963 b->adis.idle_time = dWorldGetAutoDisableTime(b->world);
964 // resetting the average calculations too
965 dBodySetAutoDisableAverageSamplesCount(b, dWorldGetAutoDisableAverageSamplesCount(b->world) );
966 }
967 else
968 {
969 b->flags |= dxBodyAutoDisable;
970 }
971}
972
973
974void dBodySetAutoDisableDefaults (dBodyID b)
975{
976 dAASSERT(b);
977 dWorldID w = b->world;
978 dAASSERT(w);
979 b->adis = w->adis;
980 dBodySetAutoDisableFlag (b, w->adis_flag);
981}
982
983//****************************************************************************
984// joints
985
986static void dJointInit (dxWorld *w, dxJoint *j)
987{
988 dIASSERT (w && j);
989 initObject (j,w);
990 j->vtable = 0;
991 j->flags = 0;
992 j->node[0].joint = j;
993 j->node[0].body = 0;
994 j->node[0].next = 0;
995 j->node[1].joint = j;
996 j->node[1].body = 0;
997 j->node[1].next = 0;
998 dSetZero (j->lambda,6);
999 addObjectToList (j,(dObject **) &w->firstjoint);
1000 w->nj++;
1001}
1002
1003
1004static dxJoint *createJoint (dWorldID w, dJointGroupID group,
1005 dxJoint::Vtable *vtable)
1006{
1007 dIASSERT (w && vtable);
1008 dxJoint *j;
1009 if (group) {
1010 j = (dxJoint*) group->stack.alloc (vtable->size);
1011 group->num++;
1012 }
1013 else j = (dxJoint*) dAlloc (vtable->size);
1014 dJointInit (w,j);
1015 j->vtable = vtable;
1016 if (group) j->flags |= dJOINT_INGROUP;
1017 if (vtable->init) vtable->init (j);
1018 j->feedback = 0;
1019 return j;
1020}
1021
1022
1023dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)
1024{
1025 dAASSERT (w);
1026 return createJoint (w,group,&__dball_vtable);
1027}
1028
1029
1030dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)
1031{
1032 dAASSERT (w);
1033 return createJoint (w,group,&__dhinge_vtable);
1034}
1035
1036
1037dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)
1038{
1039 dAASSERT (w);
1040 return createJoint (w,group,&__dslider_vtable);
1041}
1042
1043
1044dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
1045 const dContact *c)
1046{
1047 dAASSERT (w && c);
1048 dxJointContact *j = (dxJointContact *)
1049 createJoint (w,group,&__dcontact_vtable);
1050 j->contact = *c;
1051 return j;
1052}
1053
1054
1055dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)
1056{
1057 dAASSERT (w);
1058 return createJoint (w,group,&__dhinge2_vtable);
1059}
1060
1061
1062dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
1063{
1064 dAASSERT (w);
1065 return createJoint (w,group,&__duniversal_vtable);
1066}
1067
1068dxJoint * dJointCreatePR (dWorldID w, dJointGroupID group)
1069{
1070 dAASSERT (w);
1071 return createJoint (w,group,&__dPR_vtable);
1072}
1073
1074dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
1075{
1076 dAASSERT (w);
1077 return createJoint (w,group,&__dfixed_vtable);
1078}
1079
1080
1081dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)
1082{
1083 dAASSERT (w);
1084 return createJoint (w,group,&__dnull_vtable);
1085}
1086
1087
1088dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)
1089{
1090 dAASSERT (w);
1091 return createJoint (w,group,&__damotor_vtable);
1092}
1093
1094dxJoint * dJointCreateLMotor (dWorldID w, dJointGroupID group)
1095{
1096 dAASSERT (w);
1097 return createJoint (w,group,&__dlmotor_vtable);
1098}
1099
1100dxJoint * dJointCreatePlane2D (dWorldID w, dJointGroupID group)
1101{
1102 dAASSERT (w);
1103 return createJoint (w,group,&__dplane2d_vtable);
1104}
1105
1106void dJointDestroy (dxJoint *j)
1107{
1108 dAASSERT (j);
1109 if (j->flags & dJOINT_INGROUP) return;
1110 removeJointReferencesFromAttachedBodies (j);
1111 removeObjectFromList (j);
1112 j->world->nj--;
1113 dFree (j,j->vtable->size);
1114}
1115
1116
1117dJointGroupID dJointGroupCreate (int max_size)
1118{
1119 // not any more ... dUASSERT (max_size > 0,"max size must be > 0");
1120 dxJointGroup *group = new dxJointGroup;
1121 group->num = 0;
1122 return group;
1123}
1124
1125
1126void dJointGroupDestroy (dJointGroupID group)
1127{
1128 dAASSERT (group);
1129 dJointGroupEmpty (group);
1130 delete group;
1131}
1132
1133
1134void dJointGroupEmpty (dJointGroupID group)
1135{
1136 // the joints in this group are detached starting from the most recently
1137 // added (at the top of the stack). this helps ensure that the various
1138 // linked lists are not traversed too much, as the joints will hopefully
1139 // be at the start of those lists.
1140 // if any group joints have their world pointer set to 0, their world was
1141 // previously destroyed. no special handling is required for these joints.
1142
1143 dAASSERT (group);
1144 int i;
1145 dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*));
1146 dxJoint *j = (dxJoint*) group->stack.rewind();
1147 for (i=0; i < group->num; i++) {
1148 jlist[i] = j;
1149 j = (dxJoint*) (group->stack.next (j->vtable->size));
1150 }
1151 for (i=group->num-1; i >= 0; i--) {
1152 if (jlist[i]->world) {
1153 removeJointReferencesFromAttachedBodies (jlist[i]);
1154 removeObjectFromList (jlist[i]);
1155 jlist[i]->world->nj--;
1156 }
1157 }
1158 group->num = 0;
1159 group->stack.freeAll();
1160}
1161
1162
1163void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
1164{
1165 // check arguments
1166 dUASSERT (joint,"bad joint argument");
1167 dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2");
1168 dxWorld *world = joint->world;
1169 dUASSERT ( (!body1 || body1->world == world) &&
1170 (!body2 || body2->world == world),
1171 "joint and bodies must be in same world");
1172
1173 // check if the joint can not be attached to just one body
1174 dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
1175 ((body1 != 0) ^ (body2 != 0))),
1176 "joint can not be attached to just one body");
1177
1178 // remove any existing body attachments
1179 if (joint->node[0].body || joint->node[1].body) {
1180 removeJointReferencesFromAttachedBodies (joint);
1181 }
1182
1183 // if a body is zero, make sure that it is body2, so 0 --> node[1].body
1184 if (body1==0) {
1185 body1 = body2;
1186 body2 = 0;
1187 joint->flags |= dJOINT_REVERSE;
1188 }
1189 else {
1190 joint->flags &= (~dJOINT_REVERSE);
1191 }
1192
1193 // attach to new bodies
1194 joint->node[0].body = body1;
1195 joint->node[1].body = body2;
1196 if (body1) {
1197 joint->node[1].next = body1->firstjoint;
1198 body1->firstjoint = &joint->node[1];
1199 }
1200 else joint->node[1].next = 0;
1201 if (body2) {
1202 joint->node[0].next = body2->firstjoint;
1203 body2->firstjoint = &joint->node[0];
1204 }
1205 else {
1206 joint->node[0].next = 0;
1207 }
1208}
1209
1210
1211void dJointSetData (dxJoint *joint, void *data)
1212{
1213 dAASSERT (joint);
1214 joint->userdata = data;
1215}
1216
1217
1218void *dJointGetData (dxJoint *joint)
1219{
1220 dAASSERT (joint);
1221 return joint->userdata;
1222}
1223
1224
1225int dJointGetType (dxJoint *joint)
1226{
1227 dAASSERT (joint);
1228 return joint->vtable->typenum;
1229}
1230
1231
1232dBodyID dJointGetBody (dxJoint *joint, int index)
1233{
1234 dAASSERT (joint);
1235 if (index == 0 || index == 1) {
1236 if (joint->flags & dJOINT_REVERSE) return joint->node[1-index].body;
1237 else return joint->node[index].body;
1238 }
1239 else return 0;
1240}
1241
1242
1243void dJointSetFeedback (dxJoint *joint, dJointFeedback *f)
1244{
1245 dAASSERT (joint);
1246 joint->feedback = f;
1247}
1248
1249
1250dJointFeedback *dJointGetFeedback (dxJoint *joint)
1251{
1252 dAASSERT (joint);
1253 return joint->feedback;
1254}
1255
1256
1257
1258dJointID dConnectingJoint (dBodyID in_b1, dBodyID in_b2)
1259{
1260 dAASSERT (in_b1 || in_b2);
1261
1262 dBodyID b1, b2;
1263
1264 if (in_b1 == 0) {
1265 b1 = in_b2;
1266 b2 = in_b1;
1267 }
1268 else {
1269 b1 = in_b1;
1270 b2 = in_b2;
1271 }
1272
1273 // look through b1's neighbour list for b2
1274 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1275 if (n->body == b2) return n->joint;
1276 }
1277
1278 return 0;
1279}
1280
1281
1282
1283int dConnectingJointList (dBodyID in_b1, dBodyID in_b2, dJointID* out_list)
1284{
1285 dAASSERT (in_b1 || in_b2);
1286
1287
1288 dBodyID b1, b2;
1289
1290 if (in_b1 == 0) {
1291 b1 = in_b2;
1292 b2 = in_b1;
1293 }
1294 else {
1295 b1 = in_b1;
1296 b2 = in_b2;
1297 }
1298
1299 // look through b1's neighbour list for b2
1300 int numConnectingJoints = 0;
1301 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1302 if (n->body == b2)
1303 out_list[numConnectingJoints++] = n->joint;
1304 }
1305
1306 return numConnectingJoints;
1307}
1308
1309
1310int dAreConnected (dBodyID b1, dBodyID b2)
1311{
1312 dAASSERT (b1 && b2);
1313 // look through b1's neighbour list for b2
1314 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1315 if (n->body == b2) return 1;
1316 }
1317 return 0;
1318}
1319
1320
1321int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type)
1322{
1323 dAASSERT (b1 && b2);
1324 // look through b1's neighbour list for b2
1325 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1326 if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1;
1327 }
1328 return 0;
1329}
1330
1331//****************************************************************************
1332// world
1333
1334dxWorld * dWorldCreate()
1335{
1336 dxWorld *w = new dxWorld;
1337 w->firstbody = 0;
1338 w->firstjoint = 0;
1339 w->nb = 0;
1340 w->nj = 0;
1341 dSetZero (w->gravity,4);
1342 w->global_erp = REAL(0.2);
1343#if defined(dSINGLE)
1344 w->global_cfm = 1e-5f;
1345#elif defined(dDOUBLE)
1346 w->global_cfm = 1e-10;
1347#else
1348 #error dSINGLE or dDOUBLE must be defined
1349#endif
1350
1351 w->adis.idle_steps = 10;
1352 w->adis.idle_time = 0;
1353 w->adis_flag = 0;
1354 w->adis.average_samples = 1; // Default is 1 sample => Instantaneous velocity
1355 w->adis.angular_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared)
1356 w->adis.linear_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared)
1357
1358 w->qs.num_iterations = 20;
1359 w->qs.w = REAL(1.3);
1360
1361 w->contactp.max_vel = dInfinity;
1362 w->contactp.min_depth = 0;
1363
1364 return w;
1365}
1366
1367
1368void dWorldDestroy (dxWorld *w)
1369{
1370 // delete all bodies and joints
1371 dAASSERT (w);
1372 dxBody *nextb, *b = w->firstbody;
1373 while (b) {
1374 nextb = (dxBody*) b->next;
1375 if(b->average_lvel_buffer)
1376 {
1377 delete[] (b->average_lvel_buffer);
1378 b->average_lvel_buffer = 0;
1379 }
1380 if(b->average_avel_buffer)
1381 {
1382 delete[] (b->average_avel_buffer);
1383 b->average_avel_buffer = 0;
1384 }
1385 dBodyDestroy(b); // calling here dBodyDestroy for correct destroying! (i.e. the average buffers)
1386 b = nextb;
1387 }
1388 dxJoint *nextj, *j = w->firstjoint;
1389 while (j) {
1390 nextj = (dxJoint*)j->next;
1391 if (j->flags & dJOINT_INGROUP) {
1392 // the joint is part of a group, so "deactivate" it instead
1393 j->world = 0;
1394 j->node[0].body = 0;
1395 j->node[0].next = 0;
1396 j->node[1].body = 0;
1397 j->node[1].next = 0;
1398 dMessage (0,"warning: destroying world containing grouped joints");
1399 }
1400 else {
1401 dFree (j,j->vtable->size);
1402 }
1403 j = nextj;
1404 }
1405 delete w;
1406}
1407
1408
1409void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
1410{
1411 dAASSERT (w);
1412 w->gravity[0] = x;
1413 w->gravity[1] = y;
1414 w->gravity[2] = z;
1415}
1416
1417
1418void dWorldGetGravity (dWorldID w, dVector3 g)
1419{
1420 dAASSERT (w);
1421 g[0] = w->gravity[0];
1422 g[1] = w->gravity[1];
1423 g[2] = w->gravity[2];
1424}
1425
1426
1427void dWorldSetERP (dWorldID w, dReal erp)
1428{
1429 dAASSERT (w);
1430 w->global_erp = erp;
1431}
1432
1433
1434dReal dWorldGetERP (dWorldID w)
1435{
1436 dAASSERT (w);
1437 return w->global_erp;
1438}
1439
1440
1441void dWorldSetCFM (dWorldID w, dReal cfm)
1442{
1443 dAASSERT (w);
1444 w->global_cfm = cfm;
1445}
1446
1447
1448dReal dWorldGetCFM (dWorldID w)
1449{
1450 dAASSERT (w);
1451 return w->global_cfm;
1452}
1453
1454
1455void dWorldStep (dWorldID w, dReal stepsize)
1456{
1457 dUASSERT (w,"bad world argument");
1458 dUASSERT (stepsize > 0,"stepsize must be > 0");
1459 dxProcessIslands (w,stepsize,&dInternalStepIsland);
1460}
1461
1462
1463void dWorldQuickStep (dWorldID w, dReal stepsize)
1464{
1465 dUASSERT (w,"bad world argument");
1466 dUASSERT (stepsize > 0,"stepsize must be > 0");
1467 dxProcessIslands (w,stepsize,&dxQuickStepper);
1468}
1469
1470
1471void dWorldImpulseToForce (dWorldID w, dReal stepsize,
1472 dReal ix, dReal iy, dReal iz,
1473 dVector3 force)
1474{
1475 dAASSERT (w);
1476 stepsize = dRecip(stepsize);
1477 force[0] = stepsize * ix;
1478 force[1] = stepsize * iy;
1479 force[2] = stepsize * iz;
1480 // @@@ force[3] = 0;
1481}
1482
1483
1484// world auto-disable functions
1485
1486dReal dWorldGetAutoDisableLinearThreshold (dWorldID w)
1487{
1488 dAASSERT(w);
1489 return dSqrt (w->adis.linear_average_threshold);
1490}
1491
1492
1493void dWorldSetAutoDisableLinearThreshold (dWorldID w, dReal linear_average_threshold)
1494{
1495 dAASSERT(w);
1496 w->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
1497}
1498
1499
1500dReal dWorldGetAutoDisableAngularThreshold (dWorldID w)
1501{
1502 dAASSERT(w);
1503 return dSqrt (w->adis.angular_average_threshold);
1504}
1505
1506
1507void dWorldSetAutoDisableAngularThreshold (dWorldID w, dReal angular_average_threshold)
1508{
1509 dAASSERT(w);
1510 w->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
1511}
1512
1513
1514int dWorldGetAutoDisableAverageSamplesCount (dWorldID w)
1515{
1516 dAASSERT(w);
1517 return w->adis.average_samples;
1518}
1519
1520
1521void dWorldSetAutoDisableAverageSamplesCount (dWorldID w, unsigned int average_samples_count)
1522{
1523 dAASSERT(w);
1524 w->adis.average_samples = average_samples_count;
1525}
1526
1527
1528int dWorldGetAutoDisableSteps (dWorldID w)
1529{
1530 dAASSERT(w);
1531 return w->adis.idle_steps;
1532}
1533
1534
1535void dWorldSetAutoDisableSteps (dWorldID w, int steps)
1536{
1537 dAASSERT(w);
1538 w->adis.idle_steps = steps;
1539}
1540
1541
1542dReal dWorldGetAutoDisableTime (dWorldID w)
1543{
1544 dAASSERT(w);
1545 return w->adis.idle_time;
1546}
1547
1548
1549void dWorldSetAutoDisableTime (dWorldID w, dReal time)
1550{
1551 dAASSERT(w);
1552 w->adis.idle_time = time;
1553}
1554
1555
1556int dWorldGetAutoDisableFlag (dWorldID w)
1557{
1558 dAASSERT(w);
1559 return w->adis_flag;
1560}
1561
1562
1563void dWorldSetAutoDisableFlag (dWorldID w, int do_auto_disable)
1564{
1565 dAASSERT(w);
1566 w->adis_flag = (do_auto_disable != 0);
1567}
1568
1569
1570void dWorldSetQuickStepNumIterations (dWorldID w, int num)
1571{
1572 dAASSERT(w);
1573 w->qs.num_iterations = num;
1574}
1575
1576
1577int dWorldGetQuickStepNumIterations (dWorldID w)
1578{
1579 dAASSERT(w);
1580 return w->qs.num_iterations;
1581}
1582
1583
1584void dWorldSetQuickStepW (dWorldID w, dReal param)
1585{
1586 dAASSERT(w);
1587 w->qs.w = param;
1588}
1589
1590
1591dReal dWorldGetQuickStepW (dWorldID w)
1592{
1593 dAASSERT(w);
1594 return w->qs.w;
1595}
1596
1597
1598void dWorldSetContactMaxCorrectingVel (dWorldID w, dReal vel)
1599{
1600 dAASSERT(w);
1601 w->contactp.max_vel = vel;
1602}
1603
1604
1605dReal dWorldGetContactMaxCorrectingVel (dWorldID w)
1606{
1607 dAASSERT(w);
1608 return w->contactp.max_vel;
1609}
1610
1611
1612void dWorldSetContactSurfaceLayer (dWorldID w, dReal depth)
1613{
1614 dAASSERT(w);
1615 w->contactp.min_depth = depth;
1616}
1617
1618
1619dReal dWorldGetContactSurfaceLayer (dWorldID w)
1620{
1621 dAASSERT(w);
1622 return w->contactp.min_depth;
1623}
1624
1625//****************************************************************************
1626// testing
1627
1628#define NUM 100
1629
1630#define DO(x)
1631
1632
1633extern "C" void dTestDataStructures()
1634{
1635 int i;
1636 DO(printf ("testDynamicsStuff()\n"));
1637
1638 dBodyID body [NUM];
1639 int nb = 0;
1640 dJointID joint [NUM];
1641 int nj = 0;
1642
1643 for (i=0; i<NUM; i++) body[i] = 0;
1644 for (i=0; i<NUM; i++) joint[i] = 0;
1645
1646 DO(printf ("creating world\n"));
1647 dWorldID w = dWorldCreate();
1648 checkWorld (w);
1649
1650 for (;;) {
1651 if (nb < NUM && dRandReal() > 0.5) {
1652 DO(printf ("creating body\n"));
1653 body[nb] = dBodyCreate (w);
1654 DO(printf ("\t--> %p\n",body[nb]));
1655 nb++;
1656 checkWorld (w);
1657 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1658 }
1659 if (nj < NUM && nb > 2 && dRandReal() > 0.5) {
1660 dBodyID b1 = body [dRand() % nb];
1661 dBodyID b2 = body [dRand() % nb];
1662 if (b1 != b2) {
1663 DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
1664 joint[nj] = dJointCreateBall (w,0);
1665 DO(printf ("\t-->%p\n",joint[nj]));
1666 checkWorld (w);
1667 dJointAttach (joint[nj],b1,b2);
1668 nj++;
1669 checkWorld (w);
1670 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1671 }
1672 }
1673 if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
1674 dBodyID b1 = body [dRand() % nb];
1675 dBodyID b2 = body [dRand() % nb];
1676 if (b1 != b2) {
1677 int k = dRand() % nj;
1678 DO(printf ("reattaching joint %p\n",joint[k]));
1679 dJointAttach (joint[k],b1,b2);
1680 checkWorld (w);
1681 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1682 }
1683 }
1684 if (nb > 0 && dRandReal() > 0.5) {
1685 int k = dRand() % nb;
1686 DO(printf ("destroying body %p\n",body[k]));
1687 dBodyDestroy (body[k]);
1688 checkWorld (w);
1689 for (; k < (NUM-1); k++) body[k] = body[k+1];
1690 nb--;
1691 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1692 }
1693 if (nj > 0 && dRandReal() > 0.5) {
1694 int k = dRand() % nj;
1695 DO(printf ("destroying joint %p\n",joint[k]));
1696 dJointDestroy (joint[k]);
1697 checkWorld (w);
1698 for (; k < (NUM-1); k++) joint[k] = joint[k+1];
1699 nj--;
1700 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1701 }
1702 }
1703
1704 /*
1705 printf ("creating world\n");
1706 dWorldID w = dWorldCreate();
1707 checkWorld (w);
1708 printf ("creating body\n");
1709 dBodyID b1 = dBodyCreate (w);
1710 checkWorld (w);
1711 printf ("creating body\n");
1712 dBodyID b2 = dBodyCreate (w);
1713 checkWorld (w);
1714 printf ("creating joint\n");
1715 dJointID j = dJointCreateBall (w);
1716 checkWorld (w);
1717 printf ("attaching joint\n");
1718 dJointAttach (j,b1,b2);
1719 checkWorld (w);
1720 printf ("destroying joint\n");
1721 dJointDestroy (j);
1722 checkWorld (w);
1723 printf ("destroying body\n");
1724 dBodyDestroy (b1);
1725 checkWorld (w);
1726 printf ("destroying body\n");
1727 dBodyDestroy (b2);
1728 checkWorld (w);
1729 printf ("destroying world\n");
1730 dWorldDestroy (w);
1731 */
1732}
generated by cgit v1.1 at 2024-11-09 22:36:53 +0000