// 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) { dSpaceCollide (space,0,&nearCallback); if (!pause) { dWorldQuickStep (world, 0.01); // 100 Hz } dJointGroupEmpty (contactgroup); dsSetColorAlpha (1,1,0,0.5); const dReal *CPos = dBodyGetPosition(cylbody); const dReal *CRot = dBodyGetRotation(cylbody); float cpos[3] = {CPos[0], CPos[1], CPos[2]}; float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] }; dsDrawCylinder ( cpos, crot, CYLLENGTH, CYLRADIUS ); // single precision 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, SPHERERADIUS ); // single precision }
/** * @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); } }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMultiply0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMultiply0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
/*** 車輪の描画 ***/ 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); } }
static void simLoop (int pause) { dsSetColor (0,0,2); space->collide(0,&nearCallback); if (!pause) //world->quickStep(0.02); world->step(0.02); if (write_world) { FILE *f = fopen ("state.dif","wt"); if (f) { dWorldExportDIF (*world,f,"X"); fclose (f); } write_world = false; } // remove all contact joints dJointGroupEmpty (contactgroup); dsSetTexture (DS_WOOD); dsSetColor (1,0.5f,0); dsDrawCylinder(top1->getPosition(), top1->getRotation(), toplength, topradius); dsDrawCapsule(top1->getPosition(), top1->getRotation(), pinlength, pinradius); dsSetColor (0.5f,1,0); dsDrawCylinder(top2->getPosition(), top2->getRotation(), toplength, topradius); dsDrawCapsule(top2->getPosition(), top2->getRotation(), pinlength, pinradius); }
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]); } }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dConvexClass) { //dVector3 sides={0.50,0.50,0.50}; dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
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])); */ }
void dmDraw(dmObject *obj) /*** 物体の描画 ***/ { if (!obj->geom) return; obj->p = (double *) dGeomGetPosition(obj->geom); obj->R = (double *) dGeomGetRotation(obj->geom); dsSetColor(obj->color[0],obj->color[1],obj->color[2]); // 色の設定(r,g,b) // printf("color=%.1f %.1f %.1f\n",obj->color[0],obj->color[1],obj->color[2]); int type = dGeomGetClass(obj->geom); switch (type) { case dBoxClass: { dVector3 sides; dGeomBoxGetLengths(obj->geom,sides); dsDrawBox(obj->p,obj->R,sides); } break; case dSphereClass: dsDrawSphere(obj->p,obj->R,dGeomSphereGetRadius(obj->geom)); break; case dCapsuleClass: { dReal radius,length; dGeomCapsuleGetParams(obj->geom,&radius,&length); dsDrawCapsule(obj->p,obj->R,length,radius); } break; case dCylinderClass: { dReal radius,length; dGeomCylinderGetParams(obj->geom,&radius,&length); dsDrawCylinder(obj->p,obj->R,length,radius); } break; default: printf("Bad geometry type \n"); } }
// ロボットの描画 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 drawGeom (dGeomID g) { const dReal *pos = dGeomGetPosition(g); const dReal *R = dGeomGetRotation(g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g, sides); dsDrawBox (pos,R,sides); } if (type == dCylinderClass) { dReal r,l; dGeomCylinderGetParams(g, &r, &l); dsDrawCylinder (pos, R, l, r); } }
IoObject *IoDrawStuff_dsDrawCylinder(IoDrawStuff *self, IoObject *locals, IoMessage *m) { float *pos; float *R; float length; float radius; //IoSeq_assertIsVector(self, locals, m); { IoSeq *other = IoMessage_locals_vectorArgAt_(m, locals, 0); pos = IoSeq_floatPointerOfLength_(other, 3); other = IoMessage_locals_vectorArgAt_(m, locals, 1); R = IoSeq_floatPointerOfLength_(other, 12); length = IoMessage_locals_floatArgAt_(m, locals, 2); radius = IoMessage_locals_floatArgAt_(m, locals, 3); } dsDrawCylinder(pos, R, length, radius); return self; }
void desenharRobos() { dsSetTexture(DS_WOOD); for(int j=0; j < ROBOTS; j++) { dReal sides[2][3] = {{LENGTH,WIDTH,HEIGHT}, {LENGTH/2,WIDTH,HEIGHT}}; for(int i=0; i < 2; i++) { //desenhar chassi if (j<3) dsSetColor(0.5,1,0); else dsSetColor(0.5,0,1); //dsSetColor(0,0.5,1); dsDrawBox(dBodyGetPosition(robot[j].body[i]), dBodyGetRotation(robot[j].body[i]), sides[i]); //desenhar roda dsSetColor (0,0,0); //dsSetColor(1,0.5,1); dsDrawCylinder(dBodyGetPosition(robot[j].wheel[i]),dBodyGetRotation(robot[j].wheel[i]),wTHICK,RADIUS); } } }
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (!g) return; if (dGeomIsSpace(g)) { displaySpace((dSpaceID)g); return; } int type = dGeomGetClass (g); if (type == dBoxClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else show_aabb = 0; if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
// simulation loop static void simLoop (int pause) { static bool todo = false; if ( todo ) { // DEBUG static int cnt = 0; ++cnt; if (cnt == 5) command ( 'q' ); if (cnt == 10) dsStop(); } if (!pause) { double simstep = 0.01; // 10ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf (dt/simstep); if (!nrofsteps) nrofsteps = 1; for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldStep (world, simstep); dJointGroupEmpty (contactgroup); } update(); dReal radius, length; dsSetTexture (DS_WOOD); drawBox (geom[W], 1,1,0); drawBox (geom[EXT], 0,1,0); dVector3 anchorPos; dReal ang1 = 0; dReal ang2 = 0; dVector3 axisP, axisR1, axisR2; if ( dJointTypePU == type ) { dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); ang1 = pu->getAngle1(); ang2 = pu->getAngle2(); pu->getAxis1 (axisR1); pu->getAxis2 (axisR2); pu->getAxisP (axisP); dJointGetPUAnchor (pu->id(), anchorPos); } else if ( dJointTypePR == type ) { dPRJoint *pr = dynamic_cast<dPRJoint *> (joint); pr->getAxis1 (axisP); pr->getAxis2 (axisR1); dJointGetPRAnchor (pr->id(), anchorPos); } // Draw the axisR if ( geom[INT] ) { dsSetColor (1,0,1); dVector3 l; dGeomBoxGetLengths (geom[INT], l); const dReal *rotBox = dGeomGetRotation (geom[W]); dVector3 pos; for (int i=0; i<3; ++i) pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i]; dsDrawBox (pos, rotBox, l); } dsSetTexture (DS_CHECKERED); if ( geom[AXIS1] ) { dQuaternion q, qAng; dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); dGeomGetQuaternion (geom[AXIS1], q); dQuaternion qq; dQMultiply1 (qq, qAng, q); dMatrix3 R; dQtoR (qq,R); dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length); dsSetColor (1,0,0); dsDrawCylinder (anchorPos, R, length, radius); } if ( dJointTypePU == type && geom[AXIS2] ) { //dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); dQuaternion q, qAng, qq, qq1; dGeomGetQuaternion (geom[AXIS2], q); dQFromAxisAndAngle (qAng, 0, 1, 0, ang2); dQMultiply1 (qq, qAng, q); dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); dQMultiply1 (qq1, qAng, qq); dMatrix3 R; dQtoR (qq1,R); dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length); dsSetColor (0,0,1); dsDrawCylinder (anchorPos, R, length, radius); } dsSetTexture (DS_WOOD); // Draw the anchor if ( geom[ANCHOR] ) { dsSetColor (1,1,1); dVector3 l; dGeomBoxGetLengths (geom[ANCHOR], l); const dReal *rotBox = dGeomGetRotation (geom[D]); const dReal *posBox = dGeomGetPosition (geom[D]); dVector3 e; for (int i=0; i<3; ++i) e[i] = posBox[i] - anchorPos[i]; dNormalize3 (e); dVector3 pos; for (int i=0; i<3; ++i) pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i]; dsDrawBox (pos, rotBox, l); } drawBox (geom[D], 1,1,0); } }
static void simLoop (int pause) { double simstep = 0.005; // 5ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf(dt/simstep); for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldQuickStep (world, simstep); dJointGroupEmpty (contactgroup); } dsSetColor (1,1,1); #ifdef BOX const dReal *BPos = dBodyGetPosition(boxbody); const dReal *BRot = dBodyGetRotation(boxbody); float bpos[3] = {BPos[0], BPos[1], BPos[2]}; float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] }; float sides[3] = {BOXSZ, BOXSZ, BOXSZ}; dsDrawBox ( bpos, brot, sides ); // single precision #endif #ifdef CYL const dReal *CPos = dGeomGetPosition(cylgeom); const dReal *CRot = dGeomGetRotation(cylgeom); float cpos[3] = {CPos[0], CPos[1], CPos[2]}; float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] }; dsDrawCylinder ( // dBodyGetPosition(cylbody), // dBodyGetRotation(cylbody), cpos, crot, WHEELW, RADIUS ); // single precision #endif // draw world trimesh dsSetColor(0.7,0.7,0.4); dsSetTexture (DS_NONE); const dReal* Pos = dGeomGetPosition(world_mesh); float pos[3] = { Pos[0], Pos[1], Pos[2] }; const dReal* Rot = dGeomGetRotation(world_mesh); 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(dTriIndex); 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 } }
// simulation loop static void simLoop (int pause) { const dReal *rot; dVector3 ax; dReal l=0; switch (joint->getType() ) { case dJointTypeSlider : ( (dSliderJoint *) joint)->getAxis (ax); l = ( (dSliderJoint *) joint)->getPosition(); break; case dJointTypePiston : ( (dPistonJoint *) joint)->getAxis (ax); l = ( (dPistonJoint *) joint)->getPosition(); break; default: {} // keep the compiler happy } if (!pause) { double simstep = 0.01; // 1ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf (dt/simstep); if (!nrofsteps) nrofsteps = 1; for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldStep (world, simstep); dJointGroupEmpty (contactgroup); } update(); dReal radius, length; dsSetTexture (DS_WOOD); drawBox (geom[BODY2], 1,1,0); drawBox (geom[RECT], 0,0,1); if ( geom[BODY1] ) { const dReal *pos = dGeomGetPosition (geom[BODY1]); rot = dGeomGetRotation (geom[BODY1]); dsSetColor (0,0,1); dGeomCapsuleGetParams (geom[BODY1], &radius, &length); dsDrawCapsule (pos, rot, length, radius); } drawBox (geom[OBS], 1,0,1); // Draw the prismatic axis if ( geom[BODY1] ) { const dReal *pos = dGeomGetPosition (geom[BODY1]); rot = dGeomGetRotation (geom[BODY2]); dVector3 p; p[X] = pos[X] - l*ax[X]; p[Y] = pos[Y] - l*ax[Y]; p[Z] = pos[Z] - l*ax[Z]; dsSetColor (1,0,0); dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS); } if (joint->getType() == dJointTypePiston ) { dVector3 anchor; dJointGetPistonAnchor(joint->id(), anchor); // Draw the rotoide axis rot = dGeomGetRotation (geom[BODY2]); dsSetColor (1,0.5,0); dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS); dsSetColor (0,1,1); rot = dGeomGetRotation (geom[BODY1]); dsDrawSphere (anchor, rot, 1.5*RADIUS); } } }
static void simLoop (int pause) { int i, j; dsSetTexture (DS_WOOD); if (!pause) { #ifdef BOX dBodyAddForce(body[bodies-1],lspeed,0,0); #endif for (j = 0; j < joints; j++) { dReal curturn = dJointGetHinge2Angle1 (joint[j]); //dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0); dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0); dJointSetHinge2Param(joint[j],dParamFMax,dInfinity); dJointSetHinge2Param(joint[j],dParamVel2,speed); dJointSetHinge2Param(joint[j],dParamFMax2,FMAX); dBodyEnable(dJointGetBody(joint[j],0)); dBodyEnable(dJointGetBody(joint[j],1)); } if (doFast) { dSpaceCollide (space,0,&nearCallback); #if defined(QUICKSTEP) dWorldQuickStep (world,0.05); #elif defined(STEPFAST) dWorldStepFast1 (world,0.05,ITERS); #endif dJointGroupEmpty (contactgroup); } else { dSpaceCollide (space,0,&nearCallback); dWorldStep (world,0.05); dJointGroupEmpty (contactgroup); } for (i = 0; i < wb; i++) { b = dGeomGetBody(wall_boxes[i]); if (dBodyIsEnabled(b)) { bool disable = true; const dReal *lvel = dBodyGetLinearVel(b); dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2]; if (lspeed > DISABLE_THRESHOLD) disable = false; const dReal *avel = dBodyGetAngularVel(b); dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2]; if (aspeed > DISABLE_THRESHOLD) disable = false; if (disable) wb_stepsdis[i]++; else wb_stepsdis[i] = 0; if (wb_stepsdis[i] > DISABLE_STEPS) { dBodyDisable(b); dsSetColor(0.5,0.5,1); } else dsSetColor(1,1,1); } else dsSetColor(0.4,0.4,0.4); dVector3 ss; dGeomBoxGetLengths (wall_boxes[i], ss); dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); } } else { for (i = 0; i < wb; i++) { b = dGeomGetBody(wall_boxes[i]); if (dBodyIsEnabled(b)) dsSetColor(1,1,1); else dsSetColor(0.4,0.4,0.4); dVector3 ss; dGeomBoxGetLengths (wall_boxes[i], ss); dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); } } dsSetColor (0,1,1); dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; for (i = 0; i < boxes; i++) dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides); dsSetColor (1,1,1); for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]), dGeomGetRotation(sphere[i]),RADIUS); // draw the cannon dsSetColor (1,1,0); dMatrix3 R2,R3,R4; dRFromAxisAndAngle (R2,0,0,1,cannon_angle); dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); dMultiply0 (R4,R2,R3,3,3,3); dReal cpos[3] = {CANNON_X,CANNON_Y,1}; dReal csides[3] = {2,2,2}; dsDrawBox (cpos,R2,csides); for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2]; dsDrawCylinder (cpos,R4,3,0.5); // draw the cannon ball dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body), CANNON_BALL_RADIUS); }
// copied from an OpenDE demo program void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (g == nullptr) return; if (dGeomIsSpace(g) != 0) { displaySpace((dSpaceID)g); return; } int type = dGeomGetClass (g); if (type == dBoxClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } else show_aabb = 0; if (show_aabb != 0) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
static void drawGeom(dGeomID geomID) { // printf("1%lu", (uint64_t)geomID); int gclass = dGeomGetClass(geomID); // printf("2\n"); const dReal *pos = NULL; const dReal *rot = NULL; bool canDrawJoints = false; ODEUserObject* userObj = (ODEUserObject*)dGeomGetData(geomID); if (nullptr != userObj) { dsSetColorAlpha(1, 1, 1, 1); // printf("%f %f %f %f\n", userObj->colorVec[0], userObj->colorVec[1], userObj->colorVec[2], userObj->colorVec[3]); dsSetTexture (userObj->textureNum); dsSetColorAlpha(userObj->m_colorVec[0], userObj->m_colorVec[1], userObj->m_colorVec[2], userObj->m_colorVec[3]); } else { dsSetColorAlpha(1, 1, 0, 1); dsSetTexture (DS_WOOD); } switch (gclass) { case dSphereClass: if (nullptr != userObj && userObj->visible) { pos = dGeomGetPosition(geomID); rot = dGeomGetRotation(geomID); dsDrawSphere(pos, rot, dGeomSphereGetRadius(geomID)); } canDrawJoints = true; break; case dBoxClass: { if (nullptr != userObj && userObj->visible) { pos = dGeomGetPosition(geomID); rot = dGeomGetRotation(geomID); dVector3 lengths; dGeomBoxGetLengths(geomID, lengths); dsDrawBox(pos, rot, lengths); } canDrawJoints = true; break; } case dCylinderClass: { if (nullptr != userObj && userObj->visible) { pos = dGeomGetPosition(geomID); rot = dGeomGetRotation(geomID); dReal length; dReal radius; dGeomCylinderGetParams(geomID, &radius, &length); dsDrawCylinder(pos, rot, length, radius); } canDrawJoints = true; break; } default: break; } // printf("class: %d\n", gclass); if (canDrawJoints) { #ifdef DRAW_JOINTS_TOO dBodyID body = dGeomGetBody(geomID); int numJoints = dBodyGetNumJoints(body); for (int i = 0; i < numJoints; ++i) { dJointID joint = dBodyGetJoint(body, i); int jointClass = dJointGetType(joint); switch (jointClass) { case dJointTypeHinge: { dVector3 a11; dBodyID body1 = dJointGetBody(joint, 0); dBodyID body2 = dJointGetBody(joint, 1); if (body1 && body2) { const dReal* bodyPos1 = dBodyGetPosition(body1); const dReal* bodyPos2 = dBodyGetPosition(body2); dJointGetHingeAnchor(joint, a11); dsSetColor(1, 0, 0); dsDrawLine(a11, bodyPos1); dsDrawLine(a11, bodyPos2); dsSetColor(0, 1, 0); dsDrawLine(bodyPos1, bodyPos2); } } } } #endif } }
// copied from an OpenDE demo program //todo:pass trimesh as argument to this function void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb, Tmesh tm) { int i; if (!g) return; if (dGeomIsSpace(g)) { displaySpace((dSpaceID)g); return; } int type = dGeomGetClass (g); if (type == dBoxClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dTriMeshClass) { //dTriIndex* Indices = DISP.tmd[i].indices; const dReal* Pos = dGeomGetPosition(g); const dReal* Rot = dGeomGetRotation(g); for (int ii = 0; ii < (tm.indexSize/3); ii++) { const dReal v[9] = { // explicit conversion from float to dReal tm.vertices[tm.indices[ii * 3 + 0] * 3 + 0], tm.vertices[tm.indices[ii * 3 + 0] * 3 + 1], tm.vertices[tm.indices[ii * 3 + 0] * 3 + 2], tm.vertices[tm.indices[ii * 3 + 1] * 3 + 0], tm.vertices[tm.indices[ii * 3 + 1] * 3 + 1], tm.vertices[tm.indices[ii * 3 + 1] * 3 + 2], tm.vertices[tm.indices[ii * 3 + 2] * 3 + 0], tm.vertices[tm.indices[ii * 3 + 2] * 3 + 1], tm.vertices[tm.indices[ii * 3 + 2] * 3 + 2] }; dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1); } //std::cout<<"done once"<<std::endl; } else if (type == dRayClass) { dVector3 Origin, Direction; dGeomRayGet(g, Origin, Direction); dReal Length = dGeomRayGetLength(g); 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); double *ori = new double[3]; double *end = new double[4]; ori[0]=Origin[0]; ori[1]=Origin[1]; ori[2]=Origin[2]; end[0]=End[0]; end[1]=End[1]; end[2]=End[2]; end[3]=End[3]; dsDrawLine(ori, end); } else show_aabb = 0; if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
void draw_all_objects (dSpaceID space) { int i, j; draw_all_objects_called = 1; if (!graphical_test) return; int n = dSpaceGetNumGeoms (space); // draw all contact points dsSetColor (0,1,1); dSpaceCollide (space,0,&nearCallback); // draw all rays for (i=0; i<n; i++) { dGeomID g = dSpaceGetGeom (space,i); if (dGeomGetClass (g) == dRayClass) { dsSetColor (1,1,1); dVector3 origin,dir; dGeomRayGet (g,origin,dir); origin[2] += Z_OFFSET; dReal length = dGeomRayGetLength (g); for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j]; dsDrawLine (origin,dir); dsSetColor (0,0,1); dsDrawSphere (origin,dGeomGetRotation(g),0.01); } } // draw all other objects for (i=0; i<n; i++) { dGeomID g = dSpaceGetGeom (space,i); dVector3 pos; if (dGeomGetClass (g) != dPlaneClass) { memcpy (pos,dGeomGetPosition(g),sizeof(pos)); pos[2] += Z_OFFSET; } switch (dGeomGetClass (g)) { case dSphereClass: { dsSetColorAlpha (1,0,0,0.8); dReal radius = dGeomSphereGetRadius (g); dsDrawSphere (pos,dGeomGetRotation(g),radius); break; } case dBoxClass: { dsSetColorAlpha (1,1,0,0.8); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,dGeomGetRotation(g),sides); break; } case dCapsuleClass: { dsSetColorAlpha (0,1,0,0.8); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,dGeomGetRotation(g),length,radius); break; } case dCylinderClass: { dsSetColorAlpha (0,1,0,0.8); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,dGeomGetRotation(g),length,radius); break; } case dPlaneClass: { dVector4 n; dMatrix3 R,sides; dVector3 pos2; dGeomPlaneGetParams (g,n); dRFromZAxis (R,n[0],n[1],n[2]); for (j=0; j<3; j++) pos[j] = n[j]*n[3]; pos[2] += Z_OFFSET; sides[0] = 2; sides[1] = 2; sides[2] = 0.001; dsSetColor (1,0,1); for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j]; dsDrawLine (pos,pos2); dsSetColorAlpha (1,0,1,0.8); dsDrawBox (pos,R,sides); break; } } } }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } //<---- Convex Object else if (type == dConvexClass) { #if 0 dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); #else dsDrawConvex(pos,R, Sphere_planes, Sphere_planecount, Sphere_points, Sphere_pointcount, Sphere_polygons); #endif } //----> Convex Object else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_body) { dBodyID body = dGeomGetBody(g); if (body) { const dReal *bodypos = dBodyGetPosition (body); const dReal *bodyr = dBodyGetRotation (body); dReal bodySides[3] = { 0.1, 0.1, 0.1 }; dsSetColorAlpha(0,1,0,1); dsDrawBox(bodypos,bodyr,bodySides); } } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }