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 <e32std.h>

    27 #include <pls.h>

    28 #include "object.h"

    29 #include <ode/ode.h>

    30 #include "joint.h"

    31 #include <ode/odemath.h>

    32 #include <ode/matrix.h>

    33 #include "step.h"

    34 #include "quickstep.h"

    35 #include "util.h"

    36 #include <ode/memory.h>

    37 #include <ode/error.h>

    38 

    39 #ifdef __WINSCW__

    40 

    41 TInt InitializeGlobaData( TOdeStaticData* aData )

    42     {

    43     Mem::FillZ( aData, sizeof( TOdeStaticData ) );

    44     aData->autoEnableDepth = 2;

    45     return KErrNone;

    46     }

    47     

    48 struct TOdeStaticData* GetGlobalData()

    49     {

    50     return Pls<TOdeStaticData>( TUid::Uid( 0x2001CBE3 ), InitializeGlobaData );

    51     }

    52 

    53 #else

    54 

    55 static struct TOdeStaticData odeStaticData;

    56 

    57 struct TOdeStaticData* GetGlobalData()

    58     {

    59     return &odeStaticData;

    60     }

    61 

    62 #endif // __WINSCW__

    63 

    64 // misc defines

    65 //#define ALLOCA dALLOCA16

    66 

    67 //****************************************************************************

    68 // utility

    69 

    70 static inline void initObject (dObject *obj, dxWorld *w)

    71 {

    72   obj->world = w;

    73   obj->next = 0;

    74   obj->tome = 0;

    75   obj->userdata = 0;

    76   obj->tag = 0;

    77 }

    78 

    79 

    80 // add an object `obj' to the list who's head pointer is pointed to by `first'.

    81 

    82 static inline void addObjectToList (dObject *obj, dObject **first)

    83 {

    84   obj->next = *first;

    85   obj->tome = first;

    86   if (*first) (*first)->tome = &obj->next;

    87   (*first) = obj;

    88 }

    89 

    90 

    91 // remove the object from the linked list

    92 

    93 static inline void removeObjectFromList (dObject *obj)

    94 {

    95   if (obj->next) obj->next->tome = obj->tome;

    96   *(obj->tome) = obj->next;

    97   // safeguard

    98   obj->next = 0;

    99   obj->tome = 0;

   100 }

   101 

   102 

   103 // remove the joint from neighbour lists of all connected bodies

   104 

   105 static void removeJointReferencesFromAttachedBodies (dxJoint *j)

   106 {

   107   for (int i=0; i<2; i++) {

   108     dxBody *body = j->node[i].body;

   109     if (body) {

   110       dxJointNode *n = body->firstjoint;

   111       dxJointNode *last = 0;

   112       while (n) {

   113 	if (n->joint == j) {

   114 	  if (last) last->next = n->next;

   115 	  else body->firstjoint = n->next;

   116 	  break;

   117 	}

   118 	last = n;

   119 	n = n->next;

   120       }

   121     }

   122   }

   123   j->node[0].body = 0;

   124   j->node[0].next = 0;

   125   j->node[1].body = 0;

   126   j->node[1].next = 0;

   127 }

   128 

   129 //****************************************************************************

   130 // debugging

   131 

   132 

   133 // check the validity of the world data structures

   134 

   135 static void checkWorld (dxWorld */*w*/)

   136 {

   137   //dxBody *b;

   138   //dxJoint *j;

   139 

   140  // TBD

   141 

   142 }

   143 

   144 

   145 void dWorldCheck (dxWorld *w)

   146 {

   147   checkWorld (w);

   148 }

   149 

   150 //****************************************************************************

   151 // body

   152 EXPORT_C dxWorld* dBodyGetWorld (dxBody* b)

   153 {

   154 

   155   return b->world;

   156 }

   157 

   158 EXPORT_C dxBody *dBodyCreate (dxWorld *w)

   159 {

   160 

   161   dxBody *b = new dxBody;

   162   initObject (b,w);

   163   b->firstjoint = 0;

   164   b->flags = 0;

   165   b->geom = 0;

   166   b->average_lvel_buffer = 0;

   167   b->average_avel_buffer = 0;

   168   dMassSetParameters (&b->mass,REAL(1.0),0,0,0,REAL(1.0),REAL(1.0),REAL(1.0),0,0,0);

   169   dSetZero (b->invI,4*3);

   170   b->invI[0] = REAL(1.0);

   171   b->invI[5] = REAL(1.0);

   172   b->invI[10] = REAL(1.0);

   173   b->invMass = REAL(1.0);

   174   dSetZero (b->posr.pos,4);

   175   dSetZero (b->q,4);

   176   b->q[0] = REAL(1.0);

   177   dRSetIdentity (b->posr.R);

   178   dSetZero (b->lvel,4);

   179   dSetZero (b->avel,4);

   180   dSetZero (b->facc,4);

   181   dSetZero (b->tacc,4);

   182   dSetZero (b->finite_rot_axis,4);

   183   addObjectToList (b,(dObject **) &w->firstbody);

   184   w->nb++;

   185 

   186   // set auto-disable parameters

   187   b->average_avel_buffer = b->average_lvel_buffer = 0; // no buffer at beginnin

   188   dBodySetAutoDisableDefaults (b);	// must do this after adding to world

   189   b->adis_stepsleft = b->adis.idle_steps;

   190   b->adis_timeleft = b->adis.idle_time;

   191   b->average_counter = 0;

   192   b->average_ready = 0; // average buffer not filled on the beginning

   193   dBodySetAutoDisableAverageSamplesCount(b, b->adis.average_samples);

   194 

   195   return b;

   196 }

   197 

   198 

   199 EXPORT_C void dBodyDestroy (dxBody *b)

   200 {

   201 

   202 

   203   // all geoms that link to this body must be notified that the body is about

   204   // to disappear. note that the call to dGeomSetBody(geom,0) will result in

   205   // dGeomGetBodyNext() returning 0 for the body, so we must get the next body

   206   // before setting the body to 0.

   207   dxGeom *next_geom = 0;

   208   for (dxGeom *geom = b->geom; geom; geom = next_geom) {

   209     next_geom = dGeomGetBodyNext (geom);

   210     dGeomSetBody (geom,0);

   211   }

   212 

   213   // detach all neighbouring joints, then delete this body.

   214   dxJointNode *n = b->firstjoint;

   215   while (n) {

   216     // sneaky trick to speed up removal of joint references (black magic)

   217     n->joint->node[(n == n->joint->node)].body = 0;

   218 

   219     dxJointNode *next = n->next;

   220     n->next = 0;

   221     removeJointReferencesFromAttachedBodies (n->joint);

   222     n = next;

   223   }

   224   removeObjectFromList (b);

   225   b->world->nb--;

   226 

   227   // delete the average buffers

   228   if(b->average_lvel_buffer)

   229   {

   230 	  delete[] (b->average_lvel_buffer);

   231 	  b->average_lvel_buffer = 0;

   232   }

   233   if(b->average_avel_buffer)

   234   {

   235 	  delete[] (b->average_avel_buffer);

   236 	  b->average_avel_buffer = 0;

   237   }

   238 

   239   delete b;

   240 }

   241 

   242 

   243 EXPORT_C void dBodySetData (dBodyID b, void *data)

   244 {

   245   b->userdata = data;

   246 }

   247 

   248 

   249 EXPORT_C void *dBodyGetData (dBodyID b)

   250 {

   251   return b->userdata;

   252 }

   253 

   254 

   255 EXPORT_C void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)

   256 {

   257 

   258   b->posr.pos[0] = x;

   259   b->posr.pos[1] = y;

   260   b->posr.pos[2] = z;

   261 

   262   // notify all attached geoms that this body has moved

   263   for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))

   264     dGeomMoved (geom);

   265 }

   266 

   267 

   268 EXPORT_C void dBodySetRotation (dBodyID b, const dMatrix3 R)

   269 {

   270   dQuaternion q;

   271   dRtoQ (R,q);

   272   dNormalize4 (q);

   273   b->q[0] = q[0];

   274   b->q[1] = q[1];

   275   b->q[2] = q[2];

   276   b->q[3] = q[3];

   277   dQtoR (b->q,b->posr.R);

   278 

   279   // notify all attached geoms that this body has moved

   280   for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))

   281     dGeomMoved (geom);

   282 }

   283 

   284 

   285 EXPORT_C void dBodySetQuaternion (dBodyID b, const dQuaternion q)

   286 {

   287 

   288   b->q[0] = q[0];

   289   b->q[1] = q[1];

   290   b->q[2] = q[2];

   291   b->q[3] = q[3];

   292   dNormalize4 (b->q);

   293   dQtoR (b->q,b->posr.R);

   294 

   295   // notify all attached geoms that this body has moved

   296   for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))

   297     dGeomMoved (geom);

   298 }

   299 

   300 

   301 EXPORT_C void dBodySetLinearVel  (dBodyID b, dReal x, dReal y, dReal z)

   302 {

   303   b->lvel[0] = x;

   304   b->lvel[1] = y;

   305   b->lvel[2] = z;

   306 }

   307 

   308 

   309 EXPORT_C void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)

   310 {

   311   b->avel[0] = x;

   312   b->avel[1] = y;

   313   b->avel[2] = z;

   314 }

   315 

   316 

   317 EXPORT_C const dReal * dBodyGetPosition (dBodyID b)

   318 {

   319   return b->posr.pos;

   320 }

   321 

   322 

   323 EXPORT_C void dBodyCopyPosition (dBodyID b, dVector3 pos)

   324 {

   325 	dReal* src = b->posr.pos;

   326 	pos[0] = src[0];

   327 	pos[1] = src[1];

   328 	pos[2] = src[2];

   329 }

   330 

   331 

   332 EXPORT_C const dReal * dBodyGetRotation (dBodyID b)

   333 {

   334   return b->posr.R;

   335 }

   336 

   337 

   338 EXPORT_C void dBodyCopyRotation (dBodyID b, dMatrix3 R)

   339 {

   340 	const dReal* src = b->posr.R;

   341 	R[0] = src[0];

   342 	R[1] = src[1];

   343 	R[2] = src[2];

   344 	R[3] = src[3];

   345 	R[4] = src[4];

   346 	R[5] = src[5];

   347 	R[6] = src[6];

   348 	R[7] = src[7];

   349 	R[8] = src[8];

   350 	R[9] = src[9];

   351 	R[10] = src[10];

   352 	R[11] = src[11];

   353 }

   354 

   355 

   356 EXPORT_C const dReal * dBodyGetQuaternion (dBodyID b)

   357 {

   358   return b->q;

   359 }

   360 

   361 

   362 EXPORT_C void dBodyCopyQuaternion (dBodyID b, dQuaternion quat)

   363 {

   364 	dReal* src = b->q;

   365 	quat[0] = src[0];

   366 	quat[1] = src[1];

   367 	quat[2] = src[2];

   368 	quat[3] = src[3];

   369 }

   370 

   371 

   372 EXPORT_C const dReal * dBodyGetLinearVel (dBodyID b)

   373 {

   374   return b->lvel;

   375 }

   376 

   377 

   378 EXPORT_C const dReal * dBodyGetAngularVel (dBodyID b)

   379 {

   380   return b->avel;

   381 }

   382 

   383 

   384 EXPORT_C void dBodySetMass (dBodyID b, const dMass *mass)

   385 {

   386   // The centre of mass must be at the origin.

   387   // Use dMassTranslate( mass, -mass->c[0], -mass->c[1], -mass->c[2] ) to correct it.

   388 

   389   memcpy (&b->mass,mass,sizeof(dMass));

   390   if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) {

   391     dRSetIdentity (b->invI);

   392   }

   393   b->invMass = dRecip(b->mass.mass);

   394 }

   395 

   396 

   397 EXPORT_C void dBodyGetMass (dBodyID b, dMass *mass)

   398 {

   399   memcpy (mass,&b->mass,sizeof(dMass));

   400 }

   401 

   402 

   403 EXPORT_C void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)

   404 {

   405   b->facc[0] += fx;

   406   b->facc[1] += fy;

   407   b->facc[2] += fz;

   408 }

   409 

   410 

   411 EXPORT_C void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)

   412 {

   413   b->tacc[0] += fx;

   414   b->tacc[1] += fy;

   415   b->tacc[2] += fz;

   416 }

   417 

   418 

   419 EXPORT_C void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)

   420 {

   421   dVector3 t1,t2;

   422   t1[0] = fx;

   423   t1[1] = fy;

   424   t1[2] = fz;

   425   t1[3] = 0;

   426   dMULTIPLY0_331 (t2,b->posr.R,t1);

   427   b->facc[0] += t2[0];

   428   b->facc[1] += t2[1];

   429   b->facc[2] += t2[2];

   430 }

   431 

   432 

   433 EXPORT_C void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)

   434 {

   435   dVector3 t1,t2;

   436   t1[0] = fx;

   437   t1[1] = fy;

   438   t1[2] = fz;

   439   t1[3] = 0;

   440   dMULTIPLY0_331 (t2,b->posr.R,t1);

   441   b->tacc[0] += t2[0];

   442   b->tacc[1] += t2[1];

   443   b->tacc[2] += t2[2];

   444 }

   445 

   446 

   447 EXPORT_C void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,

   448 			 dReal px, dReal py, dReal pz)

   449 {

   450   b->facc[0] += fx;

   451   b->facc[1] += fy;

   452   b->facc[2] += fz;

   453   dVector3 f,q;

   454   f[0] = fx;

   455   f[1] = fy;

   456   f[2] = fz;

   457   q[0] = px - b->posr.pos[0];

   458   q[1] = py - b->posr.pos[1];

   459   q[2] = pz - b->posr.pos[2];

   460   dCROSS (b->tacc,+=,q,f);

   461 }

   462 

   463 

   464 EXPORT_C void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,

   465 			    dReal px, dReal py, dReal pz)

   466 {

   467   dVector3 prel,f,p;

   468   f[0] = fx;

   469   f[1] = fy;

   470   f[2] = fz;

   471   f[3] = 0;

   472   prel[0] = px;

   473   prel[1] = py;

   474   prel[2] = pz;

   475   prel[3] = 0;

   476   dMULTIPLY0_331 (p,b->posr.R,prel);

   477   b->facc[0] += f[0];

   478   b->facc[1] += f[1];

   479   b->facc[2] += f[2];

   480   dCROSS (b->tacc,+=,p,f);

   481 }

   482 

   483 

   484 EXPORT_C void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,

   485 			    dReal px, dReal py, dReal pz)

   486 {

   487   dVector3 frel,f;

   488   frel[0] = fx;

   489   frel[1] = fy;

   490   frel[2] = fz;

   491   frel[3] = 0;

   492   dMULTIPLY0_331 (f,b->posr.R,frel);

   493   b->facc[0] += f[0];

   494   b->facc[1] += f[1];

   495   b->facc[2] += f[2];

   496   dVector3 q;

   497   q[0] = px - b->posr.pos[0];

   498   q[1] = py - b->posr.pos[1];

   499   q[2] = pz - b->posr.pos[2];

   500   dCROSS (b->tacc,+=,q,f);

   501 }

   502 

   503 

   504 EXPORT_C void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,

   505 			       dReal px, dReal py, dReal pz)

   506 {

   507   dVector3 frel,prel,f,p;

   508   frel[0] = fx;

   509   frel[1] = fy;

   510   frel[2] = fz;

   511   frel[3] = 0;

   512   prel[0] = px;

   513   prel[1] = py;

   514   prel[2] = pz;

   515   prel[3] = 0;

   516   dMULTIPLY0_331 (f,b->posr.R,frel);

   517   dMULTIPLY0_331 (p,b->posr.R,prel);

   518   b->facc[0] += f[0];

   519   b->facc[1] += f[1];

   520   b->facc[2] += f[2];

   521   dCROSS (b->tacc,+=,p,f);

   522 }

   523 

   524 

   525 EXPORT_C const dReal * dBodyGetForce (dBodyID b)

   526 {

   527   return b->facc;

   528 }

   529 

   530 

   531 EXPORT_C const dReal * dBodyGetTorque (dBodyID b)

   532 {

   533   return b->tacc;

   534 }

   535 

   536 

   537 EXPORT_C void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)

   538 {

   539   b->facc[0] = x;

   540   b->facc[1] = y;

   541   b->facc[2] = z;

   542 }

   543 

   544 

   545 EXPORT_C void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)

   546 {

   547   b->tacc[0] = x;

   548   b->tacc[1] = y;

   549   b->tacc[2] = z;

   550 }

   551 

   552 

   553 EXPORT_C void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,

   554 			  dVector3 result)

   555 {

   556   dVector3 prel,p;

   557   prel[0] = px;

   558   prel[1] = py;

   559   prel[2] = pz;

   560   prel[3] = 0;

   561   dMULTIPLY0_331 (p,b->posr.R,prel);

   562   result[0] = p[0] + b->posr.pos[0];

   563   result[1] = p[1] + b->posr.pos[1];

   564   result[2] = p[2] + b->posr.pos[2];

   565 }

   566 

   567 

   568 EXPORT_C void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,

   569 			  dVector3 result)

   570 {

   571   dVector3 prel,p;

   572   prel[0] = px;

   573   prel[1] = py;

   574   prel[2] = pz;

   575   prel[3] = 0;

   576   dMULTIPLY0_331 (p,b->posr.R,prel);

   577   result[0] = b->lvel[0];

   578   result[1] = b->lvel[1];

   579   result[2] = b->lvel[2];

   580   dCROSS (result,+=,b->avel,p);

   581 }

   582 

   583 

   584 EXPORT_C void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,

   585 		       dVector3 result)

   586 {

   587   dVector3 p;

   588   p[0] = px - b->posr.pos[0];

   589   p[1] = py - b->posr.pos[1];

   590   p[2] = pz - b->posr.pos[2];

   591   p[3] = 0;

   592   result[0] = b->lvel[0];

   593   result[1] = b->lvel[1];

   594   result[2] = b->lvel[2];

   595   dCROSS (result,+=,b->avel,p);

   596 }

   597 

   598 

   599 EXPORT_C void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,

   600 			  dVector3 result)

   601 {

   602   dVector3 prel;

   603   prel[0] = px - b->posr.pos[0];

   604   prel[1] = py - b->posr.pos[1];

   605   prel[2] = pz - b->posr.pos[2];

   606   prel[3] = 0;

   607   dMULTIPLY1_331 (result,b->posr.R,prel);

   608 }

   609 

   610 

   611 EXPORT_C void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,

   612 			 dVector3 result)

   613 {

   614   dVector3 p;

   615   p[0] = px;

   616   p[1] = py;

   617   p[2] = pz;

   618   p[3] = 0;

   619   dMULTIPLY0_331 (result,b->posr.R,p);

   620 }

   621 

   622 

   623 EXPORT_C void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,

   624 			   dVector3 result)

   625 {

   626   dVector3 p;

   627   p[0] = px;

   628   p[1] = py;

   629   p[2] = pz;

   630   p[3] = 0;

   631   dMULTIPLY1_331 (result,b->posr.R,p);

   632 }

   633 

   634 

   635 EXPORT_C void dBodySetFiniteRotationMode (dBodyID b, int mode)

   636 {

   637   b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis);

   638   if (mode) {

   639     b->flags |= dxBodyFlagFiniteRotation;

   640     if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 ||

   641 	b->finite_rot_axis[2] != 0) {

   642       b->flags |= dxBodyFlagFiniteRotationAxis;

   643     }

   644   }

   645 }

   646 

   647 

   648 EXPORT_C void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)

   649 {

   650   b->finite_rot_axis[0] = x;

   651   b->finite_rot_axis[1] = y;

   652   b->finite_rot_axis[2] = z;

   653   if (x != 0 || y != 0 || z != 0) {

   654     dNormalize3 (b->finite_rot_axis);

   655     b->flags |= dxBodyFlagFiniteRotationAxis;

   656   }

   657   else {

   658     b->flags &= ~dxBodyFlagFiniteRotationAxis;

   659   }

   660 }

   661 

   662 

   663 EXPORT_C int dBodyGetFiniteRotationMode (dBodyID b)

   664 {

   665   return ((b->flags & dxBodyFlagFiniteRotation) != 0);

   666 }

   667 

   668 

   669 EXPORT_C void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)

   670 {

   671   result[0] = b->finite_rot_axis[0];

   672   result[1] = b->finite_rot_axis[1];

   673   result[2] = b->finite_rot_axis[2];

   674 }

   675 

   676 

   677 EXPORT_C int dBodyGetNumJoints (dBodyID b)

   678 {

   679   int count=0;

   680   for (dxJointNode *n=b->firstjoint; n ; n=n->next, count++)

   681       {

   682       }

   683   return count;

   684 }

   685 

   686 

   687 EXPORT_C dJointID dBodyGetJoint (dBodyID b, int index)

   688 {

   689   int i=0;

   690   for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) {

   691     if (i == index) return n->joint;

   692   }

   693   return 0;

   694 }

   695 

   696 

   697 EXPORT_C void dBodyEnable (dBodyID b)

   698 {

   699   b->flags &= ~dxBodyDisabled;

   700   b->adis_stepsleft = b->adis.idle_steps;

   701   b->adis_timeleft = b->adis.idle_time;

   702   // no code for average-processing needed here

   703 }

   704 

   705 

   706 EXPORT_C void dBodyDisable (dBodyID b)

   707 {

   708   b->flags |= dxBodyDisabled;

   709 }

   710 

   711 

   712 EXPORT_C int dBodyIsEnabled (dBodyID b)

   713 {

   714   return ((b->flags & dxBodyDisabled) == 0);

   715 }

   716 

   717 

   718 EXPORT_C void dBodySetGravityMode (dBodyID b, int mode)

   719 {

   720   if (mode) b->flags &= ~dxBodyNoGravity;

   721   else b->flags |= dxBodyNoGravity;

   722 }

   723 

   724 

   725 EXPORT_C int dBodyGetGravityMode (dBodyID b)

   726 {

   727   return ((b->flags & dxBodyNoGravity) == 0);

   728 }

   729 

   730 

   731 // body auto-disable functions

   732 

   733 EXPORT_C dReal dBodyGetAutoDisableLinearThreshold (dBodyID b)

   734 {

   735 	return dSqrt (b->adis.linear_average_threshold);

   736 }

   737 

   738 

   739 EXPORT_C void dBodySetAutoDisableLinearThreshold (dBodyID b, dReal linear_average_threshold)

   740 {

   741 	b->adis.linear_average_threshold = dMUL(linear_average_threshold,linear_average_threshold);

   742 }

   743 

   744 

   745 EXPORT_C dReal dBodyGetAutoDisableAngularThreshold (dBodyID b)

   746 {

   747 	return dSqrt (b->adis.angular_average_threshold);

   748 }

   749 

   750 

   751 EXPORT_C void dBodySetAutoDisableAngularThreshold (dBodyID b, dReal angular_average_threshold)

   752 {

   753 	b->adis.angular_average_threshold = dMUL(angular_average_threshold,angular_average_threshold);

   754 }

   755 

   756 

   757 EXPORT_C int dBodyGetAutoDisableAverageSamplesCount (dBodyID b)

   758 {

   759 	return b->adis.average_samples;

   760 }

   761 

   762 

   763 EXPORT_C void dBodySetAutoDisableAverageSamplesCount (dBodyID b, unsigned int average_samples_count)

   764 {

   765 	b->adis.average_samples = average_samples_count;

   766 	// update the average buffers

   767 	if(b->average_lvel_buffer)

   768 	{

   769 		delete[] b->average_lvel_buffer;

   770 		b->average_lvel_buffer = 0;

   771 	}

   772 	if(b->average_avel_buffer)

   773 	{

   774 		delete[] b->average_avel_buffer;

   775 		b->average_avel_buffer = 0;

   776 	}

   777 	if(b->adis.average_samples > 0)

   778 	{

   779 		b->average_lvel_buffer = new dVector3[b->adis.average_samples];

   780 		b->average_avel_buffer = new dVector3[b->adis.average_samples];

   781 	}

   782 	else

   783 	{

   784 		b->average_lvel_buffer = 0;

   785 		b->average_avel_buffer = 0;

   786 	}

   787 	// new buffer is empty

   788 	b->average_counter = 0;

   789 	b->average_ready = 0;

   790 }

   791 

   792 

   793 EXPORT_C int dBodyGetAutoDisableSteps (dBodyID b)

   794 {

   795 	return b->adis.idle_steps;

   796 }

   797 

   798 

   799 EXPORT_C void dBodySetAutoDisableSteps (dBodyID b, int steps)

   800 {

   801 	b->adis.idle_steps = steps;

   802 }

   803 

   804 

   805 EXPORT_C dReal dBodyGetAutoDisableTime (dBodyID b)

   806 {

   807 	return b->adis.idle_time;

   808 }

   809 

   810 

   811 EXPORT_C void dBodySetAutoDisableTime (dBodyID b, dReal time)

   812 {

   813 	b->adis.idle_time = time;

   814 }

   815 

   816 

   817 EXPORT_C int dBodyGetAutoDisableFlag (dBodyID b)

   818 {

   819 	return ((b->flags & dxBodyAutoDisable) != 0);

   820 }

   821 

   822 

   823 EXPORT_C void dBodySetAutoDisableFlag (dBodyID b, int do_auto_disable)

   824 {

   825 	if (!do_auto_disable)

   826 	{

   827 		b->flags &= ~dxBodyAutoDisable;

   828 		// (mg) we should also reset the IsDisabled state to correspond to the DoDisabling flag

   829 		b->flags &= ~dxBodyDisabled;

   830 		b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world);

   831 		b->adis.idle_time = dWorldGetAutoDisableTime(b->world);

   832 		// resetting the average calculations too

   833 		dBodySetAutoDisableAverageSamplesCount(b, dWorldGetAutoDisableAverageSamplesCount(b->world) );

   834 	}

   835 	else

   836 	{

   837 		b->flags |= dxBodyAutoDisable;

   838 	}

   839 }

   840 

   841 

   842 EXPORT_C void dBodySetAutoDisableDefaults (dBodyID b)

   843 {

   844 	dWorldID w = b->world;

   845 	b->adis = w->adis;

   846 	dBodySetAutoDisableFlag (b, w->adis_flag);

   847 }

   848 

   849 //****************************************************************************

   850 // joints

   851 

   852 static void dJointInit (dxWorld *w, dxJoint *j)

   853 {

   854   initObject (j,w);

   855   j->vtable = 0;

   856   j->flags = 0;

   857   j->node[0].joint = j;

   858   j->node[0].body = 0;

   859   j->node[0].next = 0;

   860   j->node[1].joint = j;

   861   j->node[1].body = 0;

   862   j->node[1].next = 0;

   863   dSetZero (j->lambda,6);

   864   addObjectToList (j,(dObject **) &w->firstjoint);

   865   w->nj++;

   866 }

   867 

   868 

   869 static dxJoint *createJoint (dWorldID w, dJointGroupID group,

   870 			     dxJoint::Vtable *vtable)

   871 {

   872   dxJoint *j;

   873   if (group) {

   874     j = (dxJoint*) group->stack.alloc (vtable->size);

   875     group->num++;

   876   }

   877   else j = (dxJoint*) dAlloc (vtable->size);

   878   dJointInit (w,j);

   879   j->vtable = vtable;

   880   if (group) j->flags |= dJOINT_INGROUP;

   881   if (vtable->init) vtable->init (j);

   882   j->feedback = 0;

   883   return j;

   884 }

   885 

   886 

   887 EXPORT_C dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)

   888 {

   889   return createJoint (w,group,&__dball_vtable);

   890 }

   891 

   892 

   893 EXPORT_C dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)

   894 {

   895   return createJoint (w,group,&__dhinge_vtable);

   896 }

   897 

   898 

   899 EXPORT_C dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)

   900 {

   901   return createJoint (w,group,&__dslider_vtable);

   902 }

   903 

   904 

   905 EXPORT_C dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,

   906 			       const dContact *c)

   907 {

   908   dxJointContact *j = (dxJointContact *)

   909     createJoint (w,group,&__dcontact_vtable);

   910   j->contact = *c;

   911   return j;

   912 }

   913 

   914 

   915 EXPORT_C dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)

   916 {

   917   return createJoint (w,group,&__dhinge2_vtable);

   918 }

   919 

   920 

   921 EXPORT_C dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)

   922 {

   923   return createJoint (w,group,&__duniversal_vtable);

   924 }

   925 

   926 EXPORT_C dxJoint * dJointCreatePR (dWorldID w, dJointGroupID group)

   927 {

   928   return createJoint (w,group,&__dPR_vtable);

   929 }

   930 

   931 EXPORT_C dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)

   932 {

   933   return createJoint (w,group,&__dfixed_vtable);

   934 }

   935 

   936 

   937 EXPORT_C dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)

   938 {

   939   return createJoint (w,group,&__dnull_vtable);

   940 }

   941 

   942 

   943 EXPORT_C dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)

   944 {

   945   return createJoint (w,group,&__damotor_vtable);

   946 }

   947 

   948 EXPORT_C dxJoint * dJointCreateLMotor (dWorldID w, dJointGroupID group)

   949 {

   950   return createJoint (w,group,&__dlmotor_vtable);

   951 }

   952 

   953 EXPORT_C dxJoint * dJointCreatePlane2D (dWorldID w, dJointGroupID group)

   954 {

   955   return createJoint (w,group,&__dplane2d_vtable);

   956 }

   957 

   958 EXPORT_C void dJointDestroy (dxJoint *j)

   959 {;

   960   if (j->flags & dJOINT_INGROUP) return;

   961   removeJointReferencesFromAttachedBodies (j);

   962   removeObjectFromList (j);

   963   j->world->nj--;

   964   dFree (j,j->vtable->size);

   965 }

   966 

   967 

   968 EXPORT_C dJointGroupID dJointGroupCreate (int /*max_size*/)

   969 {

   970   dxJointGroup *group = new dxJointGroup;

   971   group->num = 0;

   972   return group;

   973 }

   974 

   975 

   976 EXPORT_C void dJointGroupDestroy (dJointGroupID group)

   977 {

   978   dJointGroupEmpty (group);

   979   delete group;

   980 }

   981 

   982 

   983 EXPORT_C void dJointGroupEmpty (dJointGroupID group)

   984 {

   985   // the joints in this group are detached starting from the most recently

   986   // added (at the top of the stack). this helps ensure that the various

   987   // linked lists are not traversed too much, as the joints will hopefully

   988   // be at the start of those lists.

   989   // if any group joints have their world pointer set to 0, their world was

   990   // previously destroyed. no special handling is required for these joints.

   991 

   992   int i;

   993   dxJoint **jlist = (dxJoint**) malloc (group->num * sizeof(dxJoint*));

   994   if (jlist == NULL) {

   995   	return;

   996   }

   997   dxJoint *j = (dxJoint*) group->stack.rewind();

   998   for (i=0; i < group->num; i++) {

   999     jlist[i] = j;

  1000     j = (dxJoint*) (group->stack.next (j->vtable->size));

  1001   }

  1002   for (i=group->num-1; i >= 0; i--) {

  1003     if (jlist[i]->world) {

  1004       removeJointReferencesFromAttachedBodies (jlist[i]);

  1005       removeObjectFromList (jlist[i]);

  1006       jlist[i]->world->nj--;

  1007     }

  1008   }

  1009   group->num = 0;

  1010   

  1011   free(jlist);

  1012   

  1013   group->stack.freeAll();

  1014   

  1015 }

  1016 

  1017 

  1018 EXPORT_C void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)

  1019 {

  1020   // remove any existing body attachments

  1021   if (joint->node[0].body || joint->node[1].body) {

  1022     removeJointReferencesFromAttachedBodies (joint);

  1023   }

  1024 

  1025   // if a body is zero, make sure that it is body2, so 0 --> node[1].body

  1026   if (body1==0) {

  1027     body1 = body2;

  1028     body2 = 0;

  1029     joint->flags |= dJOINT_REVERSE;

  1030   }

  1031   else {

  1032     joint->flags &= (~dJOINT_REVERSE);

  1033   }

  1034 

  1035   // attach to new bodies

  1036   joint->node[0].body = body1;

  1037   joint->node[1].body = body2;

  1038   if (body1) {

  1039     joint->node[1].next = body1->firstjoint;

  1040     body1->firstjoint = &joint->node[1];

  1041   }

  1042   else joint->node[1].next = 0;

  1043   if (body2) {

  1044     joint->node[0].next = body2->firstjoint;

  1045     body2->firstjoint = &joint->node[0];

  1046   }

  1047   else {

  1048     joint->node[0].next = 0;

  1049   }

  1050 }

  1051 

  1052 

  1053 EXPORT_C void dJointSetData (dxJoint *joint, void *data)

  1054 {

  1055   joint->userdata = data;

  1056 }

  1057 

  1058 

  1059 EXPORT_C void *dJointGetData (dxJoint *joint)

  1060 {

  1061   return joint->userdata;

  1062 }

  1063 

  1064 

  1065 EXPORT_C int dJointGetType (dxJoint *joint)

  1066 {

  1067   return joint->vtable->typenum;

  1068 }

  1069 

  1070 

  1071 EXPORT_C dBodyID dJointGetBody (dxJoint *joint, int index)

  1072 {

  1073   if (index == 0 || index == 1) {

  1074     if (joint->flags & dJOINT_REVERSE) return joint->node[1-index].body;

  1075     else return joint->node[index].body;

  1076   }

  1077   else return 0;

  1078 }

  1079 

  1080 

  1081 EXPORT_C void dJointSetFeedback (dxJoint *joint, dJointFeedback *f)

  1082 {

  1083   joint->feedback = f;

  1084 }

  1085 

  1086 

  1087 EXPORT_C dJointFeedback *dJointGetFeedback (dxJoint *joint)

  1088 {

  1089   return joint->feedback;

  1090 }

  1091 

  1092 

  1093 

  1094 EXPORT_C dJointID dConnectingJoint (dBodyID in_b1, dBodyID in_b2)

  1095 {

  1096 	dBodyID b1, b2;

  1097 

  1098 	if (in_b1 == 0) {

  1099 		b1 = in_b2;

  1100 		b2 = in_b1;

  1101 	}

  1102 	else {

  1103 		b1 = in_b1;

  1104 		b2 = in_b2;

  1105 	}

  1106 

  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 n->joint;

  1110     }

  1111 

  1112     return 0;

  1113 }

  1114 

  1115 

  1116 

  1117 EXPORT_C int dConnectingJointList (dBodyID in_b1, dBodyID in_b2, dJointID* out_list)

  1118 {

  1119 	dBodyID b1, b2;

  1120 

  1121 	if (in_b1 == 0) {

  1122 		b1 = in_b2;

  1123 		b2 = in_b1;

  1124 	}

  1125 	else {

  1126 		b1 = in_b1;

  1127 		b2 = in_b2;

  1128 	}

  1129 

  1130     // look through b1's neighbour list for b2

  1131     int numConnectingJoints = 0;

  1132     for (dxJointNode *n=b1->firstjoint; n; n=n->next) {

  1133         if (n->body == b2)

  1134             out_list[numConnectingJoints++] = n->joint;

  1135     }