bool wheelCallBack(dGeomID o1,dGeomID o2,PSurface* s) { //s->id2 is ground const dReal* r; //wheels rotation matrix //const dReal* p; //wheels rotation matrix if ((o1==s->id1) && (o2==s->id2)) { r=dBodyGetRotation(dGeomGetBody(o1)); //p=dGeomGetPosition(o1);//never read } else if ((o1==s->id2) && (o2==s->id1)) { r=dBodyGetRotation(dGeomGetBody(o2)); //p=dGeomGetPosition(o2);//never read } else { //XXX: in this case we dont have the rotation // matrix, thus we must return return false; } s->surface.mode = dContactFDir1 | dContactMu2 | dContactApprox1 | dContactSoftCFM; s->surface.mu = fric(_w->cfg->robotSettings.WheelPerpendicularFriction); s->surface.mu2 = fric(_w->cfg->robotSettings.WheelTangentFriction); s->surface.soft_cfm = 0.002; dVector3 v={0,0,1,1}; dVector3 axis; dMultiply0(axis,r,v,4,3,1); dReal l = sqrt(axis[0]*axis[0] + axis[1]*axis[1]); s->fdir1[0] = axis[0]/l; s->fdir1[1] = axis[1]/l; s->fdir1[2] = 0; s->fdir1[3] = 0; s->usefdir1 = true; return true; }
// 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 }
//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) { // 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) { 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 odeSpringRender() { glEnable( GL_BLEND ); glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); glPointSize( 4.f ); for( int i=0; i<ODESPRINGS_MAX; i++ ) { if( odeSprings[i].active ) { if( odeSprings[i].disabled ) { continue; glColor4ub( 120, 120, 120, 128 ); } else { glColor4ub( 120, 120, 230, 255 ); } FVec3 p0; if( odeSprings[i].dBody[0] ) { dReal *pos0 = (dReal *)dBodyGetPosition( odeSprings[i].dBody[0] ); dReal *rot0 = (dReal *)dBodyGetRotation( odeSprings[i].dBody[0] ); FMat4 mat0 = zmathODERotPosToFMat4( rot0, pos0 ); p0 = mat0.mul( odeSprings[i].offset[0] ); } else { p0 = odeSprings[i].offset[0]; } FVec3 p1; if( odeSprings[i].dBody[1] ) { dReal *pos1 = (dReal *)dBodyGetPosition( odeSprings[i].dBody[1] ); dReal *rot1 = (dReal *)dBodyGetRotation( odeSprings[i].dBody[1] ); FMat4 mat1 = zmathODERotPosToFMat4( rot1, pos1 ); p1 = mat1.mul( odeSprings[i].offset[1] ); } else { p1 = odeSprings[i].offset[1]; } glBegin( GL_LINES ); glVertex3fv( p0 ); glVertex3fv( p1 ); glEnd(); glBegin( GL_POINTS ); FVec3 hack = p1; hack.sub( p0 ); hack.normalize(); hack.mul( odeSprings[i].naturalLen ); hack.add( p0 ); glVertex3fv( hack ); glEnd(); } } }
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)); }
void draw::draw_tail(){ glPushMatrix(); //Link 1 glPushMatrix(); const dReal *tail_link_1_location = dBodyGetPosition(body_bag->getTailLink1Body()); glTranslatef(tail_link_1_location[0], tail_link_1_location[1], tail_link_1_location[2]); const dReal *tail_link_1_rotation_matrix_ode = dBodyGetRotation(body_bag->getTailLink1Body()); float tail_link_1_rotation_matrix_openGL[16]; getOpenGLRotationMatrix(tail_link_1_rotation_matrix_openGL, tail_link_1_rotation_matrix_ode); //rotate the link glMultMatrixf(tail_link_1_rotation_matrix_openGL); glScalef(4, 4, 60); draw_cube(); glPopMatrix(); //Link 2 glPushMatrix(); const dReal *tail_link_2_location = dBodyGetPosition(body_bag->getTailLink2Body()); glTranslatef(tail_link_2_location[0], tail_link_2_location[1], tail_link_2_location[2]); const dReal *tail_link_2_rotation_matrix_ode = dBodyGetRotation(body_bag->getTailLink2Body()); float tail_link_2_rotation_matrix_openGL[16]; getOpenGLRotationMatrix(tail_link_2_rotation_matrix_openGL, tail_link_2_rotation_matrix_ode); //rotate the link glMultMatrixf(tail_link_2_rotation_matrix_openGL); glScalef(4, 4, 60); draw_cube(); glPopMatrix(); //Link 3 glPushMatrix(); const dReal *tail_link_3_location = dBodyGetPosition(body_bag->getTailLink3Body()); glTranslatef(tail_link_3_location[0], tail_link_3_location[1], tail_link_3_location[2]); const dReal *tail_link_3_rotation_matrix_ode = dBodyGetRotation(body_bag->getTailLink3Body()); float tail_link_3_rotation_matrix_openGL[16]; getOpenGLRotationMatrix(tail_link_3_rotation_matrix_openGL, tail_link_3_rotation_matrix_ode); //rotate the link glMultMatrixf(tail_link_3_rotation_matrix_openGL); glScalef(4, 4, 60); draw_cube(); glPopMatrix(); //Link 4 glPushMatrix(); const dReal *tail_link_4_location = dBodyGetPosition(body_bag->getTailLink4Body()); glTranslatef(tail_link_4_location[0], tail_link_4_location[1], tail_link_4_location[2]); const dReal *tail_link_4_rotation_matrix_ode = dBodyGetRotation(body_bag->getTailLink4Body()); float tail_link_4_rotation_matrix_openGL[16]; getOpenGLRotationMatrix(tail_link_4_rotation_matrix_openGL, tail_link_4_rotation_matrix_ode); //rotate the link glMultMatrixf(tail_link_4_rotation_matrix_openGL); glScalef(4, 4, 60); draw_cube(); glPopMatrix(); glPopMatrix(); }
void vmWishboneCar::drawLinkage(vm::WheelLoc loc) { // select wheel vmWishbone *snow; switch (loc) { case vm::WheelLoc::FR: snow= &frSuspension; break; case vm::WheelLoc::FL: snow= &flSuspension; break; case vm::WheelLoc::RR: snow= &rrSuspension; break; case vm::WheelLoc::RL: snow= &rlSuspension; break; default: break; } // setup linkage const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3; const dReal *pos4,*R4,*pos5,*R5; dsSetColorAlpha(0.0,0.0,1.0, 0.5); pos1 = dBodyGetPosition(snow->uplink.body); R1 = dBodyGetRotation(snow->uplink.body); dsDrawCylinderD(pos1,R1,snow->uplink.width,snow->uplink.radius); dsSetColor(0.0,1.0,0.0); pos2 = dBodyGetPosition(snow->hlink.body); R2 = dBodyGetRotation(snow->hlink.body); dsDrawCylinderD(pos2,R2,snow->hlink.width,snow->hlink.radius); dsSetColorAlpha(1.0,0.0,1.0, 0.5); pos3 = dBodyGetPosition(snow->lowlink.body); R3 = dBodyGetRotation(snow->lowlink.body); dsDrawCylinderD(pos3,R3,snow->lowlink.width,snow->lowlink.radius); // setup spring and damper dsSetColorAlpha(0.1,0.7,1.0, 0.5); pos4= dBodyGetPosition(snow->upstrut.body); R4= dBodyGetRotation(snow->upstrut.body); dsDrawCylinderD(pos4,R4,snow->upstrut.width,snow->upstrut.radius); dsSetColorAlpha(1.0,0.7,1.0, 0.5); pos5= dBodyGetPosition(snow->lowstrut.body); R5= dBodyGetRotation(snow->lowstrut.body); dsDrawCylinderD(pos5,R5,snow->lowstrut.width,snow->lowstrut.radius); }
/*** ロボットアームの描画 ***/ void drawArm() { dReal r,length; dReal box_length[3] = {0.4, 0.4, 0.4}; for (int i = 0; i < NUM-1; i++ ) { // カプセルの描画 dGeomCapsuleGetParams(rlink[i].geom, &r,&length); dsDrawCapsule(dBodyGetPosition(rlink[i].body), dBodyGetRotation(rlink[i].body),length,r); } dGeomBoxGetLengths(rlink[NUM-1].geom, box_length); dsDrawBox(dBodyGetPosition(rlink[NUM-1].body), dBodyGetRotation(rlink[NUM-1].body), box_length); }
/*** キーボードコマンドの処理関数 ***/ static void command(int cmd) { switch (cmd) { case '1': float xyz1[3],hpr1[3]; dsGetViewpoint(xyz1,hpr1); printf("xyz=%4.2f %4.2f %4.2f ",xyz1[0],xyz1[1],xyz1[2]); printf("hpr=%6.2f %6.2f %5.2f \n",hpr1[0],hpr1[1],hpr1[2]); break; case 'k': wheel[3].v -= 0.8; break; // 後進 case 's': wheel[0].v = wheel[1].v = wheel[2].v = wheel[3].v = 0.0; // 停止 break; case 'x': // ゴロシュート { const dReal *bp = dBodyGetPosition(ball.body); const dReal *p = dBodyGetPosition(wheel[0].body); const dReal *R = dBodyGetRotation(base.body); dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1], 2)); if (dist < 0.3) { dReal vmax = POWER * 0.01 * 8.0; dBodySetLinearVel(ball.body,vmax * R[0],vmax * R[4], 0.0); // printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]); } } break; case 'l': // ループシュート { const dReal *bp = dBodyGetPosition(ball.body); const dReal *p = dBodyGetPosition(wheel[0].body); const dReal *R = dBodyGetRotation(base.body); dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1],2)); if (dist < 1.0) { dReal vmax45 = POWER * 0.01 * 8.0 / sqrt(2.0); dBodySetLinearVel(ball.body,vmax45 * R[0],vmax45 * R[4], vmax45); // printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]); } } break; case 'b': dBodySetPosition(ball.body,0,0,0.14+offset_z); dBodySetLinearVel(ball.body,0,0,0); dBodySetAngularVel(ball.body,0,0,0); break; } }
vector<float> controller::getTargetPosition(int leg_id) { //Last link //translation Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4); mt(2, 3) = body_bag->getFootLinkLength(leg_id, 3); //rotation Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4); const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3)); for(int i = 0; i < 3; i++) { for(int j = 0; j < 4; j++) { mr(i, j) = rotation_matrix_ode[i*4+j]; } } mr(3, 0) = 0; mr(3, 1) = 0; mr(3, 2) = 0; mr(3, 3) = 1; Eigen::MatrixXf point = Eigen::MatrixXf::Random(4, 1); point(0, 0) = current_foot_location[leg_id][0]; point(1, 0) = current_foot_location[leg_id][1]; point(2, 0) = current_foot_location[leg_id][2]; point(3, 0) = 1; Eigen::MatrixXf transformedPoint = mr*mt*mr.inverse()*point; //Second last link //translation mt = Eigen::MatrixXf::Identity(4, 4); mt(2, 3) = body_bag->getFootLinkLength(leg_id, 2); //rotation mr = Eigen::MatrixXf::Identity(4, 4); rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2)); for(int i = 0; i < 3; i++) { for(int j = 0; j < 4; j++) { mr(i, j) = rotation_matrix_ode[i*4+j]; } } mr(3, 0) = 0; mr(3, 1) = 0; mr(3, 2) = 0; mr(3, 3) = 1; Eigen::MatrixXf endEffectorM = mr*mt*mr.inverse()*transformedPoint; vector<float> endEffector(3); endEffector[0] = endEffectorM(0,0); endEffector[1] = endEffectorM(1,0); endEffector[2] = endEffectorM(2,0); return endEffector; }
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); }
/*** 台車の描画 ***/ 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); }
void RigidBody::setMeshTransfomation() { if(m_type !=TRIANGLEMESH) { std::cerr<<"trying to set transform on non mesh\n"; return; } const dReal *rot=dBodyGetRotation(m_id); const dReal *pos=dBodyGetPosition(m_id); dReal tx[16]; tx[0]=rot[0]; tx[1]=rot[1]; tx[2]=rot[2]; tx[3]=pos[0]; tx[4]=rot[4]; tx[5]=rot[5]; tx[6]=rot[6]; tx[7]=pos[1]; tx[8]=rot[8]; tx[9]=rot[9]; tx[10]=rot[10]; tx[11]=pos[2]; tx[12]=rot[12]; tx[13]=rot[13]; tx[14]=rot[14]; tx[15]=1.0; dGeomTriMeshSetLastTransform( m_geom.getID() ,tx ); dGeomTriMeshClearTCCache (m_geom.getID()); }
ngl::Mat4 RigidBody::getRotation()const { // When getting, the returned values are pointers to internal data structures, // so the vectors are valid until any changes are made to the rigid body // system structure. const dReal *rot=dBodyGetRotation(m_id); ngl::Mat4 m; m.m_openGL[0]= rot[0]; m.m_openGL[1]= rot[4]; m.m_openGL[2]= rot[8]; m.m_openGL[3]= 0; m.m_openGL[4]= rot[1]; m.m_openGL[5]= rot[5]; m.m_openGL[6]= rot[9]; m.m_openGL[7]= 0; m.m_openGL[8]= rot[2]; m.m_openGL[9]= rot[6]; m.m_openGL[10]= rot[10]; m.m_openGL[11]= 0; m.m_openGL[12]= rot[0]; m.m_openGL[13]= rot[1]; m.m_openGL[14]= rot[2]; m.m_openGL[15]= 1; return m; }
void CProtoHapticDoc::UpdateDynamics() { for(int i= 0; i<m_shapeCount; i++) { dGeomBoxSetLengths (m_geoms[i], m_shapes[i]->getSizeX(), m_shapes[i]->getSizeY(), m_shapes[i]->getSizeZ()); dGeomSetPosition (m_geoms[i], m_shapes[i]->getLocationX(), m_shapes[i]->getLocationY(), m_shapes[i]->getLocationZ()); dGeomSetRotation (m_geoms[i], dBodyGetRotation(bodies[i])); dBodySetPosition (bodies[i], m_shapes[i]->getLocationX(), m_shapes[i]->getLocationY(), m_shapes[i]->getLocationZ()); float *rotation= m_shapes[i]->getRotation(); const dReal rot[12]= { rotation[0], rotation[4], rotation[8], rotation[12], rotation[1], rotation[5], rotation[9], rotation[13], rotation[2], rotation[6], rotation[10], rotation[14] }; dBodySetRotation (bodies[i], rot); dMass mass; dMassSetBox (&mass, m_shapes[i]->getMass(),m_shapes[i]->getSizeX(), m_shapes[i]->getSizeY(), m_shapes[i]->getSizeZ()); dBodySetMass (bodies[i], &mass); } }
/* returns the a-th principal axis of a body */ void body_axis (t_real *axis, dBodyID b, int a) { if (b == SURFACE_BODY_ID) { if (a==0) { axis[0] = 0.; axis[1] = 0.; axis[2] = -1.; } else if (a==1) { axis[0] = 0.; axis[1] = 1.; axis[2] = 0.; } else if (a==2) { axis[0] = 1.; axis[1] = 0.; axis[2] = 0.; } } else { const t_real *o = dBodyGetRotation (b); axis[0] = o[0+a]; axis[1] = o[4+a]; axis[2] = o[8+a]; } }
/* return the principal inertia tensor of a body (inverse = 0) or its inverse (inverse = 1) */ void body_inertia_tensor (t_real *RIRt, dBodyID b, int inverse) { const t_real *R; t_matrix mi; t_vector vi; /* get principal inertia components */ body_inertia (vi, b); if (inverse==0) { mi[0] = vi[0]; mi[1] = 0. ; mi[2] = 0. ; mi[4] = 0. ; mi[5] = vi[1]; mi[6] = 0. ; mi[8] = 0. ; mi[9] = 0. ; mi[10] = vi[2]; } else { mi[0] = 1./vi[0]; mi[1] = 0. ; mi[2] = 0. ; mi[4] = 0. ; mi[5] = 1./vi[1]; mi[6] = 0. ; mi[8] = 0. ; mi[9] = 0. ; mi[10] = 1./vi[2]; } /* fix extra elements */ mi[3] = mi[7] = mi[11] = 0.; /* perform the matrix multiplication to bring it to the * world reference frame */ R = dBodyGetRotation (b); matrix_ABAt (RIRt, R, mi); }
void DynamicsSolver::GetTransform ( dBodyID geom, Matrix* M ) { float outmatrix[16]; dReal* L; dReal* R; L = (dReal*)dBodyGetPosition(geom); R = (dReal*)dBodyGetRotation(geom); outmatrix[0]=(float)R[0]; outmatrix[1]=(float)R[4]; outmatrix[2]=(float)R[8]; outmatrix[3]=0; outmatrix[4]=(float)R[1]; outmatrix[5]=(float)R[5]; outmatrix[6]=(float)R[9]; outmatrix[7]=0; outmatrix[8]=(float)R[2]; outmatrix[9]=(float)R[6]; outmatrix[10]=(float)R[10]; outmatrix[11]=0; outmatrix[12]=(float)L[0]; outmatrix[13]=(float)L[1]; outmatrix[14]=(float)L[2]; outmatrix[15]=1; memcpy( M->Mat, outmatrix, sizeof(float) * 16); }
void ODERigidObject::GetTransform(RigidTransform& T) const { const dReal* pos = dBodyGetPosition(bodyID); Vector3 comPos(pos[0],pos[1],pos[2]); const dReal* rot = dBodyGetRotation(bodyID); CopyMatrix(T.R,rot); T.t = comPos - T.R*obj.com; }
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])); */ }
ODESpring *odeSpringCreate( dBodyID b0, dBodyID b1, FVec3 off0, FVec3 off1, int type, int computeNatLen ) { for( int i=0; i<ODESPRINGS_MAX; i++ ) { if( ! odeSprings[i].active ) { odeSprings[i].type = type; odeSprings[i].active = 1; odeSprings[i].disabled = 0; odeSprings[i].dBody[0] = b0; odeSprings[i].dBody[1] = b1; odeSprings[i].offset[0] = off0; odeSprings[i].offset[1] = off1; odeSprings[i].naturalLen = 0.f; odeSprings[i].breakLen = 0.f; odeSprings[i].maximumForce = 0.f; odeSprings[i].constant = 1.f; odeSprings[i].naturalLenPtr = 0; odeSprings[i].breakLenPtr = 0; odeSprings[i].maximumForcePtr = 0; odeSprings[i].constantPtr = 0; if( computeNatLen ) { FVec3 p0 = odeSprings[i].offset[0]; if( odeSprings[i].dBody[0] ) { dReal *pos0 = (dReal *)dBodyGetPosition( odeSprings[i].dBody[0] ); dReal *rot0 = (dReal *)dBodyGetRotation( odeSprings[i].dBody[0] ); FMat4 mat0 = zmathODERotPosToFMat4( rot0, pos0 ); p0 = mat0.mul( odeSprings[i].offset[0] ); } FVec3 p1 = odeSprings[i].offset[1]; if( odeSprings[i].dBody[1] ) { dReal *pos1 = (dReal *)dBodyGetPosition( odeSprings[i].dBody[1] ); dReal *rot1 = (dReal *)dBodyGetRotation( odeSprings[i].dBody[1] ); FMat4 mat1 = zmathODERotPosToFMat4( rot1, pos1 ); p1 = mat1.mul( odeSprings[i].offset[1] ); } p0.sub( p1 ); odeSprings[i].naturalLen = p0.mag(); } return &odeSprings[i]; } } assert( 0 ); return 0; }
void TSRODERigidBody::DebugRender() { Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState ); TSRMatrix4 bodyTransform; TSRMatrix4 geomTransform; const float* pBodyPosition = dBodyGetPosition( m_BodyID ); const float* pBodyRotation = dBodyGetRotation( m_BodyID ); ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation ); TSRGlobalConstants.PushMatrix(); TSRGlobalConstants.MultMatrix( bodyTransform.d ); TSRDebugDraw::RenderAxis( 1.0f ); TSRDebugDraw::RenderSphere( 0.25f ); for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ ) { dGeomID currGeomTransformID = m_GeomIDs[ i ]; dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID ); const float* pGeomPosition = dGeomGetPosition( geomID ); const float* pGeomRotation = dGeomGetRotation( geomID ); ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation ); TSRGlobalConstants.PushMatrix(); TSRGlobalConstants.MultMatrix( bodyTransform.d ); switch( dGeomGetClass( geomID ) ) { case dBoxClass: { dVector3 extents; dGeomBoxGetLengths( geomID, extents ); TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] ); TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] ); aabbMin *= 0.5f; aabbMax *= 0.5f; TSRDebugDraw::RenderAABB( aabbMin, aabbMax ); } break; case dSphereClass: { float radius; radius = dGeomSphereGetRadius( geomID ); TSRDebugDraw::RenderSphere( radius ); } break; case dCylinderClass: { float radius,length; dGeomCylinderGetParams( geomID, &radius, &length ); TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) ); } break; } TSRGlobalConstants.PopMatrix(); } TSRGlobalConstants.PopMatrix(); Graphics()->SetRasterizerState( Graphics()->m_FillSolidState ); }
Pose Primitive::getPose() const { if(!geom) { if (!body) return Pose::translate(0.0f,0.0f,0.0f); // fixes init bug else return osgPose(dBodyGetPosition(body), dBodyGetRotation(body)); } return osgPose(dGeomGetPosition(geom), dGeomGetRotation(geom)); }
SpringHook::SpringHook(dBodyID _body,const Vector3& _worldpt,const Vector3& _target,Real _k) :body(_body),target(_target),k(_k) { Matrix3 R; Vector3 t; CopyVector(t,dBodyGetPosition(body)); CopyMatrix(R,dBodyGetRotation(body)); R.mulTranspose(_worldpt-t,localpt); }
void DrawBox() { for(int i = 0; i < BOX_NUM; i++) { dsSetColorAlpha(0.3, 0.2, 0.1, 1.0); dsSetTexture(DS_WOOD); dsDrawBoxD(dBodyGetPosition(box[i].body),dBodyGetRotation(box[i].body), boxSize[i]); } }
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); }
// ロボットの描画 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); }