C++ dsSetColor函数代码示例

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldQuickStep (world,0.02);

  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }


  if (doFeedback)
  {
    if (fbnum>MAX_FEEDBACKNUM)
      printf("joint feedback buffer overflow!\n");
    else
    {
      dVector3 sum = {0, 0, 0};
      printf("\n");
      for (int i=0; i<fbnum; i++) {
        dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2;
        printf("%f %f %f\n", f[0], f[1], f[2]);
        sum[0] += f[0];
        sum[1] += f[1];
        sum[2] += f[2];
      }
      printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]);
      dMass m;
      dBodyGetMass(obj[selected].body, &m);
      printf("Object G=%f\n", GRAVITY*m.mass);
    }
    doFeedback = 0;
    fbnum = 0;
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0.8,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}

/**
 * @brief Desenhar o campo da simulacao
 *
 * Desenha toda a parte grafica da simulacao, como
 * as paredes, as linhas, os robos e a bola.
 *
 */
void desenharCampo()
{
    dVector3 ss;
    dsSetColor (0,0,0);

    for(int i=0; i < 6; i++) //Paredes
    {
        dGeomBoxGetLengths (wall[i],ss);
        dsDrawBox (dGeomGetPosition(wall[i]), dGeomGetRotation(wall[i]), ss);
    }

    for(int i=0; i < 3;i++) //Gols
    {
        dGeomBoxGetLengths (goalL[i],ss);
        dsDrawBox (dGeomGetPosition(goalL[i]), dGeomGetRotation(goalL[i]), ss);
        dGeomBoxGetLengths (goalR[i],ss);
        dsDrawBox (dGeomGetPosition(goalR[i]), dGeomGetRotation(goalR[i]), ss);
    }

    dsSetColor (1,1,1); //Círculo Central
    dsDrawCylinder(dGeomGetPosition(circle),dGeomGetRotation(circle),0.0001,CIRCLE_RADIUS);

    for (int i=0; i < 4; i++) //Quinas
    {
        dGeomBoxGetLengths (triangle[i],ss);
        dsDrawBox (dGeomGetPosition(triangle[i]), dGeomGetRotation(triangle[i]), ss);
    }
}

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  //if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}

static void simLoop (int pause)
{
  int i,j;

  for (i=0; i < NUM; i++) {
    for (j=0; j < NUM; j++) test_matrix[i][j] = 0;
  }
  dSpaceCollide (space,0,&nearCallback);
  for (i=0; i < NUM; i++) {
    for (j=i+1; j < NUM; j++) {
      if (good_matrix[i][j] && !test_matrix[i][j]) {
	printf ("failed to report collision (%d,%d) (seed=%ld)\n",i,j,seed);
      }
    }
  }

  seed++;
  init_test();

  for (i=0; i<NUM; i++) {
    dVector3 pos,side;
    dMatrix3 R;
    dRSetIdentity (R);
    for (j=0; j<3; j++) pos[j] = (bounds[i][j*2+1] + bounds[i][j*2]) * 0.5;
    for (j=0; j<3; j++) side[j] = bounds[i][j*2+1] - bounds[i][j*2];
    if (hits[i] > 0) dsSetColor (1,0,0);
    else dsSetColor (1,1,0);
    dsDrawBox (pos,R,side);
  }
}

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldQuickStep (world,0.05);

  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }
  
  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0.8,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}

static void simLoop (int pause)
{
  // stop after a given number of iterations, as long as we are not in
  // interactive mode
  if (cmd_graphics && !cmd_interactive &&
      (iteration >= max_iterations)) {
    dsStop();
    return;
  }
  iteration++;

  if (!pause) {
    // do stuff for this test and check to see if the joint is behaving well
    dReal error = doStuffAndGetError (test_num);
    if (error > max_error) max_error = error;
    if (cmd_interactive && error < dInfinity) {
      printf ("scaled error = %.4e\n",error);
    }

    // take a step
    dWorldStep (world,STEPSIZE);

    // occasionally re-orient the first body to create a deliberate error.
    if (cmd_occasional_error) {
      static int count = 0;
      if ((count % 20)==0) {
	// randomly adjust orientation of body[0]
	const dReal *R1;
	dMatrix3 R2,R3;
	R1 = dBodyGetRotation (body[0]);
	dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
			    dRandReal()-0.5,dRandReal()-0.5);
	dMultiply0 (R3,R1,R2,3,3,3);
	dBodySetRotation (body[0],R3);

	// randomly adjust position of body[0]
	const dReal *pos = dBodyGetPosition (body[0]);
	dBodySetPosition (body[0],
			  pos[0]+0.2*(dRandReal()-0.5),
			  pos[1]+0.2*(dRandReal()-0.5),
			  pos[2]+0.2*(dRandReal()-0.5));
      }
      count++;
    }
  }

  if (cmd_graphics) {
    dReal sides1[3] = {SIDE,SIDE,SIDE};
    dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
    dsSetTexture (DS_WOOD);
    dsSetColor (1,1,0);
    dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
    if (body[1]) {
      dsSetColor (0,1,1);
      dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
    }
  }
}

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }

  /*{
    for (int i = 1; i < IndexCount; i++) {
      dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
    }
  }*/

  {const dReal* Pos = dGeomGetPosition(TriMesh);
  const dReal* Rot = dGeomGetRotation(TriMesh);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
	const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
	const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
	dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
  }}}

  if (Ray){
	  dVector3 Origin, Direction;
	  dGeomRayGet(Ray, Origin, Direction);
	  
	  dReal Length = dGeomRayGetLength(Ray);
	  
	  dVector3 End;
	  End[0] = Origin[0] + (Direction[0] * Length);
	  End[1] = Origin[1] + (Direction[1] * Length);
	  End[2] = Origin[2] + (Direction[2] * Length);
	  End[3] = Origin[3] + (Direction[3] * Length);
	  
	  dsDrawLine(Origin, End);
  }
}

static void simLoop (int pause)
{
  const dReal kd = -0.3;	// angular damping constant
  const dReal ks = 0.5;	// spring constant
  if (!pause) {
    // add an oscillating torque to body 0, and also damp its rotational motion
    static dReal a=0;
    const dReal *w = dBodyGetAngularVel (body[0]);
    dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
    a += 0.01;

    // add a spring force to keep the bodies together, otherwise they will
    // fly apart along the slider axis.
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
		   ks*(p2[2]-p1[2]));
    dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
		   ks*(p1[2]-p2[2]));

    // occasionally re-orient one of the bodies to create a deliberate error.
    if (occasional_error) {
      static int count = 0;
      if ((count % 20)==0) {
	// randomly adjust orientation of body[0]
	const dReal *R1;
	dMatrix3 R2,R3;
	R1 = dBodyGetRotation (body[0]);
	dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
			    dRandReal()-0.5,dRandReal()-0.5);
	dMultiply0 (R3,R1,R2,3,3,3);
	dBodySetRotation (body[0],R3);

	// randomly adjust position of body[0]
	const dReal *pos = dBodyGetPosition (body[0]);
	dBodySetPosition (body[0],
			  pos[0]+0.2*(dRandReal()-0.5),
			  pos[1]+0.2*(dRandReal()-0.5),
			  pos[2]+0.2*(dRandReal()-0.5));
      }
      count++;
    }

    dWorldStep (world,0.05);
  }

  dReal sides1[3] = {SIDE,SIDE,SIDE};
  dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
  dsSetTexture (DS_WOOD);
  dsSetColor (1,1,0);
  dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}

 void doDraw(){
     if (!leg) return;
     dsSetColor(0.0,0.0,1.0);
     dsDrawCapsuleD(dBodyGetPosition(leg->lower_part.body), dBodyGetRotation(leg->lower_part.body), leg->lower_part.length, leg->lower_part.radius);
     dsSetColor(1.0,0.0,0.0);
     dsDrawCapsuleD(dBodyGetPosition(leg->upper_part.body), dBodyGetRotation(leg->upper_part.body), leg->upper_part.length, leg->upper_part.radius);
     //TEST 
     double k1 =  1.0,  fMax  = 10.0; // k1: proportional gain,  fMax：Max torque[Nm]
     //dJointSetHingeParam(jointHinge, dParamVel, k1); // Set angular velocity[m/s]
     //dJointSetHingeParam(jointHinge, dParamFMax, fMax); // Set max torque[N/m]
     printf("\r %6d:",dJointGetHingeAngle(leg->jointHinge)); 
 }

static void simLoop (int pause)
{
  double simstep = 0.001; // 1ms simulation steps
  double dt = dsElapsedTime();

  int nrofsteps = (int) ceilf(dt/simstep);
//  fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps);

  for (int i=0; i<nrofsteps && !pause; i++)
  {
    dSpaceCollide (space,0,&nearCallback);
    dWorldQuickStep (world, simstep);
    dJointGroupEmpty (contactgroup);
  }

  dsSetColor (1,1,1);
  const dReal *SPos = dBodyGetPosition(sphbody);
  const dReal *SRot = dBodyGetRotation(sphbody);
  float spos[3] = {SPos[0], SPos[1], SPos[2]};
  float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
  dsDrawSphere
  (
    spos, 
    srot, 
    RADIUS
  );

  // draw world trimesh
  dsSetColor(0.4,0.7,0.9);
  dsSetTexture (DS_NONE);

  const dReal* Pos = dGeomGetPosition(world_mesh);
  //dIASSERT(dVALIDVEC3(Pos));
  float pos[3] = { Pos[0], Pos[1], Pos[2] };

  const dReal* Rot = dGeomGetRotation(world_mesh);
  //dIASSERT(dVALIDMAT3(Rot));
  float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };

  int numi = sizeof(world_indices)  / sizeof(int);

  for (int i=0; i<numi/3; i++)
  {
    int i0 = world_indices[i*3+0];
    int i1 = world_indices[i*3+1];
    int i2 = world_indices[i*3+2];
    float *v0 = world_vertices+i0*3;
    float *v1 = world_vertices+i1*3;
    float *v2 = world_vertices+i2*3;
    dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
  }
}

void DisplayOpenDESpaces::displaySpace(dSpaceID space)
{
    int ngeoms = dSpaceGetNumGeoms(space);
    for (int i = 0 ; i < ngeoms ; ++i)
    {
        dGeomID geom = dSpaceGetGeom(space, i);
        std::map<dGeomID, Color>::const_iterator it = m_gcolors.find(geom);
        if (it != m_gcolors.end())
            dsSetColor(it->second.r,it->second.g,it->second.b);
        else
            dsSetColor(m_activeColor.r, m_activeColor.g, m_activeColor.b);
        drawGeom(geom, nullptr, nullptr, 0);
    }
}

void simLoop(int pause)
{
    if (!pause) {
        dSpaceCollide (space, 0, &nearCallback);
        dWorldQuickStep(world, 0.01);
        dJointGroupEmpty(contactgroup);
    }
    
    dsSetColor (1,1,0);
    for (int i=0; i<ncards; ++i) {
        dsSetColor (1, dReal(i)/ncards, 0);
        cards[i]->draw();
    }
    
}

static void simLoop (int pause)
{
    const dReal stepsize = 0.02;

    dsSetColor (0,0,2);
    dSpaceCollide (space,0,&nearCallback);
    if (!pause) {
        
        if (mov_type == 1)
            moveplat_1(stepsize);
        else
            moveplat_2(stepsize);

        dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]);
        updatecam();
        dWorldQuickStep (world,stepsize);
        //dWorldStep (world,stepsize);
    }

    if (write_world) {
        FILE *f = fopen ("state.dif","wt");
        if (f) {
            dWorldExportDIF (world,f,"X");
            fclose (f);
        }
        write_world = 0;
    }
  
    // remove all contact joints
    dJointGroupEmpty (contactgroup);

    dsSetColor (1,1,0);
    dsSetTexture (DS_WOOD);
    for (int i=0; i<num; i++) {
        for (int j=0; j < GPB; j++) {
            if (! dBodyIsEnabled (obj[i].body)) {
                dsSetColor (1,0.8,0);
            }
            else {
                dsSetColor (1,1,0);
            }
            drawGeom (obj[i].geom[j],0,0,show_aabb);
        }
    }
    dsSetColor (1,0,0);
    drawGeom (platform,0,0,show_aabb);
    //usleep(5000);
}

void simLoop (int pause)
{
  static bool DumpInfo=true;
  const dReal ss[3] = {0.02,0.02,0.02};
  dContactGeom contacts[8];
  int contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom));
  //fprintf(stdout,"Contact Count %d\n",contactcount);
  const dReal* pos;
  const dReal* R;
  dsSetTexture (DS_WOOD);
  pos = dGeomGetPosition (geoms[0]);
  R = dGeomGetRotation (geoms[0]);
  dsSetColor (0.6f,0.6f,1);
  dsDrawConvex(pos,R,planes,
	       planecount,
	       points,
	       pointcount,
	       polygons);
  pos = dGeomGetPosition (geoms[1]);
  R = dGeomGetRotation (geoms[1]);
  dsSetColor (0.4f,1,1);
  dsDrawConvex(pos,R,planes,
	       planecount,
	       points,
	       pointcount,
	       polygons);
  /*if (show_contacts) */
  dMatrix3 RI;
  dRSetIdentity (RI);
  dsSetColor (1.0f,0,0);
  for(int i=0;i<contactcount;++i)
    {
      if(DumpInfo)
	{
	  //DumpInfo=false;
	  fprintf(stdout,"Contact %d Normal %f,%f,%f Depth %f\n",
		  i,
		  contacts[i].normal[0],
		  contacts[i].normal[1],
		  contacts[i].normal[2],
		  contacts[i].depth);
	}
      dsDrawBox (contacts[i].pos,RI,ss);
    }
  if(DumpInfo)
    DumpInfo=false;

}

static void simLoop (int pause)
{
  if (!pause) {
    // add random forces and torques to all bodies
    int i;
    const dReal scale1 = 5;
    const dReal scale2 = 5;

    for (i=0; i<NUM; i++) {
      dBodyAddForce (body[i],
		     scale1*(dRandReal()*2-1),
		     scale1*(dRandReal()*2-1),
		     scale1*(dRandReal()*2-1));
      dBodyAddTorque (body[i],
		     scale2*(dRandReal()*2-1),
		     scale2*(dRandReal()*2-1),
		     scale2*(dRandReal()*2-1));
    }

    dWorldStep (world,0.05);
    createTest();
  }

  // float sides[3] = {SIDE,SIDE,SIDE};
  dsSetColor (1,1,0);
  for (int i=0; i<NUM; i++)
    dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS);
}