ode こま

Open Dynamics Engine 入門
【5日目】デモ　「こま」

http://www.natural-science.or.jp/article/20110216235836.php

Open Dynamics Engine 入門
【5日目】デモ「こま」

//http://www.natural-science.or.jp/article/20110216235836.php

//Open Dynamics Engine 入門【5日目】デモ「こま」

//上のコードから片方のコマ（コマ1）だけポインタを使わずにコードをかく

//コマ1　赤色　　コマ2　緑

//360行あたりのコードで koma_body2->setGyroscopicMode(false);

//コマ２の姿勢制御モードがオフのなっているので早く倒れる

//

//コマはどんな関数で回るのか？　ジョイントがあって回転させる関数はわかるが。

//　→　

#include

#include

//#include "texturepath.h"

#ifdef _MSC_VER

#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints

#endif

#ifndef DRAWSTUFF_TEXTURE_PATH

#define DRAWSTUFF_TEXTURE_PATH "C:/ode-0.11.1/drawstuff/textures"

#endif

// select correct drawing functions

#ifdef dDOUBLE

#define dsDrawBox dsDrawBoxD

#define dsDrawSphere dsDrawSphereD

#define dsDrawCylinder dsDrawCylinderD

#define dsDrawCapsule dsDrawCapsuleD

#define dsDrawConvex dsDrawConvexD

#endif

bool write_world = false; //このodeシュミレーションをファイルに出力するフラッグ

bool show_contacts = false; //地面と接触している点を表示。わかりにくいので元コードより大きくした

dWorld * world;

dBody *koma_body2;

dBodyID koma_body1; //koma_body1をポインタ表現しない

dSpace *space;

dJointGroup contactgroup;

const dReal zikuradius = 0.05f; //pinradiusを zikuradiusに変更、以下同様

const dReal zikulength = 1.5f;

const dReal koma_bodyradius = 1.0f; //topradiusを　koma_bodyradiusに変更、以下同様

const dReal koma_bodylength = 0.25f;

const dReal koma_bodymass = 1.0f;

#define MAX_CONTACTS 4

static void nearCallback (void *data, dGeomID o1, dGeomID o2)

{

// for drawing the contact points

dMatrix3 RI;

dRSetIdentity (RI); //行列RIを単位行列にセットする。回転していない状態に

//する

//const dReal ss[3] = {0.02,0.02,0.02};

const dReal ss[3] = {0.2,0.2,0.2};

// 94行あたりで　if (show_contacts) 床とコマの接触面を表示するdsDrawBoxdの大

// きさ

//サイズが小さくてわからないので、元コードより大きくした。

int i;

dBodyID b1 = dGeomGetBody(o1);

dBodyID b2 = dGeomGetBody(o2);

dContact contact[MAX_CONTACTS];

//以下接触点の数をnumcで得ている。定番のコード

int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,

sizeof(dContact));

   //  dContact contact[MAX_CONTACTS];

   //  //接触情報を格納する配列

   //  int numc = dCollide(

   //      o1,

   //      o2,

   //      MAX_CONTACTS,     //接触点の最大数

   //      &contact[0].geom, //接触情報を格納するdContactGeom構造体

   //      sizeof(dContact)  //引数はここはdContactかdContactGeomでいいみたい

   //      );

   //  //dCollide: 接触情報を作成する　戻り値は接触点の数　衝突しない場合は0

//接触していたら

for (i=0; i contact[i].surface.mode = dContactApprox1;
   //接触面の性質を設定する
// http://www.koj-m.sakura.ne.jp/ode/index.php?%CB%E0%BB%A4%A1%F5%C0%DC%BF%A8%CA%D4
//『dContactApprox1』のフラグを追加する.その 場合，『surface.mu=1』に対応
//する力まで物体は滑らない.それに対し『dContactApprox1』のフラグを立てな
//い場合は，静摩擦は 無く，『surface.mu=1』に対応する動摩擦力のみ適用され
//る.
//　http://hhhappyyyymoto.blogspot.jp/2015/10/so-zou.html?zx=fd636d6421be3cef
//ode 摩擦　dContactApprox1 Blogger
//要するに　摩擦が大事なプログラムでは  dContactApprox1にしたらいい？
contact[i].surface.mu = 2;
   //摩擦係数（摩擦方向1）
//最大値は1ではないか？

dJointID c = dJointCreateContact (*world,contactgroup,contact+i);
//contact+iは&contact[i]と同じか？
dJointAttach (c,b1,b2);
   //接触ジョイントの作成
if (show_contacts)
dsDrawBox (contact[i].geom.pos, RI, ss);
//dsDrawBox( dBodyGetPosition( box ), dBodyGetRotation( box ),
//box_sizes );

}
}


// start simulation - set viewpoint

static void start()
{
  static float xyz[3] = {4.777f, -2.084f, 2.18f};
  static float hpr[3] = {153.0f, -14.5f, 0.0f};
  dsSetViewpoint (xyz,hpr);
  printf ("SPACE to reset\n"); //リセット
  printf ("A to tilt the koma_bodys. katamuku\n"); //こまを傾ける
  printf ("T to toggle showing the contact points. sessyoku_tenn_hyouzi\n");//床とコマの接触点を表示
  printf ("1 to save the current state to 'state.dif'.\n"); //ファイルに保存
}


char locase (char c) //大文字を小文字に変換
{
  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
  else return c;
}


// called when a key pressed
static void reset();
static void tilt(); //tilt 意味は傾斜　傾ける　など

static void command (int cmd)
{
cmd = locase (cmd);
if (cmd == ' ')
{
reset();
}
else if (cmd == 'a') {
tilt();
}
else if (cmd == 't') {
show_contacts = !show_contacts;
}
else if (cmd == '1') {
write_world = true;
}
}

// simulation loop
static void simLoop (int pause)
{
dsSetColor (0,0,2);
space->collide(0,&nearCallback);
//●dSpaceCollide(space,0,&nearCallback);

if (!pause)
//world->quickStep(0.02);
world->step(0.02);
   //●dWorldStep(world,0.02);

if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (*world,f,"X");
//ワールドの状態を出力します。出力先は、引数のfileで指定します。
//void dWorldExportDIF(dWorldID w, FILE *file, const char *world_name);
//"X"とはどこから、持ってきた？
fclose (f);
}
write_world = false;
}

// remove all contact joints
dJointGroupEmpty (contactgroup);
//テクスチャを変えている。難しいことはわからない。
dsSetTexture (DS_WOOD);

//以下コマ1の本体　軸　を描画
dsSetColor (1.0, 0.0, 0.0); //赤
dsDrawCylinder( dBodyGetPosition(koma_body1),
dBodyGetRotation(koma_body1),
koma_bodylength, koma_bodyradius );
dsDrawCylinder( dBodyGetPosition(koma_body1),
dBodyGetRotation(koma_body1),
zikulength, zikuradius );

//以下コマ2の本体　軸　を描画
dsSetColor (0.0, 1.0, 0.0); //緑
dsDrawCylinder(koma_body2->getPosition(),
koma_body2->getRotation(),
koma_bodylength, koma_bodyradius);
dsDrawCapsule(koma_body2->getPosition(),
koma_body2->getRotation(),
zikulength, zikuradius);
}


static void reset()
{
dMatrix3 R; //dMatrix3[ 4 * 3 ]
dRSetIdentity(R); //Rを単位行列 にする

   //　剛体dBodyIDの姿勢を回転行列Rに設定する。
dBodySetRotation(koma_body1, R);
koma_body2->setRotation(R);

dBodySetPosition(koma_body1,0.8f, -2, 2);
koma_body2->setPosition(0.8f, 2, 2);

//bodyの角速度を絶対座標系の各軸周りに(x, y, z) [rad/s]と設定します
//　上がコマが回る源の関数だろう
dBodySetAngularVel( koma_body1, 0,0,5);
koma_body2->setAngularVel(0,0,5);

dBodySetLinearVel(koma_body1, 0,0.2f,0);
koma_body2->setLinearVel(0,0.2f,0);
//bodyの速度を絶対座標系で(x, y, z) [m/s]に設定します
//剛体の速度の設定はdBodySetLinearVel()、角速度の設定は
//dBodySetAngularVel()、を使います。ただし、この速度は重心の速度、角速度は重
//心周りの角速度です。また、ボディの速度を設定することは初期状態だけにし、時
//間ステップ毎に強制的にある速度を設定することは、実際の物理現象とは異なった
//挙動を生み出してしまいますので避けてください。
//もし、物体の速度をコントロールした場合は、関節に付属しているモータを使って
//ください   http://demura.net/9ode/481.html
//　上からの意味だと初期にy軸方向に少しふらつかせている。
//　なんでかな？
}

static void tilt()
{
dBodyAddTorque(koma_body1, 0, 10, 0);
koma_body2->addTorque(0, 10, 0);
//原点から手前方向にｘ　右にｙ　//上にｚ
//ボディの重心を作用点にして絶対座標(0, 10, 0)のベクトルのトルクを加える。
//ということは、手前方向に倒れるトルクが働く
//以下トルクの定義
//http://www.washimo-web.jp/Technology/Statics/No06/Statics06.htm
//何の意味が？
//上はaキーで実行されるが、やってもただ単にコマをふらつかせて遊んでいるだけ
}

int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

// create world
// ポインタを使わないmainのほうが簡単と思うが
// ①　以下であるならば
// static dWorldID world;
// static dSpaceID space;
// static dGeomID  ground;
// static dJointID fixed;
// static dJointID joint_ball; //ボールジョイント
// static dJointGroupID contactgroup;

dInitODE();
// ② ワールド作成
   //  world = dWorldCreate();
world = new dWorld();

// ③重力の設定
world->setGravity(0,0,-0.5f);
//  dWorldSetGravity(world,0,0,-0.5f);

// ④ 拘束
world->setCFM(1e-5f); //@eCFMを設定 弱い拘束にしているのか？
// dWorldSetCFM (world, 1e-5f);
// 小さな値では強い (hard) 拘束となり、大きな値では弱い (soft) 拘束となりま
// 既定値  10^-10 (単精度では10^-5)  範囲  10^-9 ～ 1
// http://demura.net/9ode/351.html　拘束について

// ⑤速度減衰率を設定0～1の間　デフォルトは0　つまり減衰しない？
world->setLinearDamping(0.00001f);

//角速度減衰率を設定
// dWorldSetLinearDamping(world, 0.00001f)
world->setAngularDamping(0.0001f);
//上の2つの設定、限りなく0に近いので設定しなくてもいいのでは？

// ⑥dSpace型のオブジェクトを生成（衝突空間の生成）
space = new dSimpleSpace(0);
// space = dSimpleSpaceCreate (0);

// ⑦
dPlane *floor = new dPlane(*space, 0,0,1,0);
   // ground = dCreatePlane(space,0,0,1,0);

// ⑧
// 以下のようにかけるかも
// typedef struct {
//   dBodyID body;
//   dGeomID geom;
//   dReal   radius,length,width,height,mass;
// } myLink;
// myLink koma_body1,koma_body2;

//ボディの生成
koma_body1 = dBodyCreate(*world);
koma_body2 = new dBody(*world);

//  ⑩
dMass m;
//  元コードにはdMassSetZero(&m);のような初期化のコードがはいっていないが？
dMassSetZero(&m);

   // ⑪シリンダー型の質量をセット
//m.setCylinderTotal(1, 3, koma_bodyradius, koma_bodylength);
dMassSetCylinderTotal(&m, 1, 3, koma_bodyradius, koma_bodylength);

// OdeMass::set_cylinder_total(float total_mass, int direction, float radius, float length);
// direction:方向
// @@dMassSetCylinderTotal(this,total,direction,radius,length)　と同じ？
// google で検索してもヒットしない。
// void dMassSetCapsuleTotal (
   //   dMass *mass,
   //   dReal total_mass, // 質量
   //   int direction,    // 長軸方向(1=x軸 2=y軸 3=z軸)
   //   dReal radius,     // 半径
   //   dReal length      // 長さ
   //   );
 
//⑫ボディに質量をセット
   dBodySetMass(koma_body1, &m);
koma_body2->setMass(m);

// ⑬
static dGeomID koma_geom1;
dGeom *g2, *ziku1, *ziku2;
//dGeom *g1, *g2, *ziku1, *ziku2;

   // ⑭ジオメトリ作成
koma_geom1 = dCreateCylinder(*space, koma_bodyradius, koma_bodylength);
g2 = new dCylinder(*space, koma_bodyradius, koma_bodylength);

   // ⑮ジオメトリgeomをボディbodyに関連付けます．
dGeomSetBody(koma_geom1, koma_body1);
g2->setBody(*koma_body2);

// ⑯軸のジオメトリ作成
ziku1 = new dCapsule(*space, zikuradius, zikulength);
ziku2 = new dCapsule(*space, zikuradius, zikulength);
// なんでkoma_body1にg1とziku1のgeomを同時にセットするのか。
// そんな事が可能なのか？
// 円柱状のgeomの上下に少しだけ細いカプセル状のgeomが突き出ている事になる

// ⑰軸のボディを作成せずにコマ本体にジオメトリを関連付ける
dGeomSetBody(*ziku1, koma_body1);
ziku2->setBody(*koma_body2);

//　コマ2だけ姿勢制御モード(緑色のこま)をはずしているみたい
koma_body2->setGyroscopicMode(false);
//上をコメントアウトすると緑色のこまが先に倒れる
//dBodySetGyroscopicMode(0)　と同じだと思う　

reset();

// run simulation
dsSimulationLoop (argc,argv,512,384,&fn);

//delete g1;
delete g2;
delete ziku1;
delete ziku2;
delete floor;
contactgroup.empty();

//koma_body1はnewしていないので、deleteはいらないのか
//でも、koma_body1だけ動的確保がされていないのか？
delete &koma_body1;

delete koma_body2;
delete space;
delete world;
//なんで手動でdeleteするのか？以下ではだめか？
//スペースのクリーンナップモードを有効にすることで、スペースを破棄したときに
//そこに含まれるジオメトリも自動的に破棄されるようになります。これは既定で有
//効となっています。
//dGeomDestroy ( dGeomID geom )k //ジオメトリの破棄
//dJointGroupDestroy( contactgroup ); // ジョイントグループの破棄
   //dSpaceDestroy( space );          // スペースの破棄
   //dWorldDestroy( world );          // 世界の破壊
//
//　私ばかよねー　オバカさんよねー
//http://www.flow.cs.is.nagoya-u.ac.jp/hamada/programming/cpp5.html
//配列の動的確保
// new をしたら　deleteをしないとだめ

dCloseODE();
}