odeのdemo_buggyを少し改変してみた。
//demo_buggy
//元コードではウィンドウが小さくジャンプ台が見えないのでウィンドウを大きく
//視点をかえ、車の移動につれ視点が動くようにした。
//コメントは以下サイト参考
//http://www.cl.mse.tcu.ac.jp/lab/edu/p4rc/dl/demo_buggy-with-comments.pdf
//使いやすいキーでbuggyを動かすようにする
//コードも自分にわかりやすくする。
//自動走行、 ジャイロ効果を使う
//wheel[i]の使い方が間違っている
#include ＜stdio.h＞
#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
#endif
// some constants
// chassis 車の車台、ボディ
//#define LENGTH 0.7 // chassis length
//#define WIDTH 0.5 // chassis width
//#define HEIGHT 0.2 // chassis height
//#define RADIUS 0.18 // wheel radius
//#define STARTZ 0.5 // starting height of chassis
//#define CMASS 1 // chassis mass
//#define WMASS 0.2 // wheel mass
#define CAR_L 0.7 // chassis length
#define CAR_W 0.5 // chassis width
#define CAR_H 0.2 // chassis height
#define CAR_M 1 // chassis mass
#define CAR_START_H 0.5 // starting height of chassis
#define WHEEL_R 0.18 // wheel radius
#define WHEEL_M 0.2 // wheel mass
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID body[4]; //車体と車輪3つの剛体
static dJointID joint[3]; // joint[0] is the front wheel
static dJointGroupID contactgroup;
static dGeomID ground;
static dSpaceID car_space;
//car_spaceというspeaceを作成し車体と車輪接触ジョイントをなくし、同時にジオメトリをグループ化して「同時」に動くようにしている
static dGeomID box[1]; //box[0]で車体のジオメトリを表現
static dGeomID sphere[3]; //sphere[0～2]で車輪のジオメトリを表現
static dGeomID ground_box;
//ジャンプ台のジオメトリ ジャンプ台は「落下」する必要がないため「剛体」がない
const int WHEE_ALL = 3; //車輪の数
typedef struct {
dBodyID body; //動力学計算用のボディ
dGeomID geom; //衝突計算用のジオメトリ
} MyObject;
MyObject car, wheel[WHEE_ALL], jump_dai;
// things that the user controls
//speedは車輪回転角速度[ rad / s ] , steerは目標操舵角度[ rad ]を表す
static dReal speed=0, steer=0; // user commands
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i, n;
// only collide things with the ground
int g1 = (o1 == ground || o1 == jump_dai.geom);
int g2 = (o2 == ground || o2 == jump_dai.geom);
if (!(g1 ^ g2)) return;
const int N = 10;
dContact contact[N];
n = dCollide (o1, o2, N, &contact[0].geom, sizeof(dContact));
if (n ＞ 0) {
for (i=0; i＜n; i++) {
contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = dInfinity;
contact[i].surface.slip1 = 0.1;
contact[i].surface.slip2 = 0.1;
contact[i].surface.soft_erp = 0.5;
contact[i].surface.soft_cfm = 0.3;
dJointID c = dJointCreateContact (world, contactgroup, &contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
//static float xyz[3] = {0.8317f, -0.9817f, 0.8000f};
//static float hpr[3] = {121.0000f, -27.5000f, 0.0000f};
static float xyz[3] = {3.10f, -2.24f, 0.95f};
static float hpr[3] = {120.0f, -25.50f, 0.00f};
dsSetViewpoint (xyz, hpr);
printf ("Press:\t'8' to increase speed.\n"
"\t'2' to decrease speed.\n"
"\t'4' to steer left.\n"
"\t'6' to steer right.\n"
"\t'0' to reset speed and steering.\n"
"\t'1' to save the current state to 'state.dif'.\n");
//buggyを操作しやすいようにキーを変える
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd) {
case '8': case 'A':
speed += 0.3;
break;
case '2': case 'Z':
speed -= 0.3;
break;
case '4':
steer -= 0.5;
break;
case '6':
steer += 0.5;
break;
case '0':
speed = 0;
steer = 0;
break;
case '1':
{
FILE *f = fopen ("state.dif", "wt");
if (f) {
dWorldExportDIF (world, f, "");
//このコマンドodeの状態をファイルにエクスポートするらしいが
//インポートのコマンドはないし、なににつかえるのか？
fclose (f);
}
}
case 's': // sキーを押すと視点，視線を表示
{
float xyz[3], hpr[3]; // 視点, 視線
dsGetViewpoint(xyz, hpr); // 視点，視線を取得
printf("xyz=%4.2f %4.2f %4.2f ", xyz[0], xyz[1], xyz[2]);
printf("hpr=%6.2f %6.2f %5.2f \n", hpr[0], hpr[1], hpr[2]);
}
break;
}
}
// simulation loop
static void simLoop (int pause)
{
int i;
if (!pause) {
// motor
// ヒンジジョイント2の画像
// file:///E:/scrapbook2/data/20150501175023/hinge2.jpg
dJointSetHinge2Param (joint[0], dParamVel2, -speed);
//前輪回転速度を- speed [ rad / s ]に設定
//dParamVel2はHinge2の軸2の回転速度の設定になるのだろう
dJointSetHinge2Param (joint[0], dParamFMax2, 0.1);
//前輪最大トルクを0.1 Nmに設定
// steering
dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
// 軸1の角度 通常のヒンジジョイントの角度のようなもの
//目標操舵角steerと実際の操舵角の差を計算
if (v ＞ 0.1) v = 0.1; //±0 . 1で飽和
if (v ＜ -0.1) v = -0.1;
v *= 10.0;
// vは±1の範囲で目標操舵角と実際の操舵角の差を10倍にする
dJointSetHinge2Param (joint[0], dParamVel, v);
//前輪操舵回転速度をｖ[rad/s]とする
dJointSetHinge2Param (joint[0], dParamFMax, 0.2);
//前輪操舵トルクの最大値を0.2 Nmにする
dJointSetHinge2Param (joint[0], dParamLoStop, -0.75);
//前輪操舵角の最小角を-0.75 radにする
dJointSetHinge2Param (joint[0], dParamHiStop, 0.75);
//前輪操舵角の最大角を0.75 radにする
dJointSetHinge2Param (joint[0], dParamFudgeFactor, 0.1);
//停止状態から動作を始めるときに発生する、過大な力を抑制する。これは実装上
//の問題であり、このパラメータはそれをごまかす (fudge)。 0 ～ 1
dSpaceCollide (space, 0, &nearCallback);
dWorldStep (world, 0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
dsSetColor (0, 1, 1);
dsSetTexture (DS_WOOD); //木目のテクスチャ
//●車台を描画
dReal sides[3] = {CAR_L, CAR_W, CAR_H};
dsDrawBox(dBodyGetPosition(car.body), dBodyGetRotation(car.body), sides);
//●車輪の描画
dsSetColor (1, 1, 1);
for (i=0; i＜3; i++){
dsDrawCylinder(
dBodyGetPosition(wheel[i].body),
dBodyGetRotation(wheel[i].body),
0.02f, WHEEL_R);
}
//for (i=0; i＜3; i++) dsDrawCylinder(dBodyGetPosition(wheel[i].body),
// dBodyGetRotation(wheel[i].body), 0.02f, WHEEL_R);
//●ジャンプ台の描画
//main関数で以下のコードがあり、ground_boxの大きさが設定されている
//ground_box = dCreateBox (space, 2, 1.5, 1);
//その前に以下のコードがでてくるのでわかりずらい。
dVector3 ss;
dGeomBoxGetLengths(jump_dai.geom, ss);
//mainで大きさ設定済みのground_boxから各ｘｙｘ辺の大きさを得ているのだろう
//そんな事しなくても、ファイル先頭で定義するばいいのにと思うが。
dsDrawBox(dGeomGetPosition(jump_dai.geom), dGeomGetRotation(jump_dai.geom), ss);
//ジャンプ台描画
/*
printf ("%.10f %.10f %.10f %.10f\n",
dJointGetHingeAngle (joint[1]),
dJointGetHingeAngle (joint[2]),
dJointGetHingeAngleRate (joint[1]),
dJointGetHingeAngleRate (joint[2]));
*/
////●視点を車の移動につれて動かす
const dReal *P = dBodyGetPosition(car.body);
float xyz[3] = {3.10f+P[0], -2.24f+P[1], 0.95f};
float hpr[3] = {120.0f, -25.50f, 0.00f};
dsSetViewpoint (xyz, hpr);
}
int main (int argc, char **argv)
{
int i;
dMass m;
// 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
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world, 0, 0, -0.5);
ground = dCreatePlane (space, 0, 0, 1, 0);
// chassis body
car.body = dBodyCreate (world); //車体
dBodySetPosition (car.body, 0, 0, CAR_START_H);
dMassSetBox (&m, 1, CAR_L, CAR_W, CAR_H);
//たぶんここの1は密度をさしているものと思う
//dMassAdjust: dMassSetXx()によって密度から設定した場合に、その質量を設定す
//るのに有効です。
//だから以下のコマンドで CAR_M=1 で全体の質量を1に調整している
//はじめから dMassSetXxTotal()※ … 質量 (Mass) から設定すればいいではないか
//わけがわからん
dMassAdjust (&m, CAR_M); //車体の質量の設定
dBodySetMass (car.body, &m);
car.geom = dCreateBox (0, CAR_L, CAR_W, CAR_H);
//最初の引数がspeaceでなく0となっているので....
//box[0]ではなくbody_geom とかわかりやすい表現してほしい
dGeomSetBody (car.geom, car.body);
// wheel bodies 車輪
// i = 0 :前輪，i = 1 :左後輪，i = 2 :右後輪
for (i=0; i＜3; i++) {
wheel[i].body = dBodyCreate (world);
dQuaternion q;
//dQuaternion: typedef dReal dQuaternion[ 4 ] dRealの配列
dQFromAxisAndAngle (q, 1, 0, 0, M_PI*0.5);
// (q, x, y , z, 回転各)となり上はｘ軸で90度の回転を示す
// 参考サイトはｚの回転と記述されていたが、まちがい
//qに q以後の4つの引数を入れていることになるのか
dBodySetQuaternion (wheel[i].body, q);
//wheel[i].bodyをクォータニオンqをつかって回転
//これで通常の車のタイヤ位置、方向になる
dMassSetSphere (&m, 1, WHEEL_R);
dMassAdjust (&m, WHEEL_M);
dBodySetMass (wheel[i].body, &m);
wheel[i].geom = dCreateSphere (0, WHEEL_R);
//球状のgeomを作成 シリンダーではないんだ。描画はシリンダーでされている
dGeomSetBody (wheel[i].geom, wheel[i].body);
}
dBodySetPosition (wheel[0].body, 0.5*CAR_L, 0, CAR_START_H-CAR_H*0.5);
dBodySetPosition (wheel[1].body, -0.5*CAR_L, CAR_W*0.5, CAR_START_H-CAR_H*0.5);
dBodySetPosition (wheel[2].body, -0.5*CAR_L, -CAR_W*0.5, CAR_START_H-CAR_H*0.5);
// front wheel hinge
/*
joint[0] = dJointCreateHinge2 (world, 0);
dJointAttach (joint[0], body[0], body[1]);
const dReal *a = dBodyGetPosition (body[1]);
dJointSetHinge2Anchor (joint[0], a[0], a[1], a[2]);
dJointSetHinge2Axis1 (joint[0], 0, 0, 1);
dJointSetHinge2Axis2 (joint[0], 0, 1, 0);
*/
// front and back wheel hinges
// ???ジョイントの位置を決定 ジョイント番号：0 =左前, 1 =右前, 2 =左後
// i = 0 :前輪，i = 1 :左後輪，i = 2 :右後輪
for (i=0; i＜3; i++) {
joint[i] = dJointCreateHinge2 (world, 0);
dJointAttach (joint[i], car.body, wheel[i].body);
const dReal *a = dBodyGetPosition (wheel[i].body);
dJointSetHinge2Anchor (joint[i], a[0], a[1], a[2]);
dJointSetHinge2Axis1 (joint[i], 0, 0, 1);
dJointSetHinge2Axis2 (joint[i], 0, 1, 0);
//このジョイントの位置だと、当然接触ジョイントが発生しないようになってい
//るのだろう？
}
// set joint suspension
// 車輪と車体の間のサスペンション
// i = 0 :前輪，i = 1 :左後輪，i = 2 :右後輪
for (i=0; i＜3; i++) {
dJointSetHinge2Param (joint[i], dParamSuspensionERP, 0.4);
dJointSetHinge2Param (joint[i], dParamSuspensionCFM, 0.8);
//大きくなるほどソフト拘束 0から1の間 たぶんソフト拘束している
}
// lock back wheels along the steering axis
// 左右の後輪が動かないように固定する
// i = 0 :前輪，i = 1 :左後輪，i = 2 :右後輪
for (i=1; i＜3; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (joint[i], dParamLoStop, 0);
dJointSetHinge2Param (joint[i], dParamHiStop, 0);
// the following alternative method is no good as the wheels may get out
// of alignment:
// 次のように速度をゼロにしてもうまくいかない
// dJointSetHinge2Param (joint[i], dParamVel, 0);
// dJointSetHinge2Param (joint[i], dParamFMax, dInfinity);
}
// create car space and add it to the top level space
// 上の設定を反映する
car_space = dSimpleSpaceCreate (space);
//space = dHashSpaceCreate (0);
// d***SpaceCreate()は
//引数のspace内に新しいスペースを作成します。新規にスペースを作成するとき
//は、引数に0を指定します。
//だから上のdHashSpaceCreateで作成されたspeace内にcar_spaceを作成している
//スペース (Space):
//ジオメトリを格納する入れ物であり、衝突検出の対象を含む空間です。また、ス
//ペース内に他のスペースを含むこともできます。
dSpaceSetCleanup (car_space, 0);
//スペースのクリーンナップモードを有効にすることで、スペースを破棄したときに
//そこに含まれるジオメトリも自動的に破棄されるようになります。これは既定で有
//効となっています。
//引数modeに1を設定することで有効となり、0で無効となります。
//なんで無効にする必要があるのか？
// モード1にして 以下の①のdGeomDestroyをコメントアウトしても変わりなかった
// けれど........
//以下 各ジオメトリをcar_spaceに入れる
dSpaceAdd (car_space, car.geom);
dSpaceAdd (car_space, wheel[0].geom);
dSpaceAdd (car_space, wheel[1].geom);
dSpaceAdd (car_space, wheel[2].geom);
// environment
// ジャンプ台
jump_dai.geom = dCreateBox (space, 2, 1.5, 1);
dMatrix3 R;
dRFromAxisAndAngle (R, 0, 1, 0, -0.15); //4.5度かな？
dGeomSetPosition (jump_dai.geom, 2, 0, -0.34);
dGeomSetRotation (jump_dai.geom, R);
// run simulation
dsSimulationLoop (argc, argv, 652, 488, &fn);
//ウィンドウを元コードより大きくした
//①以下クリーンナップモードの関係で個別にdGeomDestroyしているのか？
dGeomDestroy (car.geom);
dGeomDestroy (wheel[0].geom);
dGeomDestroy (wheel[1].geom);
dGeomDestroy (sphere[2]);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
0 件のコメント:
コメントを投稿