コマを回す
文字どおり、コマ(独楽)をODE上で回してみました。
ジャイロ効果でコマが倒れません。
ジャイロ効果でコマが倒れません。
ODEでは、こういうのもシミュレーションできるみたいです。
さすがに、シミュレーションなので本物とはちょっと動作が違う感じはしますが。。。
さすがに、シミュレーションなので本物とはちょっと動作が違う感じはしますが。。。
demoプログラムにも demo_gyroscopic.cpp があります。
説明
キー'a'を押すと、コマが回転します。
キー'b'を押すと、コマを倒そうとする力を与えます。
キー'b'を押すと、コマを倒そうとする力を与えます。
プログラムを実行すると、コマが出現します。
この時点では、コマは垂直になっているので倒れません。
キー'a'を何回か押して、コマを回転させます。
次にキー'b'を押して、コマに横方向の力を与えてみてください。
この時点では、コマは垂直になっているので倒れません。
キー'a'を何回か押して、コマを回転させます。
次にキー'b'を押して、コマに横方向の力を与えてみてください。
コマを回さずにキー'b'を押すと、当然ながらそのまま倒れます。
コマの回転速度は10程度まで上げると、安定して倒れません。
コマの回転速度は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;
}
添付ファイル