初心者さんからご質問があったので、よりわかりやすくするために赤いボールが地面に接触したら青色になるようにプログラムを改良しました。ゲーム開発やロボットの研究者にも使われているオープンソースの物理計算エンジンODE (Open Dynamics Engine、オープン ダイナミクスエンジン）を学ぶODE講座の第６回目です。
今回は衝突検出機能について勉強します。前回のプログラムでは衝突検出機能がなかったので、ボールが地面をすり抜けていったわけです。ODEでは動力学計算と衝突検出計算が別々に実装されています。動力学計算をするためには、ワールドworldをdWorldCreate()で生成し、その中に剛体bodyを生成し、dWorldStep()で動力学計算をしましたね。
ジオメトリ
ジオメトリとは物体の形状という意味で、下図の左からsphere（球）、box（直方体）、cylinder（円柱）、capsule（カプセル）などの種類があります。
一方、衝突検出計算をするためには、スペースspaceをdHashSpaceCreate()で生成し、その中に剛体bodyに対応するジオメトリgeometryを生成しなければなりません。以下のサンプルプログラムでは、８５行目で球に対応するジオメトリをdCreateSphere()で生成しています。８６行目で、動力学計算の対象となるボディball.bodyと衝突検出計算の対象となるball.geomを結びつけています。これでODEの物体が完成です。
次に、simLoop関数の中で、dSpaceCollide()を呼び出します。このAPIは衝突しそうな２つのジオメトリが発生したら、それらをnearCallback関数に渡します。nearCallback関数の引数o1, o2が衝突する可能性のある２つのジオメトリです。nearCallback関数では、接触点を算出したり、接触点の性質などを設定します。 なお、注意しなければいけないことは、引数o1、o2は接触する可能性があるだけで、実際に接触しているかどうかよくわからないことです。それを知るためにはdCollide()の戻り値、つまり、接触点数が１以上かどうかチェックすれば良いのです。
また、７４行目にあるように接触点の集まりが格納される入れ物をdJointGroupCreate()で生成し、シミュレーションループで毎回それをdJointGroupEmpty()を使って空にしなければなりません。サンプルプログラムでは５７行目でやっています。これを忘れると１ステップ前の接触点達が悪さをしますのでお忘れなく！
- 衝突検出に関するAPI
- dSpaceID dHashSpaceCreate(0)
衝突計算用スペースを生成し、そのID（識別子）を返す。 - dGeomID dCreatePlane(dSpaceID space ,dReal a, dReal b, dReal c, dReal d)
spaceにax+by+cz=dの平面ジオメトリを生成する。 - dGeomID dCreateSphere(dSpaceID space, dReal r)
spaceに半径rの球ジオメトリを生成する。 - void dGeomSetBody(dGeomID geom, dBodyID body)
物体の2つの属性であるジオメトリgeomと剛体bodyを関連づける。 - dJointGroupID dJointGroupCreate(0)
接触点のグループを格納するジョイントグループを生成し、そのIDを返す。 - void dJointGroupEmpty(dJointGroupID)
接触点が格納されているジョイントグループを空にする。
- dSpaceID dHashSpaceCreate(0)
以下にソースコードを示します。 前回のプログラムと違うところだけコメントを入れています。
さて、ソースコードをさっそく読みましょう。なお、前回のsample1 と全く同じstart関数やprepDrawStuff関数は省略しています。
[code]
// sample2.cpp by でむ
#include
#include
#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD // 単精度と倍精度の描画関数に対応するおまじない
#endif
static dWorldID world; // 動力学計算用のワールド
static dSpaceID space; // 衝突検出用のスペース
static dGeomID ground; // 地面
static dJointGroupID contactgroup; // コンタクトグループ
static int flag = 0; // 地面との接触を示すflag
dsFunctions fn;
const dReal radius = 0.2, mass = 1.0; // 半径[m]、質量[m]
// 前回は動力学計算用のbodyだけでしたが、今回は衝突検出用のgeomが加わったので ボールオブジェクトを構造体で定義しました。
typedef struct {
dBodyID body; // 動力学計算用
dGeomID geom; // 衝突検出用
} MyObject;
MyObject ball; // ボールオブジェクト
// 衝突検出のコールバック関数
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
static const int N = 4; // 接触点数の上限は4個 staticを忘れずにつけてください．
dContact contact[N];
int isGround = ((ground == o1) || (ground == o2)); // 衝突する２つのうちどちらかが地面ならisGroundのフラグを立てる
int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact)); // nは衝突点数
if (isGround) { // 大地の旗が立っていたら衝突検出機能を働かせる
if (n >= 1) flag = 1;
else flag = 0;
for (int i = 0; i < n; i++) {
contact[i].surface.mode = dContactBounce; // 地面の反発係数を設定
contact[i].surface.bounce = 0.0; // (0.0~1.0) 反発係数は0から1まで
contact[i].surface.bounce_vel = 0.0; // (0.0以上) 反発に必要な最低速度
// コンタクトジョイント生成
dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
// 接触している２つのgeometryをコンタクトジョイントで拘束
dJointAttach (c,dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
static void simLoop (int pause)
{
const dReal *pos,*R;
flag = 0; // 地面との衝突flagを０にセット
dSpaceCollide(space,0,&nearCallback); // 衝突判定、これは一番最初に書くこと。
dWorldStep(world,0.01); // シミュレーションを1ステップ進める
dJointGroupEmpty(contactgroup); // ジョイントグループを空にする
if (flag == 0) dsSetColor(1.0, 0.0, 0.0); // 赤色の設定
else dsSetColor(0.0, 0.0, 1.0); // 青色の設定
pos = dBodyGetPosition(ball.body); // 位置
R = dBodyGetRotation(ball.body); // 回転行列
dsDrawSphere(pos,R,radius); // 球の描画
}
int main (int argc, char **argv)
{
dReal x0 = 0.0, y0 = 0.0, z0 = 2.0;
dMass m1;
prepDrawStuff();
world = dWorldCreate();
dWorldSetGravity(world,0,0,-0.5);
space = dHashSpaceCreate(0); // 衝突検出スペースの生成
contactgroup = dJointGroupCreate(0); // コンタクトグループ（接触点が格納される）の生成
// 地面の生成
ground = dCreatePlane(space,0,0,1,0); // 地面用ジオメトリの生成
// 球の生成l
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1,mass,radius);
dBodySetMass(ball.body,&m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space,radius); // 玉ジオメトリの生成
dGeomSetBody(ball.geom,ball.body); // bodyにgeomをセット
dsSimulationLoop (argc,argv,352,288,&fn);
dSpaceDestroy(space); // スペースの破壊
dWorldDestroy (world);
return 0;
}
[/code]
オブジェクトも動力学計算用のbody(ボディ）の他に衝突検出計算用にgeom（ジオメトリ）を設定する必要があるのでMyObject 構造体でそれらをメンバとして定義していますね。
main関数の中で玉オブジェクトのgeomをdCreateSphere()で作り、dGeomSetBodyでbodyとgeomを関連付けていますのでオブジェクトの位置と姿勢はbodyだけで設定すればOKです。これをしないと幽体離脱現象に陥ってしまいます。
衝突検出関数dSpaceCollideはシミュレーションの各ステップで実行されるsimLoop関数の中で呼び出されています。注意する点としては、必ずsimLoopの一番始めで呼び出してください。これを後のほうにもっていくと玉が地面を突き抜けてしまいますよ。dSpaceCollideではコー ルバック関数nearCallbackを呼び出しています。31行目で接触する可能性のある２つの物体のうち、どちらかが地面groundならisGroundをtrueにセットします。32行目のdCollide()の戻り値は接触点数です。
３４行目のif (isGround)文は，衝突する可能性のある物体のうちどちらかが地面なら３５行目から４８行目までの処理をします．つまり，地面との衝突以外は考えていません．地面以外との衝突を考慮する場合は，このif文を削除してください．
３５行目で地面でかつ接触点数が１以上なら、地面と接触したことを示すflagを１にセットしています。
では、ここからソースコードsample2-080124.tgzをダウンロードして実行してください。今度は地面を突き抜けないはずです。 実行の仕方は前回とほぼ同じです。sample1をsample2と読み替えてください。
課題を１つ出します。sample2を実行すると大地が好きなのかどうかよくわかりませんがボールがまったく弾みません。 この呪縛を解いてください。
では、また次回！
更新履歴
- 2008-6-26: nearCallbackの説明を補足（赤字）．
- 2008-1-24: 接触したら色が変わるようにプログラムを変更し、拡張子をcppにした。
- 2008-1-13: ソースコードを整形した．
- 2007-9-19: sample2.tgzのmakefileを変更し怒られずに済むようにした。
- 2007-1-20: 本文の説明をより詳しくし、ジオメトリの種類の図を追加ました。
コメント
はじめましてmituです
今自分は動力学シミュレータを作成しているのですが，
動力学計算を自分のシミュレータで行い，衝突計算は
ODEでやりたいと考えています．
サンプルでは接触点からの反発力は質量を考慮していると
思うのですが，自分はこれを質量を考慮せずに接触点の沈み
具合から計算できると考えているのですが，可能でしょうか？
mituさん、
はじめました。
以下について教えてください。
1. サンプルとは具体的に何ですか？
2. 接触点の沈みとはPenetration Depthのことですか？
3. 「接触点の沈み具合から計算できる」とは「接触点の沈み具合から反発力を計算できる」ということですか？
4. 拘束力の計算はmituさんがやられるのですか？それともODEでしょうか？
でむ
mituです
サンプルというのは球体の落下のプログラムです．
今自分は衝突検出をODEで行ってもらい，そこで生じる接触力（衝撃力）
の計算は自分の動力学シミュレータの方で行おうと考えています．
接触力などは形状などの動力学を考慮しないものだけでは出せないと思いますので．
ということを考えていたのですが，たぶん接触力の計算に関してはペナルティ法の
ようなやり方になるのかと考えています．
また，質問が増えるのですが，現在は接触点の位置や法線の情報をどのように返せば
いいでしょうか？
他にも，球体に速度を与えてODEで動力学計算を行わない（dWorldStep関数を使わない）
場合，接触後の球体の速度は返せますか？
お返事ありがとうございます
そうだったんですか。知りませんでした。
他の形で試して見ます。ありがとうございました。
ミツルギさん、
円柱プリミティブが導入されたのはＯＤＥ0.6からでまだコードが熟成していないようです。
ちなみに、円柱と円柱、円柱とカプセルの衝突検出はODE0.7でもまだ実装されていません。衝突の検出状況はODE Wikiのマニュアル(http://opende.sourceforge.net/wiki/index.php/Manual_%28Collision_Detection%29)をご覧ください。
この前のものをいろいろとまた変えてみたのですがやはりまだ円柱同士ではすり抜けてしまいます。
ただデバックしてみたところnearCallbackには円柱同士の衝突でも入っているようです。
やはりsurfaceのあたりの設定がおかしいのでしょうか？
はっきりいってこの辺がまったくわかりません…よろしくお願いします。
お返事ありがとうございます。
下が自分の関数です
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if(b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return;
static const int N = 4;
dContact contact[N];
int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
if(n>0)
{
for(int i=0;i<n;i++)
{
contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
contact[i].surface.mu = dInfinity;
contact[i].surface.bounce = 0.0;
contact[i].surface.bounce_vel = 0.0;
contact[i].surface.slip1 = 0.1;
contact[i].surface.slip2 = 0.1;
contact[i].surface.soft_erp = 0.8;
contact[i].surface.soft_cfm = 1e-4;
dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
dJointAttach(c,b1,b2);
}
}
}
サンプルと大して変わっていないです。片岡さんの書き込みを見つつ変えてみたのですがやはり
検出が出来てないようです。
modeに入れるdContactSlip1などの意味がわからないまま使っています。おかしいのはその辺でしょうか？
片岡さん、
拡張子を.cか.cppにするかで迷っているのでとても参考になりました。どうもありがとうございます。
経緯を簡単に説明すると、この講座では多くの方に理解できるようにC言語でプログラムを書いています。ただ、99年に標準化されたC99を使って書いているのでコンパイラの標準設定ならエラーがでるかもしれません。gccなら-std=c99のオプションをつけないとエラーがでる場合があります。また、昨年、cygwinでサンプルプログラムを書いたときにg++でないとmakeがどうしても通らなかったのでコンパイラにはg++を使っています。コンパイラにc++言語用のg++を使っているので拡張子を.cppにする方がすっきりするし、コンパイル（ビルド）するときにエラーが出ると初心者は途方にくれるので.cppに変更していきたいと思います。
片岡さん、
VC++でのbounce_velの初期値を調べて頂きありがとうございます。ODEのソースコードを読みましたがbounce_velは明示的に初期化されていません。明示的に0で初期化すべきだと考えます。本家のメーリングリストにポストしてみます。
細かいことを言って申し訳ないのですが、
ODEのVC用プロジェクトの初期設定を使うとサンプルファイルの形式が「.c」だと
C++じゃなくてC言語と誤認してしまってエラーが出てしまうので「.cpp」に変えておいた方が
VCを使う人にもわかりやすいと思います。
ありがとうございます。
調べた結果VC++でのbounce_velの初期値は-107374176.0fでした。
片岡さん，
mingw(g++)とVisual C++では一部挙動が異なるところがあるようですね．
どうもありがとうございます．サンプルコードを修正します．
なお，反発部分のソースコードは以下となっていてbounce_velが0以上で，外に出て行く速度ベクトル(outgoing)の負の値がbounce_velより大きければ反発します．
// from ode-0.7/ode/src/joint.cpp (l1382-1398)
// deal with bounce
if (j->contact.surface.mode & dContactBounce) {
// calculate outgoing velocity (-ve for incoming contact)
dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) +
dDOT(info->J1a,j->node[0].body->avel);
if (j->node[1].body) {
outgoing += dDOT(info->J2l,j->node[1].body->lvel) +
dDOT(info->J2a,j->node[1].body->avel);
}
// only apply bounce if the outgoing velocity is greater than the
// threshold, and if the resulting c[0] exceeds what we already have.
if (j->contact.surface.bounce_vel >= 0 &&
(-outgoing) > j->contact.surface.bounce_vel) {
dReal newc = – j->contact.surface.bounce * outgoing;
if (newc > info->c[0]) info->c[0] = newc;
}
}
mingw(g++)のbounce_velのデフォルト値は0になっているのでbounce_velを明示的に設定しなくて反発するのだと考えています．Visual C++ではbounce_velのデフォルト値はいくらですか？
以下のようにbounce_velを設定しないで，その値を調べて頂ければありがたいです．
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
const int N = 10;
dContact contact[N];
int isGround = ((ground == o1) || (ground == o2));
int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
if (isGround) {
for (int i = 0; i < n; i++) {
contact[i].surface.mode = dContactBounce;
contact[i].surface.bounce = 1.0; // (0.0~1.0)
printf(“bounce_vel=%f \n”,contact[i].surface.bounce_vel);
dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
dJointAttach (c,dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
私の使用している環境は(ODE0.7，Windows XP Professional, Visual C++.net Standard Ver.2003)です。
反発しないと言っても、衝突の検出はされています。
しかし、反発係数を変えても、その場で停止してしまうという現象が発生します。
bounce_velに0以上の数値を入力すると、反発係数に応じた反応をみることができるようになりました。
ミツルギさん，
プチキャンプを読んで頂きありがとうございます．枚数の制限のため説明が十分にできず，わかりづらくありませんか？
さて，お使いのnearCallback関数だけを投稿して頂けますか？
恐らく，球と地面以外は衝突検出していないのだと思います．
でむ
片岡さん，
私の環境(ODE0.7，Windows XP Professional, MinGW-5.0.3)ではbounce_velを設定しなくても反発しました．bounce_velはおっしゃるとおりです．
環境を教えて頂けますか？
はじめましてこんにちは
豊田高専でＯＤＥを使って研究をしている者です。
ロボカップ・プチシミュレータのサンプルをベースに円柱の物体何個か作り、
速度を与えて動かしているのですが、同じ形状の円柱物体同士が当たるときに
すり抜けてしまいます。（なぜかボールとの接触時は大丈夫です）
bodyとgeomの接続はdGeomSetBodyで行っており、理由が自分では見当たりません…。
解決方法を教えていただけないでしょうか。よろしくお願いします。
はじめまして
大阪電気通信大学の学生です。
ODEの勉強の参考にさせてもらっています。
ODEの0.7から（0.7かどうか詳しくはわかりませんが）物体が反発するには反発係数のほかに
bounce_vel という変数に0以上を設定しなければ反発しないようです。
見たところbounce_velは衝突した物体の速度がこの値より小さいと反発しない、最低反発速度のように思います。