補助平面の導入 - cvl-robot's diary

２０．マウスで３次元空間の操作

今考える空間には、スクリーン座標系、ロボット座標系、世界座標系が存在します。補助平面は、ロボットを動かすためにロボットの周辺に配置したいものですから、ロボット座標系で管理することにします。したがって、出力として求めたいものもロボット座標系での位置姿勢です。

２０．１　平面の定め方

３次元空間中の平面の方程式は、一般的にax+by+cz=dで表すことができます。ただし、方程式のままでは、空間中にどのように存在するのか直感的にはわかりにくいという問題があります。面の法線ベクトルは(a,b,c)と一致しますので、面の向きの決定は簡単です。dを直感的に定めることができればいいのですが、慣れないとちょっと無理です。その代わり、特定の点p0(x0,y0,z0)を通る法線ベクトル(a,b,c)の平面と指定するようにすれば、dを計算することができます。視覚情報から補助平面を決定したい場合にもこのやり方は便利です。

[入力]ofVec3f n(a,b,c)が平面の法線

[入力]平面の原点もしくは平面上の点ofVec(x0,y0,z0)

dを計算→平面の方程式が求まる

２０．２　マウスカーソルと補助平面の交点の求め方

マウスカーソルが平面上のどこにあるのかを調べるためには、マウスカーソルの点にスクリーンを前後に貫く線が存在するものとみなして、線分と平面の交点を調べることで知ることができます。プログラムで計算する方法は、下記のページで紹介されています。

http://www.sousakuba.com/Programming/gs_plane_line_intersect.html

実装を見ていきましょう。スクリーン座標系のマウスカーソル点を手前から奥へ突き抜ける世界座標系での3次元線分を求めるためには、スクリーン座標系で同じx,y座標ながらz座標が異なる線分の端点を次のように求めます。cameraはofEasyCamでscreenToWorldはそのメンバー関数です。

ofVec3f p1 = camera.screenToWorld(ofVec3f(x,y,-1));

ofVec3f p2 = camera.screenToWorld(ofVec3f(x,y,1));

これはまだ世界座標系ですので、ロボット座標系に変換します。

ofxAssimpMeshHelper & meshHelper = model.getMeshHelper(0); // ロボットの原点座標系

ofMatrix4x4 m1 = model.getModelMatrix().getInverse(); // 線分をWorld座標系からロボット座標系へ

ofMatrix4x4 m2 = meshHelper.matrix.getInverse();

ofPoint p1r = m1*m2*p1; // ロボット座標系のレイの線分

ロボット座標系で、平面と線分の3次元交点を求めます。

bool intersect = IntersectPlaneAndLine(&Pt,p1r,p2r,ofVec4f(n.x,n.y,n.z,d));

２０．３　補助平面の表示

補助平面を描画してやると、操作の利便性があがります。そのために、openFrameworksの平面の表示モデルの準備をしておきます。平面のメッシュモデルの作り方は、

ofMesh.plane(辺Aの長さ、辺Bの長さ、分割数);

ですが、面の法線方向を指定できません。ｘｙ方向に張られるようです。描画の度に行列を掛けてもいいのですが、直感的に扱えるようにあらかじめ90度回転して寝かせたモデルを作っておきます。

この平面メッシュの表示は次のように行います。

ofQuaternion quat;

quat.makeRotate(ofVec3f(0,0,1),n); // Identityの時の平面の法線(0,0,1)が法線方向ベクトルnに向くように回転

ofMatrix4x4 m(quat);

m.setTranslation(p0); // 平面の原点位置

ofMultMatrix(m);

pick_plane_mesh.drawVertices(); // 平面を描画

２０．４　補助平面座標系における交点の求め方

また平面を２次元座標系と見立てた場合の、平面座標でのピック位置を取得したい場合があります。そのためには、次のようにします。

ofMatrix4x4 im = m.getInverse();

ofVec3f iPt = Pt*im;

cout << "plane" << iPt.x << " " << iPt.y << " " << ((iPt.z<0.1)?0:iPt.z) << endl; // 平面座標上の点の位置