ODE/転がり摩擦 - bambooflow Note

転がり摩擦

ODEは、静止摩擦はあるようですが、転がり摩擦はないみたいです。
なので、平面でボールを転がすとどこまでも転がっていってしまいます。

そこで、転がり摩擦を簡易的に作ってみました。
計算が正しいかどうかは知りません。 (計算は苦手なので。)
もっと素晴らしい方法があるかもしれません。

計算

どこかのページで、転がり摩擦は次の式であると書いてあったので、これを使ってみます。

 F = f * (W / r) 
      F : 転がり摩擦[N] 
      f : 転がり摩擦係数[mm]
      r : 物体の半径[mm]

今回は、球体のみ考えます。円柱とかはとりあえず置いておきます。

ODEで転がり摩擦をあたえるには、dBodyAddForceもしくはdBodyAddTorqueがよさそうに思います。
ここではAddTorqueを使ってみました。

rolling_function関数で、剛体に転がり摩擦をAddTorqueにて与えています。
タイミングは、ボールが物体と衝突しているときのみでよいので、nearCallback関数内で呼ぶようにしています。
物体の固定からの変更点は、rollong_function関数の追加と、nearCallback関数の修正のみです。

まぁ、永遠に転がり続けるのが気に入らなくて作ってみただけなので、精度とかはまったく考えてません。
あくまで参考まで。

全ソースコード

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
 
#define MAX_CONTACTS 4
static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;
 
static dBodyID b_sphere;
static dGeomID g_sphere;
static dGeomID g_fixed;
static dReal  radius = 0.5;
static dReal  f_box_sizes[3] = { 2.0, 2.0, 1.0 };
 
#ifdef dDOUBLE
#  define dsDrawSphere dsDrawSphereD
#  define dsDrawBox dsDrawBoxD
#endif
 
 
//
// 簡易 転がり摩擦計算関数
//  [引数]
//    o    : ジオメトリID
//    coef : 転がり摩擦係数
// 
// [計算方法]
//    F = f * (W / r)
//       F : 転がり摩擦[N]
//       f : 転がり摩擦係数
//       r : 物体の半径
//
void rolling_function( dGeomID o, dReal coef, dContact *c )
{
    if (o && dGeomGetClass(o) == dSphereClass) {
        dBodyID b = dGeomGetBody(o);
        if (!b) return;
        dMass m;
        dBodyGetMass( b, &m );
        dReal* normal = c->geom.normal;      // 垂直抗力ベクトル
 
        dReal w = m.mass*(normal[2]*9.8);    // 質量, (memo:角度差cosΘ = (normal[0]*0.0 + normal[1]*0.0 + normal[2]*1.0))
        dReal r = dGeomSphereGetRadius( o ); // 半径
        dReal F = coef * (w / r );           // 転がり摩擦(力)
        dReal T = F * r;                     // 転がり摩擦のトルク?
 
        const dReal* av = dBodyGetAngularVel(b);
        /*
        const dReal* lv = dBodyGetLinearVel(b);
        printf( "sphre w=%.3f, r=%.3f, F=%.3f, T=%.3f", w, r, F, T );
        printf( ", av=(%.3f,%.3f,%.3f)", av[0], av[1], av[2] );
        printf( ", lv=(%.3f,%.3f,%.3f)", lv[0], lv[1], lv[2] );
        printf( "\n" );
        */
        dReal a_speed = sqrt(av[0]*av[0] + av[1]*av[1] + av[2]*av[2]); // 回転スピード
        if (a_speed > 1.0e-5) {
            dReal n_av[3] = { av[0]/a_speed, av[1]/a_speed, av[2]/a_speed }; // 回転方向の正規化
            dBodyAddTorque( b, -n_av[0]*T, -n_av[1]*T, -n_av[2]*T ); // 転がり摩擦をトルクとしてあたえる
        }
        else {
            dBodySetAngularVel( b, 0.0f, 0.0f, 0.0f ); // 停止
        }
    }
}
 
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;
        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 = 0.00001;
    }
 
    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 );
 
            // 転がり摩擦
            rolling_function( o1, 0.02, contact+i );
            rolling_function( o2, 0.02, contact+i );
        }
    }
}
 
// start simulation - set viewpoint
static void start()
{
    static float xyz[3] = { 0.0f, 7.0f, 5.0f };
    static float hpr[3] = { -125.f, -15.f, 0.f };
 
    dsSetViewpoint( xyz, hpr );
}
 
// 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 );
    dsDrawSphere( dBodyGetPosition( b_sphere ), dBodyGetRotation( b_sphere ), radius );
    dsSetColor( 0.5, 0.5, 0.5 );
    dsDrawBox( dGeomGetPosition( g_fixed ), dGeomGetRotation( g_fixed ), f_box_sizes );
    /*
    const dReal* pos = dBodyGetPosition( b_sphere );
    printf( "x=%.5f, y=%.5f, z=%.5f\n", pos[0], pos[1], pos[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 = 0;
    fn.stop    = 0;
    fn.path_to_textures = "../../drawstuff/textures";
 
    dInitODE();
 
    // 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 Sphere
    b_sphere = dBodyCreate( world );
    dReal pos[3] = { 0.0, 0.0, 5.0 };
    dBodySetPosition( b_sphere, pos[0], pos[1], pos[2] );
    dMass sphere_mass;
    dMassSetSphere( &sphere_mass, 1.0, radius );
    dBodySetMass( b_sphere, &sphere_mass );
    g_sphere = dCreateSphere( space, radius );
    dGeomSetBody( g_sphere, b_sphere );
 
    // creating Fixed Box
    g_fixed = dCreateBox( space, f_box_sizes[0], f_box_sizes[1], f_box_sizes[2] );
    dMatrix3 R;
    //dRFromAxisAndAngle( R, 0, 1, 0, -0.30 );
    dRFromAxisAndAngle( R, 0, 1, 0.5, -0.30 );
    dGeomSetRotation( g_fixed, R);              // 方向指定
    dGeomSetPosition( g_fixed, 0.0, 0.0, 0.5 ); // 位置指定
 
    // starting simulation
    dsSimulationLoop( argc, argv, 320, 240, &fn );
 
    dJointGroupDestroy( contactgroup );
    dSpaceDestroy( space );
    dWorldDestroy( world );
    return 0;
}
 

以上