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); }
void SlopeField::drawObjects() { for (size_t i = 0; i < LENGTH(slopes); ++i) { slopes[i]->drawInStuff(); } #define LINE_DRAW_OFFSET 0.005 dVector3 slopeLines[][2] = { {{1 , 0.985, LINE_DRAW_OFFSET}, {1 , -0.985, LINE_DRAW_OFFSET}}, {{1.5, 0.985, 0.15 + LINE_DRAW_OFFSET}, {1.5, -0.985, 0.15 + LINE_DRAW_OFFSET}}, {{2 , 0.985, 0.3 + LINE_DRAW_OFFSET}, {2 , -0.985, 0.3 + LINE_DRAW_OFFSET}}, {{2.5, 0.985, 0.3 + LINE_DRAW_OFFSET}, {2.5, -0.985, 0.3 + LINE_DRAW_OFFSET}}, {{3 , 0.985, 0.3 + LINE_DRAW_OFFSET}, {3 , -0.985, 0.3 + LINE_DRAW_OFFSET}}, {{3.5, 0.985, 0.15 + LINE_DRAW_OFFSET}, {3.5, -0.985, 0.15 + LINE_DRAW_OFFSET}}, {{4 , 0.985, LINE_DRAW_OFFSET}, {4 , -0.985, LINE_DRAW_OFFSET}}, {{1 , 0 , 0.00 + LINE_DRAW_OFFSET}, {2 , 0 , 0.3 + LINE_DRAW_OFFSET}}, {{2 , 0 , 0.30 + LINE_DRAW_OFFSET}, {3 , 0 , 0.3 + LINE_DRAW_OFFSET}}, {{3 , 0 , 0.30 + LINE_DRAW_OFFSET}, {4 , 0 , 0.0 + LINE_DRAW_OFFSET}}, {{1 , 0.5 , 0.00 + LINE_DRAW_OFFSET}, {2 , 0.5 , 0.3 + LINE_DRAW_OFFSET}}, {{2 , 0.5 , 0.30 + LINE_DRAW_OFFSET}, {3 , 0.5 , 0.3 + LINE_DRAW_OFFSET}}, {{3 , 0.5 , 0.30 + LINE_DRAW_OFFSET}, {4 , 0.5 , 0.0 + LINE_DRAW_OFFSET}}, {{1 , -0.5 , 0.00 + LINE_DRAW_OFFSET}, {2 , -0.5 , 0.3 + LINE_DRAW_OFFSET}}, {{2 , -0.5 , 0.30 + LINE_DRAW_OFFSET}, {3 , -0.5 , 0.3 + LINE_DRAW_OFFSET}}, {{3 , -0.5 , 0.30 + LINE_DRAW_OFFSET}, {4 , -0.5 , 0.0 + LINE_DRAW_OFFSET}}, }; dsSetColor(1,1,1); for (size_t i = 0; i < LENGTH(slopeLines); ++i) { dsDrawLine(slopeLines[i][0], slopeLines[i][1]); } }
void DrawOmni(){ /* 車輪の描画 */ dReal radius, length; dsSetColor(0.3, 0.3, 0.3); for (int i=0; i<OMNI_NUM; i++) { for (int j=0; j< WHEEL_NUM; j++) { dGeomCylinderGetParams(wheel[i][j].geom, &radius, &length); dsDrawCylinder(dGeomGetPosition(wheel[i][j].geom), dGeomGetRotation(wheel[i][j].geom),length, radius); } /* 土台の描画 */ dsSetColor(0.3, 0.1, 0.1); dsDrawBox(dBodyGetPosition(base[i].body),dBodyGetRotation(base[i].body),baseSize[i]); } }
/*** ゴールの描画 ***/ static void drawGoal() { dsSetTexture(DS_NONE); for (int i = 0; i < GOAL_PARTS_NUM; i++) { if (i < 4) dsSetColor(1.3, 1.3, 1.3); else dsSetColor(1.3, 1.3, 0.0); dsDrawBox(dGeomGetPosition(goal_parts[i]), dGeomGetRotation(goal_parts[i]),GOAL_SIDES[i]); if (i < 4) dsSetColor(1.3, 1.3, 1.3); else dsSetColor(0.0, 0.0, 1.3); dsDrawBox(dGeomGetPosition(goal_parts2[i]), dGeomGetRotation(goal_parts2[i]),GOAL_SIDES[i]); } }
/*** 台車の描画 ***/ static void drawBase() { // dsSetColor(1.3, 1.3, 0.0); // 黄色 // dsSetColor(0.0, 0.0, 1.3); // 青 dsSetColor(0.1, 0.1, 0.1); // 黒 dsDrawBox(dBodyGetPosition(base.body),dBodyGetRotation(base.body),SIDE); }
// simulation loop static void simLoop (int pause) { const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3; //@a sphere dsSetColor(1,0,0); // set color (red, green, blue�j a value is between 0 and 1 dsSetSphereQuality(3); // set sphere quality. 3 is enough pos1 = dBodyGetPosition(sphere.body); // get a body position R1 = dBodyGetRotation(sphere.body); // get a body rotation matrix dsDrawSphere(pos1,R1,radius); // draw a sphere // a cylinder dsSetColorAlpha (0,1,0,1); pos2 = dBodyGetPosition(cylinder.body); R2 = dBodyGetRotation(cylinder.body); dsDrawCylinder(pos2,R2,length,radius); // draw a cylinder // a capsule dsSetColorAlpha (1,1,1,1); pos2 = dBodyGetPosition(capsule.body); R2 = dBodyGetRotation(capsule.body); dsDrawCapsule(pos2,R2,length,radius); // draw a capsule // a box dsSetColorAlpha (0,0,1,1); pos3 = dBodyGetPosition(box.body); R3 = dBodyGetRotation(box.body); dsDrawBox(pos3,R3,sides); // draw a box // a ray dReal posA[3] = {0, 5, 0}, posB[3]= {0, 5, 1.9}; dsDrawLine(posA,posB); // draw a ray }
static void simLoop (int pause) { int i; if (!pause) { // motor dJointSetHinge2Param (joint[0],dParamVel2,-speed); dJointSetHinge2Param (joint[0],dParamFMax2,0.1); // steering dReal v = steer - dJointGetHinge2Angle1 (joint[0]); if (v > 0.1) v = 0.1; if (v < -0.1) v = -0.1; v *= 10.0; dJointSetHinge2Param (joint[0],dParamVel,v); dJointSetHinge2Param (joint[0],dParamFMax,0.2); dJointSetHinge2Param (joint[0],dParamLoStop,-0.75); dJointSetHinge2Param (joint[0],dParamHiStop,0.75); dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1); dSpaceCollide (space,0,&nearCallback); dWorldStep (world,0.05); // remove all contact joints dJointGroupEmpty (contactgroup); } dsSetColor (0,1,1); dsSetTexture (DS_WOOD); dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); dsSetColor (1,1,1); for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),0.02f,RADIUS); dVector3 ss; dGeomBoxGetLengths (ground_box,ss); dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); /* printf ("%.10f %.10f %.10f %.10f\n", dJointGetHingeAngle (joint[1]), dJointGetHingeAngle (joint[2]), dJointGetHingeAngleRate (joint[1]), dJointGetHingeAngleRate (joint[2])); */ }
//Fonction de dessin avec drawstuff void dessin_env( Bloc *bloc1, Bloc *bloc2 ){ dReal sides[3] = {LARGEUR, LONGUEUR, EPAISSEUR }; dsSetColor (COULEUR_BLOC); dsSetTexture (DS_WOOD); dsDrawBox ( dBodyGetPosition( bloc1->bodyID ) , dBodyGetRotation( bloc1->bodyID ), sides); dsDrawBox ( dBodyGetPosition( bloc2->bodyID ) , dBodyGetRotation( bloc2->bodyID ), sides); }
static void simLoop (int pause) { if (!pause) { dSpaceCollide(space,0,&nearCallback); dWorldQuickStep (world,0.05); dJointGroupEmpty(contactgroup); } dReal sides1[3]; dGeomBoxGetLengths(geom[0], sides1); dReal sides2[3]; dGeomBoxGetLengths(geom[1], sides2); 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); }
IoObject *IoDrawStuff_dsSetColor(IoDrawStuff *self, IoObject *locals, IoMessage *m) { float red = IoMessage_locals_intArgAt_(m, locals, 0); float green = IoMessage_locals_intArgAt_(m, locals, 1); float blue = IoMessage_locals_intArgAt_(m, locals, 2); dsSetColor(red, green, blue); return self; }
// ロボットの描画 static void draw() { const dReal *pos, *rot; dReal side[3]; // SHIELDの描画 pos = dBodyGetPosition(rod[0].body); rot = dBodyGetRotation(rod[0].body); dsSetColor(1.0 ,0.0 ,0.0); dsDrawCylinder(pos, rot, SHIELD_LENGTH, SHIELD_RADIUS); // RODの描画 pos = dBodyGetPosition(rod[1].body); rot = dBodyGetRotation(rod[1].body); side[0] = ROD_WIDTH; side[1] = ROD_WIDTH; side[2] = ROD_LENGTH; dsSetColor(0.9 ,0.7 ,0.13); dsDrawBox(pos, rot, side); // BODYの描画 pos = dBodyGetPosition(rod[2].body); rot = dBodyGetRotation(rod[2].body); side[0] = BODY_WIDTH; side[1] = BODY_LENGTH; side[2] = BODY_HEIGHT; dsSetColor(0.0 ,0.0 ,1.0); dsDrawBox(pos, rot, side); // BULLETの描画 pos = dBodyGetPosition(bullet.body); rot = dBodyGetRotation(bullet.body); dsSetColor(0 ,0 ,0); dsDrawSphere(pos, rot, BULLET_RADIUS+0.1); // RAYの描画 dVector3 pos_s,pos_e; pos_s[0] = CANNON_X; pos_s[1]=CANNON_Y; pos_s[2]=CANNON_Z; pos_e[0] = CANNON_X + CANNON_Y * tan(cannon_q_d[0]); pos_e[1] = 0.0; pos_e[2] = CANNON_Z + CANNON_Y * tan(- 1.0 * cannon_q_d[1]) / cos(cannon_q_d[0]); dsSetColor(1 ,0 ,0); dsDrawLine(pos_s, pos_e); }
void desenharMundo() { desenharRobos(); //desenhar bolas dsSetColor (1,0.5,0.0); dsDrawSphere (dBodyGetPosition(bola.corpo),dBodyGetRotation(bola.corpo), BOLA_RAIO); desenharCampo(); }
// Simulation loop void simLoop(int pause) { control(); dWorldStep(world, 0.02); const double* var; // Draw a robot dsSetColor(1.0,1.0,1.0); // Set color (r, g, b), In this case white is set for (int i = 0; i < NUM; i++ ){// Draw capsules for links dsDrawCapsuleD(var, var , l[i], r[i]); } }
void SkidSteeringVehicle::draw() { { dsSetColor(0, 0, 1); const dReal *pos = dGeomGetPosition(this->vehicleGeom); const dReal *R = dGeomGetRotation(this->vehicleGeom); dReal sides[3]; dGeomBoxGetLengths(this->vehicleGeom, sides); dsDrawBoxD(pos, R, sides); } dsSetColor(1, 1, 0); for(int fr = 0; fr < 2; fr++) { for(int lr = 0; lr < 2; lr++) { const dReal *pos = dGeomGetPosition(this->wheelGeom[fr][lr]); const dReal *R = dGeomGetRotation(this->wheelGeom[fr][lr]); dReal radius, length; dGeomCylinderGetParams(this->wheelGeom[fr][lr], &radius, &length); dsDrawCylinderD(pos, R, length, radius); } } }
void DisplayOpenDESpaces::displaySpace(std::vector<dGeomID> g, std::vector<Tmesh> *tm) { int ngeoms = g.size(); for (int i = 0 ; i < ngeoms-1 ; ++i) { dGeomID geom = g[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); if(tm==NULL) drawGeom(geom, NULL, NULL, 0); else drawGeom(geom, NULL, NULL, 0,tm->at(i)); } drawGeom(g[ngeoms-1], NULL, NULL, 0); }
/*** 車輪の描画 ***/ void drawWheel() { dReal radius, length; dsSetColor(1.1,1.1,1.1); for (int i=0; i< WHEEL_NUM; i++) { dGeomCylinderGetParams(wheel[i].geom, &radius, &length); dsDrawCylinder(dGeomGetPosition(wheel[i].geom), dGeomGetRotation(wheel[i].geom),length, radius); } }