ODE/複数のオブジェクトと異なる衝突処理 - ずくなしのーと

複数のオブジェクトと異なる衝突処理

ODEでは、衝突検出は次のように行います。

1. dSpaceCollideにより大まかな衝突判定を行い、geom同士が近づいたら引数の関数をコールする(nearCallback関数)
2. 呼ばれたnearCallbackで細かな衝突判定を行い、具体的な形状の衝突を判定(dJointCreateContact)
3. もし、衝突を検出したらContactJointにより衝突計算を行う

衝突時、オブジェクトごとに反発係数や摩擦係数は異なるのが自然ですが、
ODEの衝突処理は、nearCalllback関数の１カ所で行うことになるため、
異なる衝突処理を表現するのになかなかに困惑します。

このページでは、オブジェクトごとに衝突処理を切り替えるコード例を示したいと思います。
あくまでも一例です。ほかに良い方法は見つかるかもしれませんが参考にしていただければ幸いです。

３つの球体と異なるバウンド処理

ここで紹介する方法は、dGeomSetDataとdGeomGetDataを使って異なる設定を行うというものです。
オブジェクト生成時にdSurfaceParametersを含むクラスのポインタをdGeomIDに設定（dGeomSetData）し、
nearCallback関数が呼ばれたら、dGeomIDのクラスポインタを取得(dGeomGetData)し、
dSurfaceParameters値を設定します。
そうすると、オブジェクトごとに設定された異なるサーフェイスパラメータが反映されます。

// v0.13
#include <vector>
#include <iostream>
#include <cassert>
 
#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#define dsDrawBox dsDrawBoxD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#endif
 
class Shape {
public:
  dSurfaceParameters m_surface;
  virtual void draw() {}
  Shape() {
    m_surface.mode = 0;
    m_surface.mu = dInfinity;
  }
};
 
class Sphere : public Shape {
protected:
  dBodyID m_body;
  dGeomID m_geom;
  int m_kind;
 
public:
  Sphere(dWorldID world, dSpaceID space, int kind=0)
    : m_kind(kind)
  {
    dReal radius = 0.5;
 
    // 生成
    m_body = dBodyCreate( world );
    m_geom = dCreateSphere(space, radius);
    dGeomSetBody(m_geom, m_body);
 
    // データ設定
    dGeomSetData(m_geom, this);
 
    // 位置設定
    if (m_kind == 0) {
      dBodySetPosition(m_body, -2.0, 0.0, 3.0);
    } else if (m_kind == 1) {
      dBodySetPosition(m_body,  0.0, 0.0, 3.0);
    } else if (m_kind == 2) {
      dBodySetPosition(m_body,  2.0, 0.0, 3.0);
    }
 
   // 質量設定
    dMass m;
    dMassSetZero(&m);
    dMassSetSphereTotal(&m, 1.0, radius);
    dBodySetMass(m_body, &m);
 
    // 衝突時のサーフェスパラメータ設定 
    if (m_kind == 0) {
      //m_surface.mode = dContactBounce | dContactSoftCFM;
      m_surface.mode = dContactBounce;
      m_surface.mu = dInfinity;     // 摩擦係数
      m_surface.bounce = 1.00;      // 反発係数
      m_surface.bounce_vel = 0.1;      // 反発係数しきい値
      //m_surface.soft_cfm = 0.005;  // CFM設定
    } else if (m_kind == 1) {
      m_surface.mode = dContactBounce | dContactSoftCFM;
      m_surface.mu = dInfinity;    // 摩擦係数
      m_surface.bounce = 1.00;     // 反発係数
      m_surface.bounce_vel = 0.1;      // 反発係数しきい値
      m_surface.soft_cfm = 0.005;  // CFM設定
    } else if (m_kind == 2) {
      m_surface.mode = dContactBounce | dContactSoftCFM;
      m_surface.mu = dInfinity;    // 摩擦係数
      m_surface.bounce = 0.80;     // 反発係数
      m_surface.bounce_vel = 0.1;      // 反発係数しきい値
      m_surface.soft_cfm = 0.001;  // CFM設定
    }
  }
 
  ~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
    if (m_kind == 0) {
      dsSetColorAlpha(0.75, 0.3, 0.3, 1.0);  //赤
    } else if (m_kind == 1) {
      dsSetColorAlpha(0.3, 0.75, 0.3, 1.0);  //緑
    } else if (m_kind == 2) {
      dsSetColorAlpha(0.3, 0.3, 0.75, 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;
dSurfaceParameters default_surface = {
  .mode = 0,
  .mu = dInfinity
};
 
// 衝突検出用関数
static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
  static Shape defaultShape;
  static const int MAX_CONTACTS = 4;
  dBodyID b1 = dGeomGetBody( o1 );
  dBodyID b2 = dGeomGetBody( o2 );
 
  if ( b1 && b2 && dAreConnectedExcluding( b1, b2, dJointTypeContact ) )
    return;
 
  // オブジェクトのサーフェイスパラメータを取得
  Shape *s;
  s = (Shape*)dGeomGetData(o1);
  if (!s) { s = (Shape*)dGeomGetData(o2); }
  if (!s) { s = &defaultShape; }
  assert(s);
 
  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 = s->m_surface;
      // 衝突の発生
      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, 5.f, 3.f };
  static float hpr[3] = {-90.f, -15.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 );
    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);
  contact_group = dJointGroupCreate(0);
 
  // 地面生成
  dGeomID ground = dCreatePlane(space, 0, 0, 1, 0);
  dGeomSetData(ground, NULL);
 
  // シェイプ生成
  shapeList.push_back(new Sphere(world, space, 0));
  shapeList.push_back(new Sphere(world, space, 1));
  shapeList.push_back(new Sphere(world, space, 2));
 
  dsSimulationLoop(argc, argv, 320, 240, &fn);
 
  dJointGroupDestroy(contact_group);
  dWorldDestroy(world);
  dSpaceDestroy(space);
  dCloseODE();
  return 0;
}