読者です 読者をやめる 読者になる 読者になる

cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

カプセル衝突判定

高価なロボットを壊さないために、簡単な衝突判定ぐらいはしておきたいところです。学生に聞いたところ、カプセル(球スイープ)衝突判定が簡単で速いということなので真似してこれを作ってみます。ロボットの各ジョイント間を接続する線分に、適当な肉厚の仮想球を与えて、それらの衝突を調べることで自己干渉回避をします。

[1]によると、

カプセルカプセル

  • 線分間の距離が半径以下なら交差

とあります。

3次元の線分間の距離はどうやって求めればいいのでしょう?意外や検索してもスパッとした回答は出てきませんでした。[2]のように、直線と点の距離は計算できます。直線と点の距離は、点と直線上の点の最短の長さとして定義されています。[3]のように、直線と直線の計算もできます。

 直線と直線と見なした時の距離、と、各端点と相手の直線の距離を調べてその最近傍点が線分上に乗っているかどうかを最大4回調べる、とか、いろいろ条件分けして・・・とか考えていけば格好悪くても動くものは出来そうですが、書籍[4]にズバリの回答が載っているそうです。

書籍[4]は高価な著作物なので引用や解説は避けます。

float ClosestPtSegmentSegment()

という関数を調べてください。返り値が2線分間の距離の二乗です。

 

衝突判定のテスト

原著では3次元ベクトルをPointという構造体で扱っています。多少使いやすくするために、PointをofVec3fに書き換えます。

float ClosestPtSegmentSegment(const ofVec3f p1, const ofVec3f q1, const ofVec3f p2, const ofVec3f q2, float &s, float &t, ofVec3f &c1, ofVec3f &c2){

typedef ofVec3f Vector;

        ・・・

}

 また原著では、内積の計算をDotという関数で扱っていますが、float f=d2.dot(r);などのように書き換えます。また、EPSILONという定義値を極小値として扱っていますが、#include <float.h>してFLT_EPSILONに置き換えます。この辺は自由に。

HIRONXのモデルで衝突判定のテストを行ってみます。少し長いですが、ほとんど単にデバッグ用の表示です。

#include "ClosestPtSegmentSegment.h"

int HIRONXGUIControler::collisionCheck()

{

  std::map<string, ofVec3f> P;

 

  // Update joints position

  for(int i=0; i<(int)model.getMeshCount(); i++){

    aiString name = model.getMeshHelper(i).mesh->mName;

    P[name.data] = (model.getMeshHelper(i).matrix * model.getModelMatrix()).getTranslation();

  }

 

  // for debug

  ofDrawSphere(P["CHEST_JOINT0_Link"], 0.01);

  ofDrawSphere(P["HEAD_JOINT0_Link"], 0.01);

  ofDrawSphere(P["HEAD_JOINT1_Link"], 0.01);

  ofDrawSphere(P["RARM_JOINT0_Link"], 0.01);

  ofDrawSphere(P["RARM_JOINT1_Link"], 0.01);

  ofDrawSphere(P["RARM_JOINT2_Link"], 0.01);

  ofDrawSphere(P["RARM_JOINT3_Link"], 0.01);

  ofDrawSphere(P["RARM_JOINT4_Link"], 0.01);

  ofDrawSphere(P["RARM_JOINT5_Link"], 0.01);

  ofDrawSphere(P["LARM_JOINT0_Link"], 0.01);

  ofDrawSphere(P["LARM_JOINT1_Link"], 0.01);

  ofDrawSphere(P["LARM_JOINT2_Link"], 0.01);

  ofDrawSphere(P["LARM_JOINT3_Link"], 0.01);

  ofDrawSphere(P["LARM_JOINT4_Link"], 0.01);

  ofDrawSphere(P["LARM_JOINT5_Link"], 0.01);

 

  ofLine(P["CHEST_JOINT0_Link"], P["HEAD_JOINT0_Link"]);

  ofLine(P["HEAD_JOINT0_Link"], P["HEAD_JOINT1_Link"]);

  // ofLine(P["RARM_JOINT0_Link"], P["RARM_JOINT1_Link"]);

  ofLine(P["RARM_JOINT1_Link"], P["RARM_JOINT2_Link"]);

  ofLine(P["RARM_JOINT2_Link"], P["RARM_JOINT3_Link"]);

  ofLine(P["RARM_JOINT3_Link"], P["RARM_JOINT4_Link"]);

  ofLine(P["RARM_JOINT4_Link"], P["RARM_JOINT5_Link"]);

  // ofLine(P["LARM_JOINT0_Link"], P["LARM_JOINT1_Link"]);

  ofLine(P["LARM_JOINT1_Link"], P["LARM_JOINT2_Link"]);

  ofLine(P["LARM_JOINT2_Link"], P["LARM_JOINT3_Link"]);

  ofLine(P["LARM_JOINT3_Link"], P["LARM_JOINT4_Link"]);

  ofLine(P["RARM_JOINT4_Link"], P["RARM_JOINT5_Link"]);

  ofLine(P["LARM_JOINT0_Link"], P["RARM_JOINT0_Link"]);

 

  // collision check

  float s, t;

  ofVec3f c1, c2;

  float sq_len = ClosestPtSegmentSegment(P["RARM_JOINT4_Link"], P["RARM_JOINT5_Link"], P["LARM_JOINT4_Link"], P["LARM_JOINT5_Link"], s, t, c1, c2);

  cout << len << " " << s << " " << t << " " << c1 << " " << c2 << endl;

 

  if(sqrt(sq_len) < 0.1/*r*/) ofSetColor(ofColor::red);

  ofLine(P["RARM_JOINT4_Link"], P["RARM_JOINT5_Link"]);

}

 線表示なので地味ですが、手先が近づいたため衝突を示す赤い表示になっています。

f:id:cvl-robot:20140501102043p:plain

 

使用例:右手を左手にぶつけようと近づけると勝手に避ける

f:id:cvl-robot:20140502170435g:plain

float t=1.1*r; // rは衝突判定のためのカプセルの半径

ofVec3f pt = c1 + t* (c2 - c1).normalized(); // 左手の逃げる先

 衝突判定に用いた線分の距離を表す2つの交点を媒介変数表示して、ぶつからない距離に逃げる先の座標を求めて、左手のIKを解く。2点を結ぶベクトルを正規化しておく。

感覚的に避けてほしいところにあまり行ってくれないので、単純に右からよってきたら左に避ける、などの単純ルールの方が実用的かもしれません。

 

[1]http://harumune.s56.xrea.com/assari/index.php?%BE%D7%C6%CD%C8%BD%C4%EA

[2]http://www.infra.kochi-tech.ac.jp/takagi/Survey2/7Parameter.pdf

[3]http://d.hatena.ne.jp/obelisk2/20101228/1293521247

[4]著者Christer Ericson, 翻訳中村達也: ゲームプログラミングのためのリアルタイム衝突判定, ボーンデジタル,pp.148--151, 2つの線分の最近傍点