カプセル衝突判定 - cvl-robot's diary

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

[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"]);

}