ODE環境下、
.\ode-0.12\ode\demo\demo_crash.cpp
を改変したサンプルプログラムを使ってODEの使い方を確認する。
int dInitODE2( unsigned int uiInitFlags /*=0*/ );引数のuiInitFlagsには、dInitODEFlags列挙体の値を指定する。 処理が成功した場合にはゼロ以外の値を返す。
enum dInitODEFlags
{
// dCleanupODEAllDataForThread()関数の呼び出し時に、
// スレッドのローカルデータが確実に消去されるようにする
dInitFlagManualThreadCleanup = 0x00000001,}}
void dCloseODE( void )ODE終了処理。 このAPIを呼び出す前にほか全てのワールド、スペース、ジョイントグループなどの開放を行う必要がある。
dWorldID dWorldCreate( void );ボディとジョイントを格納する動力学演算世界を生成する。
void dWorldDestroy ( dWorldID world );世界を破棄する。格納されるているボディと、ジョイント グループに含まれないジョイントも同時に破棄される。
void dWorldStep ( dWorldID world, dReal stepsize );worldに対してstepsize分の演算を進める。
void dWorldQuickStep ( dWorldID world, dReal stepsize );worldに対してstepsize分の処理速度を重視した演算を進める。
void dWorldSetQuickStepNumIterations ( dWorldID, int num );QuickStepのくり返し (iteration) 数を増加させることで、演算の精度を上げることができる。 しかし、その分処理時間がかかる。デフォルトはiteration=20
dSpaceID dSimpleSpaceCreate ( dSpaceID space );新規にスペースを生成する。スペースは物体を格納する空間のこと。 スペースの内部にスペースを配置することも可能。
dSpaceID dHashSpaceCreate ( dSpaceID space );引数のspace内に新しいスペースを作成する。 新規にスペースを作成するときは、引数に0を指定する。
？？？？
void dSpaceDestroy ( dSpaceID );スペースを破棄する。
void dSpaceCollide (
dSpaceID space, // 検査対象とするスペース
void *data, // コールバック関数に渡す任意のデータ
dNearCallback *callback // 衝突時に呼び出されるコールバック関数
);シミュレーションステップごとに衝突検出を実行する。 衝突が検出されると渡したコールバック関数が呼ばれる。 衝突する可能性のあるジオメトリが検出されたときには、 そのジオメトリの組ごとにそれを引数として指定のコールバック関数が呼び出される。
typedef void dNearCallback (
void *data, // dSpaceCollideから渡される任意のデータ
dGeomID o1, // 衝突する可能性のあるジオメトリ(スペース)1
dGeomID o2 // 衝突する可能性のあるジオメトリ(スペース)2
);この関数はジオメトリが衝突する可能性があるときに呼び出される。 本当に衝突しているかどうかは、接触情報を調べて確認する必要がある。
dGeomID dCreateBox ( dSpaceID space, dReal lx, dReal ly, dReal lz );直方体の生成。
dGeomID dCreateSphere ( dSpaceID space, dReal radius );球体の生成。
dGeomID dCreateCapsule ( dSpaceID space, dReal radius, dReal length );カプセルの生成。
dGeomID dCreateCylinder ( dSpaceID space, dReal radius, dReal length );円柱の生成。
dGeomID dCreatePlane ( dSpaceID space, dReal a, dReal b, dReal c, dReal d );平面の生成。
dBodyID dBodyCreate ( dWorldID );ボディーの生成。ワールドの破棄によって自動破棄される。
void dBodySetPosition ( dBodyID, dReal x, dReal y, dReal z );ボディの座標を指定する。
void dBodySetLinearVel ( dBodyID, dReal x, dReal y, dReal z );ボディに速度を設定する。
void dBodySetAngularVel ( dBodyID, dReal x, dReal y, dReal z );ボディに角速度を設定する。
void dBodyEnable ( dBodyID );ボディを有効化する。生成時は基本的に有効化状態。
void dBodyDisable ( dBodyID );ボディを無効化する。無効化されたボディはシミュレーション対象外となる。
int dBodyIsEnabled ( dBodyID );ボディの有効化状態を確認する。1=有効 0=無効
void dBodySetMass ( dBodyID, const dMass *mass );ボディに質量パラメータを設定する。
void dMassSetSphereTotal (
dMass *mass,
dReal total_mass, // 質量
dReal radius // 半径
);球体 (Sphere)
void dMassSetBoxTotal (
dMass *mass,
dReal total_mass, // 質量
dReal lx, // x軸方向の長さ
dReal ly, // y軸方向の長さ
dReal lz // z軸方向の長さ
);直方体 (Box)
void dMassSetCapsuleTotal (
dMass *mass,
dReal total_mass, // 質量
int direction, // 長軸方向(1=x軸 2=y軸 3=z軸)
dReal radius, // 半径
dReal length // 長さ
);カプセル (Capsule)
void dMassSetCylinderTotal (
dMass *mass,
dReal total_mass, // 質量
int direction, // 長軸方向(1=x軸 2=y軸 3=z軸)
dReal radius, // 半径
dReal length // 長さ
);円柱 (Cylinder)
void dGeomSetBody ( dGeomID geom, dBodyID body );ボディとジオメトリを関連付ける。
dBodyID dGeomGetBody ( dGeomID geom );ジオメトリに関連付けられたボディを取得する。
dJointCreateContact(
dWorldID,
dJointGroupID,
const dContact * // 接触面の定義
);接触のジョイントを生成する。
void dJointAttach (
dJointID joint, // 接続するジョイント
dBodyID body1, // 接続されるボディ1
dBodyID body2 // 接続されるボディ2
);2つのボディをジョイントで接続する。 指定したジョイントがすでに他のボディの接続に使用されていた場合には、そのボディの接続は解除される。 body1またはbody2に0が指定された場合には、他方のボディは静的な環境 (static environment) と接続され固定される。
dJointGroupID dJointGroupCreate ( int max_size );ジョイントグループを作成する。 max_sizeは互換性のために残されているもので、現在では常に0を指定。
void dJointGroupDestroy ( dJointGroupID );ジョイントグループを破棄する。
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: [email protected] Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// This is a demo of the QuickStep and StepFast methods,
// originally by David Whittaker.
#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
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define LENGTH 3.5 // chassis length
#define WIDTH 2.5 // chassis width
#define HEIGHT 1.0 // chassis height
#define RADIUS 0.5 // wheel radius
#define STARTZ 1.0 // starting height of chassis
#define CMASS 1 // chassis mass
#define WMASS 1 // wheel mass
#define COMOFFSET -5 // center of mass offset
#define WALLMASS 1 // wall box mass
#define BALLMASS 1 // ball mass
#define FMAX 25 // car engine fmax
#define ROWS 1 // rows of cars
#define COLS 1 // columns of cars
#define ITERS 15 // number of iterations
#define WBOXSIZE 0.5 // size of wall boxes
#define WALLWIDTH 20 // width of wall
#define WALLHEIGHT 20 // height of wall
#define DISABLE_THRESHOLD 0.008 // maximum velocity (squared) a body can have and be disabled
#define DISABLE_STEPS 20 // number of steps a box has to have been disable-able before it will be disabled
#define CANNON_X 15 // x position of cannon
#define CANNON_Y 0 // y position of cannon
#define CANNON_BALL_MASS 10 // mass of the cannon ball
#define CANNON_BALL_RADIUS 0.5
#define WALL
#define CANNON
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID body[10000];
static int bodies;
static dJointID joint[100000];
static int joints;
static dJointGroupID contactgroup;
static dGeomID ground;
static dGeomID box[10000];
static int boxes;
static dGeomID sphere[10000];
static int spheres;
static dGeomID wall_boxes[10000];
static dBodyID wall_bodies[10000];
static dGeomID cannon_ball_geom;
static dBodyID cannon_ball_body;
static int wb_stepsdis[10000];
static int wb;
static bool doFast;
static dBodyID b;
static dMass m;
// things that the user controls
static dReal turn = 0, speed = 0; // user commands
static dReal cannon_angle=0,cannon_elevation=-1.2;
// 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;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected(b1, b2))
return;
const int N = 4;
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;
if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass)
contact[i].surface.mu = 20;
else
contact[i].surface.mu = 0.5;
contact[i].surface.slip1 = 0.0;
contact[i].surface.slip2 = 0.0;
contact[i].surface.soft_erp = 0.8;
contact[i].surface.soft_cfm = 0.01;
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2));
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {3.8548f,9.0843f,7.5900f};
static float hpr[3] = {-145.5f,-22.5f,0.25f};
dsSetViewpoint (xyz,hpr);
printf ("Press:\t'a' to increase speed.\n"
"\t'z' to decrease speed.\n"
"\t',' to steer left.\n"
"\t'.' to steer right.\n"
"\t' ' to reset speed and steering.\n"
"\t'[' to turn the cannon left.\n"
"\t']' to turn the cannon right.\n"
"\t'1' to raise the cannon.\n"
"\t'2' to lower the cannon.\n"
"\t'x' to shoot from the cannon.\n"
"\t'f' to toggle fast step mode.\n"
"\t'r' to reset simulation.\n");
}
void resetSimulation()
{
int i;
i = 0;
// destroy world if it exists
if (bodies)
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
}
for (i = 0; i < 1000; i++)
wb_stepsdis[i] = 0;
// recreate world
world = dWorldCreate();
// space = dHashSpaceCreate( 0 );
// space = dSimpleSpaceCreate( 0 );
space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ );
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-1.5);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
dWorldSetQuickStepNumIterations (world,ITERS);
ground = dCreatePlane (space,0,0,1,0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
wb = 0;
#ifdef WALL
bool offset = false;
for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
{
offset = !offset;
for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
{
wall_bodies[wb] = dBodyCreate (world);
dBodySetPosition (wall_bodies[wb],-20,y,z);
dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dMassAdjust (&m, WALLMASS);
dBodySetMass (wall_bodies[wb],&m);
wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
//dBodyDisable(wall_bodies[wb++]);
wb++;
}
}
dMessage(0,"wall boxes: %i", wb);
#endif
#ifdef CANNON
cannon_ball_body = dBodyCreate (world);
cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
dBodySetMass (cannon_ball_body,&m);
dGeomSetBody (cannon_ball_geom,cannon_ball_body);
dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
#endif
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd) {
case 'a': case 'A':
speed += 0.3;
break;
case 'z': case 'Z':
speed -= 0.3;
break;
case ',':
turn += 0.1;
if (turn > 0.3)
turn = 0.3;
break;
case '.':
turn -= 0.1;
if (turn < -0.3)
turn = -0.3;
break;
case ' ':
speed = 0;
turn = 0;
break;
case 'f': case 'F':
doFast = !doFast;
break;
case 'r': case 'R':
resetSimulation();
break;
case '[':
cannon_angle += 0.1;
break;
case ']':
cannon_angle -= 0.1;
break;
case '1':
cannon_elevation += 0.1;
break;
case '2':
cannon_elevation -= 0.1;
break;
case 'x': case 'X': {
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2];
dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]);
dReal force = 10;
dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]);
dBodySetAngularVel (cannon_ball_body,0,0,0);
break;
}
}
}
// simulation loop
static void simLoop (int pause)
{
int i, j;
dsSetTexture (DS_WOOD);
if (!pause) {
for (j = 0; j < joints; j++)
{
dReal curturn = dJointGetHinge2Angle1 (joint[j]);
//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
dJointSetHinge2Param(joint[j],dParamVel2,speed);
dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
dBodyEnable(dJointGetBody(joint[j],0));
dBodyEnable(dJointGetBody(joint[j],1));
}
if (doFast)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
else
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
{
bool disable = true;
const dReal *lvel = dBodyGetLinearVel(b);
dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
if (lspeed > DISABLE_THRESHOLD)
disable = false;
const dReal *avel = dBodyGetAngularVel(b);
dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
if (aspeed > DISABLE_THRESHOLD)
disable = false;
if (disable)
wb_stepsdis[i]++;
else
wb_stepsdis[i] = 0;
if (wb_stepsdis[i] > DISABLE_STEPS)
{
dBodyDisable(b);
// dsSetColor(0.5,0.5,1);
}
// else
// dsSetColor(1,1,1);
}
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
else
{
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
dsSetColor(1,1,1);
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
dsSetColor (0,1,1);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
for (i = 0; i < boxes; i++)
dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
dsSetColor (1,1,1);
for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
dGeomGetRotation(sphere[i]),RADIUS);
// draw the cannon
dsSetColor (1,1,0);
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
dReal csides[3] = {2,2,2};
dsDrawBox (cpos,R2,csides);
for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
dsDrawCylinder (cpos,R4,3,0.5);
// draw the cannon ball
dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
CANNON_BALL_RADIUS);
}
int main (int argc, char **argv)
{
doFast = true;
// 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;
dInitODE2(0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
resetSimulation();
// run simulation
dsSimulationLoop (argc,argv,1600,900,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}