C++ dAASSERT函数代码示例

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);
}