「がんばれない」けど「がんばりたい」

ITエンジニアの仕事のこと。AI、機械学習、ディープラーニング。地頭力。車のこと。

Path Following #01|Nature of Code

PathFollowing from Kazuyoshi Ueno on Vimeo.

前回は内積について復習。今回は再びVehicleを使用したアルゴリズムを。前々回で画面を適当なグリッドで分割し、各々のグリッドにランダムな、またはある程度規則を持たせたベクトルを配置し、

それをdesiredとしてVehicleを制御してみました。Flow Fieldと命名していますが、このアルゴリズムは流体的なシミュレートに使ったりされます。(と書きながら思ったの事は、もう少し記事が進んだら、それぞれのアルゴリズムを使用した、より具体的な作品的なものを考察しないと…と思っています。)FlowFieldが画面全体を環境としていたのに対して、あるPath(パス)=道を作成し、その道に沿ってVehicleを動かす事を考えます。

まずPathについての定義ですが、シンプルに考えて直線のものを考えます。start位置とend位置のみを繋ぐ直線でpath radiusという道幅を持ったものにします。この道幅内をVehicleが進む事になります。Path上にキッチリVehicleを乗せるのであればpath radiusを小さい値にすればイイ事になります。

ここまでのpathクラスを書いてみます。

class Path {

public:
  Path()
  {
  }

  virtual~Path(){}

  /**
   * variables
   */
  ofVec3f start;
  ofVec3f end;
  float radius;


  /**
   * methods
   */
   void setup()
   {
      radius = 20;
      start.set(0, ofGetHeight()/3.0);
      end.set(ofGetWidth(), 2*ofGetHeight()/3);
   }


   void draw()
   {
      ofSetColor(127,127);
      ofSetLineWidth(radius*2);
      ofLine(start.x, start.y, end.x, end.y);

      ofSetColor(255);
      ofSetLineWidth(1);
      ofLine(start.x, start.y, end.x, end.y);
   }
};

ではpathの外側にemmitされたVehicleがpathの方へ向かい、path内に入った後は、そのまま道に沿って移動させる事を考えてみます。Rainoldsのアルゴリズムを見ると、以下の順でVehicleのパラメータを考慮するように書かれてあります。

■ 1)未来(次フレーム)の位置を予測する。

コードで表すと以下のようになります。currentのvelocityを参照して算出します。まず現在のvelocityを正規化し方向を出します。その方向に対して25をかけています。25を掛ける=velocity方向に25ピクセル進んだところを未来の位置としています。 ところで、この25という値ですが、適当?かと。

// first work - predict location
ofVec3f predict = velocity;
predict.normalize();
predict *= 25;

ofVec3f predictLoc = location + predict;

■ 2)1)の位置からpathまでの距離を求める。

この距離が道幅内(radius)であればPathに沿って進ませる仕事のみを行い、そうでなければ再び1)からの仕事をおこないます。では、1)で算出した【未来の位置】から道までの距離はどう算出すればいいでしょうか。Vehicleの位置からPathにぶつかるまでのベクトルはPathに到達するベクトルであれば、その全てがVehicleからPathまでの距離と認識できますが、そのうち最短距離のものを採用します。理由は得になく、そうしない理由がある場合は、そうすればイイです。

では最短距離はどのようにして求めるのか? ここで内積を利用します。下記図を見てください。まず最短距離はnormal from point to lineと書かれたところにあたります。手順は以下になります。

path01

2-1)predicted locationを求める ※これは1)の計算で求まります。

2-2)start位置からpredicted locationまでのベクトルを求める。 ※ベクトルA

2-3)startからendまでのベクトルを求める。 ※ベクトルB

2-4)ベクトルAとベクトルBが成す角を求める ※θ

2-5)predicted locationからpathに対して垂線を下ろしたポイントを求める ※2-1〜2-4を利用すると求まる

2-6)predicted locationから2-5)で求めた位置までの距離を求める

以上の手順で最短距離を求める事が出来ます。 コードにしてみます。

//2-1|first work - predict location
ofVec3f predict = velocity;
predict.normalize();
predict *= 25;
ofVec3f predictLoc = location + predict;


//2-2|vector a
ofVec3f vecA = predictLoc - path.start;


//2-3|vector b (eq path)
ofVec3f vecB = path.end - path.start;


//2-4|θ ※内積からθを求める
float theta = angleBetween(a, b);
※http://of.studio23c.com/index.php/archives/1084を参照


//2-5|三角形の定理より
//start位置からnormalPointまでの距離
float disFromStart2NormPoint = vecA.length() * cos(theta);

vecB.normalize();
vecB *= disFromStart2NormPoint;

ofVec3f normalPoint = path.start + vecB;


//2-6|最短距離
float distance = (normalPoint - predictLoc).length();

■ 実は上記のコードはもっと簡略化できます。

startからnormalPointまでの距離disFromStart2NormPointは上述のとおり、

vecA.length()*cos(theta); ※1

と三角形の定理から求めています。この式が内積の式に似ているんです!
※1を内積の書き方で書くと…

|A|*cos(theta);

内積の式を思い出すと、
vecA・vecB = |A| x |B| x cos(theta);

ここでベクトルBの大きさが1だと仮定すると、
vecA・vecB = |A| x cos(theta);

つまりstartからnormalPointまでの距離disFromStart2NormPointは、 ベクトルBの大きさを正規化した状態での内積と同じなのです。しかも2-1〜の手順内でvecBは正規化しています。

disFromStart2NormPoint = vecA.dot(vecB);

これをふまえてコードを再構築すると2-4が必要なくなり2-5の求め方が三角形定理から内積に変化します。

//2-1|first work - predict location
ofVec3f predict = velocity;
predict.normalize();
predict *= 25;
ofVec3f predictLoc = location + predict;


//2-2|vector a
ofVec3f vecA = predictLoc - path.start;


//2-3|vector b (eq path)
ofVec3f vecB = path.end - path.start;


//2-5|内積を使用して求める
vecB.normalize();
float disFromStart2NormPoint = vecA.dot(vecB);
vecB *= disFromStart2NormPoint;
ofVec3f normalPoint = path.start + vecB;


//2-6|最短距離
float distance = (normalPoint - predictLoc).length();

■ これで最短距離が求められたので、この距離がpathの幅内になければVehicleをpathに向けて動かします。方法はseek(target)メソッドを使います。targetはnormalPointとしても良いのですがReonaldのアルゴリズムではnormalPointから少し先の位置(normalPoint + 10)をtargetとしています。