ODE(Open Dynamics Engine)講座の１４回目です。今回は重心の位置を移動する方法について説明します。
AKさんから重心の移動についてご質問があったので回答がてら起き上がり小法師（こぼし）のプログラムを作りました．民主党の前原前代表に配られた起き上がり小法師が倒れたままだった事件はまだ記憶に新しいですね．
さて，ODEにはdMassTranslateというAPIがありますが，これを使うだけでは重心を移動できません．ODE User Guideの10.7.7 Geometry Transform Classに” for example, you can offset the center of a sphere, or rotate a cylinder so that its axis is something other than the default.”という記述があるようにGeometry Transformオブジェクトと一緒に使わなければいけません．
具体的にはGeometry Transformオブジェクト（例では小法師の胴体）にカプセル化(encapsulate)される球オブジェクト（例では頭）の重心をGeometry Transformオブジェクトの重心からずらして設定し，Geometry Transformの位置もそのオフセットを打ち消すように設定しなければなりません．この辺の詳しい説明はODEのメーリングリスト（英語）で5月末から議論されています．
この例では， 小法師の胴体に相当する球の質量を10kg，半径を0.4m，重心を球の中心より0.4m下に設定しています．小法師の頭に相当する球の質量は1gに設定しています．j, f キーを押すと小法師を左右に転倒させるモーメントが働きますが，この小法師なら必ず起き上がってくれます．重心のオフセットをいろいろ変えて動作を確認してみてください．
なお，メーリングリストを読むとODEの次期バージョン0.06のRC2ではdGeomSetOffsetPositionが使え，もっと簡単に重心のオフセットを設定できるようなことが書かれていました．それについてはまだ試していません．
// koboshi.c by Kosei Demura 2006-6-4
// My web site is http://demura.net
// This program uses the Open Dynamics Engine (ODE) by Russell Smith.
// The ODE web site is http://ode.org/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#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 trans_ball; // Geometry Transformオブジェクト
MyLink ball;
dGeomID encap_ball_geom; // Geometry Transformオブジェクトに格納されるオブジェクト
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<MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = 5;
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<numc; i++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
}
}
}
static void drawGeom(dGeomID g, const dReal *pos, const dReal *R)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition(g);
if (!R) R = dGeomGetRotation(g);
int type = dGeomGetClass(g);
if (type == dCCylinderClass) {
dReal radius,length;
dGeomCCylinderGetParams(g,&radius,&length);
dsDrawCappedCylinderD(pos,R,length,radius);
}
else if (type == dSphereClass) {
dsDrawSphereD(pos,R,dGeomSphereGetRadius (g));
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom(g);
const dReal *pos2 = dGeomGetPosition(g2);
const dReal *R2 = dGeomGetRotation(g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331(actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom(g2,actual_pos,actual_R);
}
}
static void simLoop (int pause)
{
dSpaceCollide(space,0,&nearCallback);
dWorldStep(world,0.01);
dJointGroupEmpty(contactgroup);
dsSetColor(1.0,0.0,0.0);
drawGeom(trans_ball.geom, 0, 0);
drawGeom(ball.geom, 0, 0);
}
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 createKoboshi() {
dMass m,m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 1.0; // 胴体重心の初期位置[m]
dReal dx = 0.0, dy = 0.0, dz = 0.4; // 重心のオフセット [m]
trans_ball.geom = dCreateGeomTransform(space); // Geometry Transformオブジェクトのジオメトリを作成
trans_ball.body = dBodyCreate(world); // Geometry Transformオブジェクトのボディを作成
dGeomTransformSetCleanup(trans_ball.geom,1); // Geomery Transformのお掃除モード設定
// 胴体: 重心を移動したボール
trans_ball.radius = 0.4; // 球の半径0.4m
trans_ball.mass = 10.0; // 球の質量 10kg
dMassSetZero(&m); // 質量パラメータの初期化
dMassSetSphereTotal(&m,trans_ball.mass,trans_ball.radius); // 球の質量パラメータの計算
encap_ball_geom = dCreateSphere(0,trans_ball.radius); // カプセル化されるオブジェクトのスペースには0を入れ，ボディを作ってはいけない
// カプセル化される球のジオメトリencap_ball_geomをGeometry Transformオブジェクトであるtrans_ball.geomジオメトリに設定する（これによりカプセル化される）
dGeomTransformSetGeom(trans_ball.geom,encap_ball_geom);
dGeomSetPosition(encap_ball_geom, dx, dy, dz); // ジオメトリをオフセット分移動
dMassTranslate(&m, dx, dy, dz); // 重心をオフセット分移動
dGeomSetBody(trans_ball.geom,trans_ball.body); // ボディとジオメトリの対応づけ
dBodySetMass(trans_ball.body,&m); // ボディに質量パラメータを設定
dGeomSetPosition(trans_ball.geom, x0 -dx, y0 -dy, z0 – dz); // オフセットの分を考慮してGeometry Tranformオブジェクトの位置を設定
// 頭
ball.radius = 0.3; // 球の半径 0.3m
ball.mass = 0.001; // 質量 0.001kg
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1,ball.mass, ball.radius);
dBodySetMass(ball.body,&m1);
dBodySetPosition(ball.body, x0, y0, z0 + trans_ball.radius + ball.radius);
ball.geom = dCreateSphere(space,ball.radius);
dGeomSetBody(ball.geom,ball.body);
// fixed joint
joint = dJointCreateFixed(world, 0);
dJointAttach(joint, trans_ball.body,ball.body);
dJointSetFixed(joint);
}
void command(int cmd)
{
switch(cmd) {
case ‘f’:
dBodyAddTorque(ball.body, 100, 0, 0); break;
case ‘j’:
dBodyAddTorque(ball.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);
createKoboshi();
dsSimulationLoop (argc,argv,640,480,&fn);
dWorldDestroy (world);
return 0;
}
おしまい！
コメント