ODE/コマを回す - bambooflow Note

コマを回す

文字どおり、コマ(独楽)をODE上で回してみました。
ジャイロ効果でコマが倒れません。

ODEでは、こういうのもシミュレーションできるみたいです。
さすがに、シミュレーションなので本物とはちょっと動作が違う感じはしますが。。。

demoプログラムにも demo_gyroscopic.cpp があります。

説明

キー'a'を押すと、コマが回転します。
キー'b'を押すと、コマを倒そうとする力を与えます。

プログラムを実行すると、コマが出現します。
この時点では、コマは垂直になっているので倒れません。
キー'a'を何回か押して、コマを回転させます。
次にキー'b'を押して、コマに横方向の力を与えてみてください。

コマを回さずにキー'b'を押すと、当然ながらそのまま倒れます。
コマの回転速度は10程度まで上げると、安定して倒れません。

全ソースコード

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
 
#define MAX_CONTACTS 4
static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;
 
// コマ
static dBodyID body_koma;          // Body:コマ本体
static dGeomID geom_body;          // Geom:コマの胴体部分
static dGeomID geom_axis;          // Geom:軸
static dReal   body_mass = 10.0;   // 質量
static dReal   body_radius = 3.0;  // 胴体の半径
static dReal   body_length = 0.5;  // 胴体の高さ
static dReal   axis_radius = 0.1;  // 軸の半径
static dReal   axis_length = 4.0;  // 軸の長さ
 
#ifdef dDOUBLE
#  define dsDrawCylinder dsDrawCylinderD
#  define dsDrawCapsule dsDrawCapsuleD
#endif
 
 
static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
    dBodyID b1 = dGeomGetBody( o1 );
    dBodyID b2 = dGeomGetBody( o2 );
 
    if ( b1 && b2 && dAreConnectedExcluding( b1, b2, dJointTypeContact ) )
        return;
 
    dContact contact[MAX_CONTACTS];
    for ( int i=0; i<MAX_CONTACTS; i++ )
    {
        contact[i].surface.mode
            = dContactBounce | dContactSoftERP | dContactSoftCFM | dContactApprox1
            | dContactSlip1 | dContactSlip2;
        //contact[i].surface.mode = dContactBounce | dContactApprox1;
        contact[i].surface.mu = 1.0; // dInfinity;
        contact[i].surface.bounce = 0.5;
        contact[i].surface.soft_erp = 0.2;
        contact[i].surface.soft_cfm = 1.0e-5;
        contact[i].surface.slip1 = 0.01;
        contact[i].surface.slip2 = 0.01;
    }
 
    int numc = dCollide( o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof( dContact ) ); 
    if ( numc > 0 )
    {
        for ( int i=0; i<numc; i++ )
        {
            dJointID c = dJointCreateContact( world, contactgroup, contact+i );
            dJointAttach( c, b1, b2 );
        }
    }
}
 
// start simulation - set viewpoint
static void start()
{
    static float xyz[3] = { 0.0f, 8.0f, 5.0f };
    static float hpr[3] = { -90.f, -20.f, 0.f };
 
    //dAllocateODEDataForThread(dAllocateMaskAll);
 
    dsSetViewpoint( xyz, hpr );
}
 
static void command( int cmd )
{
    if (cmd == 'a') { // コマを回転させる
        printf( "a pushed\n" );
        dBodyAddTorque( body_koma, 0.0, 0.0, 1000.0 );
        //const dReal *rot = dBodyGetRotation( body_koma );
        //printf( "a pushed %f, %f, %f\n", rot[0], rot[1], rot[2] );
    }
    else if (cmd == 'b') { // コマを倒す力をあたえる
        printf( "b pushed\n" );
        dBodyAddTorque( body_koma, 1000.0, 0.0, 0.0 );
    }
}
 
// simulation loop
static void simLoop( int pause )
{
    dSpaceCollide( space, 0, &nearCallback );
 
    if (!pause)
    {
        dWorldStep( world, 0.005 );
    }
 
    dJointGroupEmpty( contactgroup );
 
    dsSetColor( 1.0, 1.0, 0.0 );
    dsDrawCylinder( dGeomGetPosition( geom_body ), dGeomGetRotation( geom_body ), body_length, body_radius );
    dsSetColor( 0.5, 0.5, 0.5 );
    dsDrawCapsule( dGeomGetPosition( geom_axis ), dGeomGetRotation( geom_axis ), axis_length, axis_radius );
    /*
    const dReal* pos = dBodyGetPosition( b_sphere );
    printf( "x=%.5f, y=%.5f, z=%.5f\n", pos[0], pos[1], pos[2] );
    */
    const dReal *vel = dBodyGetAngularVel( body_koma );
    printf( "v0=%.5f, v1=%.5f, v2=%.5f\n", vel[0], vel[1], vel[2] );
}
 
 
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 = "../../drawstuff/textures";
 
    dInitODE2(0);
 
    // creating world
    world = dWorldCreate();
    space = dHashSpaceCreate( 0 );
    contactgroup = dJointGroupCreate( 0 );
    dCreatePlane( space, 0, 0, 1, 0 );
    dWorldSetGravity( world, 0.0, 0.0, -9.8 );
    dWorldSetERP( world, 0.8 );
    dWorldSetCFM( world, 0.00001 );
 
    // creating Koma
    body_koma = dBodyCreate( world );
    dReal pos[3] = { 0.0, 0.0, 5.0 };
    dBodySetPosition( body_koma, pos[0], pos[1], pos[2] );
    // 胴(重り）設定
    dMass m;
    dMassSetCylinder( &m, 1.0, 1, body_radius, body_length );
    dMassAdjust( &m, body_mass );
    dBodySetMass( body_koma, &m );
    geom_body = dCreateCylinder( space, body_radius, body_length );
    dGeomSetBody( geom_body, body_koma );
    // 軸設定
    geom_axis = dCreateCapsule( space, axis_radius, axis_length );
    dGeomSetBody( geom_axis, body_koma );
 
    // starting simulation
    dsSimulationLoop( argc, argv, 320, 240, &fn );
 
    dJointGroupDestroy( contactgroup );
    dSpaceDestroy( space );
    dWorldDestroy( world );
    return 0;
}