Ejemplo n.º 1
0
int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = 0;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }

  // create world
  dInitODE2(0);

  int i;
  contactgroup.create ();
  world.setGravity (0,0,-0.5);
  dWorldSetCFM (world.id(),1e-5);
  dPlane plane (space,0,0,1,0);

  for (i=0; i<NUM; i++) {
    body[i].create (world);
    dReal k = i*SIDE;
    body[i].setPosition (k,k,k+0.4);
    dMass m;
    m.setBox (1,SIDE,SIDE,SIDE);
    m.adjust (MASS);
    body[i].setMass (&m);
    body[i].setData ((void*)(size_t)i);

    box[i].create (space,SIDE,SIDE,SIDE);
    box[i].setBody (body[i]);
  }
  for (i=0; i<(NUM-1); i++) {
    joint[i].create (world);
    joint[i].attach (body[i],body[i+1]);
    dReal k = (i+0.5)*SIDE;
    joint[i].setAnchor (k,k,k+0.4);
  }

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dCloseODE();
  return 0;
}
Ejemplo n.º 2
0
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnected (b1,b2)) return;

  // @@@ it's still more convenient to use the C interface here.

  dContact contact;
  contact.surface.mode = 0;
  contact.surface.mu = dInfinity;
  if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
    dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact);
    dJointAttach (c,b1,b2);
  }
}
Ejemplo n.º 3
0
//===========================================================================================
//! \brief シミュレーションオブジェクトを作成
void create_world( void )
//===========================================================================================
{
  // acrobot の回転軸(以下,支柱)は ここでは 直方体(Box)にしています.
  const dReal param_h0     = 0.05; // 支柱(直方体)の高さ[m]
  const dReal param_wx0    = 0.05; // 同幅(x)
  const dReal param_wy0    = 0.80; // 同幅(y)
  const dReal param_z0     = 1.20; // 支柱の垂直位置[m]

  const dReal param_l1     = 0.50; // 第1リンク(支柱に近いリンク)の長さ[m]
  const dReal param_d1     = 0.15; // 同直径[m]
  const dReal param_l2     = 0.50; // 第2リンク(支柱に近いリンク)の長さ[m]
  const dReal param_d2     = 0.15; // 同直径[m]

  const dReal density      = 1000.0;  // 各リンクの密度[kg/m^3]. 参考(?)`人体の密度' は 900~1100 kg/m^3 (wikipedia)

  int i;
  contactgroup.create (0);
  world.setGravity (0,0,-9.8);  // 重力 [m/s^2]
  dWorldSetCFM (world.id(),1e-5);
  plane.create (space,0,0,1,0); // 地面(平面).

  i=0; {
    body[i].create (world);
    body[i].setPosition (0.0, 0.0, param_z0); // 支柱の中心座標
    dReal xx=param_wx0, yy=param_wy0, zz=param_h0;
    dMass m;
    m.setBox (density,xx,yy,zz);
    body[i].setMass (&m);
    LinkBase.create (space,xx,yy,zz);
    LinkBase.setBody (body[i]);
  }
  i=1; {
    body[i].create (world);
    body[i].setPosition (0.0, 0.0, param_z0-0.5*param_l1); // リンク1の中心座標
    dReal rad=0.5*param_d1, len=param_l1-2.0*rad;
    dMass m;
    m.setCappedCylinder (density,3,rad,len);  // direction(3): z-axis
    body[i].setMass (&m);
    Link1.create (space,rad,len);
    Link1.setBody (body[i]);
  }
  i=2; {
    body[i].create (world);
    body[i].setPosition (0.0, 0.0, param_z0-param_l1-0.5*param_l2); // リンク2の中心座標
    dReal rad=0.5*param_d2, len=param_l2-2.0*rad;
    dMass m;
    m.setCappedCylinder (density,3,rad,len);  // direction(3): z-axis
    body[i].setMass (&m);
    Link2.create (space,rad,len);
    Link2.setBody (body[i]);
  }

  i=0; {
    const dReal *pos = body[0].getPosition();
    joint[i].create (world);
    joint[i].attach (body[0],body[1]);
    joint[i].setAnchor (pos[0],pos[1],pos[2]); // 回転中心=支柱の中心(=原点)
    joint[i].setAxis (0.0,1.0,0.0); // 回転軸=y軸
    // joint[i].setParam (dParamHiStop, +0.5*M_PI); // 関節の可動範囲を制約するときに使う
    // joint[i].setParam (dParamLoStop, -0.5*M_PI); // acrobot の場合は省略
  }
  i=1; {
    const dReal *pos = body[1].getPosition();
    joint[i].create (world);
    joint[i].attach (body[1],body[2]);
    joint[i].setAnchor (pos[0],pos[1],pos[2]-0.5*param_l1); // 回転中心=リンク1とリンク2の間
    joint[i].setAxis (0.0,1.0,0.0); // 回転軸=y軸
  }
  base_joint.create(world);
  base_joint.attach(body[0].id(),0); // 支柱(body[0]) と 平面(0)の間の固定リンク.支柱が固定される.
  base_joint.set();
}