ODE/複数のボールと衝突 - ずくなしのーと

複数のボールと衝突

複数のオブジェクトと衝突処理を試す。
x, y, z方向に10x10x10の球体を配置して衝突実験。

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <vector>
 
#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#define dsDrawBox dsDrawBoxD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#endif
 
class Shape {
public:
  virtual void draw() = 0;
};
 
class Sphere : public Shape {
protected:
  dBodyID m_body;
  dGeomID m_geom;
 
public:
  Sphere(dWorldID world, dSpaceID space, dReal x, dReal y, dReal z) {
    dReal radius = 0.5;
 
    // 生成
    m_body = dBodyCreate( world );
    m_geom = dCreateSphere(space, radius);
    dGeomSetBody(m_geom, m_body);
 
    // 位置設定
    dBodySetPosition(m_body, x, y, z);
 
    // 質量設定
    dMass m;
    dMassSetZero(&m);
    dMassSetSphereTotal(&m, 1.0, radius);
    dBodySetMass(m_body, &m);
  }
  ~Sphere() {
    dBodyDestroy(m_body);
    dGeomDestroy(m_geom);
  }
 
  virtual void draw() {
    //dsSetTexture (DS_CHECKERED);
    dsSetTexture (DS_WOOD);
    //dsSetColor( 1.0f, 1.0f, 0.0f ); // RGB Color
    dsSetColorAlpha(0.75, 0.3, 0.3, 1.0);
    dsDrawSphere( dGeomGetPosition( m_geom ),
        dGeomGetRotation( m_geom ),
        dGeomSphereGetRadius(m_geom) );
  }
};
 
static std::vector<Shape*> shapeList;
static dWorldID world;
static dSpaceID space;
static dJointGroupID contact_group;
 
// 衝突検出用関数
static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
  static const int MAX_CONTACTS = 4;
  dBodyID b1 = dGeomGetBody( o1 );
  dBodyID b2 = dGeomGetBody( o2 );
 
  if ( b1 && b2 && dAreConnectedExcluding( b1, b2, dJointTypeContact ) )
    return;
 
  dContact contact[MAX_CONTACTS];
  int numc = dCollide( o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof( dContact ) ); 
  if (numc > 0) {
    for (int i=0; i<numc; i++) {
      // 物体同士の接触時のパラメータ設定
      contact[i].surface.mode = dContactBounce | dContactSoftCFM;
      contact[i].surface.mu = dInfinity;   // 摩擦係数
      contact[i].surface.bounce = 0.5;     // 反発係数
      contact[i].surface.bounce_vel = 0.01; 
      contact[i].surface.soft_cfm = 0.0001;  // CFM設定
      // 衝突の発生
      dJointID c = dJointCreateContact( world, contact_group, contact+i );
      dJointAttach( c, b1, b2 ); 
    }
  }
}
 
// start simulation - set viewpoint
static void start()
{
  static float xyz[3] = { 0.f, 20.f, 10.f };
  static float hpr[3] = {-90.f, -20.f, 0.f };
 
  dsSetViewpoint( xyz, hpr ); // カメラ位置と方向設定
}
 
// simulation loop
static void simLoop( int pause )
{
  // Ctl+p が押されたらifに入らない
  if (!pause) {
    dSpaceCollide( space, 0, &nearCallback ); // 衝突検出
    //dWorldStep( world, 0.01 );
    dWorldQuickStep( world, 0.01 );
    dJointGroupEmpty( contact_group );
  }
 
  // Shapeの表示
  std::vector<Shape*>::iterator it = shapeList.begin();
  while (it != shapeList.end()) {
    (*it)->draw();
    ++it;
  }
}
 
 
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 = "../../../textures";
 
  // ODE初期化
  dInitODE2(0);
  dAllocateODEDataForThread(dAllocateMaskAll);
 
  // World,Space生成
  world = dWorldCreate();
  dWorldSetGravity(world, 0.0, 0.0, -9.8);
  dWorldSetCFM (world,1e-5);
  dWorldSetLinearDamping(world, 0.00001);
  dWorldSetAngularDamping(world, 0.005);
  dWorldSetMaxAngularSpeed(world, 200);
  dWorldSetERP(world, 0.2);
  dWorldSetContactSurfaceLayer (world,0.001);
  //space = dSimpleSpaceCreate (0);
  space = dHashSpaceCreate (0);
  contact_group = dJointGroupCreate(0);
 
  // 地面生成
  dCreatePlane(space, 0, 0, 1, 0);
 
  // シェイプ生成
  for (int z=0; z<10; z++) {
    for (int y=0; y<10; y++) {
      for (int x=0; x<10; x++) {
        dReal px = (dReal)x*1.1-5.0 + (dReal)(rand()%100)/100000.0;
        dReal py = (dReal)y*1.1-5.0 + (dReal)(rand()%100)/100000.0;
        dReal pz = (dReal)z*1.1+1.0 ;
        shapeList.push_back(new Sphere(world, space, px, py, pz));
      }
    }
  }
 
  dsSimulationLoop(argc, argv, 320, 240, &fn);
 
  dJointGroupDestroy(contact_group);
  dWorldDestroy(world);
  dSpaceDestroy(space);
  return 0;
}