so-zou.jp

demo_boxstack (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

#include "icosahedron_geom.h"

// 凸形状オブジェクト

// 立方体の面
dReal planes[] =
{
   1.0f,  0.0f,  0.0f, 0.25f,
   0.0f,  1.0f,  0.0f, 0.25f,
   0.0f,  0.0f,  1.0f, 0.25f,
  -1.0f,  0.0f,  0.0f, 0.25f,
   0.0f, -1.0f,  0.0f, 0.25f,
   0.0f,  0.0f, -1.0f, 0.25f,
};
const unsigned int planecount = 6;

// 立方体の点
dReal points[] =
{
   0.25f, 0.25f, 0.25f,     // 点0
  -0.25f, 0.25f, 0.25f,     // 点1

   0.25f, -0.25f, 0.25f,    // 点2
  -0.25f, -0.25f, 0.25f,    // 点3

   0.25f, 0.25f, -0.25f,    // 点4
  -0.25f, 0.25f, -0.25f,    // 点5

   0.25f, -0.25f, -0.25f,   // 点6
  -0.25f, -0.25f, -0.25f,   // 点7
};
const unsigned int pointcount = 8;

// 立方体のポリゴン (6つの四角形)
unsigned int polygons[] =
{
  4, 0, 2, 6, 4,  // +X
  4, 1, 0, 4, 5,  // +Y
  4, 0, 1, 3, 2,  // +Z
  4, 3, 1, 5, 7,  // -X
  4, 2, 3, 7, 6,  // -Y
  4, 5, 4, 6, 7,  // -Z
};

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

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

// 定数

#define NUM             100     // オブジェクトの最大数
#define DENSITY         ( 5.0 ) // すべてのオブジェクトの密度
#define GPB             3       // 1つのボディの最大のジオメトリ数
#define MAX_CONTACTS    8       // 1つのボディの最大の接点数
#define MAX_FEEDBACKNUM 20      // フィードバックの最大数

#define GRAVITY         REAL( 0.5 )
#define USE_GEOM_OFFSET 1       // 混成オブジェクトの重心をオフセットさせるか？

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

struct MyObject
{
  dBodyID body;            // ボディ
  dGeomID geom[ GPB ];     // ボディのジオメトリ
};


static int num = 0;        // シミュレーション内のオブジェクトの数
static int nextobj = 0;    // num == NUMとなったときに再利用される次のオブジェクト

static dWorldID world;
static dSpaceID space;
static MyObject obj[ NUM ];
static dJointGroupID contactgroup;

static int selected = -1;       // 選択されているオブジェクト
static int show_aabb = 0;       // ジオメトリのAABBを表示するか？
static int show_contacts = 0;   // 接触点を表示するか？
static int random_pos = 1;      // ランダムな位置からオブジェクトを落下させるか？
static int write_world = 0;     // 現在の状態をファイルに書き出すか？
static int show_body = 0;       // ボディを表示するか？


struct MyFeedback
{
  dJointFeedback fb;   // ジョイントのフィードバック
  bool first;          // 選択されているオブジェクトか？
};

static MyFeedback feedbacks[ MAX_FEEDBACKNUM ];

static int doFeedback = 0;      // フィードバックを表示するか？
static int fbnum = 0;           // フィードバックの数

• 力とトルクの取得 (dJointFeedback)

nearCallback

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

static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
  int i;
// if( o1->body && o2->body ) return;

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

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


  dContact contact[ MAX_CONTACTS ];   // up to MAX_CONTACTS contacts per box-box

  for( i = 0; i < MAX_CONTACTS; i++ )
  {
    contact[ i ].surface.mode = dContactBounce | dContactSoftCFM;
    contact[ i ].surface.mu = dInfinity;
    contact[ i ].surface.mu2 = 0;
    contact[ i ].surface.bounce = 0.1;
    contact[ i ].surface.bounce_vel = 0.1;
    contact[ i ].surface.soft_cfm = 0.01;
  }

  // 接触情報を作成する
  int numc = dCollide( o1, o2, MAX_CONTACTS, &contact[ 0 ].geom, sizeof( dContact ) );
  if( numc )
  {
    dMatrix3 RI;
    dRSetIdentity( RI );

    const dReal ss[ 3 ] = { 0.02, 0.02, 0.02 };

    for( i = 0; i < numc; i++ )
    {
      // 接触ジョイントを作成する
      dJointID c = dJointCreateContact( world, contactgroup, contact + i );

      dJointAttach( c, b1, b2 );


      if( show_contacts )
      {
        // 接触した位置に直方体を描画する
        dsDrawBox( contact[ i ].geom.pos, RI, ss );
      }

      if( doFeedback
       && ( b1 == obj[ selected ].body
         || b2 == obj[ selected ].body ) )
      {
        if( fbnum < MAX_FEEDBACKNUM )
        {
          feedbacks[ fbnum ].first = ( b1 == obj[ selected ].body );

          // 作成した接触ジョイントにフィードバックを設定する
          dJointSetFeedback( c, &feedbacks[ fbnum++ ].fb );
        }
        else
        {
          fbnum++;
        }
      }
    }
  }
}

• ボディ同士がジョイントによって接続されているか確認する (dAreConnectedExcluding)
• 接触面の定義 (dSurfaceParameters)
• 接触情報を作成する (dCollide)
• フィードバックの設定 (dJointSetFeedback)

start

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

static void start()
{
  dAllocateODEDataForThread( dAllocateMaskAll );


  // 視点を設定する
  static float xyz[ 3 ] = { 2.1640f, -1.3079f, 1.7600f };
  static float hpr[ 3 ] = { 125.5000f, -17.0000f, 0.0000f };
  dsSetViewpoint( xyz, hpr );


  printf( "To drop another object, press:\n" );
  printf( "   b for box.\n" );
  printf( "   s for sphere.\n" );
  printf( "   c for capsule.\n" );
  printf( "   y for cylinder.\n" );
  printf( "   v for a convex object.\n" );
  printf( "   x for a composite object.\n" );
  printf( "To select an object, press space.\n" );
  printf( "To disable the selected object, press d.\n" );
  printf( "To enable the selected object, press e.\n" );
  printf( "To dump transformation data for the selected object, press p.\n" );
  printf( "To toggle showing the geom AABBs, press a.\n" );
  printf( "To toggle showing the contact points, press t.\n" );
  printf( "To toggle dropping from random position/orientation, press r.\n" );
  printf( "To save the current state to 'state.dif', press 1.\n" );
  printf( "To show joint feedbacks of selected object, press f.\n" );
}

locase

// 小文字に変換する

char locase( char c )
{
  if( c >= 'A' && c <= 'Z' )
  {
    return c - ( 'a' - 'A' );
  }
  else
  {
    return c;
  }
}

command

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

static void command( int cmd )
{
  size_t i;     // オブジェクトを識別するための番号
  int j, k;
  dReal sides[ 3 ];
  dMass m;
  int setBody;  // ボディとの関連付け (dGeomSetBody) は不要か？

  cmd = locase( cmd );
  if( cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v' )
  {
    setBody = 0;  // ボディとの関連付け必要

    if( num < NUM )
    {
      i = num;
      num++;
    }
    else
    {
      i = nextobj;
      nextobj++;

      if( nextobj >= num )
      {
        nextobj = 0;
      }


      // iの位置にあるボディとジオメトリを破棄する
      dBodyDestroy( obj[ i ].body );

      for( k = 0; k < GPB; k++ )
      {
        if( obj[ i ].geom[ k ] )
        {
          dGeomDestroy( obj[ i ].geom[ k ] );
        }
      }

      // 破棄したオブジェクトのメモリを0でクリアする
      memset( &obj[ i ], 0, sizeof( obj[ i ] ) );
    }

    // ボディを作成する
    obj[ i ].body = dBodyCreate( world );
    for( k = 0; k < 3; k++ )
    {
      sides[ k ] = dRandReal() * 0.5 + 0.1;
    }

    dMatrix3 R;
    if( random_pos )
    {
      dBodySetPosition(
        obj[ i ].body,
        dRandReal() * 2 - 1,
        dRandReal() * 2 - 1,
        dRandReal() + 2
        );

      dRFromAxisAndAngle(
        R,
        dRandReal() * 2.0 - 1.0,
        dRandReal() * 2.0 - 1.0,
        dRandReal() * 2.0 - 1.0,
        dRandReal() * 10.0 - 5.0
        );
    }
    else
    {
      dReal maxheight = 0;

      for( k = 0; k < num; k++ )
      {
        const dReal *pos = dBodyGetPosition( obj[ k ].body );

        if( pos[ 2 ] > maxheight )
        {
          maxheight = pos[ 2 ];
        }
      }

      dBodySetPosition( obj[ i ].body, 0, 0, maxheight + 1 );

      dRSetIdentity( R );
//    dRFromAxisAndAngle( R, 0, 0, 1, /* dRandReal() * 10.0 - 5.0 */ 0 );
    }

    dBodySetRotation( obj[ i ].body, R );
    dBodySetData( obj[ i ].body, ( void* )i );

    if( cmd == 'b' )  // 直方体 (Box)
    {
      dMassSetBox( &m, DENSITY, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
      obj[ i ].geom[ 0 ] = dCreateBox( space, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
    }
    else if( cmd == 'c' )  // カプセル (Capsule)
    {
      sides[ 0 ] *= 0.5;

      dMassSetCapsule( &m, DENSITY, 3, sides[ 0 ], sides[ 1 ] );
      obj[ i ].geom[ 0 ] = dCreateCapsule( space, sides[ 0 ], sides[ 1 ] );
    }
    else if( cmd == 'v' )  // 凸形状オブジェクト (Convex Object)
    {
      dMassSetBox( &m, DENSITY, 0.25, 0.25, 0.25 );

#if 0
    obj[ i ].geom[ 0 ] = dCreateConvex(
      space,
      planes,
      planecount,
      points,
      pointcount,
      polygons
      );
#else
    obj[ i ].geom[ 0 ] = dCreateConvex(
      space,
      Sphere_planes,
      Sphere_planecount,
      Sphere_points,
      Sphere_pointcount,
      Sphere_polygons
      );
#endif
    }
    else if( cmd == 'y' )  // 円柱 (Cylinder)
    {
      dMassSetCylinder( &m, DENSITY, 3, sides[ 0 ], sides[ 1 ] );
      obj[ i ].geom[ 0 ] = dCreateCylinder( space, sides[ 0 ], sides[ 1 ] );
    }
    else if( cmd == 's' )  // 球 (Sphere)
    {
      sides[ 0 ] *= 0.5;

      dMassSetSphere( &m, DENSITY, sides[ 0 ] );
      obj[ i ].geom[ 0 ] = dCreateSphere( space, sides[ 0 ] );
    }
    else if( cmd == 'x' && USE_GEOM_OFFSET ) // (重心オフセット) 混成オブジェクト (Composite Object)
    {
      setBody = 1;  // ボディとの関連付け不要

      dMass m2;
      dMassSetZero( &m );

      dReal dpos[ GPB ][ 3 ];    // ジオメトリ間の位置のずれ
      dMatrix3 drot[ GPB ];

      // ランダムな位置を設定する
      for( j = 0; j < GPB; j++ )
      {
        for( k = 0; k < 3; k++ )
        {
          dpos[ j ][ k ] = dRandReal() * 0.3 - 0.15;
        }
      }

      // 総質量を求める
      for( k = 0; k < GPB; k++ )
      {
        if( k == 0 )  // 球 (Sphere)
        {
          dReal radius = dRandReal() * 0.25 + 0.05;

          obj[ i ].geom[ k ] = dCreateSphere( space, radius );
          dMassSetSphere( &m2, DENSITY, radius );
        }
        else if( k == 1 )  // 直方体 (Box)
        {
          obj[ i ].geom[ k ] = dCreateBox( space, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
          dMassSetBox( &m2, DENSITY, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
        }
        else  // カプセル (Capsule)
        {
          dReal radius = dRandReal() * 0.1 + 0.05;
          dReal length = dRandReal() * 1.0 + 0.1;

          obj[ i ].geom[ k ] = dCreateCapsule( space, radius, length );
          dMassSetCapsule( &m2, DENSITY, 3, radius, length );
        }


        dRFromAxisAndAngle(
          drot[ k ],
          dRandReal() * 2.0 - 1.0,
          dRandReal() * 2.0 - 1.0,
          dRandReal() * 2.0 - 1.0,
          dRandReal() * 10.0 - 5.0
          );

        dMassRotate( &m2, drot[ k ] );
        dMassTranslate( &m2, dpos[ k ][ 0 ], dpos[ k ][ 1 ], dpos[ k ][ 2 ] );

        // 総質量に加算する
        dMassAdd( &m, &m2 );
      }

      for( k = 0; k < GPB; k++ )
      {
        dGeomSetBody( obj[ i ].geom[ k ], obj[ i ].body );

        // ジオメトリの重心をオフセットさせる
        dGeomSetOffsetPosition(
          obj[ i ].geom[ k ],
          dpos[ k ][ 0 ] - m.c[ 0 ],
          dpos[ k ][ 1 ] - m.c[ 1 ],
          dpos[ k ][ 2 ] - m.c[ 2 ]
          );

        dGeomSetOffsetRotation( obj[ i ].geom[ k ], drot[ k ] );
      }

      dMassTranslate( &m, -m.c[ 0 ], -m.c[ 1 ], -m.c[ 2 ] );
      dBodySetMass( obj[ i ].body, &m );
    }
    else if( cmd == 'x' )  // 混成オブジェクト (Composite Object)
    {
      dGeomID g2[ GPB ];        // ジオメトリ
      dReal dpos[ GPB ][ 3 ];   // ジオメトリ間の位置のずれ

      dMass m2;
      dMassSetZero( &m );

      // ランダムな位置を設定する
      for( j = 0; j < GPB; j++ )
      {
        for( k = 0; k < 3; k++ )
        {
          dpos[ j ][ k ] = dRandReal() * 0.3 - 0.15;
        }
      }

      for( k = 0; k < GPB; k++ )
      {
        // ジオメトリ変換オブジェクトを作成する
        obj[ i ].geom[ k ] = dCreateGeomTransform( space );
        dGeomTransformSetCleanup( obj[ i ].geom[ k ], 1 );


        if( k == 0 )  // 球(Sphere)
        {
          dReal radius = dRandReal() * 0.25 + 0.05;
          g2[ k ] = dCreateSphere( 0, radius );
          dMassSetSphere( &m2, DENSITY, radius );
        }
        else if( k == 1 )  // 直方体(Box)
        {
          g2[ k ] = dCreateBox( 0, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
          dMassSetBox( &m2, DENSITY, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
        }
        else  // カプセル(Capsule)
        {
          dReal radius = dRandReal() * 0.1 + 0.05;
          dReal length = dRandReal() * 1.0 + 0.1;
          g2[ k ] = dCreateCapsule( 0, radius, length );
          dMassSetCapsule( &m2, DENSITY, 3, radius, length );
        }


        // ジオメトリ変換オブジェクトにジオメトリを格納する
        dGeomTransformSetGeom( obj[ i ].geom[ k ], g2[ k ] );


        dMatrix3 Rtx;
        dRFromAxisAndAngle(
          Rtx,
          dRandReal() * 2.0 - 1.0,
          dRandReal() * 2.0 - 1.0,
          dRandReal() * 2.0 - 1.0,
          dRandReal() * 10.0 - 5.0
          );

        // ジオメトリの位置と姿勢を設定する
        dGeomSetPosition( g2[ k ], dpos[ k ][ 0 ], dpos[ k ][ 1 ], dpos[ k ][ 2 ] );
        dGeomSetRotation( g2[ k ], Rtx );


        // 質量パラメータを回転させ移動する
        dMassRotate( &m2, Rtx );
        dMassTranslate( &m2, dpos[ k ][ 0 ], dpos[ k ][ 1 ], dpos[ k ][ 2 ] );

        // 総質量に加算する
        dMassAdd( &m, &m2 );
      }

      // 重心が ( 0, 0, 0 ) となるように、すべてのカプセル化されたオブジェクトを移動する
      for( k = 0; k < GPB; k++ )
      {
        dGeomSetPosition(
          g2[ k ],
          dpos[ k ][ 0 ] - m.c[ 0 ],
          dpos[ k ][ 1 ] - m.c[ 1 ],
          dpos[ k ][ 2 ] - m.c[ 2 ]
          );
      }

      dMassTranslate( &m, -m.c[ 0 ], -m.c[ 1 ], -m.c[ 2 ] );
    }

    // ボディとの関連付けが不要とされていないならば、関連付けを行う
    if( !setBody )
    {
      for( k = 0; k < GPB; k++ )
      {
        if( obj[ i ].geom[ k ] )
        {
          dGeomSetBody( obj[ i ].geom[ k ], obj[ i ].body );
        }
      }
    }

    dBodySetMass( obj[ i ].body, &m );
  }

  if( cmd == ' ' )
  {
    // 選択されているオブジェクトを変更する
    selected++;

    if( selected >= num )
    {
      selected = 0;
    }

    if( selected < 0 )
    {
      selected = 0;
    }
  }
  else if( cmd == 'd' && selected >= 0 && selected < num )
  {
    // 選択されているオブジェクトを無効にする
    dBodyDisable( obj[ selected ].body );
  }
  else if( cmd == 'e' && selected >= 0 && selected < num )
  {
    // 選択されているオブジェクトを有効にする
    dBodyEnable( obj[ selected ].body );
  }
  else if( cmd == 'a' )
  {
    show_aabb ^= 1;
  }
  else if( cmd == 't' )
  {
    show_contacts ^= 1;
  }
  else if( cmd == 'r' )
  {
    random_pos ^= 1;
  }
  else if( cmd == '1' )
  {
    write_world = 1;
  }
  else if( cmd == 'p' && selected >= 0 )
  {
    // 選択されているオブジェクトの位置と姿勢を表示する
    const dReal *pos = dGeomGetPosition( obj[ selected ].geom[ 0 ] );
    const dReal *rot = dGeomGetRotation( obj[ selected ].geom[ 0 ] );

    printf( "POSITION:\n\t[ %f, %f, %f ]\n\n", pos[ 0 ], pos[ 1 ], pos[ 2 ] );
    printf( "ROTATION:\n\t[ %f, %f, %f, %f ]\n\t[ %f, %f, %f, %f ]\n\t[ %f, %f, %f, %f ]\n\n",
      rot[ 0 ], rot[ 1 ], rot[ 2 ], rot[ 3 ],
      rot[ 4 ], rot[ 5 ], rot[ 6 ], rot[ 7 ],
      rot[ 8 ], rot[ 9 ], rot[ 10 ], rot[ 11 ]
      );
  }
  else if( cmd == 'f' && selected >= 0 && selected < num )
  {
    if( dBodyIsEnabled( obj[ selected ].body ) )
    {
      doFeedback = 1;
    }
  }
}

• ジオメトリの作成 (dCreate)
• 質量 (dMass)
• ジオメトリ変換オブジェクト
• ボディの有効化/無効化 (dBodyEnable、dBodyDisable)

drawGeom

// ジオメトリを描画する

void drawGeom( dGeomID g, const dReal *pos, const dReal *R, int show_aabb )
{
  int i;

  if( !g )
  {
    return;
  }


  if( !pos )
  {
    pos = dGeomGetPosition( g );
  }

  if( !R )
  {
    R = dGeomGetRotation( g );
  }


  // ジオメトリの種類を取得する
  int type = dGeomGetClass( g );

  if( type == dBoxClass )  // 直方体
  {
    dVector3 sides;
    dGeomBoxGetLengths( g, sides );

    dsDrawBox( pos, R, sides );
  }
  else if( type == dSphereClass )  // 球
  {
    dsDrawSphere( pos, R, dGeomSphereGetRadius( g ) );
  }
  else if( type == dCapsuleClass )  // カプセル
  {
    dReal radius, length;
    dGeomCapsuleGetParams( g, &radius, &length );

    dsDrawCapsule( pos, R, length, radius );
  }
  else if( type == dConvexClass )  // 凸形状
  {
#if 0
    dsDrawConvex(
      pos,
      R,
      planes,
      planecount,
      points,
      pointcount,
      polygons
    );
#else
    dsDrawConvex(
      pos,
      R,
      Sphere_planes,
      Sphere_planecount,
      Sphere_points,
      Sphere_pointcount,
      Sphere_polygons
    );
#endif
  }
  else if( type == dCylinderClass )  // 円柱
  {
    dReal radius, length;
    dGeomCylinderGetParams( g, &radius, &length );

    dsDrawCylinder( pos, R, length, radius );
  }
  else if( type == dGeomTransformClass )  // ジオメトリ変換オブジェクト
  {
    // ジオメトリ変換オブジェクトに格納されているジオメトリを取得する
    dGeomID g2 = dGeomTransformGetGeom( g );

    const dReal *pos2 = dGeomGetPosition( g2 );
    const dReal *R2 = dGeomGetRotation( g2 );

    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331( actual_pos, R, pos2 );

    actual_pos[ 0 ] += pos[ 0 ];
    actual_pos[ 1 ] += pos[ 1 ];
    actual_pos[ 2 ] += pos[ 2 ];
    dMULTIPLY0_333( actual_R, R, R2 );

    // ジオメトリ変換オブジェクトに格納されていたジオメトリで描画し直す
    drawGeom( g2, actual_pos, actual_R, 0 );
  }


  // ジオメトリgのボディを描画する
  if( show_body )
  {
    dBodyID body = dGeomGetBody( g );

    if( body )
    {
      const dReal *bodypos = dBodyGetPosition( body );
      const dReal *bodyr = dBodyGetRotation( body );

      dReal bodySides[ 3 ] = { 0.1, 0.1, 0.1 };  // 描画サイズ
      dsSetColorAlpha( 0, 1, 0, 1 );

      dsDrawBox( bodypos, bodyr, bodySides );
    }
  }

  // ジオメトリgの境界ボックスを描画する
  if( show_aabb )
  {
    // 境界ボックスを取得する
    dReal aabb[ 6 ];
    dGeomGetAABB( g, aabb );

    dVector3 bbpos;
    for( i = 0; i < 3; i++ )
    {
      bbpos[ i ] = 0.5 * ( aabb[ i * 2 ] + aabb[ i * 2 + 1 ] );
    }

    dVector3 bbsides;
    for( i = 0; i < 3; i++ )
    {
      bbsides[ i ] = aabb[ i * 2 + 1 ] - aabb[ i * 2 ];
    }

    dMatrix3 RI;
    dRSetIdentity( RI );

    dsSetColorAlpha( 1, 0, 0, 0.5 );
    dsDrawBox( bbpos, RI, bbsides );
  }
}

• ジオメトリの種類の取得 (dGeomGetClass)
• 境界ボックスの取得 (dGeomGetAABB)

simLoop

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

static void simLoop( int pause )
{
  dsSetColor( 0, 0, 2 );

  // 衝突するボディを検出する
  dSpaceCollide( space, 0, &nearCallback );

  if( !pause )
  {
    // シミュレーションのステップを進める
    dWorldQuickStep( world, 0.02 );
  }


  // ワールドの状態をファイルに出力する
  if( write_world )
  {
    FILE *f = fopen( "state.dif", "wt" );

    if( f )
    {
      dWorldExportDIF( world, f, "X" );
      fclose( f );
    }

    write_world = 0;
  }


  // フィードバックを表示する
  if( doFeedback )
  {
    if( fbnum>MAX_FEEDBACKNUM )
    {
      printf( "joint feedback buffer overflow!\n" );
    }
    else
    {
      dVector3 sum = { 0, 0, 0 };
      printf( "\n" );

      for( int i = 0; i < fbnum; i++ )
      {
        dReal *f =
          feedbacks[ i ].first ?
          feedbacks[ i ].fb.f1 :  // ジョイントがボディ1に作用する力
          feedbacks[ i ].fb.f2;   // ジョイントがボディ2に作用する力

        printf( "%f %f %f\n", f[ 0 ], f[ 1 ], f[ 2 ] );

        sum[ 0 ] += f[ 0 ];
        sum[ 1 ] += f[ 1 ];
        sum[ 2 ] += f[ 2 ];
      }

      printf( "Sum: %f %f %f\n", sum[ 0 ], sum[ 1 ], sum[ 2 ] );

      dMass m;
      dBodyGetMass( obj[ selected ].body, &m );

      printf( "Object G=%f\n", GRAVITY * m.mass );
    }

    doFeedback = 0;
    fbnum = 0;
  }

  // ジョイント グループを破棄する (すべての接触ジョイントを破棄する)
  dJointGroupEmpty( contactgroup );


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


  for( int i = 0; i < num; i++ )
  {
    for( int j = 0; j < GPB; j++ )
    {
      // 描画色を設定する
      if( i == selected )  // オブジェクトが選択されている
      {
        dsSetColor( 0, 0.7, 1 );
      }
      else if( ! dBodyIsEnabled( obj[ i ].body ) )  // ボディが無効状態
      {
        dsSetColor( 1, 0.8, 0 );
      }
      else
      {
        dsSetColor( 1, 1, 0 );
      }

      // ジオメトリを描画する
      drawGeom( obj[ i ].geom[ j ], 0, 0, show_aabb );
    }
  }
}

• ワールドのエクスポート (dWorldExportDIF)
• ジョイントのフィードバック
• ボディの有効/無効 (dBodyIsEnabled)

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();
  space = dHashSpaceCreate( 0 );
  contactgroup = dJointGroupCreate( 0 );

  dWorldSetGravity( world, 0, 0, -GRAVITY );
  dWorldSetCFM( world, 1e-5 );           // CFM
  dWorldSetAutoDisableFlag( world, 1 );  // 自動無効化を有効

#if 1
  dWorldSetAutoDisableAverageSamplesCount( world, 10 );
#endif

  dWorldSetLinearDamping( world, 0.00001 ); // 速度の減衰率
  dWorldSetAngularDamping( world, 0.005 );  // 角速度の減衰率
  dWorldSetMaxAngularSpeed( world, 200 );   // 角速度の最大値

  dWorldSetContactMaxCorrectingVel( world, 0.1 ); // 修正速度
  dWorldSetContactSurfaceLayer( world, 0.001 );   // 表面層の厚さ


  dCreatePlane( space, 0, 0, 1, 0 );

  memset( obj, 0, sizeof( obj ) );


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


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

  dCloseODE();

  return 0;
}

• ワールドの減衰 (damping)
• 接触時の挙動

• 導入
  • ライブラリのビルド
  • 初期化と終了処理
• 数学関数
  • データ型
• オブジェクト
  • ボディ
    • 質量
  • ジオメトリ
  • ワールド
  • スペース
• ジョイント
  • ジョイント グループ
  • 接触ジョイント
    • ERP/CFM
  • 角度モーター
  • 固定ジョイント
• 衝突検出
  • サンプルコード
  • 衝突検出ライブラリ
• デモプログラム
  • demo_boxstack
  • demo_chain1
  • demo_cylvssphere
  • demo_hinge
• Drawstuff
  • シミュレーション
  • 描画処理
  • メッセージ
• エラー処理
  • エラーメッセージ
• Ode.NET

• お問い合わせ
• プライバシーポリシー
• サイトマップ