Open Dynamics Engine 入門
【4日目】デモ「いもむし」
デモ「いもむし」
任意のボディに力を加えることで、運動させることができます。
プログラムを考察してみる 余分なコードあり。
素人がしたもの
// 2015/08/02
// 芋虫が動く
#include <vector>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
int WindowWidth = 640; //ウィンドウの幅
int WindowHeight = 480; //ウィンドウの高さ
#ifndef DRAWSTUFF_TEXTURE_PATH
#define DRAWSTUFF_TEXTURE_PATH "C:/ode-0.11.1/drawstuff/textures"
#endif
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
#define NUM 10 //いもむしの長さ
#define NN 5 //いもむしの数
#define SIDE (0.2) //
#define MASS (1.0)
#define RADIUS (0.1732f)
static dWorldID world;
static dSpaceID space;
//dGeomID ground;
static dBodyID body[NN][NUM]; //いもむしを作っている球の配列
static dJointID joint[NN][NUM-1];//上の球同士のジョイントの配列
static dJointGroupID contactgroup;
static dGeomID sphere[NN][NUM];
//struct Card {
// dBodyID body;
// dGeomID geom;
// static const dReal sides[3];
//
// Card()
// {
// body = dBodyCreate(world);
// geom = dCreateBox(space, sides[0], sides[1], sides[2]);
// dGeomSetBody(geom, body);
// dGeomSetData(geom, this);
// //dGeomSetData geomのデータをセットしているみただが、
// //今まで使わなくてもだいじょうぶだったが
// dMass mass;
// //以下のコードがはいっていないがいいのか？
// //dMassSetZero(&mass); // 全質量パラメータを0にする
//
// //mass.setBox(1, sides[0], sides[1], sides[2]);
// dMassSetBox(&mass, 1, sides[0], sides[1], sides[2]);
// //Boxに質量を与える
// //void dMassSetBox (dMass *, dReal density, dReal lx, dReal ly, dReal lz);
// //void dMassSetBoxTotal (dMass *, dReal total_mass, dReal lx, dReal ly, dReal lz);
// // density : 密度
// // lx, ly, lz : サイズ(x, y, z)
// // total_mass : 剛体全体での質量
// dBodySetMass(body, &mass);
// }
//
// ~Card()
// {
// dBodyDestroy(body);
// dGeomDestroy(geom);
// }
//
// void draw() const
// {
// dsDrawBox(dBodyGetPosition(body),
// dBodyGetRotation(body), sides);
// }
//};
////カードの大きさ
//static const dReal cwidth=.5, cthinkness=.02, clength=1;
////static const dReal cwidth=.1, cthinkness=.1, clength=1.0;
//const dReal Card::sides[3] = {cwidth, cthinkness, clength};
//
//std::vector<Card*> cards;
//// Card*(構造体のポインタ)の可変長配列の宣言
//// ポインタを使わないとだめなのか？
//
////カードの総数を計算
//int getncards(int levels)
//{
// return (3*levels*levels + levels) / 2;
// // 1段 2段 3段 床枚数
// //levels=1 2枚 (3*1*1+1)/2=2 正解 3枚 -1枚
// //levels=2 7枚 (3*2*2+2)/2=7 正解 3枚+3枚*2個 -2枚
// //levels=3 15枚 (3*3*3+3)/2=15正解 3枚+3枚*2個+3枚*3個 -3枚
// //グーグル先生によると 1+2+3+...+n=1/2n(n+1)だから
// //3*(1+2+...levels)-levels
// // =3*(1/2*levels*(levels+1))-levels
// // =(levels*levels+levels)*3/2-levels*2/2
// // =(3*levels*levels+levels)/2
//}
//
////カードを設置（再配置）
////地面に黄色の点が7つ 赤の点が一個 青が一個ある で真ん中の黄点が原点
////赤がｘ軸+方向 青がｙ軸+方向 上がｚ軸+方向
//void place_cards()
//{
// ncards = getncards(levels);
// int oldcards = cards.size();
// for (int i=ncards; i<oldcards; ++i)
// delete cards[i]; //メモリの開放
// cards.resize(ncards); //cardsのサイズ変更
// for (int i=oldcards; i<ncards; ++i)
// cards[i] = new Card; //メモリの確保
//
// int c = 0;
// dMatrix3 right, left, hrot; //回転行列用の変数
// dReal angle = 20*M_PI/180.; //円周率 odeのほうのヘッダーファイルでインクルードされているみたい
// //dRFromAxisAndAngleの引数angleの単位がラジアンだから、角度20度を
// //ラジアンに変換
// dRFromAxisAndAngle(right, 1, 0, 0, -angle);//x軸右回転行列の取得 rightに代入
// dRFromAxisAndAngle(left, 1, 0, 0, angle); //左/回転行列の取得 leftに代入
// dRFromAxisAndAngle(hrot, 1, 0, 0, 91*M_PI/180.);
// //①void dRFromAxisAndAngle(dMatrix3 R, dReal ax, dReal ay, dReal
// //az, dReal angle);
// // 回転軸ベクトル(ax, ay, az)の回りを反時計方向にangle[rad]回転した
// //ときの回転変換行列Rを取得する。
// //②void dBodySetRotation(dBodyID, const dMatrix3 R);
// //剛体dBodyIDの姿勢を回転行列Rに設定する。
// //①と②は対になっているのだろう
//
// //dReal eps = 0.2; //余スペース
// dReal eps = 0.05; //余スペース
// //dReal vstep = 2.0;
// //dReal hstep = 0.5;
// dReal vstep = cos(angle)*clength + eps; //横方向への移動幅 0.9896926...
// dReal hstep = sin(angle)*clength + eps; //縦方向への移動幅 0.3920201....
// //clengthはカードの縦の長さ それをカードを20度に傾けて積んだら横方向の幅はvstepとなる
// //hstepも同様の考え方
// //注意 原文ではcosを横（ｙ軸方向）sinを縦（ｚ軸方向）とかいてある
// //がｚ軸垂直方向からの回転角みたいなので、縦横の表示が逆と思う。こ
// //の結論にほぼ1日かかる。はじめは？？？？？なんでだったが。
//
// for (int lvl=0; lvl<levels; ++lvl){ //各階層ごとにカードを配置
// int n = (levels-lvl);
// dReal height = (lvl)*vstep + vstep/2;
// //カードの位置をレベル（階層）があがるごとにvstepプラスするん
// //だけれどカードの位置はvstep/2の引数となるかな。
// for (int i=0; i<2*n; ++i, ++c){
// dBodySetPosition(cards[c]->body,//カードの位置を設定
// 0, //x座標
// -n*hstep + hstep*i, //y座標 なんでここで横方向と縦方向の移動パラメ
// //ータがx座標とｚ座標でいれかわっているのか
// //上の結論のごとくok
// height //z座標
// );
// if (i%2)
// dBodySetRotation(cards[c]->body, left);
// else
// dBodySetRotation(cards[c]->body, right);
// //どうしても カードの位置の決定の仕方が不明
// //いや 以下でいいのか
// //levels=3とすると
// //for (int lvl=0; lvl<levels; ++lvl) lvl=0～2
// //int n = (levels-lvl);
// // lvl=0 n=3 height=0*0.98+0.98/2=0.49
// // for (int i=0; i<2*n; ++i, ++c)
// // i=0～5
// // -n*hstep+hstep*i -3*hstep～2*hstep
// // lvl=1 n=2 height=1*0.98+0.98/2=1.47
// // for (int i=0; i<2*n; ++i, ++c)
// // i=0～3
// // -n*hstep+hstep*i -2*hstep～1*hstep
// //
// // lvl=2 n=1 height=2*0.98+0.98/2=2.45
// // for (int i=0; i<2*n; ++i, ++c)
// // i=0～1
// // -n*hstep+hstep*i -1*hstep～0
// //
// //level=1とすると
// //for (int lvl=0; lvl<levels; ++lvl) lvl=0～0
// //int n = (levels-lvl); n=1
// //height=1
// //for (int i=0; i<2*n; ++i, ++c) i=0～1
// // i=0 i=1
// // -n*hstep+hstep*i -hstep 0
// }
//
// if (n==1)
// break;
// //n==1の時は以下のコードを実行しないと言うことか
//
// //床のカードの座標計算
// //アバウトな感じがする
// for (int i=0; i<n-1; ++i, ++c){
// dBodySetPosition(cards[c]->body,
// 0,
// -(n-1 - (clength-hstep)/2)*hstep + 2*hstep*i,
// height + vstep/2);
// dBodySetRotation(cards[c]->body, hrot);
// //levels=3とすると
// //for (int lvl=0; lvl<levels; ++lvl) lvl=0～2
// //int n = (levels-lvl);
// // for (int i=0; i<n-1; ++i, ++c)
// // lvl=0 n=3 i=0～1
// // -(n-1 - (clength-hstep)/2)*hstep + 2*hstep*i
// // -(2-(c-h)/2)*h
// // -n*hstep + hstep*i
// // -3*hstep+hstep/2
// // lvl=1 n=2 i=0～0
// // lvl=2 n=1 if (n==1)のコードにより実行しない
// }
// }
//}
static void start()
{
static float xyz[3] = {2.1640f, -1.3079f, 1.7600f}; //カメラの位置
static float hpr[3] = {125.5000f, -17.0000f, 0.0000f}; //カメラの方向
// 上の場合はカメラはｙ軸の方向を向いている
dsSetViewpoint(xyz, hpr);
//puts("Controls");
//puts(" SPACE - reposition cards");
//puts(" - - one less level");
//puts(" - - one more level");
}
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
//const int MAX_CONTACTS = 8;
//dContact contact[MAX_CONTACTS];
dContact contact;
if (b1 && b2 && dAreConnected(b1,b2)) return;
//dAreConnected 2つの剛体がジョイントによって接続されているとき1を
//返して、そうで//ない場合は0を返す接続されているボディ同士は衝突
//検出しない
//return文でこの関数を抜ける
contact.surface.mode = 0;
contact.surface.mu = 1;
contact.surface.mu2 = 0;
if (dCollide(o1, o2, 1, &contact.geom, sizeof(dContactGeom))){
dJointID c = dJointCreateContact(world, contactgroup, &contact);
dJointAttach(c, b1, b2);
//2接触情報の作成
//dColide()によって、衝突する可能性のある2つのジオメトリの接触情報
//を作成できます。その接触情報は、dContactGeom構造体の配列として取
//得できます。
//
//戻り値は接触点の数であり、衝突していない場合には0が返されます。
//なおこの関数は、対象のジオメトリが属するスペースを考慮しません。
//
//int dCollide (
// dGeomID o1, // 検査対象のジオメトリ(スペース)1
// dGeomID o2, // 検査対象のジオメトリ(スペース)2
// int flags, // 接触情報の作成を指示するフラグ (接触点の最大数)
// dContactGeom *contact, // 接触情報を格納するdContactGeom構造体の配列
// int skip // dContactGeom構造体の大きさ
// );
//3 dJointCreateContact 接触ジョイントの作成
//4 dJointAttach 接触したボディを接触ジョイントで接続
}
// int numc = dCollide(o1, o2, MAX_CONTACTS,
// &contact[0].geom,
// sizeof(dContact));
// int isGround = ((ground==o1) || (ground==o2));
// for (int i=0; i<numc; i++){
// contact[i].surface.mode = dContactApprox1;
// if(isGround) contact[i].surface.mu =5; //カードと地面との摩擦係数
// else contact[i].surface.mu = 5; //カード同士の摩擦係数
// //0.1だとカードが滑ってタワーが壊れる
// dJointID c = dJointCreateContact(world, contactgroup, contact+i);
// dJointAttach(c, b1, b2);
// }
}
void simLoop(int pause)
{
int i;
if (!pause){
for (int j=0; j<NN; j++){
//NN いもむしの数 NUM いもむしの長さ
static double angle = 0;
angle += 0.01;
dBodyAddForce( body[j][NUM-1], 0, 0, 5.0*( sin(angle*(j+1))+1.0) );
//ボディに力を加える
//力はボディごとに加算されて作用します。累積された力は、シ
//ミュレーションのステップごとに0にクリアされます。
//5.0*( sin(angle*(j+1))+1.0)をpythonで計算
// >>> import math
// >>> angle = 0
// >>> for j in range(5):
// ... angle = angle + 0.01
// ... ans=5.0*(math.sin(angle*(j+1)+1.0))
// ... print ans
// ...
// 4.23415922309
// 4.31202113622
// 4.43313457225
// 4.58401554386
// 4.74492309678
dSpaceCollide(space, 0, &nearCallback);
dWorldStep(world, 0.05);
dJointGroupEmpty(contactgroup);
}
}
//オブジェクトのテクスチャを設定する．texture_number はDS_WOOD(木
//目) やDS_NONE(テクスチャなし) などのテクスチャ定数でなければいけ
//ない．現在のテクスチャは現在の色で塗られる
//NN いもむしの数 いもむしごとに色分けしている
for (int j=0; j<NN; j++){
for (int i=0; i<NUM; i++){
switch(j){
case 0:
dsSetTexture(DS_CHECKERED);
break;
case 1:
dsSetTexture(DS_GROUND);
break;
case 2:
dsSetTexture(DS_SKY);
break;
case 3:
dsSetTexture(DS_WOOD);
break;
default:
dsSetColor(1, 1, 0);
}
dsDrawSphere(dBodyGetPosition(body[j][i]),
dBodyGetRotation(body[j][i]),
RADIUS);
}
}
// if (!pause) {
// //下3行のコードはどうしてもいる。きまり文句
// dSpaceCollide(space, 0, &nearCallback);
// //衝突検出関数dSpaceCollide()を実行する。衝突が検出されると、
// //そのコールバック関数が呼ばれる。
// dWorldQuickStep(world, 0.01);
// dJointGroupEmpty(contactgroup);
// }
// dsSetColor(1, 1, 0);
// for(int i=0; i<ncards; ++i){
// //ncards カードの総数
// dsSetColor(1, dReal(i)/ncards, 0);
// //1番目は赤，2番目は緑，3番目は青成分で0以上1以下の値となりま
// //す．ちなみに，全部0だと黒，全部1.0だと白です．
// cards[i]->draw(); //各カードを描いている
// }
}
// //キーボード「-」「=」で押すと、タワーの階層を「減らす」「増やす」し、
// //「(スペース)」でタワーをリセットできます。
// void command(int c)
// {
// switch(c){
// case '=':
// levels++;
// place_cards();
// break;
// case '-':
// levels--;
// if(levels <= 0)
// levels++;
// place_cards();
// break;
// case ' ':
// place_cards();
// break;
// }
// }
int main(int argc, char **argv)
{
int i;
dReal k;
dMass m;
dsFunctions fn;
//dsFunctionsではイニシャライズ関数、シミュレーションループ関数やコ
//マンドを受け付ける関数の指定を
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
//fn.command = &command;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dInitODE();
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
//単純な衝突検出計算用スペースを生成し，そのID 番号を返す．
//space = dHashSpaceCreate(0);
//高速に計算可能なハッシュテーブルを備えた衝突検出計算用スペースの
//生成し，そのID 番号を返す
//どちらでも、かわらないような....
dWorldSetGravity(world, 0, 0, -0.5); //世界にｚ方向に-0.5の重力
dCreatePlane(space, 0, 0, 1, 0);
//ground = dCreatePlane(space, 0, 0, 1, 0);
//dWorldSetQuickStepNumIterations(world, 50);
//dWorldStep(dWorldID, dReal stepsize);で世界をすすめるとメモリをたくさん必要とするらしい。
//そこでdWorldQuickStep (dWorldID, dReal stepsize);を使うが
//早くなる分、正確さにかける。で
//dWorldSetQuickStepNumIterations(dWorldID, int num);を使うみたい
//mumでステップ毎にQuickStep方式を実行する繰り返し数の取得と設定を
//しているようだ
//contactgroup = dJointGroupCreate(0);
////平面のジオメトリを作成 地面の剛体はつくらなくてもいいみたい
//place_cards();
//NN いもむしの数
//NUM いもむしの長さ
//SIDE 0.2
//RADIUS (0.1732f)
for (int j=0; j<NN; j++){
contactgroup = dJointGroupCreate(0);
for (i=0; i<NUM; i++){
body[j][i] = dBodyCreate(world);
k = i * SIDE;
dBodySetPosition(body[j][i], k-j*0.1, k, k+0.4+j*1.0);
dMassSetBox(&m, 1, SIDE, SIDE, SIDE);
dMassAdjust(&m, MASS);
//すでに密度で与えられたdMassに対して全質量を再度与えるのに有用
//質量がnewmassになるように調整します。これはdMassSetXx()
//によって密度から設定した場合に、その質量を設定するのに有
//効です。
dBodySetMass(body[j][i], &m);
sphere[j][i] = dCreateSphere(space, RADIUS);
dGeomSetBody(sphere[j][i], body[j][i]);
//sphereなどと書くのでややこしい。geomをセットしている
//下のコードと比較 下のコードが私にとってなじんだコード
// ball.body = dBodyCreate(world);
// dMassSetZero(&m1);
// dMassSetSphereTotal(&m1,mass,radius);
// dBodySetMass(ball.body,&m1);
// dBodySetPosition(ball.body, x0, y0, z0);
//
// ball.geom = dCreateSphere(space,radius);
// dGeomSetBody(ball.geom,ball.body);
}
for (i=0; i<(NUM-1); i++){
joint[j][i] = dJointCreateBall(world, 0);
//ボールジョイント
dJointAttach(joint[j][i], body[j][i], body[j][i+1]);
//ボールジョイントでつなで一匹のいもむしを作成
k = (i + 0.5) * SIDE;
dJointSetBallAnchor(joint[j][i], k-j*0.1, k, k+0.4+j*1.0);
//軸の位置をセット
}
}
dsSimulationLoop(argc, argv, WindowWidth, WindowHeight, &fn);
//////シミュレーション用の無限ループ
//levels = 0;
//place_cards();
dJointGroupDestroy(contactgroup);
dWorldDestroy(world);
dSpaceDestroy(space);
dCloseODE();
return 0;
}
//dWorldID world; // 動力学の世界
//dBodyID apple; // リンゴ
//dReal r = 0.2, m = 1.0; // リンゴの半径,質量
//dsFunctions fn; // ドロースタッフ用の構造体
//
//void simLoop(int pause) /*** シミュレーションループ ***/
//{
// dWorldStep(world,0.01); // シミュレーションを1ステップ進める
//
// dsSetColor(1.0,0.0,0.0); // 赤色の設定(r,g,b)
// const dReal *pos = dBodyGetPosition(apple); // リンゴの位置を取得
// const dReal *R = dBodyGetRotation(apple); // リンゴの姿勢を取得
// dsDrawSphereD(pos,R,r); // リンゴの描画
//}
//
//void start() /*** 前処理 ***/
//{
// static float xyz[3] = {3.0,0.0,1.0}; // 視点の位置
// static float hpr[3] = {-180, 0, 0}; // 視線の方向
// dsSetViewpoint(xyz,hpr); // カメラの設定
//}
//
//void setDrawStuff() /*** 描画関数の設定 ***/
//{
// fn.version = DS_VERSION; // ドロースタッフのバージョン
// fn.start = &start; // 前処理 start関数のポインタ
// fn.step = &simLoop; // simLoop関数のポインタ
// fn.path_to_textures = "../../drawstuff/textures"; // テクスチャ
//}
//
//int main(int argc, char **argv) /*** main関数 ***/
//{
// setDrawStuff(); // 描画関数の設定
// world = dWorldCreate(); // 世界の創造
// dWorldSetGravity(world,0,0,-0.2); // 重力設定
//
// apple = dBodyCreate(world); // ボールの生成
// dMass mass; // 構造体massの宣言
// dMassSetZero(&mass); // 構造体massの初期化
// dMassSetSphereTotal(&mass,m,r); // 構造体massに質量を設定
// dBodySetMass(apple,&mass); // ボールにmassを設定
// dBodySetPosition(apple, 0.0, 0.0, 2.0); // ボールの位置(x,y,z)を設定
//
// dsSimulationLoop(argc,argv,320, 240,&fn); // シミュレーションループ
// dWorldDestroy(world); // 世界の終焉
// return 0;
//}
0 件のコメント:
コメントを投稿