demo_chain1 (ODE 0.11.1)

#include <stdio.h>
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"
#include "texturepath.h"

#ifdef _MSC_VER
  #pragma warning(disable:4244 4305)  // 型変換に伴う精度喪失の警告を抑制する (VC++)
#endif

// 適切な描画関数を選択する

#ifdef dDOUBLE
  #define dsDrawBox dsDrawBoxD
  #define dsDrawSphere dsDrawSphereD
  #define dsDrawCylinder dsDrawCylinderD
  #define dsDrawCapsule dsDrawCapsuleD
#endif

// 定数

#define NUM 10           // ボックスの数
#define SIDE (0.2)       // ボックスの側面の長さ
#define MASS (1.0)       // ボックスの質量
#define RADIUS (0.1732f) // 球の半径

// 動力学と衝突検出のオブジェクト

static dWorldID world;
static dSpaceID space;

static dBodyID body[ NUM ];
static dJointID joint[ NUM - 1 ];
static dJointGroupID contactgroup;
static dGeomID sphere[ NUM ];

nearCallback

// スペースの2つのオブジェクトが衝突する可能性があるときに、
// dSpaceCollideによって呼び出される

static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
  dBodyID b1, b2;
  dContact contact;

  b1 = dGeomGetBody( o1 );
  b2 = dGeomGetBody( o2 );

  // 2つのボディがジョイントによって接続されているならば、何もせずに関数を抜ける
  if( b1 && b2 && dAreConnected( b1, b2 ) )
  {
    return;
  }

  contact.surface.mode = 0;
  contact.surface.mu = 0.1;
  contact.surface.mu2 = 0;

  if( dCollide( o1, o2, 1, &contact.geom, sizeof( dContactGeom ) ) )
  {
    // 接触ジョイントを作成する
    dJointID c = dJointCreateContact( world, contactgroup, &contact );

    dJointAttach( c, b1, b2 );
  }
}

start

// シミュレーションを開始する

static void start()
{
  static float xyz[ 3 ] = { 2.1640f,  - 1.3079f, 1.7600f };
  static float hpr[ 3 ] = { 125.5000f,  - 17.0000f, 0.0000f };

  dAllocateODEDataForThread( dAllocateMaskAll );

  // 視点を設定する
  dsSetViewpoint( xyz, hpr );
}

simLoop

// シミュレーション ループ

static void simLoop( int pause )
{
  int i;

  if( !pause )
  {
    static double angle = 0;
    angle += 0.05;

    // ボディに力を加える
    dBodyAddForce(
      body[ NUM - 1 ],                   // 対象とするボディ
      0, 0, 1.5 * ( sin( angle ) + 1.0 ) // 力ベクトル
      );

    dSpaceCollide( space, 0, &nearCallback );
    dWorldStep( world, 0.05 );

    // すべての接触ジョイントを削除する
    dJointGroupEmpty( contactgroup );
  }


  dsSetColor( 1, 1, 0 );
  dsSetTexture( DS_WOOD );

  for( i = 0; i < NUM; i++ )
  {
    // 球を描画する
    dsDrawSphere(
      dBodyGetPosition( body[ i ] ),
      dBodyGetRotation( body[ i ] ),
      RADIUS
      );
  }
}

main

int main( int argc, char **argv )
{
  int i;
  dReal k;
  dMass m;

  // drawstuffの設定をする
  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;


  dInitODE2( 0 );

  // ワールドを作成する
  world = dWorldCreate();
  space = dHashSpaceCreate( 0 );

  contactgroup = dJointGroupCreate( 1000000 );

  dWorldSetGravity( world, 0, 0,  - 0.5 );
  dCreatePlane( space, 0, 0, 1, 0 );


  for( i = 0; i < NUM; i++ )
  {
    // ボディを作成する
    body[ i ] = dBodyCreate( world );

    k = i * SIDE;
    dBodySetPosition( body[ i ], k, k, k + 0.4 );
    dMassSetBox( &m, 1, SIDE, SIDE, SIDE );
    dMassAdjust( &m, MASS );
    dBodySetMass( body[ i ], &m );

    // 球のジオメトリを作成する
    sphere[ i ] = dCreateSphere( space, RADIUS );

    // ジオメトリをボディと関連付ける
    dGeomSetBody( sphere[ i ], body[ i ] );
  }

  for( i = 0; i < ( NUM - 1 ); i++ )
  {
    // ボール ジョイントを作成する
    joint[ i ] = dJointCreateBall( world, 0 );

    dJointAttach( joint[ i ], body[ i ], body[ i + 1 ] );

    k = ( i + 0.5 ) * SIDE;
    dJointSetBallAnchor( joint[ i ], k, k, k + 0.4 );
  }

  // シミュレーションを開始する
  dsSimulationLoop( argc, argv, 352, 288, &fn );

  dJointGroupDestroy( contactgroup );
  dSpaceDestroy( space );
  dWorldDestroy( world );
  dCloseODE();

  return 0;
}