ODE講座17:重心位置の移動(オフセット)

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

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

  • ジオメトリの位置を移動させるAPI
void dGeomSetOffsetPosition (dGeomID, dReal x, dReal y, dReal z);
dGeomIDの位置をbodyの位置から(x, y, z)だけずらして設定する。このAPIを使用する前にジオメトリはボディと関連づけられていなければいけない。つまり、dGeomSetBodyが先に呼び出されている必要あり。

では、サンプルプログラムとして上図の起き上がり小法師を紹介します。ソースコードはここからダウンロードしてください。この例では、小法師の胴体に相当するの球(質量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キーを押すとx軸の正方向に100Nの力を加える
dBodyAddTorque(head.body, 100, 0, 0); break;
case ‘j’: // fキーを押すとx軸の正方向に-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]

おしまい。

コメントを残す

メールアドレスが公開されることはありません。