2326 lines
52 KiB
C++
2326 lines
52 KiB
C++
/*************************************************************************
|
|
* *
|
|
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
|
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of EITHER: *
|
|
* (1) The GNU Lesser General Public License as published by the Free *
|
|
* Software Foundation; either version 2.1 of the License, or (at *
|
|
* your option) any later version. The text of the GNU Lesser *
|
|
* General Public License is included with this library in the *
|
|
* file LICENSE.TXT. *
|
|
* (2) The BSD-style license that is included with this library in *
|
|
* the file LICENSE-BSD.TXT. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
|
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
|
* *
|
|
*************************************************************************/
|
|
|
|
#ifdef _MSC_VER
|
|
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
|
|
#endif
|
|
|
|
// this source file is mostly concerned with the data structures, not the
|
|
// numerics.
|
|
|
|
#include <ode/ode.h>
|
|
#include <ode/memory.h>
|
|
#include <ode/error.h>
|
|
#include "config.h"
|
|
#include "matrix.h"
|
|
#include "odemath.h"
|
|
#include "objects.h"
|
|
#include "joints/joints.h"
|
|
#include "step.h"
|
|
#include "quickstep.h"
|
|
#include "util.h"
|
|
#include "odetls.h"
|
|
|
|
// misc defines
|
|
#define ALLOCA dALLOCA16
|
|
|
|
//****************************************************************************
|
|
// utility
|
|
|
|
|
|
// add an object `obj' to the list who's head pointer is pointed to by `first'.
|
|
|
|
void addObjectToList (dObject *obj, dObject **first)
|
|
{
|
|
obj->next = *first;
|
|
obj->tome = first;
|
|
if (*first) (*first)->tome = &obj->next;
|
|
(*first) = obj;
|
|
}
|
|
|
|
|
|
// remove the object from the linked list
|
|
|
|
static inline void removeObjectFromList (dObject *obj)
|
|
{
|
|
if (obj->next) obj->next->tome = obj->tome;
|
|
*(obj->tome) = obj->next;
|
|
// safeguard
|
|
obj->next = NULL;
|
|
obj->tome = NULL;
|
|
}
|
|
|
|
|
|
// remove the joint from neighbour lists of all connected bodies
|
|
|
|
static void removeJointReferencesFromAttachedBodies (dxJoint *j)
|
|
{
|
|
for (int i=0; i<2; i++) {
|
|
dxBody *body = j->node[i].body;
|
|
if (body) {
|
|
dxJointNode *n = body->firstjoint;
|
|
dxJointNode *last = NULL;
|
|
while (n) {
|
|
if (n->joint == j) {
|
|
if (last) last->next = n->next;
|
|
else body->firstjoint = n->next;
|
|
break;
|
|
}
|
|
last = n;
|
|
n = n->next;
|
|
}
|
|
}
|
|
}
|
|
j->node[0].body = NULL;
|
|
j->node[0].next = NULL;
|
|
j->node[1].body = NULL;
|
|
j->node[1].next = NULL;
|
|
}
|
|
|
|
//****************************************************************************
|
|
// debugging
|
|
|
|
// see if an object list loops on itself (if so, it's bad).
|
|
|
|
static int listHasLoops (dObject *first)
|
|
{
|
|
if (first==0 || first->next==0) return 0;
|
|
dObject *a=first,*b=first->next;
|
|
int skip=0;
|
|
while (b) {
|
|
if (a==b) return 1;
|
|
b = b->next;
|
|
if (skip) a = a->next;
|
|
skip ^= 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
// check the validity of the world data structures
|
|
|
|
static int g_world_check_tag_generator = 0;
|
|
|
|
static inline int generateWorldCheckTag()
|
|
{
|
|
// Atomicity is not necessary here
|
|
return ++g_world_check_tag_generator;
|
|
}
|
|
|
|
static void checkWorld (dxWorld *w)
|
|
{
|
|
dxBody *b;
|
|
dxJoint *j;
|
|
|
|
// check there are no loops
|
|
if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops");
|
|
if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops");
|
|
|
|
// check lists are well formed (check `tome' pointers)
|
|
for (b=w->firstbody; b; b=(dxBody*)b->next) {
|
|
if (b->next && b->next->tome != &b->next)
|
|
dDebug (0,"bad tome pointer in body list");
|
|
}
|
|
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
|
|
if (j->next && j->next->tome != &j->next)
|
|
dDebug (0,"bad tome pointer in joint list");
|
|
}
|
|
|
|
// check counts
|
|
int n = 0;
|
|
for (b=w->firstbody; b; b=(dxBody*)b->next) n++;
|
|
if (w->nb != n) dDebug (0,"body count incorrect");
|
|
n = 0;
|
|
for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++;
|
|
if (w->nj != n) dDebug (0,"joint count incorrect");
|
|
|
|
// set all tag values to a known value
|
|
int count = generateWorldCheckTag();
|
|
for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count;
|
|
for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count;
|
|
|
|
// check all body/joint world pointers are ok
|
|
for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w)
|
|
dDebug (0,"bad world pointer in body list");
|
|
for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w)
|
|
dDebug (0,"bad world pointer in joint list");
|
|
|
|
/*
|
|
// check for half-connected joints - actually now these are valid
|
|
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
|
|
if (j->node[0].body || j->node[1].body) {
|
|
if (!(j->node[0].body && j->node[1].body))
|
|
dDebug (0,"half connected joint found");
|
|
}
|
|
}
|
|
*/
|
|
|
|
// check that every joint node appears in the joint lists of both bodies it
|
|
// attaches
|
|
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
|
|
for (int i=0; i<2; i++) {
|
|
if (j->node[i].body) {
|
|
int ok = 0;
|
|
for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) {
|
|
if (n->joint == j) ok = 1;
|
|
}
|
|
if (ok==0) dDebug (0,"joint not in joint list of attached body");
|
|
}
|
|
}
|
|
}
|
|
|
|
// check all body joint lists (correct body ptrs)
|
|
for (b=w->firstbody; b; b=(dxBody*)b->next) {
|
|
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
|
|
if (&n->joint->node[0] == n) {
|
|
if (n->joint->node[1].body != b)
|
|
dDebug (0,"bad body pointer in joint node of body list (1)");
|
|
}
|
|
else {
|
|
if (n->joint->node[0].body != b)
|
|
dDebug (0,"bad body pointer in joint node of body list (2)");
|
|
}
|
|
if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body");
|
|
}
|
|
}
|
|
|
|
// check all body pointers in joints, check they are distinct
|
|
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
|
|
if (j->node[0].body && (j->node[0].body == j->node[1].body))
|
|
dDebug (0,"non-distinct body pointers in joint");
|
|
if ((j->node[0].body && j->node[0].body->tag != count) ||
|
|
(j->node[1].body && j->node[1].body->tag != count))
|
|
dDebug (0,"bad body pointer in joint");
|
|
}
|
|
}
|
|
|
|
|
|
void dWorldCheck (dxWorld *w)
|
|
{
|
|
checkWorld (w);
|
|
}
|
|
|
|
//****************************************************************************
|
|
// body
|
|
|
|
dxBody::dxBody(dxWorld *w) :
|
|
dObject(w)
|
|
{
|
|
|
|
}
|
|
|
|
|
|
dxWorld* dBodyGetWorld (dxBody * b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->world;
|
|
}
|
|
|
|
dxBody *dBodyCreate (dxWorld *w)
|
|
{
|
|
dAASSERT (w);
|
|
dxBody *b = new dxBody(w);
|
|
b->firstjoint = NULL;
|
|
b->flags = 0;
|
|
b->geom = NULL;
|
|
b->average_lvel_buffer = NULL;
|
|
b->average_avel_buffer = NULL;
|
|
dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0);
|
|
dSetZero (b->invI,4*3);
|
|
b->invI[0] = 1;
|
|
b->invI[5] = 1;
|
|
b->invI[10] = 1;
|
|
b->invMass = 1;
|
|
dSetZero (b->posr.pos,4);
|
|
dSetZero (b->q,4);
|
|
b->q[0] = 1;
|
|
dRSetIdentity (b->posr.R);
|
|
dSetZero (b->lvel,4);
|
|
dSetZero (b->avel,4);
|
|
dSetZero (b->facc,4);
|
|
dSetZero (b->tacc,4);
|
|
dSetZero (b->finite_rot_axis,4);
|
|
addObjectToList (b,(dObject **) &w->firstbody);
|
|
w->nb++;
|
|
|
|
// set auto-disable parameters
|
|
b->average_avel_buffer = b->average_lvel_buffer = NULL; // no buffer at beginning
|
|
dBodySetAutoDisableDefaults (b); // must do this after adding to world
|
|
b->adis_stepsleft = b->adis.idle_steps;
|
|
b->adis_timeleft = b->adis.idle_time;
|
|
b->average_counter = 0;
|
|
b->average_ready = 0; // average buffer not filled on the beginning
|
|
dBodySetAutoDisableAverageSamplesCount(b, b->adis.average_samples);
|
|
|
|
b->moved_callback = NULL;
|
|
|
|
dBodySetDampingDefaults(b); // must do this after adding to world
|
|
|
|
b->flags |= w->body_flags & dxBodyMaxAngularSpeed;
|
|
b->max_angular_speed = w->max_angular_speed;
|
|
|
|
b->flags |= dxBodyGyroscopic;
|
|
|
|
return b;
|
|
}
|
|
|
|
|
|
void dBodyDestroy (dxBody *b)
|
|
{
|
|
dAASSERT (b);
|
|
|
|
// all geoms that link to this body must be notified that the body is about
|
|
// to disappear. note that the call to dGeomSetBody(geom,0) will result in
|
|
// dGeomGetBodyNext() returning 0 for the body, so we must get the next body
|
|
// before setting the body to 0.
|
|
dxGeom *next_geom = NULL;
|
|
for (dxGeom *geom = b->geom; geom; geom = next_geom) {
|
|
next_geom = dGeomGetBodyNext (geom);
|
|
dGeomSetBody (geom,0);
|
|
}
|
|
|
|
// detach all neighbouring joints, then delete this body.
|
|
dxJointNode *n = b->firstjoint;
|
|
while (n) {
|
|
// sneaky trick to speed up removal of joint references (black magic)
|
|
n->joint->node[(n == n->joint->node)].body = NULL;
|
|
|
|
dxJointNode *next = n->next;
|
|
n->next = NULL;
|
|
removeJointReferencesFromAttachedBodies (n->joint);
|
|
n = next;
|
|
}
|
|
removeObjectFromList (b);
|
|
b->world->nb--;
|
|
|
|
// delete the average buffers
|
|
if(b->average_lvel_buffer)
|
|
{
|
|
delete[] (b->average_lvel_buffer);
|
|
b->average_lvel_buffer = NULL;
|
|
}
|
|
if(b->average_avel_buffer)
|
|
{
|
|
delete[] (b->average_avel_buffer);
|
|
b->average_avel_buffer = NULL;
|
|
}
|
|
|
|
delete b;
|
|
}
|
|
|
|
|
|
void dBodySetData (dBodyID b, void *data)
|
|
{
|
|
dAASSERT (b);
|
|
b->userdata = data;
|
|
}
|
|
|
|
|
|
void *dBodyGetData (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->userdata;
|
|
}
|
|
|
|
|
|
void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)
|
|
{
|
|
dAASSERT (b);
|
|
b->posr.pos[0] = x;
|
|
b->posr.pos[1] = y;
|
|
b->posr.pos[2] = z;
|
|
|
|
// notify all attached geoms that this body has moved
|
|
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
|
|
dGeomMoved (geom);
|
|
}
|
|
|
|
|
|
void dBodySetRotation (dBodyID b, const dMatrix3 R)
|
|
{
|
|
dAASSERT (b && R);
|
|
|
|
memcpy(b->posr.R, R, sizeof(dMatrix3));
|
|
|
|
bool bOrthogonalizeResult = dxOrthogonalizeR(b->posr.R);
|
|
dAVERIFY(bOrthogonalizeResult);
|
|
|
|
dRtoQ (R, b->q);
|
|
dNormalize4 (b->q);
|
|
|
|
// notify all attached geoms that this body has moved
|
|
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) {
|
|
dGeomMoved (geom);
|
|
}
|
|
}
|
|
|
|
|
|
void dBodySetQuaternion (dBodyID b, const dQuaternion q)
|
|
{
|
|
dAASSERT (b && q);
|
|
b->q[0] = q[0];
|
|
b->q[1] = q[1];
|
|
b->q[2] = q[2];
|
|
b->q[3] = q[3];
|
|
dNormalize4 (b->q);
|
|
dQtoR (b->q,b->posr.R);
|
|
|
|
// notify all attached geoms that this body has moved
|
|
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
|
|
dGeomMoved (geom);
|
|
}
|
|
|
|
|
|
void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z)
|
|
{
|
|
dAASSERT (b);
|
|
b->lvel[0] = x;
|
|
b->lvel[1] = y;
|
|
b->lvel[2] = z;
|
|
}
|
|
|
|
|
|
void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)
|
|
{
|
|
dAASSERT (b);
|
|
b->avel[0] = x;
|
|
b->avel[1] = y;
|
|
b->avel[2] = z;
|
|
}
|
|
|
|
|
|
const dReal * dBodyGetPosition (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->posr.pos;
|
|
}
|
|
|
|
|
|
void dBodyCopyPosition (dBodyID b, dVector3 pos)
|
|
{
|
|
dAASSERT (b);
|
|
dReal* src = b->posr.pos;
|
|
pos[0] = src[0];
|
|
pos[1] = src[1];
|
|
pos[2] = src[2];
|
|
}
|
|
|
|
|
|
const dReal * dBodyGetRotation (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->posr.R;
|
|
}
|
|
|
|
|
|
void dBodyCopyRotation (dBodyID b, dMatrix3 R)
|
|
{
|
|
dAASSERT (b);
|
|
const dReal* src = b->posr.R;
|
|
R[0] = src[0];
|
|
R[1] = src[1];
|
|
R[2] = src[2];
|
|
R[3] = src[3];
|
|
R[4] = src[4];
|
|
R[5] = src[5];
|
|
R[6] = src[6];
|
|
R[7] = src[7];
|
|
R[8] = src[8];
|
|
R[9] = src[9];
|
|
R[10] = src[10];
|
|
R[11] = src[11];
|
|
}
|
|
|
|
|
|
const dReal * dBodyGetQuaternion (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->q;
|
|
}
|
|
|
|
|
|
void dBodyCopyQuaternion (dBodyID b, dQuaternion quat)
|
|
{
|
|
dAASSERT (b);
|
|
dReal* src = b->q;
|
|
quat[0] = src[0];
|
|
quat[1] = src[1];
|
|
quat[2] = src[2];
|
|
quat[3] = src[3];
|
|
}
|
|
|
|
|
|
const dReal * dBodyGetLinearVel (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->lvel;
|
|
}
|
|
|
|
|
|
const dReal * dBodyGetAngularVel (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->avel;
|
|
}
|
|
|
|
|
|
void dBodySetMass (dBodyID b, const dMass *mass)
|
|
{
|
|
dAASSERT (b && mass );
|
|
dIASSERT(dMassCheck(mass));
|
|
|
|
// The centre of mass must be at the origin.
|
|
// Use dMassTranslate( mass, -mass->c[0], -mass->c[1], -mass->c[2] ) to correct it.
|
|
dUASSERT( fabs( mass->c[0] ) <= dEpsilon &&
|
|
fabs( mass->c[1] ) <= dEpsilon &&
|
|
fabs( mass->c[2] ) <= dEpsilon, "The centre of mass must be at the origin." );
|
|
|
|
b->mass = *mass;
|
|
if (dInvertPDMatrix (b->mass.I,b->invI,3,NULL)==0) {
|
|
dDEBUGMSG ("inertia must be positive definite!");
|
|
dRSetIdentity (b->invI);
|
|
}
|
|
b->invMass = dRecip(b->mass.mass);
|
|
}
|
|
|
|
|
|
void dBodyGetMass (dBodyID b, dMass *mass)
|
|
{
|
|
dAASSERT (b && mass);
|
|
*mass = b->mass;
|
|
}
|
|
|
|
|
|
void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)
|
|
{
|
|
dAASSERT (b);
|
|
b->facc[0] += fx;
|
|
b->facc[1] += fy;
|
|
b->facc[2] += fz;
|
|
}
|
|
|
|
|
|
void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
|
|
{
|
|
dAASSERT (b);
|
|
b->tacc[0] += fx;
|
|
b->tacc[1] += fy;
|
|
b->tacc[2] += fz;
|
|
}
|
|
|
|
|
|
void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 t1,t2;
|
|
t1[0] = fx;
|
|
t1[1] = fy;
|
|
t1[2] = fz;
|
|
t1[3] = 0;
|
|
dMultiply0_331 (t2,b->posr.R,t1);
|
|
b->facc[0] += t2[0];
|
|
b->facc[1] += t2[1];
|
|
b->facc[2] += t2[2];
|
|
}
|
|
|
|
|
|
void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 t1,t2;
|
|
t1[0] = fx;
|
|
t1[1] = fy;
|
|
t1[2] = fz;
|
|
t1[3] = 0;
|
|
dMultiply0_331 (t2,b->posr.R,t1);
|
|
b->tacc[0] += t2[0];
|
|
b->tacc[1] += t2[1];
|
|
b->tacc[2] += t2[2];
|
|
}
|
|
|
|
|
|
void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
|
|
dReal px, dReal py, dReal pz)
|
|
{
|
|
dAASSERT (b);
|
|
b->facc[0] += fx;
|
|
b->facc[1] += fy;
|
|
b->facc[2] += fz;
|
|
dVector3 f,q;
|
|
f[0] = fx;
|
|
f[1] = fy;
|
|
f[2] = fz;
|
|
q[0] = px - b->posr.pos[0];
|
|
q[1] = py - b->posr.pos[1];
|
|
q[2] = pz - b->posr.pos[2];
|
|
dAddVectorCross3(b->tacc,q,f);
|
|
}
|
|
|
|
|
|
void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
|
|
dReal px, dReal py, dReal pz)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 prel,f,p;
|
|
f[0] = fx;
|
|
f[1] = fy;
|
|
f[2] = fz;
|
|
f[3] = 0;
|
|
prel[0] = px;
|
|
prel[1] = py;
|
|
prel[2] = pz;
|
|
prel[3] = 0;
|
|
dMultiply0_331 (p,b->posr.R,prel);
|
|
b->facc[0] += f[0];
|
|
b->facc[1] += f[1];
|
|
b->facc[2] += f[2];
|
|
dAddVectorCross3(b->tacc,p,f);
|
|
}
|
|
|
|
|
|
void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
|
|
dReal px, dReal py, dReal pz)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 frel,f;
|
|
frel[0] = fx;
|
|
frel[1] = fy;
|
|
frel[2] = fz;
|
|
frel[3] = 0;
|
|
dMultiply0_331 (f,b->posr.R,frel);
|
|
b->facc[0] += f[0];
|
|
b->facc[1] += f[1];
|
|
b->facc[2] += f[2];
|
|
dVector3 q;
|
|
q[0] = px - b->posr.pos[0];
|
|
q[1] = py - b->posr.pos[1];
|
|
q[2] = pz - b->posr.pos[2];
|
|
dAddVectorCross3(b->tacc,q,f);
|
|
}
|
|
|
|
|
|
void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
|
|
dReal px, dReal py, dReal pz)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 frel,prel,f,p;
|
|
frel[0] = fx;
|
|
frel[1] = fy;
|
|
frel[2] = fz;
|
|
frel[3] = 0;
|
|
prel[0] = px;
|
|
prel[1] = py;
|
|
prel[2] = pz;
|
|
prel[3] = 0;
|
|
dMultiply0_331 (f,b->posr.R,frel);
|
|
dMultiply0_331 (p,b->posr.R,prel);
|
|
b->facc[0] += f[0];
|
|
b->facc[1] += f[1];
|
|
b->facc[2] += f[2];
|
|
dAddVectorCross3(b->tacc,p,f);
|
|
}
|
|
|
|
|
|
const dReal * dBodyGetForce (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->facc;
|
|
}
|
|
|
|
|
|
const dReal * dBodyGetTorque (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->tacc;
|
|
}
|
|
|
|
|
|
void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)
|
|
{
|
|
dAASSERT (b);
|
|
b->facc[0] = x;
|
|
b->facc[1] = y;
|
|
b->facc[2] = z;
|
|
}
|
|
|
|
|
|
void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
|
|
{
|
|
dAASSERT (b);
|
|
b->tacc[0] = x;
|
|
b->tacc[1] = y;
|
|
b->tacc[2] = z;
|
|
}
|
|
|
|
|
|
void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
|
|
dVector3 result)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 prel,p;
|
|
prel[0] = px;
|
|
prel[1] = py;
|
|
prel[2] = pz;
|
|
prel[3] = 0;
|
|
dMultiply0_331 (p,b->posr.R,prel);
|
|
result[0] = p[0] + b->posr.pos[0];
|
|
result[1] = p[1] + b->posr.pos[1];
|
|
result[2] = p[2] + b->posr.pos[2];
|
|
}
|
|
|
|
|
|
void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
|
|
dVector3 result)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 prel,p;
|
|
prel[0] = px;
|
|
prel[1] = py;
|
|
prel[2] = pz;
|
|
prel[3] = 0;
|
|
dMultiply0_331 (p,b->posr.R,prel);
|
|
result[0] = b->lvel[0];
|
|
result[1] = b->lvel[1];
|
|
result[2] = b->lvel[2];
|
|
dAddVectorCross3(result,b->avel,p);
|
|
}
|
|
|
|
|
|
void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
|
|
dVector3 result)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 p;
|
|
p[0] = px - b->posr.pos[0];
|
|
p[1] = py - b->posr.pos[1];
|
|
p[2] = pz - b->posr.pos[2];
|
|
p[3] = 0;
|
|
result[0] = b->lvel[0];
|
|
result[1] = b->lvel[1];
|
|
result[2] = b->lvel[2];
|
|
dAddVectorCross3(result,b->avel,p);
|
|
}
|
|
|
|
|
|
void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
|
|
dVector3 result)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 prel;
|
|
prel[0] = px - b->posr.pos[0];
|
|
prel[1] = py - b->posr.pos[1];
|
|
prel[2] = pz - b->posr.pos[2];
|
|
prel[3] = 0;
|
|
dMultiply1_331 (result,b->posr.R,prel);
|
|
}
|
|
|
|
|
|
void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
|
|
dVector3 result)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 p;
|
|
p[0] = px;
|
|
p[1] = py;
|
|
p[2] = pz;
|
|
p[3] = 0;
|
|
dMultiply0_331 (result,b->posr.R,p);
|
|
}
|
|
|
|
|
|
void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
|
|
dVector3 result)
|
|
{
|
|
dAASSERT (b);
|
|
dVector3 p;
|
|
p[0] = px;
|
|
p[1] = py;
|
|
p[2] = pz;
|
|
p[3] = 0;
|
|
dMultiply1_331 (result,b->posr.R,p);
|
|
}
|
|
|
|
|
|
void dBodySetFiniteRotationMode (dBodyID b, int mode)
|
|
{
|
|
dAASSERT (b);
|
|
b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis);
|
|
if (mode) {
|
|
b->flags |= dxBodyFlagFiniteRotation;
|
|
if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 ||
|
|
b->finite_rot_axis[2] != 0) {
|
|
b->flags |= dxBodyFlagFiniteRotationAxis;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)
|
|
{
|
|
dAASSERT (b);
|
|
b->finite_rot_axis[0] = x;
|
|
b->finite_rot_axis[1] = y;
|
|
b->finite_rot_axis[2] = z;
|
|
if (x != 0 || y != 0 || z != 0) {
|
|
dNormalize3 (b->finite_rot_axis);
|
|
b->flags |= dxBodyFlagFiniteRotationAxis;
|
|
}
|
|
else {
|
|
b->flags &= ~dxBodyFlagFiniteRotationAxis;
|
|
}
|
|
}
|
|
|
|
|
|
int dBodyGetFiniteRotationMode (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return ((b->flags & dxBodyFlagFiniteRotation) != 0);
|
|
}
|
|
|
|
|
|
void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)
|
|
{
|
|
dAASSERT (b);
|
|
result[0] = b->finite_rot_axis[0];
|
|
result[1] = b->finite_rot_axis[1];
|
|
result[2] = b->finite_rot_axis[2];
|
|
}
|
|
|
|
|
|
int dBodyGetNumJoints (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
int count=0;
|
|
for (dxJointNode *n=b->firstjoint; n; n=n->next, count++);
|
|
return count;
|
|
}
|
|
|
|
|
|
dJointID dBodyGetJoint (dBodyID b, int index)
|
|
{
|
|
dAASSERT (b);
|
|
int i=0;
|
|
for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) {
|
|
if (i == index) return n->joint;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void dBodySetDynamic (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
|
|
dBodySetMass(b,&b->mass);
|
|
}
|
|
|
|
void dBodySetKinematic (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
dSetZero (b->invI,4*3);
|
|
b->invMass = 0;
|
|
}
|
|
|
|
int dBodyIsKinematic (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return b->invMass == 0;
|
|
}
|
|
|
|
void dBodyEnable (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
b->flags &= ~dxBodyDisabled;
|
|
b->adis_stepsleft = b->adis.idle_steps;
|
|
b->adis_timeleft = b->adis.idle_time;
|
|
// no code for average-processing needed here
|
|
}
|
|
|
|
|
|
void dBodyDisable (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
b->flags |= dxBodyDisabled;
|
|
}
|
|
|
|
|
|
int dBodyIsEnabled (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return ((b->flags & dxBodyDisabled) == 0);
|
|
}
|
|
|
|
|
|
void dBodySetGravityMode (dBodyID b, int mode)
|
|
{
|
|
dAASSERT (b);
|
|
if (mode) b->flags &= ~dxBodyNoGravity;
|
|
else b->flags |= dxBodyNoGravity;
|
|
}
|
|
|
|
|
|
int dBodyGetGravityMode (dBodyID b)
|
|
{
|
|
dAASSERT (b);
|
|
return ((b->flags & dxBodyNoGravity) == 0);
|
|
}
|
|
|
|
|
|
// body auto-disable functions
|
|
|
|
dReal dBodyGetAutoDisableLinearThreshold (dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return dSqrt (b->adis.linear_average_threshold);
|
|
}
|
|
|
|
|
|
void dBodySetAutoDisableLinearThreshold (dBodyID b, dReal linear_average_threshold)
|
|
{
|
|
dAASSERT(b);
|
|
b->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
|
|
}
|
|
|
|
|
|
dReal dBodyGetAutoDisableAngularThreshold (dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return dSqrt (b->adis.angular_average_threshold);
|
|
}
|
|
|
|
|
|
void dBodySetAutoDisableAngularThreshold (dBodyID b, dReal angular_average_threshold)
|
|
{
|
|
dAASSERT(b);
|
|
b->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
|
|
}
|
|
|
|
|
|
int dBodyGetAutoDisableAverageSamplesCount (dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return b->adis.average_samples;
|
|
}
|
|
|
|
|
|
void dBodySetAutoDisableAverageSamplesCount (dBodyID b, unsigned int average_samples_count)
|
|
{
|
|
dAASSERT(b);
|
|
b->adis.average_samples = average_samples_count;
|
|
// update the average buffers
|
|
if(b->average_lvel_buffer)
|
|
{
|
|
delete[] b->average_lvel_buffer;
|
|
b->average_lvel_buffer = NULL;
|
|
}
|
|
if(b->average_avel_buffer)
|
|
{
|
|
delete[] b->average_avel_buffer;
|
|
b->average_avel_buffer = NULL;
|
|
}
|
|
if(b->adis.average_samples > 0)
|
|
{
|
|
b->average_lvel_buffer = new dVector3[b->adis.average_samples];
|
|
b->average_avel_buffer = new dVector3[b->adis.average_samples];
|
|
}
|
|
else
|
|
{
|
|
b->average_lvel_buffer = NULL;
|
|
b->average_avel_buffer = NULL;
|
|
}
|
|
// new buffer is empty
|
|
b->average_counter = 0;
|
|
b->average_ready = 0;
|
|
}
|
|
|
|
|
|
int dBodyGetAutoDisableSteps (dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return b->adis.idle_steps;
|
|
}
|
|
|
|
|
|
void dBodySetAutoDisableSteps (dBodyID b, int steps)
|
|
{
|
|
dAASSERT(b);
|
|
b->adis.idle_steps = steps;
|
|
}
|
|
|
|
|
|
dReal dBodyGetAutoDisableTime (dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return b->adis.idle_time;
|
|
}
|
|
|
|
|
|
void dBodySetAutoDisableTime (dBodyID b, dReal time)
|
|
{
|
|
dAASSERT(b);
|
|
b->adis.idle_time = time;
|
|
}
|
|
|
|
|
|
int dBodyGetAutoDisableFlag (dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return ((b->flags & dxBodyAutoDisable) != 0);
|
|
}
|
|
|
|
|
|
void dBodySetAutoDisableFlag (dBodyID b, int do_auto_disable)
|
|
{
|
|
dAASSERT(b);
|
|
if (!do_auto_disable)
|
|
{
|
|
b->flags &= ~dxBodyAutoDisable;
|
|
// (mg) we should also reset the IsDisabled state to correspond to the DoDisabling flag
|
|
b->flags &= ~dxBodyDisabled;
|
|
b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world);
|
|
b->adis.idle_time = dWorldGetAutoDisableTime(b->world);
|
|
// resetting the average calculations too
|
|
dBodySetAutoDisableAverageSamplesCount(b, dWorldGetAutoDisableAverageSamplesCount(b->world) );
|
|
}
|
|
else
|
|
{
|
|
b->flags |= dxBodyAutoDisable;
|
|
}
|
|
}
|
|
|
|
|
|
void dBodySetAutoDisableDefaults (dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
dWorldID w = b->world;
|
|
dAASSERT(w);
|
|
b->adis = w->adis;
|
|
dBodySetAutoDisableFlag (b, w->body_flags & dxBodyAutoDisable);
|
|
}
|
|
|
|
|
|
// body damping functions
|
|
|
|
dReal dBodyGetLinearDamping(dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return b->dampingp.linear_scale;
|
|
}
|
|
|
|
void dBodySetLinearDamping(dBodyID b, dReal scale)
|
|
{
|
|
dAASSERT(b);
|
|
if (scale)
|
|
b->flags |= dxBodyLinearDamping;
|
|
else
|
|
b->flags &= ~dxBodyLinearDamping;
|
|
b->dampingp.linear_scale = scale;
|
|
}
|
|
|
|
dReal dBodyGetAngularDamping(dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return b->dampingp.angular_scale;
|
|
}
|
|
|
|
void dBodySetAngularDamping(dBodyID b, dReal scale)
|
|
{
|
|
dAASSERT(b);
|
|
if (scale)
|
|
b->flags |= dxBodyAngularDamping;
|
|
else
|
|
b->flags &= ~dxBodyAngularDamping;
|
|
b->dampingp.angular_scale = scale;
|
|
}
|
|
|
|
void dBodySetDamping(dBodyID b, dReal linear_scale, dReal angular_scale)
|
|
{
|
|
dAASSERT(b);
|
|
dBodySetLinearDamping(b, linear_scale);
|
|
dBodySetAngularDamping(b, angular_scale);
|
|
}
|
|
|
|
dReal dBodyGetLinearDampingThreshold(dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return dSqrt(b->dampingp.linear_threshold);
|
|
}
|
|
|
|
void dBodySetLinearDampingThreshold(dBodyID b, dReal threshold)
|
|
{
|
|
dAASSERT(b);
|
|
b->dampingp.linear_threshold = threshold*threshold;
|
|
}
|
|
|
|
|
|
dReal dBodyGetAngularDampingThreshold(dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return dSqrt(b->dampingp.angular_threshold);
|
|
}
|
|
|
|
void dBodySetAngularDampingThreshold(dBodyID b, dReal threshold)
|
|
{
|
|
dAASSERT(b);
|
|
b->dampingp.angular_threshold = threshold*threshold;
|
|
}
|
|
|
|
void dBodySetDampingDefaults(dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
dWorldID w = b->world;
|
|
dAASSERT(w);
|
|
b->dampingp = w->dampingp;
|
|
const unsigned mask = dxBodyLinearDamping | dxBodyAngularDamping;
|
|
b->flags &= ~mask; // zero them
|
|
b->flags |= w->body_flags & mask;
|
|
}
|
|
|
|
dReal dBodyGetMaxAngularSpeed(dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return b->max_angular_speed;
|
|
}
|
|
|
|
void dBodySetMaxAngularSpeed(dBodyID b, dReal max_speed)
|
|
{
|
|
dAASSERT(b);
|
|
if (max_speed < dInfinity)
|
|
b->flags |= dxBodyMaxAngularSpeed;
|
|
else
|
|
b->flags &= ~dxBodyMaxAngularSpeed;
|
|
b->max_angular_speed = max_speed;
|
|
}
|
|
|
|
void dBodySetMovedCallback(dBodyID b, void (*callback)(dBodyID))
|
|
{
|
|
dAASSERT(b);
|
|
b->moved_callback = callback;
|
|
}
|
|
|
|
|
|
dGeomID dBodyGetFirstGeom(dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return b->geom;
|
|
}
|
|
|
|
|
|
dGeomID dBodyGetNextGeom(dGeomID geom)
|
|
{
|
|
dAASSERT(geom);
|
|
return dGeomGetBodyNext(geom);
|
|
}
|
|
|
|
|
|
int dBodyGetGyroscopicMode(dBodyID b)
|
|
{
|
|
dAASSERT(b);
|
|
return b->flags & dxBodyGyroscopic;
|
|
}
|
|
|
|
void dBodySetGyroscopicMode(dBodyID b, int enabled)
|
|
{
|
|
dAASSERT(b);
|
|
if (enabled)
|
|
b->flags |= dxBodyGyroscopic;
|
|
else
|
|
b->flags &= ~dxBodyGyroscopic;
|
|
}
|
|
|
|
|
|
|
|
//****************************************************************************
|
|
// joints
|
|
|
|
|
|
|
|
template<class T>
|
|
dxJoint* createJoint(dWorldID w, dJointGroupID group)
|
|
{
|
|
dxJoint *j;
|
|
if (group) {
|
|
j = group->alloc<T>(w);
|
|
} else {
|
|
j = new T(w);
|
|
}
|
|
return j;
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointBall>(w,group);
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointHinge>(w,group);
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointSlider>(w,group);
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
|
|
const dContact *c)
|
|
{
|
|
dAASSERT (w && c);
|
|
dxJointContact *j = (dxJointContact *)
|
|
createJoint<dxJointContact> (w,group);
|
|
j->contact = *c;
|
|
return j;
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointHinge2> (w,group);
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointUniversal> (w,group);
|
|
}
|
|
|
|
dxJoint * dJointCreatePR (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointPR> (w,group);
|
|
}
|
|
|
|
dxJoint * dJointCreatePU (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointPU> (w,group);
|
|
}
|
|
|
|
dxJoint * dJointCreatePiston (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointPiston> (w,group);
|
|
}
|
|
|
|
dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointFixed> (w,group);
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointNull> (w,group);
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointAMotor> (w,group);
|
|
}
|
|
|
|
dxJoint * dJointCreateLMotor (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointLMotor> (w,group);
|
|
}
|
|
|
|
dxJoint * dJointCreatePlane2D (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointPlane2D> (w,group);
|
|
}
|
|
|
|
dxJoint * dJointCreateDBall (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointDBall> (w,group);
|
|
}
|
|
|
|
dxJoint * dJointCreateDHinge (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointDHinge> (w,group);
|
|
}
|
|
|
|
|
|
dxJoint * dJointCreateTransmission (dWorldID w, dJointGroupID group)
|
|
{
|
|
dAASSERT (w);
|
|
return createJoint<dxJointTransmission> (w,group);
|
|
}
|
|
|
|
static void FinalizeAndDestroyJointInstance(dxJoint *j, bool delete_it)
|
|
{
|
|
// if any group joints have their world pointer set to 0, their world was
|
|
// previously destroyed. no special handling is required for these joints.
|
|
if (j->world != NULL) {
|
|
removeJointReferencesFromAttachedBodies (j);
|
|
removeObjectFromList (j);
|
|
j->world->nj--;
|
|
}
|
|
if (delete_it) {
|
|
delete j;
|
|
} else {
|
|
j->~dxJoint();
|
|
}
|
|
}
|
|
|
|
void dJointDestroy (dxJoint *j)
|
|
{
|
|
dAASSERT (j);
|
|
if (!(j->flags & dJOINT_INGROUP)) {
|
|
FinalizeAndDestroyJointInstance(j, true);
|
|
}
|
|
}
|
|
|
|
|
|
dJointGroupID dJointGroupCreate (int /*max_size*/)
|
|
{
|
|
// not anymore ... dUASSERT (max_size > 0,"max size must be > 0");
|
|
dxJointGroup *group = new dxJointGroup();
|
|
return group;
|
|
}
|
|
|
|
|
|
void dJointGroupDestroy (dJointGroupID group)
|
|
{
|
|
dAASSERT (group);
|
|
dJointGroupEmpty (group);
|
|
delete group;
|
|
}
|
|
|
|
void dJointGroupEmpty (dJointGroupID group)
|
|
{
|
|
dAASSERT (group);
|
|
|
|
const sizeint num_joints = group->getJointCount();
|
|
if (num_joints != 0) {
|
|
// Local array is used since ALLOCA leads to mysterious NULL values in first array element and crashes under VS2005 :)
|
|
const sizeint max_stack_jlist_size = 1024;
|
|
dxJoint *stack_jlist[max_stack_jlist_size];
|
|
|
|
const sizeint jlist_size = num_joints * sizeof(dxJoint*);
|
|
dxJoint **jlist = num_joints <= max_stack_jlist_size ? stack_jlist : (dxJoint **)dAlloc(jlist_size);
|
|
|
|
if (jlist != NULL) {
|
|
// the joints in this group are detached starting from the most recently
|
|
// added (at the top of the stack). this helps ensure that the various
|
|
// linked lists are not traversed too much, as the joints will hopefully
|
|
// be at the start of those lists.
|
|
sizeint num_exported = group->exportJoints(jlist);
|
|
dIVERIFY(num_exported == num_joints);
|
|
|
|
for (sizeint i = num_joints; i != 0; ) {
|
|
--i;
|
|
dxJoint *j = jlist[i];
|
|
FinalizeAndDestroyJointInstance(j, false);
|
|
}
|
|
} else {
|
|
// ...else if there is no memory, go on detaching the way it is possible
|
|
sizeint joint_bytes;
|
|
for (dxJoint *j = (dxJoint *)group->beginEnum(); j != NULL; j = (dxJoint *)group->continueEnum(joint_bytes)) {
|
|
joint_bytes = j->size(); // Get size before object is destroyed!
|
|
FinalizeAndDestroyJointInstance(j, false);
|
|
}
|
|
}
|
|
|
|
group->freeAll();
|
|
|
|
if (jlist != stack_jlist && jlist != NULL) {
|
|
dFree(jlist, jlist_size);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
int dJointGetNumBodies(dxJoint *joint)
|
|
{
|
|
// check arguments
|
|
dUASSERT (joint,"bad joint argument");
|
|
|
|
if ( !joint->node[0].body )
|
|
return 0;
|
|
else if ( !joint->node[1].body )
|
|
return 1;
|
|
else
|
|
return 2;
|
|
}
|
|
|
|
|
|
void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
|
|
{
|
|
// check arguments
|
|
dUASSERT (joint,"bad joint argument");
|
|
dUASSERT (body1 == NULL || body1 != body2, "can't have body1==body2");
|
|
dxWorld *world = joint->world;
|
|
dUASSERT ( (body1 == NULL || body1->world == world) &&
|
|
(body2 == NULL || body2->world == world),
|
|
"joint and bodies must be in same world");
|
|
|
|
// check if the joint can not be attached to just one body
|
|
dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
|
|
((body1 != NULL) != (body2 != NULL))),
|
|
"joint can not be attached to just one body");
|
|
|
|
// remove any existing body attachments
|
|
if (joint->node[0].body != NULL || joint->node[1].body != NULL) {
|
|
removeJointReferencesFromAttachedBodies (joint);
|
|
}
|
|
|
|
// if a body is zero, make sure that it is body2, so 0 --> node[1].body
|
|
if (body1 == NULL) {
|
|
body1 = body2;
|
|
body2 = NULL;
|
|
joint->flags |= dJOINT_REVERSE;
|
|
}
|
|
else {
|
|
joint->flags &= (~dJOINT_REVERSE);
|
|
}
|
|
|
|
// attach to new bodies
|
|
joint->node[0].body = body1;
|
|
joint->node[1].body = body2;
|
|
|
|
if (body1 != NULL) {
|
|
joint->node[1].next = body1->firstjoint;
|
|
body1->firstjoint = &joint->node[1];
|
|
}
|
|
else {
|
|
joint->node[1].next = NULL;
|
|
}
|
|
|
|
if (body2 != NULL) {
|
|
joint->node[0].next = body2->firstjoint;
|
|
body2->firstjoint = &joint->node[0];
|
|
}
|
|
else {
|
|
joint->node[0].next = NULL;
|
|
}
|
|
|
|
// Since the bodies are now set.
|
|
// Calculate the values depending on the bodies.
|
|
// Only need to calculate relative value if a body exist
|
|
if (body1 != NULL || body2 != NULL) {
|
|
joint->setRelativeValues();
|
|
}
|
|
}
|
|
|
|
void dJointEnable (dxJoint *joint)
|
|
{
|
|
dAASSERT (joint);
|
|
joint->flags &= ~dJOINT_DISABLED;
|
|
}
|
|
|
|
void dJointDisable (dxJoint *joint)
|
|
{
|
|
dAASSERT (joint);
|
|
joint->flags |= dJOINT_DISABLED;
|
|
}
|
|
|
|
int dJointIsEnabled (dxJoint *joint)
|
|
{
|
|
dAASSERT (joint);
|
|
return (joint->flags & dJOINT_DISABLED) == 0;
|
|
}
|
|
|
|
void dJointSetData (dxJoint *joint, void *data)
|
|
{
|
|
dAASSERT (joint);
|
|
joint->userdata = data;
|
|
}
|
|
|
|
|
|
void *dJointGetData (dxJoint *joint)
|
|
{
|
|
dAASSERT (joint);
|
|
return joint->userdata;
|
|
}
|
|
|
|
|
|
dJointType dJointGetType (dxJoint *joint)
|
|
{
|
|
dAASSERT (joint);
|
|
return joint->type();
|
|
}
|
|
|
|
|
|
dBodyID dJointGetBody (dxJoint *joint, int index)
|
|
{
|
|
dAASSERT (joint);
|
|
if (index == 0 || index == 1) {
|
|
if (joint->flags & dJOINT_REVERSE) return joint->node[1-index].body;
|
|
else return joint->node[index].body;
|
|
}
|
|
else return 0;
|
|
}
|
|
|
|
|
|
void dJointSetFeedback (dxJoint *joint, dJointFeedback *f)
|
|
{
|
|
dAASSERT (joint);
|
|
joint->feedback = f;
|
|
}
|
|
|
|
|
|
dJointFeedback *dJointGetFeedback (dxJoint *joint)
|
|
{
|
|
dAASSERT (joint);
|
|
return joint->feedback;
|
|
}
|
|
|
|
|
|
|
|
dJointID dConnectingJoint (dBodyID in_b1, dBodyID in_b2)
|
|
{
|
|
dAASSERT (in_b1 || in_b2);
|
|
|
|
dBodyID b1, b2;
|
|
|
|
if (in_b1 == 0) {
|
|
b1 = in_b2;
|
|
b2 = in_b1;
|
|
}
|
|
else {
|
|
b1 = in_b1;
|
|
b2 = in_b2;
|
|
}
|
|
|
|
// look through b1's neighbour list for b2
|
|
for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
|
|
if (n->body == b2) return n->joint;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
int dConnectingJointList (dBodyID in_b1, dBodyID in_b2, dJointID* out_list)
|
|
{
|
|
dAASSERT (in_b1 || in_b2);
|
|
|
|
|
|
dBodyID b1, b2;
|
|
|
|
if (in_b1 == 0) {
|
|
b1 = in_b2;
|
|
b2 = in_b1;
|
|
}
|
|
else {
|
|
b1 = in_b1;
|
|
b2 = in_b2;
|
|
}
|
|
|
|
// look through b1's neighbour list for b2
|
|
int numConnectingJoints = 0;
|
|
for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
|
|
if (n->body == b2)
|
|
out_list[numConnectingJoints++] = n->joint;
|
|
}
|
|
|
|
return numConnectingJoints;
|
|
}
|
|
|
|
|
|
int dAreConnected (dBodyID b1, dBodyID b2)
|
|
{
|
|
dAASSERT (b1/* && b2*/); // b2 can be NULL to test for connection to environment
|
|
// look through b1's neighbour list for b2
|
|
for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
|
|
if (n->body == b2) return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type)
|
|
{
|
|
dAASSERT (b1/* && b2*/); // b2 can be NULL to test for connection to environment
|
|
// look through b1's neighbour list for b2
|
|
for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
|
|
if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
//****************************************************************************
|
|
// world
|
|
|
|
dxWorld * dWorldCreate()
|
|
{
|
|
dxWorld *w = new dxWorld();
|
|
|
|
return w;
|
|
}
|
|
|
|
|
|
void dWorldDestroy (dxWorld *w)
|
|
{
|
|
// delete all bodies and joints
|
|
dAASSERT (w);
|
|
dxBody *nextb, *b = w->firstbody;
|
|
while (b) {
|
|
nextb = (dxBody*) b->next;
|
|
dBodyDestroy(b); // calling here dBodyDestroy for correct destroying! (i.e. the average buffers)
|
|
b = nextb;
|
|
}
|
|
|
|
dxJoint *nextj, *j = w->firstjoint;
|
|
while (j) {
|
|
nextj = (dxJoint*)j->next;
|
|
if (j->flags & dJOINT_INGROUP) {
|
|
// the joint is part of a group, so "deactivate" it instead
|
|
j->world = NULL;
|
|
j->node[0].body = NULL;
|
|
j->node[0].next = NULL;
|
|
j->node[1].body = NULL;
|
|
j->node[1].next = NULL;
|
|
dMessage (0,"warning: destroying world containing grouped joints");
|
|
}
|
|
else {
|
|
// TODO: shouldn't we call dJointDestroy()?
|
|
sizeint sz = j->size();
|
|
j->~dxJoint();
|
|
dFree (j,sz);
|
|
}
|
|
j = nextj;
|
|
}
|
|
|
|
delete w;
|
|
}
|
|
|
|
|
|
void dWorldSetData (dWorldID w, void *data)
|
|
{
|
|
dAASSERT (w);
|
|
w->userdata = data;
|
|
}
|
|
|
|
|
|
void* dWorldGetData (dWorldID w)
|
|
{
|
|
dAASSERT (w);
|
|
return w->userdata;
|
|
}
|
|
|
|
|
|
void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
|
|
{
|
|
dAASSERT (w);
|
|
w->gravity[0] = x;
|
|
w->gravity[1] = y;
|
|
w->gravity[2] = z;
|
|
}
|
|
|
|
|
|
void dWorldGetGravity (dWorldID w, dVector3 g)
|
|
{
|
|
dAASSERT (w);
|
|
g[0] = w->gravity[0];
|
|
g[1] = w->gravity[1];
|
|
g[2] = w->gravity[2];
|
|
}
|
|
|
|
|
|
void dWorldSetERP (dWorldID w, dReal erp)
|
|
{
|
|
dAASSERT (w);
|
|
w->global_erp = erp;
|
|
}
|
|
|
|
|
|
dReal dWorldGetERP (dWorldID w)
|
|
{
|
|
dAASSERT (w);
|
|
return w->global_erp;
|
|
}
|
|
|
|
|
|
void dWorldSetCFM (dWorldID w, dReal cfm)
|
|
{
|
|
dAASSERT (w);
|
|
w->global_cfm = cfm;
|
|
}
|
|
|
|
|
|
dReal dWorldGetCFM (dWorldID w)
|
|
{
|
|
dAASSERT (w);
|
|
return w->global_cfm;
|
|
}
|
|
|
|
|
|
void dWorldSetStepIslandsProcessingMaxThreadCount(dWorldID w, unsigned count)
|
|
{
|
|
dAASSERT (w);
|
|
w->islands_max_threads = count;
|
|
}
|
|
|
|
unsigned dWorldGetStepIslandsProcessingMaxThreadCount(dWorldID w)
|
|
{
|
|
dAASSERT (w);
|
|
return w->islands_max_threads;
|
|
}
|
|
|
|
int dWorldUseSharedWorkingMemory(dWorldID w, dWorldID from_world)
|
|
{
|
|
dUASSERT (w,"bad world argument");
|
|
|
|
bool result = false;
|
|
|
|
if (from_world)
|
|
{
|
|
dUASSERT (!w->wmem, "world does already have working memory allocated"); // Prevent replacement of one memory object with another to avoid cases when smaller buffer replaces a larger one or memory manager changes.
|
|
|
|
dxStepWorkingMemory *wmem = AllocateOnDemand(from_world->wmem);
|
|
|
|
if (wmem)
|
|
{
|
|
// Even though there is an assertion check on entry still release existing
|
|
// memory object for extra safety.
|
|
if (w->wmem)
|
|
{
|
|
w->wmem->Release();
|
|
w->wmem = NULL;
|
|
}
|
|
|
|
wmem->Addref();
|
|
w->wmem = wmem;
|
|
|
|
result = true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
dxStepWorkingMemory *wmem = w->wmem;
|
|
|
|
if (wmem)
|
|
{
|
|
wmem->Release();
|
|
w->wmem = NULL;
|
|
}
|
|
|
|
result = true;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
void dWorldCleanupWorkingMemory(dWorldID w)
|
|
{
|
|
dUASSERT (w,"bad world argument");
|
|
|
|
dxStepWorkingMemory *wmem = w->wmem;
|
|
|
|
if (wmem)
|
|
{
|
|
wmem->CleanupMemory();
|
|
}
|
|
}
|
|
|
|
int dWorldSetStepMemoryReservationPolicy(dWorldID w, const dWorldStepReserveInfo *policyinfo)
|
|
{
|
|
dUASSERT (w,"bad world argument");
|
|
dUASSERT (!policyinfo || (policyinfo->struct_size >= sizeof(*policyinfo) && policyinfo->reserve_factor >= 1.0f), "Bad policy info");
|
|
|
|
bool result = false;
|
|
|
|
dxStepWorkingMemory *wmem = policyinfo ? AllocateOnDemand(w->wmem) : w->wmem;
|
|
|
|
if (wmem)
|
|
{
|
|
if (policyinfo)
|
|
{
|
|
wmem->SetMemoryReserveInfo(policyinfo->reserve_factor, policyinfo->reserve_minimum);
|
|
result = wmem->GetMemoryReserveInfo() != NULL;
|
|
}
|
|
else
|
|
{
|
|
wmem->ResetMemoryReserveInfoToDefault();
|
|
result = true;
|
|
}
|
|
}
|
|
else if (!policyinfo)
|
|
{
|
|
result = true;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
int dWorldSetStepMemoryManager(dWorldID w, const dWorldStepMemoryFunctionsInfo *memfuncs)
|
|
{
|
|
dUASSERT (w,"bad world argument");
|
|
dUASSERT (!memfuncs || memfuncs->struct_size >= sizeof(*memfuncs), "Bad memory functions info");
|
|
|
|
bool result = false;
|
|
|
|
dxStepWorkingMemory *wmem = memfuncs ? AllocateOnDemand(w->wmem) : w->wmem;
|
|
|
|
if (wmem)
|
|
{
|
|
if (memfuncs)
|
|
{
|
|
wmem->SetMemoryManager(memfuncs->alloc_block, memfuncs->shrink_block, memfuncs->free_block);
|
|
result = wmem->GetMemoryManager() != NULL;
|
|
}
|
|
else
|
|
{
|
|
wmem->ResetMemoryManagerToDefault();
|
|
result = true;
|
|
}
|
|
}
|
|
else if (!memfuncs)
|
|
{
|
|
result = true;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
void dWorldSetStepThreadingImplementation(dWorldID w,
|
|
const dxThreadingFunctionsInfo *functions_info, dThreadingImplementationID threading_impl)
|
|
{
|
|
dUASSERT (w,"bad world argument");
|
|
dUASSERT (!functions_info || functions_info->struct_size >= sizeof(*functions_info), "Bad threading functions info");
|
|
|
|
#if dTHREADING_INTF_DISABLED
|
|
dUASSERT(functions_info == NULL && threading_impl == NULL, "Threading interface is not available");
|
|
#else
|
|
w->assignThreadingImpl(functions_info, threading_impl);
|
|
#endif
|
|
}
|
|
|
|
|
|
int dWorldStep (dWorldID w, dReal stepsize)
|
|
{
|
|
dUASSERT (w,"bad world argument");
|
|
dUASSERT (stepsize > 0,"stepsize must be > 0");
|
|
|
|
bool result = false;
|
|
|
|
dxWorldProcessIslandsInfo islandsinfo;
|
|
if (dxReallocateWorldProcessContext (w, islandsinfo, stepsize, &dxEstimateStepMemoryRequirements))
|
|
{
|
|
if (dxProcessIslands (w, islandsinfo, stepsize, &dxStepIsland, &dxEstimateStepMaxCallCount))
|
|
{
|
|
result = true;
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
int dWorldQuickStep (dWorldID w, dReal stepsize)
|
|
{
|
|
dUASSERT (w,"bad world argument");
|
|
dUASSERT (stepsize > 0,"stepsize must be > 0");
|
|
|
|
bool result = false;
|
|
|
|
dxWorldProcessIslandsInfo islandsinfo;
|
|
if (dxReallocateWorldProcessContext (w, islandsinfo, stepsize, &dxEstimateQuickStepMemoryRequirements))
|
|
{
|
|
if (dxProcessIslands (w, islandsinfo, stepsize, &dxQuickStepIsland, &dxEstimateQuickStepMaxCallCount))
|
|
{
|
|
result = true;
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
void dWorldImpulseToForce (dWorldID w, dReal stepsize,
|
|
dReal ix, dReal iy, dReal iz,
|
|
dVector3 force)
|
|
{
|
|
dAASSERT (w);
|
|
stepsize = dRecip(stepsize);
|
|
force[0] = stepsize * ix;
|
|
force[1] = stepsize * iy;
|
|
force[2] = stepsize * iz;
|
|
// @@@ force[3] = 0;
|
|
}
|
|
|
|
|
|
// world auto-disable functions
|
|
|
|
dReal dWorldGetAutoDisableLinearThreshold (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return dSqrt (w->adis.linear_average_threshold);
|
|
}
|
|
|
|
|
|
void dWorldSetAutoDisableLinearThreshold (dWorldID w, dReal linear_average_threshold)
|
|
{
|
|
dAASSERT(w);
|
|
w->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
|
|
}
|
|
|
|
|
|
dReal dWorldGetAutoDisableAngularThreshold (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return dSqrt (w->adis.angular_average_threshold);
|
|
}
|
|
|
|
|
|
void dWorldSetAutoDisableAngularThreshold (dWorldID w, dReal angular_average_threshold)
|
|
{
|
|
dAASSERT(w);
|
|
w->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
|
|
}
|
|
|
|
|
|
int dWorldGetAutoDisableAverageSamplesCount (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->adis.average_samples;
|
|
}
|
|
|
|
|
|
void dWorldSetAutoDisableAverageSamplesCount (dWorldID w, unsigned int average_samples_count)
|
|
{
|
|
dAASSERT(w);
|
|
w->adis.average_samples = average_samples_count;
|
|
}
|
|
|
|
|
|
int dWorldGetAutoDisableSteps (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->adis.idle_steps;
|
|
}
|
|
|
|
|
|
void dWorldSetAutoDisableSteps (dWorldID w, int steps)
|
|
{
|
|
dAASSERT(w);
|
|
w->adis.idle_steps = steps;
|
|
}
|
|
|
|
|
|
dReal dWorldGetAutoDisableTime (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->adis.idle_time;
|
|
}
|
|
|
|
|
|
void dWorldSetAutoDisableTime (dWorldID w, dReal time)
|
|
{
|
|
dAASSERT(w);
|
|
w->adis.idle_time = time;
|
|
}
|
|
|
|
|
|
int dWorldGetAutoDisableFlag (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->body_flags & dxBodyAutoDisable;
|
|
}
|
|
|
|
|
|
void dWorldSetAutoDisableFlag (dWorldID w, int do_auto_disable)
|
|
{
|
|
dAASSERT(w);
|
|
if (do_auto_disable)
|
|
w->body_flags |= dxBodyAutoDisable;
|
|
else
|
|
w->body_flags &= ~dxBodyAutoDisable;
|
|
}
|
|
|
|
|
|
// world damping functions
|
|
|
|
dReal dWorldGetLinearDampingThreshold(dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return dSqrt(w->dampingp.linear_threshold);
|
|
}
|
|
|
|
void dWorldSetLinearDampingThreshold(dWorldID w, dReal threshold)
|
|
{
|
|
dAASSERT(w);
|
|
w->dampingp.linear_threshold = threshold*threshold;
|
|
}
|
|
|
|
dReal dWorldGetAngularDampingThreshold(dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return dSqrt(w->dampingp.angular_threshold);
|
|
}
|
|
|
|
void dWorldSetAngularDampingThreshold(dWorldID w, dReal threshold)
|
|
{
|
|
dAASSERT(w);
|
|
w->dampingp.angular_threshold = threshold*threshold;
|
|
}
|
|
|
|
dReal dWorldGetLinearDamping(dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->dampingp.linear_scale;
|
|
}
|
|
|
|
void dWorldSetLinearDamping(dWorldID w, dReal scale)
|
|
{
|
|
dAASSERT(w);
|
|
if (scale)
|
|
w->body_flags |= dxBodyLinearDamping;
|
|
else
|
|
w->body_flags &= ~dxBodyLinearDamping;
|
|
w->dampingp.linear_scale = scale;
|
|
}
|
|
|
|
dReal dWorldGetAngularDamping(dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->dampingp.angular_scale;
|
|
}
|
|
|
|
void dWorldSetAngularDamping(dWorldID w, dReal scale)
|
|
{
|
|
dAASSERT(w);
|
|
if (scale)
|
|
w->body_flags |= dxBodyAngularDamping;
|
|
else
|
|
w->body_flags &= ~dxBodyAngularDamping;
|
|
w->dampingp.angular_scale = scale;
|
|
}
|
|
|
|
void dWorldSetDamping(dWorldID w, dReal linear_scale, dReal angular_scale)
|
|
{
|
|
dAASSERT(w);
|
|
dWorldSetLinearDamping(w, linear_scale);
|
|
dWorldSetAngularDamping(w, angular_scale);
|
|
}
|
|
|
|
dReal dWorldGetMaxAngularSpeed(dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->max_angular_speed;
|
|
}
|
|
|
|
void dWorldSetMaxAngularSpeed(dWorldID w, dReal max_speed)
|
|
{
|
|
dAASSERT(w);
|
|
if (max_speed < dInfinity)
|
|
w->body_flags |= dxBodyMaxAngularSpeed;
|
|
else
|
|
w->body_flags &= ~dxBodyMaxAngularSpeed;
|
|
w->max_angular_speed = max_speed;
|
|
}
|
|
|
|
|
|
void dWorldSetQuickStepNumIterations (dWorldID w, int num)
|
|
{
|
|
dAASSERT(w);
|
|
w->qs.num_iterations = num;
|
|
}
|
|
|
|
|
|
int dWorldGetQuickStepNumIterations (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->qs.num_iterations;
|
|
}
|
|
|
|
|
|
void dWorldSetQuickStepW (dWorldID w, dReal param)
|
|
{
|
|
dAASSERT(w);
|
|
w->qs.w = param;
|
|
}
|
|
|
|
|
|
dReal dWorldGetQuickStepW (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->qs.w;
|
|
}
|
|
|
|
|
|
void dWorldSetContactMaxCorrectingVel (dWorldID w, dReal vel)
|
|
{
|
|
dAASSERT(w);
|
|
w->contactp.max_vel = vel;
|
|
}
|
|
|
|
|
|
dReal dWorldGetContactMaxCorrectingVel (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->contactp.max_vel;
|
|
}
|
|
|
|
|
|
void dWorldSetContactSurfaceLayer (dWorldID w, dReal depth)
|
|
{
|
|
dAASSERT(w);
|
|
w->contactp.min_depth = depth;
|
|
}
|
|
|
|
|
|
dReal dWorldGetContactSurfaceLayer (dWorldID w)
|
|
{
|
|
dAASSERT(w);
|
|
return w->contactp.min_depth;
|
|
}
|
|
|
|
//****************************************************************************
|
|
// testing
|
|
|
|
#define NUM 100
|
|
|
|
#define DO(x)
|
|
|
|
|
|
extern "C" void dTestDataStructures()
|
|
{
|
|
int i;
|
|
DO(printf ("testDynamicsStuff()\n"));
|
|
|
|
dBodyID body [NUM];
|
|
int nb = 0;
|
|
dJointID joint [NUM];
|
|
int nj = 0;
|
|
|
|
for (i=0; i<NUM; i++) body[i] = NULL;
|
|
for (i=0; i<NUM; i++) joint[i] = NULL;
|
|
|
|
DO(printf ("creating world\n"));
|
|
dWorldID w = dWorldCreate();
|
|
checkWorld (w);
|
|
|
|
for (;;) {
|
|
if (nb < NUM && dRandReal() > 0.5) {
|
|
DO(printf ("creating body\n"));
|
|
body[nb] = dBodyCreate (w);
|
|
DO(printf ("\t--> %p\n",body[nb]));
|
|
nb++;
|
|
checkWorld (w);
|
|
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
|
}
|
|
if (nj < NUM && nb > 2 && dRandReal() > 0.5) {
|
|
dBodyID b1 = body [dRand() % nb];
|
|
dBodyID b2 = body [dRand() % nb];
|
|
if (b1 != b2) {
|
|
DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
|
|
joint[nj] = dJointCreateBall (w,0);
|
|
DO(printf ("\t-->%p\n",joint[nj]));
|
|
checkWorld (w);
|
|
dJointAttach (joint[nj],b1,b2);
|
|
nj++;
|
|
checkWorld (w);
|
|
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
|
}
|
|
}
|
|
if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
|
|
dBodyID b1 = body [dRand() % nb];
|
|
dBodyID b2 = body [dRand() % nb];
|
|
if (b1 != b2) {
|
|
int k = dRand() % nj;
|
|
DO(printf ("reattaching joint %p\n",joint[k]));
|
|
dJointAttach (joint[k],b1,b2);
|
|
checkWorld (w);
|
|
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
|
}
|
|
}
|
|
if (nb > 0 && dRandReal() > 0.5) {
|
|
int k = dRand() % nb;
|
|
DO(printf ("destroying body %p\n",body[k]));
|
|
dBodyDestroy (body[k]);
|
|
checkWorld (w);
|
|
for (; k < (NUM-1); k++) body[k] = body[k+1];
|
|
nb--;
|
|
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
|
}
|
|
if (nj > 0 && dRandReal() > 0.5) {
|
|
int k = dRand() % nj;
|
|
DO(printf ("destroying joint %p\n",joint[k]));
|
|
dJointDestroy (joint[k]);
|
|
checkWorld (w);
|
|
for (; k < (NUM-1); k++) joint[k] = joint[k+1];
|
|
nj--;
|
|
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
|
}
|
|
}
|
|
|
|
/*
|
|
printf ("creating world\n");
|
|
dWorldID w = dWorldCreate();
|
|
checkWorld (w);
|
|
printf ("creating body\n");
|
|
dBodyID b1 = dBodyCreate (w);
|
|
checkWorld (w);
|
|
printf ("creating body\n");
|
|
dBodyID b2 = dBodyCreate (w);
|
|
checkWorld (w);
|
|
printf ("creating joint\n");
|
|
dJointID j = dJointCreateBall (w);
|
|
checkWorld (w);
|
|
printf ("attaching joint\n");
|
|
dJointAttach (j,b1,b2);
|
|
checkWorld (w);
|
|
printf ("destroying joint\n");
|
|
dJointDestroy (j);
|
|
checkWorld (w);
|
|
printf ("destroying body\n");
|
|
dBodyDestroy (b1);
|
|
checkWorld (w);
|
|
printf ("destroying body\n");
|
|
dBodyDestroy (b2);
|
|
checkWorld (w);
|
|
printf ("destroying world\n");
|
|
dWorldDestroy (w);
|
|
*/
|
|
}
|
|
|
|
//****************************************************************************
|
|
// configuration
|
|
#if 1
|
|
#define REGISTER_EXTENSION( __a ) #__a " "
|
|
#else
|
|
#define REGISTER_EXTENSION( __a ) "__a "
|
|
#endif
|
|
static const char ode_configuration[] = "ODE "
|
|
|
|
// EXTENSION LIST BEGIN
|
|
//**********************************
|
|
|
|
#ifdef dNODEBUG
|
|
REGISTER_EXTENSION( ODE_EXT_no_debug )
|
|
#endif // dNODEBUG
|
|
|
|
#if dTRIMESH_ENABLED
|
|
REGISTER_EXTENSION( ODE_EXT_trimesh )
|
|
|
|
// tri-mesh extensions
|
|
#if dTRIMESH_OPCODE
|
|
REGISTER_EXTENSION( ODE_EXT_opcode )
|
|
|
|
// opcode extensions
|
|
#if dTRIMESH_16BIT_INDICES
|
|
REGISTER_EXTENSION( ODE_OPC_16bit_indices )
|
|
#endif
|
|
|
|
#if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
|
|
REGISTER_EXTENSION( ODE_OPC_new_collider )
|
|
#endif
|
|
|
|
#endif // dTRIMESH_OPCODE
|
|
|
|
#if dTRIMESH_GIMPACT
|
|
REGISTER_EXTENSION( ODE_EXT_gimpact )
|
|
|
|
// gimpact extensions
|
|
#endif
|
|
|
|
#endif // dTRIMESH_ENABLED
|
|
|
|
#if dTLS_ENABLED
|
|
REGISTER_EXTENSION( ODE_EXT_mt_collisions )
|
|
#endif // dTLS_ENABLED
|
|
|
|
#if !dTHREADING_INTF_DISABLED
|
|
REGISTER_EXTENSION( ODE_EXT_threading )
|
|
|
|
#if dBUILTIN_THREADING_IMPL_ENABLED
|
|
REGISTER_EXTENSION( ODE_THR_builtin_impl )
|
|
#endif // #if dBUILTIN_THREADING_IMPL_ENABLED
|
|
#endif // #if !dTHREADING_INTF_DISABLED
|
|
|
|
//**********************************
|
|
// EXTENSION LIST END
|
|
|
|
// These tokens are mutually exclusive, and always present
|
|
#ifdef dSINGLE
|
|
"ODE_single_precision"
|
|
#else
|
|
"ODE_double_precision"
|
|
#endif // dDOUBLE
|
|
|
|
; // END
|
|
|
|
const char* dGetConfiguration (void)
|
|
{
|
|
return ode_configuration;
|
|
}
|
|
|
|
|
|
// Helper to check for a feature of ODE
|
|
int dCheckConfiguration( const char* extension )
|
|
{
|
|
const char *start;
|
|
char *where, *terminator;
|
|
|
|
/* Feature names should not have spaces. */
|
|
where = (char*)strchr(extension, ' ');
|
|
if ( where || *extension == '\0')
|
|
return 1;
|
|
|
|
const char* config = dGetConfiguration();
|
|
|
|
const sizeint ext_length = strlen(extension);
|
|
|
|
/* It takes a bit of care to be fool-proof. Don't be fooled by sub-strings, etc. */
|
|
start = config;
|
|
for ( ; ; )
|
|
{
|
|
where = (char*)strstr((const char *) start, extension);
|
|
if (!where)
|
|
break;
|
|
|
|
terminator = where + ext_length;
|
|
|
|
if ( (where == start || *(where - 1) == ' ') &&
|
|
(*terminator == ' ' || *terminator == '\0') )
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
start = terminator;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
// Local Variables:
|
|
// c-basic-offset:4
|
|
// End:
|