Exemple #1
0
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];
}
Exemple #2
0
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);
}
Exemple #3
0
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;
}
Exemple #5
0
const dReal * dGeomGetOffsetPosition (dxGeom *g)
{
    dAASSERT (g);
    if (g->offset_posr)
    {
        return g->offset_posr->pos;
    }
    return OFFSET_POSITION_ZERO;
}
Exemple #6
0
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);
}
Exemple #7
0
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);
}
Exemple #8
0
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];
}
Exemple #9
0
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));
}
Exemple #10
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);
}
Exemple #11
0
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*/);
}
Exemple #13
0
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];
    }
}
Exemple #14
0
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];
}
Exemple #17
0
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--;
}
Exemple #21
0
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];
}
Exemple #22
0
void _dSetValue (dReal *a, int n, dReal value)
{
  dAASSERT (a && n >= 0);
  dReal *acurr = a;
  int ncurr = n;
  while (ncurr > 0) {
    *(acurr++) = value;
    --ncurr;
  }
}
Exemple #23
0
void _dSetZero (dReal *a, int n)
{
  dAASSERT (a && n >= 0);
  dReal *acurr = a;
  int ncurr = n;
  while (ncurr > 0) {
    *(acurr++) = 0;
    --ncurr;
  }
}
Exemple #24
0
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 );
}
Exemple #25
0
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 );
}
Exemple #26
0
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];
}
Exemple #28
0
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);
}
Exemple #29
0
dJointID dJointCreatePlanar(dWorldID world, dJointGroupID group) {
    dAASSERT (world);

    dxJoint *joint;
    if (group) {
        joint = group->alloc<dxPlanarJoint>(world);
    } else {
        joint = new dxPlanarJoint(world);
    }
    return joint;
}
Exemple #30
0
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();
}