void dMassAdd (dMass *a, const dMass *b)
{
int i;
dAASSERT (a && b);
dReal denom = dRecip (a->mass + b->mass);
for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom;
a->mass += b->mass;
for (i=0; i<12; i++) a->I[i] += b->I[i];
}
dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1)
{
dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
type = dBoxClass;
side[0] = lx;
side[1] = ly;
side[2] = lz;
updateZeroSizedFlag(!lx || !ly || !lz);
}
void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length)
{
dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
dAASSERT (radius > 0 && length > 0);
dxCapsule *c = (dxCapsule*) g;
c->radius = radius;
c->lz = length;
dGeomMoved (g);
}
void dJointSetLMotorNumAxes( dJointID j, int num )
{
dxJointLMotor* joint = ( dxJointLMotor* )j;
dAASSERT( joint && num >= 0 && num <= 3 );
checktype( joint, LMotor );
if ( num < 0 ) num = 0;
if ( num > 3 ) num = 3;
joint->num = num;
}
const dReal * dGeomGetOffsetPosition (dxGeom *g)
{
dAASSERT (g);
if (g->offset_posr)
{
return g->offset_posr->pos;
}
return OFFSET_POSITION_ZERO;
}
int dIsPositiveDefinite (const dReal *A, int n)
{
dReal *Acopy;
dAASSERT (n > 0 && A);
int nskip = dPAD (n);
Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal));
memcpy (Acopy,A,nskip*n * sizeof(dReal));
return dFactorCholesky (Acopy,n);
}
void dGeomSphereSetRadius (dGeomID g, dReal radius)
{
dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
dAASSERT (radius >= 0);
dxSphere *s = (dxSphere*) g;
s->radius = radius;
s->updateZeroSizedFlag(!radius);
dGeomMoved (g);
}
dReal dJointGetAMotorAngle( dJointID j, int anum )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 3 ) anum = 3;
return joint->angle[anum];
}
dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1)
{
dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
type = dBoxClass;
side[0] = lx;
side[1] = ly;
side[2] = lz;
updateZeroSizedFlag(_dequal(lx, 0.0) || _dequal(ly, 0.0) || _dequal(lz, 0.0));
}
dxCylinder::dxCylinder (dSpaceID space, dReal _radius, dReal _length) :
dxGeom (space,1)
{
dAASSERT (_radius >= 0 && _length >= 0);
type = dCylinderClass;
radius = _radius;
lz = _length;
updateZeroSizedFlag(!_radius || !_length);
}
int dJointGetAMotorAxisRel( dJointID j, int anum )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
return joint->rel[anum];
}
dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) :
dxGeom (space,1)
{
dAASSERT (_radius >= 0 && _length >= 0);
type = dCapsuleClass;
radius = _radius;
lz = _length;
updateZeroSizedFlag(!_radius/* || !_length -- zero length capsule is not a zero sized capsule*/);
}
void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
// If we're in Euler mode, joint->axis[1] doesn't
// have anything sensible in it. So don't just return
// that, find the actual effective axis.
// Likewise, the actual axis of rotation for the
// the other axes is different from what's stored.
if ( joint->mode == dAMotorEuler ) {
dVector3 axes[3];
joint->computeGlobalAxes(axes);
if (anum == 1) {
result[0]=axes[1][0];
result[1]=axes[1][1];
result[2]=axes[1][2];
} else if (anum == 0) {
// This won't be unit length in general,
// but it's what's used in getInfo2
// This may be why things freak out as
// the body-relative axes get close to each other.
dCalcVectorCross3( result, axes[1], axes[2] );
} else if (anum == 2) {
// Same problem as above.
dCalcVectorCross3( result, axes[0], axes[1] );
}
} else if ( joint->rel[anum] > 0 ) {
if ( joint->rel[anum] == 1 )
{
dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
}
else
{
if ( joint->node[1].body ) // jds
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
result[3] = joint->axis[anum][3];
}
}
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
}
}
const dReal * dGeomGetOffsetRotation (dxGeom *g)
{
dAASSERT (g);
if (g->offset_posr)
{
return g->offset_posr->R;
}
return OFFSET_ROTATION_ZERO;
}
void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length)
{
dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
dAASSERT (radius >= 0 && length >= 0);
dxCapsule *c = (dxCapsule*) g;
c->radius = radius;
c->lz = length;
c->updateZeroSizedFlag(!radius/* || !length -- zero length capsule is not a zero sized capsule*/);
dGeomMoved (g);
}
void dGeomCopyPosition(dxGeom *g, dVector3 pos)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
const dVector3 &src = g->buildUpdatedPosition();
pos[0] = src[dV3E_X];
pos[1] = src[dV3E_Y];
pos[2] = src[dV3E_Z];
}
void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length)
{
dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
dAASSERT (radius >= 0 && length >= 0);
dxCylinder *c = (dxCylinder*) cylinder;
c->radius = radius;
c->lz = length;
c->updateZeroSizedFlag(!radius || !length);
dGeomMoved (cylinder);
}
void dxQuadTreeSpace::add(dxGeom* g){
CHECK_NOT_LOCKED (this);
dAASSERT(g);
dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space");
DirtyList.push(g);
Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block
dxSpace::add(g);
}
void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans )
{
dAASSERT(g)
dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
for (int i=0; i<16; i++)
(((dxTriMesh*)g)->last_trans)[ i ] = last_trans[ i ];
return;
}
void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
dAASSERT(Callback);
lock_count++;
cleanGeoms();
Blocks[0].Collide(UserData, Callback);
lock_count--;
}
void dGeomCopyPosition(dxGeom *g, dVector3 pos)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
g->recomputePosr();
const dReal* src = g->final_posr->pos;
pos[0] = src[0];
pos[1] = src[1];
pos[2] = src[2];
}
void _dSetValue (dReal *a, int n, dReal value)
{
dAASSERT (a && n >= 0);
dReal *acurr = a;
int ncurr = n;
while (ncurr > 0) {
*(acurr++) = value;
--ncurr;
}
}
void _dSetZero (dReal *a, int n)
{
dAASSERT (a && n >= 0);
dReal *acurr = a;
int ncurr = n;
while (ncurr > 0) {
*(acurr++) = 0;
--ncurr;
}
}
void dJointSetAMotorParam( dJointID j, int parameter, dReal value )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint );
checktype( joint, AMotor );
int anum = parameter >> 8;
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
parameter &= 0xff;
joint->limot[anum].set( parameter, value );
}
dReal dJointGetAMotorParam( dJointID j, int parameter )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint );
checktype( joint, AMotor );
int anum = parameter >> 8;
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
parameter &= 0xff;
return joint->limot[anum].get( parameter );
}
void dMassAdjust (dMass *m, dReal newmass)
{
dAASSERT (m);
dReal scale = newmass / m->mass;
m->mass = newmass;
for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dJointGetLMotorAxis( dJointID j, int anum, dVector3 result )
{
dxJointLMotor* joint = ( dxJointLMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, LMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
}
void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)
{
dUASSERT (g && g->type == dBoxClass,"argument not a box");
dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
dxBox *b = (dxBox*) g;
b->side[0] = lx;
b->side[1] = ly;
b->side[2] = lz;
b->updateZeroSizedFlag(!lx || !ly || !lz);
dGeomMoved (g);
}
dJointID dJointCreatePlanar(dWorldID world, dJointGroupID group) {
dAASSERT (world);
dxJoint *joint;
if (group) {
joint = group->alloc<dxPlanarJoint>(world);
} else {
joint = new dxPlanarJoint(world);
}
return joint;
}
dxConvex::dxConvex (dSpaceID space,
dReal *_planes,
unsigned int _planecount,
dReal *_points,
unsigned int _pointcount,
unsigned int *_polygons) :
dxGeom (space,1)
{
dAASSERT (_planes != NULL);
dAASSERT (_points != NULL);
dAASSERT (_polygons != NULL);
//fprintf(stdout,"dxConvex Constructor planes %X\n",_planes);
type = dConvexClass;
planes = _planes;
planecount = _planecount;
// we need points as well
points = _points;
pointcount = _pointcount;
polygons=_polygons;
FillEdges();
}