ホーム < ゲームつくろー！< 衝突判定編

2D衝突編
その７ 楕円と楕円の衝突


　楕円同士の衝突は、円同士、また楕円と点などと比べると難しさの度合いが桁違いです。もちろん、2つの楕円の式を連立方程式として解き、実数解が存在するか否かを判定すれば衝突はわかりますが、それはあまりに辛い解法です。

　楕円同士の衝突に関して直接答えを掲載している学術論文を見つけました：

「On-Line Collision-Avoidance Trajectory Planning of Two Planer Robots Based on Geometric Modeling.(PDF)」
Kao-Shing Hwang and Ming-Dar Tsai. Journal of Information Science Engineering 15, 131-152 (1999).
（幾何学モデルに基づく2つの平面化ロボットのオンライン衝突回避軌道計画）

この論文内の「3 FAST COLLISION-DETECTION APPROACH」に楕円衝突を高速に判定する式展開があります。この章の話は、この論文に掲載されているアイデアと数式を参考にしています。すばらしい解法を公表して下さったKao-Shing Hwang氏及びMing-Dar Tsai氏に感謝申し上げます。



① 楕円を円にして点にするアイデア

　上記論文のアイデアをまずは紹介します。今2つの有向境界楕円（Oriented Bounding Ellipes : OBE）があるとしましょう。有向楕円というのは長径軸と短径軸がXY軸と平衡になっていない楕円の事です。そのままでは埒が明きませんので、まず一方の楕円を原点に平行移動させて、さらに原点中心で回転させることで姿勢をまっすぐに直します：

今は右側の楕円がその対象になっています。この楕円は標準楕円（Standard Ellipse）もしくは軸並行境界楕円（Axis-Aligned Bounding Ellipse:AABE）と呼べるかもしれません。AABEになると、XY軸それぞれでうまくスケール変換することで簡単に半径1の円に変換できます：

楕円同士の衝突判定はこの段階で楕円と円の衝突に変わり少し簡単になるのですが、このままでもまだ判定は辛いんです。そこで、思い切って左側の楕円を長径と短径のそれぞれに1を足した拡張楕円にしてしまいます。そうすると、衝突判定に関して言えば右側の円を「点」と考えても構わなくなります：

右の点は原点にありますから、これは左の拡張楕円の中に原点が含まれているか否かという簡単な問題に衝突判定のレベルが落ちています。こうなると非常に簡単でしかも高速に衝突判定が可能になります。参照論文はこのアイデアを提供してくれています。

　なるほどと唸るアイデアです。ただ、この段階まで持っていくプロセスがまぁ～大変なんです(^-^;;



② とっても大事な事前練習：関数を行列で変換するとどんな式になるの？

　これから論文のアイデアを説明していく事になりますが、実はそのためにこの章のほぼ全てで必須の重要なセオリーを学んでおく必要があります。それは「ある関数で表される図形を行列などで変換すると、元の式はどうなるか？」という事なんです。このルールをここでしっかり理解しておかないと、以後の話はきっとさっぱりわからないと思います。ですから、それをイメージする練習を先にやっておきましょう。

　楕円はつらいので、ここは皆が知っている直線で考えてみます。今、y=2x+1という直線があるとしましょう：

水色の線がその直線です。中学校の時にやった事を思い出しますと、Y軸との交点（Y切片）は1になりますね。よって、この直線を下に1だけ平行移動させると原点を通る水色点線の直線になります。さらに水色直線上の点のX座標値をすべて2倍スケールにすると、横に少し寝た赤い直線になります。見た目でも計算でもわかるかもしれませんが、この赤い直線はxが1増えるとyも1増える直線y=xになっています。水色の直線からの動き、すなわち①Y軸方向に-1オフセット、②Xを2倍。この動きを覚えておいて下さい。

　さて、突然で恐縮ですが、今の動きの全く逆の動きを(x,y,1)というベクトルに掛け算します。何でそんな事をするか、そして何をしたいかは最後にわかります。上の逆の動きというと、まず最初にXを1/2倍にします。次にY軸方向に+1オフセットします。これを行列で表してみましょう：


　結果(x,y,1)というベクトルはX=x/2、Y=y+1という位置に移動しました。さて、この新しいXとYを最初の青い線の式y=2x+1におもむろに代入するとどうなるか？

　(y+1) =2*(x/2) + 1
  y = x

と、なんと先に計算しておいた変換後の直線の式が出現します。

　実は、ある式y=f(x)をオフセット行列、回転行列、スケール変換行列など行列変換した時の新しい式というのは、(x,y,1)というベクトルに対してその「逆行列」を「逆の順序」で掛け算した結果のXとYを元の式に代入すると得られるんです。これはf(x)が2次関数だろうがルートだろうがどのような式でも成り立ちます。もちろん、楕円でも成り立ちます（ただし行列に逆行列が存在する必要があります。上記3つの基本変換はどれも逆行列が保証されています）。

　以下からは、楕円を行列で変換しまくります。そして変換後の楕円の式を求める時にこの考え方を多用します。数学的な理屈は置いておいて構いません。それよりも、今のうちに「逆行列を逆順で」という大切なセオリーを覚えてしまって下さい。では、本番です(^-^)



③ ステップ1：楕円を行列変換して出来る式

　ここに有向楕円OBE1とOBE2があります。この2つの楕円は元々原点を中心とした軸並行境界楕円で長径、短径がわかっています。そしてそれぞれが回転、オフセットすることで有向楕円となりました。その回転角度とオフセット値もわかっているとします。このままだと辛いのは冒頭に申したとおりです。そこでまずは①で紹介したアイデアにならって、OBE2を単位円にしてみます。これはOBE2を①原点に逆オフセット、②θ2だけ逆回転、③長径と短径分だけ逆数スケール変換すると達成できます。これはイメージできますよね。

　同じ変換はOBE1にも施されます。こちらの方が重要です。OBE1にその変換を施した後の式を得るには、それら変換行列の逆行列を逆順で(x,y,1)ベクトルに掛け算し、X,Yを得るとわかります。具体的には、(x,y,1)というベクトルに対して①OBE2の長径と短径分だけスケール変換、②θ2だけ順回転、③OBE2の制御点分オフセット、という変換行列をこの順番に掛け算します：

Scl2x、Scl2yはOBE2のX軸径とY軸径、C2とS2はそれぞれcos(θ2)、sin（θ2）の意味です。O2xとO2yはOBE2のオフセット値です。この展開結果をOBE1を表す楕円の式のx,yに代入すれば良いのですが･･･有向楕円であるOBE1を表す式ってちょっと想像できないですよね。実はOBE1にX,Yを代入するというのはちょっと面倒なんです。

　そこで、もっと簡単な式で表せるローカル座標にある軸並行楕円のLocal_OBE1からまずOBE1に変換し、それにOBE2を単位円にする変換をさらに施す流れを考えてみます。

　ローカル座標にあるこの標準楕円から、①θ1だけ順回転、②(O1x,O1y)へオフセットすれば、OBE1の位置に到達します。そのOBE1にOBE2の変換である(-O2x,-O2y)オフセット、θ2逆回転、逆数スケール変換をさらに加えれば、同じOBE1の変換結果が得られます。違うのは出発点である式の簡単さです。先のセオリーに従うと、変換後OBE1の式を得るには、この合計5回の逆変換を逆順に掛け算して得られたX,Yを、標準楕円と言う簡単な式で表せるLocal_OBE1の式にそのままはめ込めば得られてしまうわけです。

　逆変換行列の掛け順は次の通りです：

①OBE2の順スケール
②OBE2の回転角度θ2だけ順回転
③(O2x,O2y)だけオフセット
④(-O1x,-O1y)だけオフセット
⑤θ1だけ逆回転

この5つの行列を(x,y,1)というベクトルにこの順で掛け算し、Local_OBE1の標準楕円のｘとyに放り込むと、欲しい変換後の式が出現します！どばっとやってしまいましょう：


さすがにこれだけになると正直見るのも嫌になりますので、行列を全部掛け算した結果を中段のようなn,p,oの記号でまとめます。ちなみに、各記号の展開式はこちらです。(x,y,1)にこの合成行列を掛け算すると、下段のようなわりとすっきりとしたベクトルになります。これをOBE1の標準楕円の式に突っ込むと次に示すこのステップでの最終式が得られます：

この式が変換後OBE1を表す式なんです。r1xとr1yはそれぞれ標準楕円であるOBE1のX軸長とY軸長です。でも変換後の式の分子にはxもyも含まれています。ちょっと不思議な式です。ちなみに、そのイメージ図はこちら：

　OBE2は単位円になり最高にすっきりしました。一方でOBE1はひしゃげてしまって最初と形も回転角度も変わってしまっています。うれしい事に実はこれも楕円なのですが、その径長も回転角度もこのままでは全然わかりません。それをどうすれば求められるのか？次のステップで説明します。



④ ステップ2：変換後OBE1の長径と短径、回転角度、オフセットをどばっと求める

　③の最後に示した楕円式の分母にあるr1xやr2yは変換後のOBE1の長径と短径ではありません。図からもわかるように、スケール変換してしまった楕円は元の径とは全然違ってしまうんです。回転角度も変わってしまいます。ですからこのままでは次に行いたい「左の楕円の長径と短径を+1して新しい楕円にする」という作業をする術が無いんです：


径がわからないと拡張のしようもありません･･･


　そこで、③の最終式から長径と短径、ひしゃげた楕円の回転角度そしてオフセット位置までを求めてみます。これを実現するには、「楕円の一般式」を引き合いに出します。楕円の一般式は、次のように表されます：

Cが抜けているのは単にcosとかぶるのが嫌だなと私が思っただけでして、大きな意味はありません。上の一般式は楕円のオフセットや回転なども全部表現できるマルチな式ですが、色々な情報が折り重なって圧縮されているために、このままだと何のことやら全然わかりません。そこで、まずは標準楕円を上の形に直してみます：

これを見るとAとBとGの成分しかありませんね。つまりA、B、Gしかないこの形ならばそれは「標準楕円」であると言えるわけです（係数の関係も含めてですが）。次に、制御点が原点にあって回転している楕円を展開してみます：

見辛くて申し訳ありませんが、上の式にはA,B,D,Gの成分のみが含まれます。これら成分のみで表される楕円は制御点が原点にある楕円と言えるわけです。逆に言えば、E,Fが含まれる一般式は、楕円がオフセットしている事も暗示しています。係数の意味が少し見えてきますよね。

　オフセットと回転をうまく施すと、有向楕円はかならず標準楕円に戻ります。そして上の係数はどんどん消えていきます。その事実をうまく利用しているのが冒頭の学術論文なんです。ここではそれにならって係数を消す作業を進めます。

　まず、ステップ1で導いた変換後OBE1の式をもう一度持ち出します：

この式を一生懸命に展開すると先に示した楕円の一般式になります。つまり上の式と

を完全に等価にできるという事です（展開式はこちらに示します）。

　今この一般式にf(x,y)と名前を付けておきます。f(x,y)を逆平行移動して原点に戻せば、EとFの成分は消えます。まだ良くわからないその平行移動量をとりあえず(h,k)としておきましょう。ここで②で説明したセオリーを思い出します。平行移動後の式を知りたい時には、(x,y,1)というベクトルを(-h,-k)だけ平行移動してできるX,Yをもとの式に代入すれば良いのでした。これはとても簡単な行列計算なので答えだけ示しますと、X=x-h, Y=y-kとなります。これを上の式にそのまま代入して展開すると次のようになります：

これが楕円を(h,k)だけ平行移動させた後の式なんです。上の記載は奇妙な段分けをしていますが、これはちゃんと意味があります。まず①ですが、この一行は平行移動量である(h,k)だけで構成されていて、xやyは含まれていません（Gも定数）。つまり、①は単に１つの定数なんです。次に②と③の括弧内に注目して下さい。この括弧はそれぞれxとyが掛けられていますが、もし括弧の中が同時に0となるh,kがあれば、②と③はすっぱりと無くなります。それには、両方の括弧を=0と置いて、h,kについての連立方程式を解けばいいんです。それを解いたのがこちらです：

結論として、上式にh及びkを代入すると、その楕円の一般式から②、③の部分がごそっと消えて無くなり、次のように式が簡略化されます：

Kは先の①部分の定数を表しています。この式にはE,F成分は確かにありません。つまりこれは「制御点が原点にある回転中の楕円」になったと言う事です。

　さて、続いてDを消す努力をしてみます。今度は「回転」を与えるとうまい事消えてくれます。何らかの順回転を与えて有向楕円を標準楕円にするという事は、x,yそれぞれに逆回転の要素を代入すれば良いので、

を上式に代入します：


　sinとcosで目がくらみそうですが、これが楕円の順回転公式なんです。ただ単に代入して展開しているだけですから大した事はしていません。最終的にA'、B'、D'という係数がまた出てきます。今やりたいことは適当なθ順回転を与えてD'=0にすることです。D'=0ならば、楕円は標準楕円（中心点が原点で回転していない楕円）となってくれます。そこで、D'=0としてθを具体的に求めてみましょう：

2倍角の公式が入っています（懐かしいですね）。θは上式にあるようにA,B,Dの分数式をatanする事で具体的に計算できます。このθだけ楕円を順回転させると、理解不能の有向楕円一般式は非常に簡単な標準楕円に化けてくれます：


式をさらに下段のように標準形に戻してみると、変換後のOBE1はX軸径が√-K/A'、Y軸径が√-K/B'である事もここでわかります。この楕円がθだけ逆回転し、さらに(-h,-k)だけ平行移動すると変換後OBE1になるわけです！



⑤ ファイナルステップ：標準楕円の径を+1して元の位置に戻す

　ここまでわかれば、後は簡単です。もう忘れかけているかもしれませんが、今やりたかったのは変換後OBE1の径を+1する事でした。今やそれは簡単です。先ほど標準形にした標準楕円の分母にある径に1を足した楕円をθだけ逆回転させた後、(-h,-k)だけ平行移動させればいいんです：

　径を+1した標準楕円を①θだけ逆回転して②(-h,-k)だけ平行移動する、という青い矢印の変換によってできるオレンジ色の楕円（変換後OBE1）の式は、ここまで何度も出てきたセオリー通り変換の逆行列を逆順に掛け算する事で成しえます。つまり、①(h,k)だけ平行移動、②θだけ順回転という変換を(x,y,1)に施して、出来たX,YをOBE1の標準式、


に突っ込むと完成します。完成形はこちらです：



この式こそ変換後のOBE1の径を+1させた楕円を現す真の式です。

　肝心の衝突判定はここまで来ると極めて簡単です。OBE2はすでに縮退して原点上にある点になってしまっています。ですから上の式を(x,y)=(0,0)と代入して、その値が1より大きいか小さいかを判定すれば衝突がわかります。楕円の中に点が含まれていれば、左辺は1よりも小さくなりますので、衝突と判定されます。

　最終的な判定式は次のようになります：


できた～～！！！ですね。



⑥ 楕円同士の衝突判定関数を公開します

　もう理屈はお腹いっぱいです。最後に楕円同士の衝突判定関数を公開してこの章を閉めたいと思います：

楕円衝突判定関数
///////////////////////////
// 楕円衝突判定関数
////////////////////
bool IKD::CollisionEllipse( ELLIPSE &E1, ELLIPSE &E2 )
{
   // STEP1 : E2を単位円にする変換をE1に施す
   float DefAng = E1.fAngle-E2.fAngle;
   float Cos = cos( DefAng );
   float Sin = sin( DefAng );
   float nx = E2.fRad_X * Cos;
   float ny = -E2.fRad_X * Sin;
   float px = E2.fRad_Y * Sin;
   float py = E2.fRad_Y * Cos;
   float ox = cos( E1.fAngle )*(E2.fCx-E1.fCx) + sin(E1.fAngle)*(E2.fCy-E1.fCy);
   float oy = -sin( E1.fAngle )*(E2.fCx-E1.fCx) + cos(E1.fAngle)*(E2.fCy-E1.fCy);

   // STEP2 : 一般式A～Gの算出
   float rx_pow2 = 1/(E1.fRad_X*E1.fRad_X);
   float ry_pow2 = 1/(E1.fRad_Y*E1.fRad_Y);
   float A = rx_pow2*nx*nx + ry_pow2*ny*ny;
   float B = rx_pow2*px*px + ry_pow2*py*py;
   float D = 2*rx_pow2*nx*px + 2*ry_pow2*ny*py;
   float E = 2*rx_pow2*nx*ox + 2*ry_pow2*ny*oy;
   float F = 2*rx_pow2*px*ox + 2*ry_pow2*py*oy;
   float G = (ox/E1.fRad_X)*(ox/E1.fRad_X) + (oy/E1.fRad_Y)*(oy/E1.fRad_Y) - 1;

   // STEP3 : 平行移動量(h,k)及び回転角度θの算出
   float tmp1 = 1/(D*D-4*A*B);
   float h = (F*D-2*E*B)*tmp1;
   float k = (E*D-2*A*F)*tmp1;
   float Th = (B-A)==0?0:atan( D/(B-A) ) * 0.5f;

   // STEP4 : +1楕円を元に戻した式で当たり判定
   float CosTh = cos(Th);
   float SinTh = sin(Th);
   float A_tt = A*CosTh*CosTh + B*SinTh*SinTh - D*CosTh*SinTh;
   float B_tt = A*SinTh*SinTh + B*CosTh*CosTh + D*CosTh*SinTh;
   float KK = A*h*h + B*k*k + D*h*k - E*h - F*k + G;
   if(KK>0) KK = 0; // 念のため
   float Rx_tt = 1+sqrt(-KK/A_tt);
   float Ry_tt = 1+sqrt(-KK/B_tt);
   float x_tt = CosTh*h-SinTh*k;
   float y_tt = SinTh*h+CosTh*k;
   float JudgeValue = x_tt*x_tt/(Rx_tt*Rx_tt) + y_tt*y_tt/(Ry_tt*Ry_tt);

   if( JudgeValue <= 1 )
      return true; // 衝突

   return false;
}


この関数の引数にあるELLIPSE型は以下の構造体です：

ELLIPSE構造体
// 楕円構造体
struct ELLIPSE
{
   float fRad_X; // X軸径
   float fRad_Y; // Y軸径
   float fAngle; // 回転角度
   float fCx; // 制御点X座標
   float fCy; // 制御点Y座標

   ELLIPSE()
   {
      fRad_X = 100.0f;
      fRad_Y = 100.0f;
      fAngle = 0.0f;
      fCx = 0.0f;
      fCy = 0.0f;
   }
};

この構造体と関数を使ったテストプログラムはこちらです。上記プログラムもヘッダーファイルと実装ファイルとして公開しておりますので、ご自由にお使い下さい。



⑦ 謝辞

　本章内容につきましてご提案下さいましたえおなゆさんに感謝申し上げます。また、参照論文に関しまして数々のご教授を頂きました藍川翡翠さんに深くお礼申し上げます。