ODE講座17：重心位置の移動（オフセット）

ゲーム開発やロボットの研究者にも広く使われているオープンソースの物理計算エンジンODE(Open Dynamics  Engine、オープン ダイナミクスエンジン）を学ぶODE講座の第１７回目です。

今回は重心の位置を移動する方法について説明します。dGeomSetOffsetPositionというAPIを使うと重心の位置をずらして設定することができます。初期の重心位置からずれした量をオフセット(offset)とよびます。

• ジオメトリの位置を移動させるAPI

では、サンプルプログラムとして上図の起き上がり小法師を紹介します。ソースコードはここからダウンロードしてください。この例では、小法師の胴体に相当するの球（質量10kg、半径0.4m)の重心を球の中心より0.4m下に設定し、頭に相当する球の質量は1gしかないので、この小法師は必ず起き上がってくれます。具体的には、dBodySetPositionで重心位置を球の中心から半径分(0.4m)下にずらして設定し、dGeomSetOffsetPositionでそのずらした量（オフセット,ここではdz）を設定しています。

キーボードからfキーやjキーを押すと、 起き上がり小法師へ左右方向へ100Nの力が加えられますが、復元力があるので起き上がるようすを確認できます。

[code]
// koboshi2.cpp by Kosei Demura 2007-1-20
#include
#include

#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#endif

#define MAX_CONTACTS 4

static dWorldID world;
static dSpaceID space;
static dGeomID ground;
static dJointGroupID contactgroup;
dsFunctions fn;

typedef struct {
dBodyID body;
dGeomID geom;
dReal radius;
dReal length;
dReal mass;
} MyLink;

MyLink head, torso; // だるまの頭、胴体
dJointID joint;　　　　　// 頭と胴体の固定用ジョイント

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i;

// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = 1.0; // 摩擦係数
contact[i].surface.bounce = 0.01; // 跳ね返り係数
contact[i].surface.bounce_vel = 0.01; // 跳ね返りの最小速度
contact[i].surface.soft_cfm = 0.00001;
}
if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
for (i=0; i
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
}
}
}

static void drawSphere(dGeomID g)
{
if (!g) return;
const dReal *pos = dGeomGetPosition(g);
const dReal *R = dGeomGetRotation(g);

dsDrawSphere(pos,R,dGeomSphereGetRadius(g));
}

static void simLoop (int pause)
{
dSpaceCollide(space,0,&nearCallback);
dWorldStep(world,0.01);
dJointGroupEmpty(contactgroup);

dsSetColor(1.0,0.0,0.0);
drawSphere(torso.geom);
dsSetColor(1.0, 0.0, 0.0);
drawSphere(head.geom);
}

void start()
{
static float xyz[3] = { 3.5,0.0,1.0};
static float hpr[3] = {-180.0,0.0,0.0};
dsSetSphereQuality(3);
dsSetViewpoint(xyz,hpr);
}

// Create an object
void createDaruma()
{
dMass m,m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 0.4;
dReal dx = 0.0, dy = 0.0, dz = 0.4; // オフセット[m]

// 胴体：重心位置を移動して設定
torso.body = dBodyCreate(world);
torso.radius = 0.4;　　// 半径
torso.mass = 10.0;　// 質量

dMassSetZero(&m);
dMassSetSphereTotal(&m,torso.mass,torso.radius);
dBodySetMass(torso.body, &m);

torso.geom = dCreateSphere(space,torso.radius);
dGeomSetBody(torso.geom,torso.body);
// 胴体（球）ボディの重心位置を、球の半径分下に設定する。
dBodySetPosition(torso.body, x0, y0, z0 – dz);
// このままでは、胴体のジオメトリ（形状）が地面に埋まるため、球の半径分上に設定する。この移動により球の重心が半径分下に移動し、胴体が復元力を持つ。
dGeomSetOffsetPosition(torso.geom, 0, 0, dz);

// 頭
head.body = dBodyCreate(world);
head.radius = 0.3;
head.mass = 0.001;

dMassSetZero(&m1);
dMassSetSphereTotal(&m1,head.mass, head.radius);
dBodySetMass(head.body,&m1);

head.geom = dCreateSphere(space,head.radius);
dGeomSetBody(head.geom,head.body);
dGeomSetPosition(head.geom, x0, y0, z0 + torso.radius + head.radius);

// 固定ジョイント　（頭と胴体固定用）
joint = dJointCreateFixed(world, 0);
dJointAttach(joint, torso.body,head.body);
dJointSetFixed(joint);
}

void command(int cmd)
{
switch(cmd) {
case ‘f’:　// fキーを押すとｘ軸の正方向に100Nの力を加える
dBodyAddTorque(head.body, 100, 0, 0); break;
case ‘j’:　// fキーを押すとｘ軸の正方向に-100Nの力を加える
dBodyAddTorque(head.body, -100, 0, 0); break;
}
}

void setDrawStuff() {
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = NULL;
fn.path_to_textures = “../../drawstuff/textures”;
}

int main(int argc, char **argv)
{
setDrawStuff(); // 描画関数のセット
world = dWorldCreate();　// 　動力学計算用ワールドの生成
space = dHashSpaceCreate(0);　// 　衝突計算用スペースの生成
contactgroup = dJointGroupCreate(0);　// 接触点グループの生成
dWorldSetGravity(world,0,0,-9.8);　// 重力加速度の設定
ground = dCreatePlane(space,0,0,1,0);　// 地面用平面ジオメトリの生成
createDaruma();　// 小法師の生成
dsSimulationLoop (argc,argv,640,480,&fn);　// シミュレーションループ
dSpaceDestroy(space); // スペースの破壊
dWorldDestroy (world); 　// ワールドの破壊
return 0;
}

[/code]

おしまい。