緑の棒と青の棒はヒンジジョイント（コントロールして90度に固定）
してみた。
ジャイロ効果によりコマの回転軸がまわる。
コマ回転方向はｚ軸右ねじ回転、重力はもちろん下、でコマの軸は
重力と90度方向の力を受ける。
//http://www.natural-science.or.jp/article/20110216235836.php
//Open Dynamics Engine 入門【5日目】デモ「こま」
//上のコードから片方のコマ（コマ1）だけポインタを使わずにコードをかく
//コマを ひとつにする
//
//コマはどんな関数で回るのか？ ジョイントがあって回転させる関数はわかるが。
//以下のコマンドでこまは回る
// → dBodySetAngularVel( koma_body1, 0,0,5);
//
//元コードのポインタ表現のコードをコメントアウトして残しておく
//構造体を使ってみる。そしてコードを少し改変。
//
//なんで、丁度軸の下が地面に接触するように設定されているのに、実行したとき
//こまが落ちてくるように描画されるのか？ 以下に設定していて勘違いをしているので
//は？
// dBodySetPosition(koma[i].body,0.8f, -2, 2) → -2, 2, 0.8f
// 修正する。
//
// コマの下の軸に.ボールジョイントをつけてそれを空中に固定する
//
// ジャイロ効果を見るため、最初からコマを傾かせておく
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
//#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
static dWorldID world; //ODE世界のID
//dWorld * world;
static dSpaceID space;
static dGeomID ground;
static dBodyID koma_body1;
static dGeomID koma_geom1;
static dGeomID ziku_geom1;
dJointGroupID contactgroup;
//dJointGroup contactgroup;
// ここで以下のコードなら main関数で contactgroup = dJointGroupCreate(0);
// として 衝突のときに使用される接触点グループを生成しなければいけないが。
// コメントアウトのコードでは いきなり接触点グループを作成しているみたい。
static dJointID joint_ball;
static dJointID joint_hinge;
static dJointID fixed;
dsFunctions fn;
//コマ本体、軸の各寸法
//コマは円筒 軸はカプセルを使う
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;
//台2と床との間
const dReal dai2_dsps = 0.02f;
//台1と台2との隙間
const dReal dai1_dsps = 0.02f;
//軸と台1との隙間
const dReal ziku_dsps = 0.02f;
//台2
const dReal dai2_xl = 0.05f;
const dReal dai2_yl = 0.05f;
const dReal dai2_zl = 2.00f;
const dReal dai2_sides[3] = {dai2_xl, dai2_yl, dai2_zl};
//台1
const dReal dai1_xl = 0.05f;
const dReal dai1_yl = 0.05f;
const dReal dai1_zl = 0.30f;
const dReal dai1_sides[3] = {dai1_xl, dai1_yl, dai1_zl};
//台2位置
const dReal dai2_xpos = 0.0f;
const dReal dai2_ypos = 0.0f;
//const dReal dai2_zpos = 0.45f; //台2とコマの軸の「隙間」を0.2とする
const dReal dai2_zpos = dai2_dsps + dai2_zl/2 ;
//台1位置
const dReal dai1_xpos = 0.0f;
const dReal dai1_ypos = 0.0f;
//const dReal dai1_zpos = 0.75f; //台1とコマの軸の「隙間」を0.2とする
const dReal dai1_zpos = dai2_zpos + dai2_zl/2 + dai1_dsps + dai1_zl/2;
//const dReal dai1_zpos = dai2_zpos + dai2_zl/2 + dai1_dsps + dai1_zl/2;
//コマの位置
const dReal koma_xpos = 0.0f;
const dReal koma_ypos = 0.0f;
const dReal koma_zpos = dai1_zpos + dai1_zl/2 + ziku_dsps
+ zikuradius + zikulength/2;
//const dReal koma_zpos = 1.80f; //空中の固定台1上でコマを回転させよう
//const dReal koma_zpos = 0.801f;
//コマの軸のgeomはkomaのbodyに関連づけられている。軸はカプセルであるから
//コマの中心からコマの軸最下部までは、ｚ方向で1.5/2+0.05 で0.80 となる。
//台1とコマの軸最下部をボールジョイントする時の、ジョイントの位置
//台1と軸をボールジョイントするときの、ジョイントの位置
const dReal jb_xpos = 0.0f;
const dReal jb_ypos = 0.0f;
const dReal jb_zpos = dai1_zpos + dai1_zl/2 + ziku_dsps/2;
//const dReal jb_zpos = 0.9f; //ジョイントの位置を「隙間」の真ん中にする
//台1と台2をヒンジジョイントする時の、ジョイントの位置
const dReal jh_xpos = 0.0f;
const dReal jh_ypos = 0.0f;
const dReal jh_zpos = dai2_zpos + dai2_zl/2 + dai1_dsps/2;
//const dReal jh_zpos = 0.6f; //ジョイントの位置を「隙間」の真ん中にする
const dReal H_ANGLE = M_PI/2 ; //ヒンジジョイントを90度に固定
#define MAX_CONTACTS 4
const int N = 1;//描画するコマの数
typedef struct {
dBodyID body; //動力学計算用のボディ
dGeomID geom; //衝突計算用のジオメトリ
} MyObject;
MyObject koma[N], ziku[N], dai1[N], dai2[N];//球を描画するための配列
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の大
// きさ
//サイズが小さくてわからないので、元コードより大きくした。
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 (int i=0; i<numc; 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;
//}
//elsh if (cmd == '1') {
// write_world = true;
//}
}
// ヒンジジョイントの制御
static void controlHinge(dReal target)
//void control()
{
dReal velCoeff = 1.0; // 比例定数
dReal p; //現在の関節角度
static dReal angle = target;
p = dJointGetHingeAngle(joint_hinge); // 現在の関節角度の取得
dReal z = angle - p; // angleは目標角度[rad]
//if (p > 0.25 * M_PI) angle = - 0.25 * M_PI; // 目標角度の下限
//if (p < - 0.25 * M_PI) angle = 0.25 * M_PI; // 目標角度の上限
dJointSetHingeParam(joint_hinge, dParamVel, velCoeff*z);// 目標角速度[rad/s]の設定
dJointSetHingeParam(joint_hinge, dParamFMax, 100); // 最大トルク[Nm]の設定
}
// simulation loop
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide(space,0,&nearCallback);
//space->collide(0,&nearCallback);
if (!pause){
dWorldStep(world,0.02);
//world->step(0.02);
controlHinge(H_ANGLE); // ヒンジジョイントの制御
//control();
}
dJointGroupEmpty(contactgroup);
//contactgroup.empty();
//以下コマの本体 軸 を描画
dsSetColor (1.0, 0.0, 0.0); //赤
for(int i=0;i<N;i++){
dsDrawCylinder( dBodyGetPosition(koma[i].body),
dBodyGetRotation(koma[i].body),
koma_bodylength, koma_bodyradius );
dsDrawCapsule( dBodyGetPosition(koma[i].body),
dBodyGetRotation(koma[i].body),
zikulength, zikuradius );
dsSetColor (0.0, 1.0, 0.0); //緑
dsDrawBox( dBodyGetPosition(dai1[i].body),
dBodyGetRotation(dai1[i].body),
dai1_sides);
dsSetColor (0.0, 0.0, 1.0); //青
dsDrawBox( dBodyGetPosition(dai2[i].body),
dBodyGetRotation(dai2[i].body),
dai2_sides);
////以下コマの本体 軸 を描画
//dsDrawCylinder(koma_body1->getPosition(),
// koma_body1->getRotation(),
// koma_bodylength, koma_bodyradius);
//dsDrawCapsule(koma_body1->getPosition(),
// koma_body1->getRotation(),
// zikulength, zikuradius);
}
}
static void reset()
{
dMatrix3 R; //dMatrix3[ 4 * 3 ]
dRSetIdentity(R); //Rを単位行列 にする
for (int i=0; i<N; i++) {
// 剛体dBodyIDの姿勢を回転行列Rに設定する。
dBodySetRotation(koma[i].body, R);
//koma_body1->setRotation(R);
dBodySetPosition(koma[i].body, koma_xpos, koma_ypos, koma_zpos);
//dBodySetPosition(koma[i].body, 0, 0, 0.801f);
//koma_body1->setPosition(0.8f, 2, 2);
dBodySetPosition(dai1[i].body, dai1_xpos, dai1_ypos, dai1_zpos);
dBodySetPosition(dai2[i].body, dai2_xpos, dai2_ypos, dai2_zpos);
//軸と台1をボールジョイント
joint_ball = dJointCreateBall (world,0);
dJointAttach (joint_ball, koma[i].body, dai1[i].body);
dJointSetBallAnchor (joint_ball, jb_xpos, jb_ypos, jb_zpos);
//台1と台2をヒンジ
joint_hinge = dJointCreateHinge(world, 0); // ジョイントの作成
dJointAttach(joint_hinge, dai1[i].body, dai2[i].body ); // ジョイントで2つのボディを接続
dJointSetHingeAnchor(joint_hinge, jh_xpos, jh_ypos, jh_zpos); // 中心点の設定
dJointSetHingeAxis(joint_hinge, 1, 0, 0); // 回転軸の設定
//台2を空中で固定ジョイント
fixed = dJointCreateFixed(world, 0);
dJointAttach(fixed, NULL, dai2[i].body); // 固定したいボディをNULLと接続
dJointSetFixed(fixed); // Fixedジョイント設定
////台1を空中で固定ジョイント
//fixed = dJointCreateFixed(world, 0);
//dJointAttach(fixed, NULL, dai1[i].body); // 固定したいボディをNULLと接続
//dJointSetFixed(fixed); // Fixedジョイント設定
dBodySetAngularVel( koma[i].body, 0,0,5);
//bodyの角速度を絶対座標系の各軸周りに(x, y, z) [rad/s]と設定します
// 上がコマが回る源の関数だろう
//koma_body1->setAngularVel(0,0,5);
//dBodySetLinearVel(koma[i].body, 0,0.2f,0);
//これは、殺しておこう
//koma_body1->setLinearVel(0,0.2f,0);
//bodyの速度を絶対座標系で(x, y, z) [m/s]に設定します
//剛体の速度の設定はdBodySetLinearVel()、角速度の設定は
//dBodySetAngularVel()、を使います。ただし、この速度は重心の速度、角速度は重
//心周りの角速度です。また、ボディの速度を設定することは初期状態だけにし、時
//間ステップ毎に強制的にある速度を設定することは、実際の物理現象とは異なった
//挙動を生み出してしまいますので避けてください。
//もし、物体の速度をコントロールした場合は、関節に付属しているモータを使って
//ください http://demura.net/9ode/481.html
// 上からの意味だと初期にy軸方向に少しふらつかせている。
// なんでかな？
//
//koma_body1->setLinearVel(0,0.9f,0); とやるとこま2（緑色）が最初からぶっ飛
//んだ。 このコードを殺すとゆっくりまわっても倒れることがない。
//自然の状態ではありえない！！！！！！！！！！！！！！
//dRFromAxisAndAngle(R, 1.0, 0.0, 0.0, M_PI/4);
//x軸周りに90度回転
dBodySetRotation(koma[i].body, R);
}
}
static void tilt()
{
for(int i=0;i<N;i++){
dBodyAddTorque(koma[i].body, 0, 10, 0);
//koma_body1->addTorque(0, 10, 0);
//原点から手前方向にｘ 右にｙ //上にｚ
//ボディの重心を作用点にして絶対座標(0, 10, 0)のベクトルのトルクを加える。
//ということは、手前方向に倒れるトルクが働く
//以下トルクの定義
//http://www.washimo-web.jp/Technology/Statics/No06/Statics06.htm
//何の意味が？
//上はaキーで実行されるが、やってもただ単にコマをふらつかせて遊んでいるだけ
}
}
void makeKoma()
{
for(int i=0;i<N;i++){
koma[i].body = dBodyCreate(world);
//koma_body2 = new dBody(world);
dMass m;
dMassSetZero(&m);
//コマ本体
dMassSetCylinderTotal(&m, 1, 3, koma_bodyradius, koma_bodylength);
// ⑪シリンダー型の質量をセット
//m.setCylinderTotal(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[i].body, &m);
//⑫ボディに質量をセット
//koma_body2->setMass(m);
//dGeom *g1, *g2, *ziku1, *ziku2;
koma[i].geom = dCreateCylinder(space, koma_bodyradius, koma_bodylength);
// ⑭ジオメトリ作成
//g1 = new dCylinder(*space, koma_bodyradius, koma_bodylength);
dGeomSetBody(koma[i].geom, koma[i].body);
// ⑮ジオメトリgeomをボディbodyに関連付けます．
//g1->setBody(*koma_body1);
//コマの台1
dai1[i].body = dBodyCreate(world);
dMassSetBoxTotal(&m, 1, dai1_xl, dai1_yl, dai1_zl);
dBodySetMass(dai1[i].body, &m);
//dai1[i].geom = dCreateBox(space, dai1_xl, dai1_yl, dai1_zl);
//dGeomSetBody(dai1[i].geom, dai1[i].body);
//コマの台2
dai2[i].body = dBodyCreate(world);
dMassSetBoxTotal(&m, 2, dai2_xl, dai2_yl, dai2_zl);
dBodySetMass(dai2[i].body, &m);
//dai2[i].geom = dCreateBox(space, dai2_xl, dai2_yl, dai2_zl);
//dGeomSetBody(dai2[i].geom, dai2[i].body);
//コマの軸
ziku[i].geom = dCreateCapsule(space, zikuradius, zikulength);
// ⑯軸のジオメトリ作成
//ziku1 = new dCapsule(*space, zikuradius, zikulength);
// なんでkoma_body1にg1とziku1のgeomを同時にセットするのか。
// そんな事が可能なのか？
// 円柱状のgeomの上下に少しだけ細いカプセル状のgeomが突き出ている事になる
dGeomSetBody(ziku[i].geom, koma[i].body);
// ⑰軸のボディを作成せずにコマ本体にジオメトリを関連付ける
//ziku2->setBody(*koma_body2);
dBodySetGyroscopicMode(koma[i].body, 0);
//koma_body2->setGyroscopicMode(false);
// コマを姿勢制御モードにする
//上をコメントアウトするとこまが倒れやすい
}
}
//描画の初期化
void prepDrawStuff()
{
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
}
int main (int argc, char **argv)
{
prepDrawStuff();
dInitODE();
space = dSimpleSpaceCreate (0);
// ⑥dSpace型のオブジェクトを生成（衝突空間の生成）
//space = new dSimpleSpace(0);
contactgroup = dJointGroupCreate(0);
ground = dCreatePlane(space,0,0,1,0);
// ⑦
//dPlane *floor = new dPlane(*space, 0,0,1,0);
world = dWorldCreate();
//world = new dWorld();
dWorldSetGravity(world,0,0,-0.5f);
//world->setGravity(0,0,-0.5f);
dWorldSetCFM (world, 1e-5f);
//world->setCFM(1e-5f); //@eCFMを設定 弱い拘束にしているのか？
// http://demura.net/9ode/351.html 拘束について
// 小さな値では強い (hard) 拘束となり、大きな値では弱い (soft) 拘束となりま
// 既定値 10^-10 (単精度では10^-5) 範囲 10^-9 ～ 1
dWorldSetLinearDamping(world, 0.00001f);
// ⑤速度減衰率を設定0～1の間 デフォルトは0 つまり減衰しない？
//world->setLinearDamping(0.00001f);
dWorldSetLinearDamping(world, 0.00001f);
//角速度減衰率を設定
//world->setAngularDamping(0.0001f);
//上の2つの設定、限りなく0に近いので設定しなくてもいいのでは？
//こまの生成
makeKoma();
reset();
dsSimulationLoop (argc,argv,512,384,&fn);
dJointGroupEmpty(contactgroup);
//contactgroup.empty();
dWorldDestroy( world ); // 世界の破壊
//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();
}
0 件のコメント:
コメントを投稿