C++ dUASSERT函数代码示例

void dJointSetUniversalParam( dJointID j, int parameter, dReal value )
{
    dxJointUniversal* joint = ( dxJointUniversal* )j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Universal );
    if (( parameter & 0xff00 ) == 0x100 )
    {
        joint->limot2.set( parameter & 0xff, value );
    }
    else
    {
        joint->limot1.set( parameter, value );
    }
}

dReal dJointGetUniversalParam( dJointID j, int parameter )
{
    dxJointUniversal* joint = ( dxJointUniversal* )j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Universal );
    if (( parameter & 0xff00 ) == 0x100 )
    {
        return joint->limot2.get( parameter & 0xff );
    }
    else
    {
        return joint->limot1.get( parameter );
    }
}

void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull)
{
  dUASSERT (g && g->type == dRayClass,"argument not a ray");

  if (FirstContact){
    g->gflags |= RAY_FIRSTCONTACT;
  }
  else g->gflags &= ~RAY_FIRSTCONTACT;

  if (BackfaceCull){
    g->gflags |= RAY_BACKFACECULL;
  }
  else g->gflags &= ~RAY_BACKFACECULL;
}

dReal dJointGetGearboxParam( dJointID j, int parameter )
{
    dxJointGearbox* joint = static_cast<dxJointGearbox*>(j);
    dUASSERT( joint, "bad joint argument" );

    switch ( parameter ) {
        case dParamCFM:
            return joint->cfm;
        case dParamERP:
            return joint->erp;
        default:
            return 0;
    }
}

void dJointGetUniversalAngles( dJointID j, dReal *angle1, dReal *angle2 )
{
    dxJointUniversal* joint = ( dxJointUniversal* )j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Universal );
    if ( joint->flags & dJOINT_REVERSE )
    {
        joint->getAngles( angle2, angle1 );
        *angle2 = -(*angle2);
        return;
    }
    else
        return joint->getAngles( angle1, angle2 );
}

void dJointSetGearboxReferenceBody2( dJointID j, dBodyID b )
{
    dxJointGearbox* joint = dynamic_cast<dxJointGearbox*>(j);
    dUASSERT( joint, "bad joint argument" );
    dUASSERT( b, "bad body argument" );

    joint->refBody2 = b;

    if ( joint->node[1].body )
    {
      if ( b )
        dQMultiply1( joint->qrel2, joint->refBody2->q, joint->node[1].body->q );
      else
      {
        // set qrel1 to the transpose of the first body q
        joint->qrel2[0] = joint->node[1].body->q[0];
        joint->qrel2[1] = joint->node[1].body->q[1];
        joint->qrel2[2] = joint->node[1].body->q[2];
        joint->qrel2[3] = joint->node[1].body->q[3];
      }
    }
    else
    {
      if ( b )
      {
        // set qrel2 to the transpose of the second body q
        joint->qrel2[0] =   joint->refBody2->q[0];
        joint->qrel2[1] = - joint->refBody2->q[1];
        joint->qrel2[2] = - joint->refBody2->q[2];
        joint->qrel2[3] = - joint->refBody2->q[3];
      }
      else
      {
        // both refBody2 and node[1].body are null, nothing happens
      }
    }
}

void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
{
    dUASSERT(g, "argument not trimesh data");

    double *elem;
    
    switch (data_id) {
    case TRIMESH_FACE_NORMALS:
	g->Normals = (dReal *) in_data;
	break;

    case TRIMESH_LAST_TRANSFORMATION:
	elem = (double *) in_data;
    for (int i=0; i<16; i++)
        g->last_trans[i] = (dReal) elem[i];
	
	break;
    default:
	dUASSERT(data_id, "invalid data type");
	break;
    }

    return;
}

int dCreateGeomClass (const dGeomClass *c)
{
    dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class");

    if (num_user_classes >= dMaxUserClasses) {
        dDebug (0,"too many user classes, you must increase the limit and "
            "recompile ODE");
    }
    user_classes[num_user_classes] = *c;
    int class_number = num_user_classes + dFirstUserClass;
    setAllColliders (class_number,&dCollideUserGeomWithGeom);

    num_user_classes++;
    return class_number;
}

dReal dJointGetScrewParam( dJointID j, int parameter )
{
    dxJointScrew* joint = ( dxJointScrew* )j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Screw );
    switch (parameter)
    {
      case dParamERP:
        return joint->erp;
      case dParamCFM:
        return joint->cfm;
      default:
        return joint->limot.get( parameter );
    }
}

void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  if (g->body) {
    const dReal * body_quat = dBodyGetQuaternion (g->body);
    quat[0] = body_quat[0];
    quat[1] = body_quat[1];
    quat[2] = body_quat[2];
    quat[3] = body_quat[3];
  }
  else {
    dRtoQ (g->R, quat);
  }
}

void dGeomCopyRotation(dxGeom *g, dMatrix3 R)
{
    dAASSERT (g);
    dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");

    const dMatrix3 &src = g->buildUpdatedRotation();
    R[0]  = src[dM3E_XX];
    R[1]  = src[dM3E_XY];
    R[2]  = src[dM3E_XZ];
    R[4]  = src[dM3E_YX];
    R[5]  = src[dM3E_YY];
    R[6]  = src[dM3E_YZ];
    R[8]  = src[dM3E_ZX];
    R[9]  = src[dM3E_ZY];
    R[10] = src[dM3E_ZZ];
}

dReal dJointGetHinge2Param( dJointID j, int parameter )
{
    dxJointHinge2* joint = ( dxJointHinge2* )j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Hinge2 );
    if (( parameter & 0xff00 ) == 0x100 )
    {
        return joint->limot2.get( parameter & 0xff );
    }
    else
    {
        if ( parameter == dParamSuspensionERP ) return joint->susp_erp;
        else if ( parameter == dParamSuspensionCFM ) return joint->susp_cfm;
        else return joint->limot1.get( parameter );
    }
}

void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
    dAASSERT (g);
    dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
    if (g->body && !g->offset_posr) {
        const dReal * body_quat = dBodyGetQuaternion (g->body);
        quat[0] = body_quat[0];
        quat[1] = body_quat[1];
        quat[2] = body_quat[2];
        quat[3] = body_quat[3];
    }
    else {
        g->recomputePosr();
        dRtoQ (g->final_posr->R, quat);
    }
}

void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  CHECK_NOT_LOCKED (g->parent_space);
  if (g->body) {
    // this will call dGeomMoved (g), so we don't have to
    dBodySetPosition (g->body,x,y,z);
  }
  else {
    g->pos[0] = x;
    g->pos[1] = y;
    g->pos[2] = z;
    dGeomMoved (g);
  }
}

void dGeomCopyRotation(dxGeom *g, dMatrix3 R)
{
    dAASSERT (g);
    dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
    g->recomputePosr();
    const dReal* src = g->final_posr->R;
    R[0]  = src[0];
    R[1]  = src[1];
    R[2]  = src[2];
    R[4]  = src[4];
    R[5]  = src[5];
    R[6]  = src[6];
    R[8]  = src[8];
    R[9]  = src[9];
    R[10] = src[10];
}