基本 球体転がり

摩擦あり

#include
#include

#define MAX_CONTACTS 4
static dWorldID world; //世界ID
static dSpaceID space;
static dJointGroupID contactgroup;

static dBodyID b_sphere; //球ID
static dGeomID g_sphere;
static dGeomID g_fixed; //ボックス固定用ID

static dReal radius = 0.5; //球の半径
static dReal f_box_sizes[3] = { 2.0, 2.0, 1.0 };//ボックスのサイズ

#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#endif

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);

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 0 )
{
for ( int i=0; i