//振り子を作成してみる
//sample11org.cppコードままだと 手前向こう側にｙの＋方向、右がｘの＋
//方向になる 参考 以下のサイト
//振り子 http://www10.atwiki.jp/bambooflow/pages/232.html
//ジョイント http://demura.net/tutorials/ode7
//球の描画と衝突判定 http://www.natural-science.or.jp/article/20110211121736.php
//000000000000ファイルの分割化をしたい。
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // VC++用警告を止める
#endif
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#endif
void objSetParameters();
void makeBody(int n);
void makeBodyMain();
void makeFixedJoint(dBodyID b1);
void makeBallJoint();
void makeJointMain();
void drawObj(int i);
void drawObjMain();
void nearCallback (void *data, dGeomID o1, dGeomID o2);
void simLoop (int pause);
void start();
void setDrawStuff();
static dWorldID world;
static dSpaceID space;
static dGeomID ground;
static dJointID fixed;
static dJointID joint_ball; //ボールジョイント
static dJointGroupID contactgroup;
const int obj_n = 200; //作成するオブジェクトの数 ｃ言語では動的に配列を
//作成することがむずかしい。とりあえず200ということで・
dsFunctions fn;
//各オブジェクトのパラメータ
typedef struct {
dBodyID body;
dGeomID geom;
dReal radius,length,width,height,mass;
dReal posX, posY, posZ;
//dReal color[3];//配列はこれではエラーが出る
dReal color1;
dReal color2;
dReal color3;
} MyObject;
MyObject obj[obj_n];
//obj[0].posX=0.0;
////じかに上のようにposXを代入すると エラーがでるようだ
////関数の中ですると エラーがでない
//構造体をクラスにかえては、エラーがでる
//パラメータをセットする
void objSetParameters()
{
//dReal color[][3] = {{1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}};
//上のボックス
obj[0].posX=0.0; obj[0].posY=0.0; obj[0].posZ=2.0;
obj[0].length = 0.2; obj[0].width = 0.2;
obj[0].height = 0.2; obj[0].mass = 1.0;
obj[0].color1 = 1.0; obj[0].color2 = 0.0; obj[0].color3 = 0.0;
//下のボックス
obj[1].posX=1.0; obj[1].posY=1.0; obj[1].posZ=2.0;
obj[1].length = 0.2; obj[1].width = 0.2;
obj[1].height = 0.2; obj[1].mass = 1.0;
obj[1].color1 = 0.0; obj[1].color2 = 0.0; obj[1].color3 = 1.0;
}
//剛体 ジオメトリの作成
void makeBodyMain()
{
makeBody(0);
makeBody(1);
}
void makeBody(int n)
{
dMass m1;
obj[n].body = dBodyCreate(world);
dMassSetZero(&m1);
//質量構造体の初期化
dMassSetBoxTotal(&m1,obj[n].mass,obj[n].length,obj[n].width,obj[n].height);
dBodySetMass(obj[n].body,&m1);
dBodySetPosition(obj[n].body,
obj[n].posX, obj[n].posY, obj[n].posZ);
obj[n].geom = dCreateBox(space,obj[n].length,obj[n].width,obj[n].height);
dGeomSetBody(obj[n].geom,obj[n].body);
}
//描画
void drawObjMain()
{
drawObj(0);
drawObj(1);
const dReal *posA;
const dReal *posB;
//ひもを描く
dsSetColor(0.0,1.0,0.0);
posA = dBodyGetPosition(obj[0].body);
posB = dBodyGetPosition(obj[1].body);
dsDrawLine(posA, posB);
}
void drawObj(int i)
{
dsSetColor(obj[i].color1, obj[i].color2, obj[i].color3);
dReal sides1[] = {obj[i].length,obj[i].width,obj[i].height};
dsDrawBoxD(dBodyGetPosition(obj[i].body),
dBodyGetRotation(obj[i].body),sides1);
}
//ジョイント
void makeJointMain()
{
makeFixedJoint(obj[0].body);
makeBallJoint();
}
void makeFixedJoint(dBodyID b1)
{
// fixed joint(固定ジョイント)
fixed = dJointCreateFixed(world,0);//0はdJointGroupID たいがい0でいいみたい
dJointAttach(fixed,NULL,b1);
dJointSetFixed(fixed);
}
void makeBallJoint()
{
//ボールジョイント
dReal x1 = 0.0, y1 = 0.0, z1 = 2.0;
//次回ここから00000000000000000
joint_ball = dJointCreateBall(world,0);//0はdJointGroupID たいがい0でいいみたい
dJointSetBallAnchor(joint_ball, x1, y1, z1);
dJointAttach(joint_ball, obj[0].body, obj[1].body);
}
//以下 ファイルの最後までodeのデフォルトの関数郡が連なる
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
static const int MAX_CONTACTS = 10;
int i;
// 2つのボディがジョイントで結合されていたら衝突検出しない
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
// dAreConnected: ボディbody1とbody2がジョイントで結合されていたら
// 1 を返 し，それ以外は0を返す
// つまり このif文はボディがジョイントで結合していたらこの関数から
// 抜ける、つまり衝突検出をしない。
dContact contact[MAX_CONTACTS];
//接触情報を格納する配列
int numc = dCollide(
o1,
o2,
MAX_CONTACTS, //接触点の最大数
&contact[0].geom, //接触情報を格納するdContactGeom構造体
sizeof(dContact) //引数はここはdContactかdContactGeomでいいみたい
);
//dCollide: 接触情報を作成する 戻り値は接触点の数 衝突しない場合は0
if (numc > 0)
{
//接触していたら
for (i=0; i<numc; i++)
{
//接触面の性質を設定する
contact[i].surface.mode = dContactSoftCFM | dContactSoftERP;
//接触面の性質
contact[i].surface.mu = dInfinity;
//摩擦係数（摩擦方向1）
contact[i].surface.soft_cfm = 1e-8;
//CFMの値
contact[i].surface.soft_erp = 1.0;
//ERPの値
dJointID c = dJointCreateContact(
world,
contactgroup,
&contact[i] //接触情報
);
//接触ジョイントの作成
//接触しボディを接触ジョイントにより接続する
dJointAttach (
c,
dGeomGetBody(contact[i].geom.g1),
//dContact構造体 contact[i]の中のdContactGeom構造体
//のジオメトリg1（「の」は「.」に対応）
dGeomGetBody(contact[i].geom.g2)
);
}
}
}
static void simLoop (int pause)
{
static int steps = 0;
dSpaceCollide(space,0,&nearCallback);
dWorldStep(world,0.001);
dJointGroupEmpty(contactgroup);
//情報を取得するジョイントを設定
drawObjMain();
}
void start()
{
static float xyz[3] = {0.0,-3.0,1.0};
static float hpr[3] = {90.0,0.0,0.0};
dsSetViewpoint (xyz,hpr);
}
void setDrawStuff()
{
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../../drawstuff/textures";
}
int main (int argc, char **argv)
{
setDrawStuff();
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dWorldSetGravity(world,0,0,-9.8);
// Create a ground
ground = dCreatePlane(space,0,0,1,0);
objSetParameters();
makeBodyMain();
makeJointMain();
// set feed back
dsSimulationLoop(argc,argv,352,288,&fn);//なかにdrawObjMainを含む
dWorldDestroy(world);
dCloseODE();
return 0;
}
0 件のコメント:
コメントを投稿