demo_hinge (ODE 0.11.1)

#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
#endif

// 定数
#define SIDE( 0.5f )  // side length of a box
#define MASS( 1.0 )   // mass of a box


// 動力学と衝突検出のオブジェクト
static dWorldID world;
static dBodyID body[ 2 ];  // 2つのボディ
static dJointID hinge;     // ヒンジジョイント


// 誤差 (キーボードからの入力によって設定される)
static int occasional_error = 0;

start

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

static void start()
{
  dAllocateODEDataForThread( dAllocateMaskAll );


  // 視点を設定する
  static float xyz[ 3 ] = { 1.0382f, -1.0811f, 1.4700f };
  static float hpr[ 3 ] = { 135.0000f, -19.5000f, 0.0000f };
  dsSetViewpoint( xyz, hpr );


  printf( "Press 'e' to start/stop occasional error.\n" );
}

command

// キー押下時に呼び出される

static void command( int cmd )
{
  if( cmd == 'e' || cmd == 'E' )
  {
    occasional_error ^= 1;
  }
}

simLoop

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

static void simLoop( int pause )
{
  if( !pause )
  {
    const dReal kd = -0.3;  // angular damping constant

    // 振動するトルクをボディに加える (それは回転運動を弱める働きもする)
    static dReal a = 0;
    const dReal *w = dBodyGetAngularVel( body[ 0 ] );

    dBodyAddTorque(
      body[ 0 ],
      kd * w[ 0 ],
      kd * w[ 1 ] + 0.1 * cos( a ),
      kd * w[ 2 ] + 0.1 * sin( a )
      );


    // シミュレーションのステップを進める
    dWorldStep( world, 0.05 );
    a += 0.01;


    // 意図的な誤差を発生させるためにボディを動かす
    if( occasional_error )
    {
      static int count = 0;

      if( ( count % 20 ) == 0 )
      {
        // ボディ0の姿勢をランダムに調整する
        const dReal *R1;
        dMatrix3 R2;
        dMatrix3 R3;

        R1 = dBodyGetRotation( body[ 0 ] );
        dRFromAxisAndAngle(
          R2,
          dRandReal() - 0.5,
          dRandReal() - 0.5,
          dRandReal() - 0.5,
          dRandReal() - 0.5
          );
        dMultiply0( R3, R1, R2, 3, 3, 3 );

        dBodySetRotation( body[ 0 ], R3 );


        // ボディ0の位置をランダムに調整する
        const dReal *pos = dBodyGetPosition( body[ 0 ] );

        dBodySetPosition(
          body[ 0 ],
          pos[ 0 ] + 0.2 * ( dRandReal() - 0.5 ),
          pos[ 1 ] + 0.2 * ( dRandReal() - 0.5 ),
          pos[ 2 ] + 0.2 * ( dRandReal() - 0.5 )
          );
      }

      count++;
    }
  }


  dReal sides1[ 3 ] = { SIDE, SIDE, SIDE };
  dReal sides2[ 3 ] = { SIDE, SIDE, SIDE * 0.8f };
  dsSetTexture( DS_WOOD );

  // ボディ0を描画する
  dsSetColor( 1, 1, 0 );
  dsDrawBox(
    dBodyGetPosition( body[ 0 ] ),
    dBodyGetRotation( body[ 0 ] ),
    sides1
    );

  // ボディ1を描画する
  dsSetColor( 0, 1, 1 );
  dsDrawBox(
    dBodyGetPosition( body[ 1 ] ),
    dBodyGetRotation( body[ 1 ] ),
    sides2
    );
}

main

int main( int argc, char **argv )
{
  // drawstuffの設定をする
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;


  dInitODE2( 0 );

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


  dMass m;
  dMassSetBox( &m, 1, SIDE, SIDE, SIDE );
  dMassAdjust( &m, MASS );

  dQuaternion q;
  dQFromAxisAndAngle( q, 1, 1, 0, 0.25 * M_PI );

  // ボディ0を作成する
  body[ 0 ] = dBodyCreate( world );
  dBodySetMass( body[ 0 ], &m );
  dBodySetPosition( body[ 0 ], 0.5 * SIDE, 0.5 * SIDE, 1 );
  dBodySetQuaternion( body[ 0 ], q );

  // ボディ1を作成する
  body[ 1 ] = dBodyCreate( world );
  dBodySetMass( body[ 1 ], &m );
  dBodySetPosition( body[ 1 ], -0.5 * SIDE, -0.5 * SIDE, 1 );
  dBodySetQuaternion( body[ 1 ], q );


  // ヒンジジョイントを作成する
  hinge = dJointCreateHinge( world, 0 );
  dJointAttach( hinge, body[ 0 ], body[ 1 ] );

  dJointSetHingeAnchor( hinge, 0, 0, 1 );
  dJointSetHingeAxis( hinge, 1, -1, 1.41421356 );


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


  dWorldDestroy( world );
  dCloseODE();

  return 0;
}