ODE/コンポジット・オブジェクトを使った空箱の作成 - ずくなしのーと

コンポジット・オブジェクトを使った空箱の作成

空の箱サンプルプログラムを用意してみました。
ここではコンポジット・オブジェクト（GeomTransform）を利用しています。
Massの設定はうまくいかなかったので一部コメントアウトしました。

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <list>
 
#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.2;
 
    // 生成
    m_body = dBodyCreate( world );
    m_geom = dCreateSphere(space, radius);
    dGeomSetBody(m_geom, m_body);
 
    // 位置設定
    //dBodySetPosition(m_body, 0.0, 0.0, 3.0);
    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) );
  }
};
 
class Hako : public Shape {
protected:
  static const int NUM = 5;
  dBodyID m_body;
  dGeomID m_geom[NUM];
 
public:
  Hako(dWorldID world, dSpaceID space) {
    const dReal scale = 2.0;
    const dVector3 lengths = {1.0*scale, 1.0*scale, 0.01};
    // offset{x,y,z}, angle{ax, ay, az}
    const dReal offsets[NUM][3] = {
      {-0.5*scale, 0.0*scale, 0.0*scale}, 
      { 0.5*scale, 0.0*scale, 0.0*scale}, 
      { 0.0*scale,-0.5*scale, 0.0*scale}, 
      { 0.0*scale, 0.5*scale, 0.0*scale}, 
      { 0.0*scale, 0.0*scale,-0.5*scale}
    };
    const dReal angles[NUM][3] = {
      { 0.0,90.0, 0.0},
      { 0.0,90.0, 0.0},
      {90.0, 0.0, 0.0},
      {90.0, 0.0, 0.0},
      { 0.0, 0.0, 0.0}
    };
 
    // 生成
    m_body = dBodyCreate( world );
 
    // Transform 生成
    dMass m, tran_m;
    dMassSetZero(&m);
    dGeomID t_geom[NUM];
    for (int k=0; k<NUM; k++) {
      m_geom[k] = dCreateGeomTransform(space);
      dGeomTransformSetCleanup(m_geom[k],1);
      t_geom[k] = dCreateBox(0, lengths[0], lengths[1], lengths[2]); 
      //dMassSetBox(&tran_m, 1.0, lengths[0], lengths[1], lengths[2]); 
      dGeomTransformSetGeom(m_geom[k], t_geom[k]); 
      dGeomSetPosition(t_geom[k], offsets[k][0], offsets[k][1], offsets[k][2]);
      //dMassTranslate(&tran_m, offsets[k][0], offsets[k][1], offsets[k][2]);
      dMatrix3 rotation;
      dMatrix3 r1, r2, r3, r4;
      dRFromAxisAndAngle(r1, 1.0, 0.0, 0.0, angles[k][0]*M_PI/180.0); // x-axis
      dRFromAxisAndAngle(r2, 0.0, 1.0, 0.0, angles[k][1]*M_PI/180.0); // y-axis
      dRFromAxisAndAngle(r3, 0.0, 0.0, 1.0, angles[k][2]*M_PI/180.0); // z-axis
      dMultiply0_333(r4, r1, r2);
      dMultiply0_333(rotation, r3, r4);
      dGeomSetRotation(t_geom[k], rotation);
      //dMassRotate(&tran_m, rotation);
      //dMassAdd(&m, &tran_m);
    }
 
    // body, geom, mass関連づけ
    for (int k=0; k<NUM; k++) {
      dGeomSetBody(m_geom[k], m_body);
    }
    dMassSetBoxTotal(&m, 10.0, lengths[0], lengths[1], lengths[2]); 
    dBodySetMass(m_body, &m);
 
    dBodySetPosition(m_body, 0.0, 0.0, 0.51*scale);
  }
 
  ~Hako() {
    dBodyDestroy(m_body);
    for (int k=0; k<NUM;k++) {
      dGeomDestroy(m_geom[k]);
    }
  }
 
  virtual void draw() {
    for (int k=0; k<NUM; k++) {
      dGeomID t_geom = dGeomTransformGetGeom( m_geom[k] );
 
      dVector3 lengths;
      dGeomBoxGetLengths(t_geom, lengths);
 
      // Get world position/orientation of parent body
      const dReal* pBodyPos = dBodyGetPosition( m_body );
      const dReal* pBodyRotMat = dBodyGetRotation( m_body );
      // Get local position/orientation of component within geom transform
      const dReal* pLocalPos = dGeomGetPosition (t_geom);
      const dReal* pLocalRotMat = dGeomGetRotation (t_geom);
 
      dVector3 worldPos;
      dMatrix3 worldRotMat;
      dMULTIPLY0_331( worldPos, pBodyRotMat, pLocalPos );
      worldPos[0] += pBodyPos[0];
      worldPos[1] += pBodyPos[1];
      worldPos[2] += pBodyPos[2];
      dMULTIPLY0_333( worldRotMat, pBodyRotMat, pLocalRotMat );
      //dQuaternion worldQuat;
      //dQfromR(worldQuat, worldRotMat); 
 
      dsSetColorAlpha(0.75, 0.75, 1.0, 0.5);
      dsDrawBox( worldPos, worldRotMat, lengths );
    }
  }
};
 
 
 
static std::list<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.7;     // 反発係数
      contact[i].surface.bounce_vel = 0.1; 
      contact[i].surface.soft_cfm = 0.001;  // 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, 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 ); // 衝突検出
    dWorldQuickStep( world, 0.01 );
    dJointGroupEmpty( contact_group );
  }
 
  // Shapeの表示
  std::list<Shape*>::iterator it = shapeList.begin();
  while (it != shapeList.end()) {
    (*it)->draw();
    ++it;
  }
}
 
static void command(int key)
{
  dReal x = dReal(rand()%10) / 10000.0;
  dReal y = dReal(rand()%10) / 10000.0;
  dReal z = dReal(rand()%10) / 10000.0 + 5.0;
  shapeList.push_front(new Sphere(world, space, x, y, z));
}
 
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 = command;
  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 = dHashSpaceCreate (0);
  contact_group = dJointGroupCreate(0);
 
  // 地面生成
  dCreatePlane(space, 0, 0, 1, 0);
 
  // シェイプ生成
  shapeList.push_back(new Hako(world, space));
 
 
  dsSimulationLoop(argc, argv, 320, 240, &fn);
 
  dJointGroupDestroy(contact_group);
  dWorldDestroy(world);
  dSpaceDestroy(space);
  return 0;
}