(57)【要約】【課題】凸立体が衝突又は接触している状況において、両者の間に形成される接触多角形を完全に且つ高速に算出する。【解決手段】衝突検出アルゴリズムで検出された衝突代表点対を結ぶベクトルを法線ベクトルとする接触平面と定義し、立体毎に接触平面との接触形状を与える半接触多角形を求める。そして、各立体についての半接触多角形の共通領域を立体間の接触形状として算出する。立体の頂点を全探索すると頂点数の増大により計算量が増大するから、接触相手となる立体に最も浸透した頂点を起点として、隣接頂点リストを局所探索する。【選択図】  図４
2007102394.tif 000002
【特許請求の範囲】
【請求項１】
複数の３次元立体モデル間の接触を検出し、接触状態にある２立体間の接触形状を算出する接触形状算出装置であって、
前記２立体間の衝突又は接触を検出する衝突検出手段と、
前記２立体の共通領域を通過する接触平面を定義する接触平面設定手段と、
前記の各立体と接触平面との接触状態をそれぞれ個別の半接触問題として解く接触問題解決手段と、
前記接触問題解決手段によって求められた立体毎の半接触問題の解を統合して、前記２立体間の接触形状を算出する接触形状算出手段と、
を具備することを特徴とする接触形状算出装置。
【請求項２】
前記衝突検出手段は、衝突又は接触する２立体間の最近傍点又は最浸透点からなる衝突代表点対を検出し、
前記接触平面設定手段は、前記衝突代表点対の中点を通り、前記衝突代表点対を結ぶ直線方向を法線ベクトルとする接触平面を定義する、
ことを特徴とする請求項１に記載の接触形状算出装置。
【請求項３】
前記接触問題解決手段は、半接触問題の解として、立体と接触平面との接触形状を与える半接触多角形を求める、
ことを特徴とする請求項２に記載の接触形状算出装置。
【請求項４】
立体毎に頂点の接続関係を表した隣接頂点グラフを保持し、
前記接触問題解決手段は、処理対象とする立体に関して、隣接頂点グラフを探索して半接触多角形内に存在する頂点集合を求める、
ことを特徴とする請求項３に記載の接触形状算出装置。
【請求項５】
前記接触問題解決手段は、処理対象とする立体に関して、適当な頂点を起点として、接触平面内に存在しない頂点に到達するまで隣接頂点グラフを再帰的に局所的探索し、半接触多角形内に存在する頂点集合を求める、
ことを特徴とする請求項４に記載の接触形状算出装置。
【請求項６】
前記接触問題解決手段は、処理対象とする立体のうち前記法線ベクトル方向に接触相手となる立体に最も浸透した頂点を起点に設定する、
ことを特徴とする請求項５に記載の接触形状算出装置。
【請求項７】
前記接触問題解決手段は、前記法線ベクトル又はその逆方向へのサポート写像から得られる頂点を起点に設定する、
ことを特徴とする請求項６に記載の接触形状算出装置。
【請求項８】
前記接触問題解決手段は、処理対象とする立体の頂点のうち、接触平面より接触相手となる立体側にある頂点を半接触多角形内に存在する頂点として抽出する、
ことを特徴とする請求項４に記載の接触形状算出装置。
【請求項９】
前記接触形状算出手段は、立体毎に得られた半接触多角形の共通領域に基づいて前記２立体間の接触形状を算出する、
ことを特徴とする請求項３に記載の接触形状算出装置。
【請求項１０】
前記接触問題解決手段は、抽出された立体の頂点集合のうち、半接触多角形の内部に存在する頂点を取り除く、
ことを特徴とする請求項４に記載の接触形状算出装置。
【請求項１１】
前記接触問題解決手段は、半接触多角形内に存在するとして抽出された頂点集合を接触平面へ正射影し、正射影した頂点群を包含する最小の凸多角形頂点集合を得るための演算を実行して、接触平面上で内部の点を含まない、半接触多角形の輪郭頂点集合を算出する、
ことを特徴とする請求項１０に記載の接触形状算出装置。
【請求項１２】
前記複数の３次元立体モデルを所定のオフセット量だけ膨張させた膨張立体を生成する立体膨張手段をさらに備え、
前記衝突検出手段は、膨張立体間の衝突又は接触に基づいて原立体間の衝突又は接触を検出し、
前記接触問題解決手段は、膨張前の原立体と、接触平面を平行移動させたオフセット接触平面との間の半接触多角形を算出する、
ことを特徴とする請求項３に記載の接触形状算出装置。
【請求項１３】
前記衝突検出手段は、衝突又は接触を検出した原立体上の最近接点対を算出し、
前記接触平面設定手段は、最近接点対の中点を通り、該最近接点対を結ぶ直線方向を法線ベクトルとする接触平面を定義する、
ことを特徴とする請求項１２に記載の接触形状算出装置。
【請求項１４】
前記接触問題解決手段は、前記接触平面設定手段により設定された接触平面を前記法線ベクトル方向に前記オフセット量だけ平行移動させたオフセット接触平面を設定する、
ことを特徴とする請求項１２に記載の接触形状算出装置。
【請求項１５】
前記立体膨張手段は、オフセット接触平面と原立体が必ず交点を有するようにオフセット量を設定し、
前記接触問題解決手段は、原立体側に前記法線方向に該オフセット量だけ接触平面を平行移動させたオフセット接触平面と該原立体との半接触多角形を算出する、
ことを特徴とする請求項１２に記載の接触形状算出装置。
【請求項１６】
複数の３次元立体モデル間の接触を検出し、接触状態にある２立体間の接触形状を算出する接触形状算出方法であって、
前記２立体間の衝突又は接触を検出する衝突検出ステップと、
前記２立体の共通領域を通過する接触平面を定義する接触平面設定ステップと、
前記の各立体と接触平面との接触状態をそれぞれ個別の半接触問題として解く接触問題解決ステップと、
前記接触問題解決ステップにおいて求められた立体毎の半接触問題の解を統合して、前記２立体間の接触形状を算出する接触形状算出ステップと、
を具備することを特徴とする接触形状算出方法。
【請求項１７】
前記衝突検出ステップでは、衝突又は接触する２立体間の最近傍点又は最浸透点からなる衝突代表点対を検出し、
前記接触平面設定ステップでは、前記衝突代表点対の中点を通り、前記衝突代表点対を結ぶ直線方向を法線ベクトルとする接触平面を定義する、
ことを特徴とする請求項１６に記載の接触形状算出方法。
【請求項１８】
前記接触問題解決ステップでは、半接触問題の解として、立体と接触平面との接触形状を与える半接触多角形を求める、
ことを特徴とする請求項１７に記載の接触形状算出方法。
【請求項１９】
前記接触問題解決ステップでは、処理対象とする立体に関して、立体毎に頂点の接続関係を表した隣接頂点グラフを探索して半接触多角形内に存在する頂点集合を求める、
ことを特徴とする請求項１８に記載の接触形状算出方法。
【請求項２０】
前記接触問題解決ステップでは、処理対象とする立体に関して、適当な頂点を起点として、接触平面内に存在しない頂点に到達するまで隣接頂点グラフを再帰的に局所的探索し、半接触多角形内に存在する頂点集合を求める、
ことを特徴とする請求項１９に記載の接触形状算出方法。
【請求項２１】
前記接触問題解決ステップでは、処理対象とする立体のうち前記法線ベクトル方向に接触相手となる立体に最も浸透した頂点を起点に設定する、
ことを特徴とする請求項２０に記載の接触形状算出方法。
【請求項２２】
前記接触問題解決ステップでは、前記法線ベクトル又はその逆方向へのサポート写像から得られる頂点を起点に設定する、
ことを特徴とする請求項２１に記載の接触形状算出方法。
【請求項２３】
前記接触問題解決ステップでは、処理対象とする立体の頂点のうち、接触平面より接触相手となる立体側にある頂点を半接触多角形内に存在する頂点として抽出する、
ことを特徴とする請求項１９に記載の接触形状算出方法。
【請求項２４】
前記接触形状算出ステップでは、立体毎に得られた半接触多角形の共通領域に基づいて前記２立体間の接触形状を算出する、
ことを特徴とする請求項１８に記載の接触形状算出方法。
【請求項２５】
前記接触問題解決ステップでは、抽出された立体の頂点集合のうち、半接触多角形の内部に存在する頂点を取り除く、
ことを特徴とする請求項１９に記載の接触形状算出方法。
【請求項２６】
前記接触問題解決ステップでは、半接触多角形内に存在するとして抽出された頂点集合を接触平面へ正射影し、正射影した頂点群を包含する最小の凸多角形頂点集合を得るための演算を実行して、接触平面上で内部の点を含まない、半接触多角形の輪郭頂点集合を算出する、
ことを特徴とする請求項２５に記載の接触形状算出方法。
【請求項２７】
前記複数の３次元立体モデルを所定のオフセット量だけ膨張させた膨張立体を生成する立体膨張ステップをさらに備え、
前記衝突検出ステップでは、膨張立体間の衝突又は接触に基づいて原立体間の衝突又は接触を検出し、
前記接触問題解決ステップでは、膨張前の原立体と、接触平面を平行移動させたオフセット接触平面との間の半接触多角形を算出する、
ことを特徴とする請求項１８に記載の接触形状算出方法。
【請求項２８】
前記衝突検出ステップでは、衝突又は接触を検出した原立体上の最近接点対を算出し、
前記接触平面設定ステップでは、最近接点対の中点を通り、該最近接点対を結ぶ直線方向を法線ベクトルとする接触平面を定義する、
ことを特徴とする請求項２７に記載の接触形状算出方法。
【請求項２９】
前記接触問題解決ステップでは、前記接触平面設定ステップにおいて設定された接触平面を前記法線ベクトル方向に前記オフセット量だけ平行移動させたオフセット接触平面を設定する、
ことを特徴とする請求項２７に記載の接触形状算出方法。
【請求項３０】
前記立体膨張ステップでは、オフセット接触平面と原立体が必ず交点を有するようにオフセット量を設定し、
前記接触問題解決ステップでは、原立体側に前記法線方向に該オフセット量だけ接触平面を平行移動させたオフセット接触平面と該原立体との半接触多角形を算出する、
ことを特徴とする請求項２７に記載の接触形状算出方法。
【請求項３１】
複数の３次元立体モデル間の接触を検出し、接触状態にある２立体間の接触形状を算出するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記２立体間の衝突又は接触を検出する衝突検出手順と、
前記２立体の共通領域を通過する接触平面を定義する接触平面設定手順と、
前記の各立体と接触平面との接触状態をそれぞれ個別の半接触問題として解く接触問題解決手順と、
前記接触問題解決手順によって求められた立体毎の半接触問題の解を統合して、前記２立体間の接触形状を算出する接触形状算出手順と、
を実行させることを特徴とするコンピュータ・プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の３次元立体モデル間の接触を検出し、接触状態にある２立体間の接触形状を算出する接触形状算出装置及び接触形状算出方法、並びにコンピュータ・プログラムに係り、特に、２つの立体が衝突又は接触している状況において、両者の間に形成される接触多角形を完全に且つ高速に算出する接触形状算出装置及び接触形状算出方法、並びにコンピュータ・プログラムに関する。
【背景技術】
【０００２】
最近、脚式移動ロボットの構造やその安定歩行制御に関する研究開発が進展し、実用化への期待も高まってきている。これら脚式移動ロボットは、クローラ式ロボットに比し不安定で姿勢制御や歩行制御が難しくなるが、階段の昇降や障害物の乗り越えなど、柔軟な歩行・走行動作を実現できるという点で優れている。例えば、移動手段としての脚を備え、接地・無接地間を遷移する安定な運動パターンを実時間で生成することができるヒューマノイド・ロボットについて提案がなされている。
【０００３】
この種の移動ロボットにおいては、オブジェクト間及びオブジェクトとロボット間が相互に力を及ぼす領域の形状すなわち「接触多角形」を必要とする状況が多く存在する。このため、仮想環境内に複数の物体が共存し、それらの間の衝突や接触といった物理的相互作用を計算機上で再現する力学シミュレーションなどを取り入れることにより、立体間の衝突が発生したことを検出するとともに、立体間の接触状態を把握して、衝突箇所に適切な反力を作用させる必要がある。
【０００４】
また、脚式移動ロボットの姿勢制御における安定度判別規範として、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）が用いられる。ＺＭＰとは床反力によるモーメントがゼロとなる床面上の点のことである（例えば、非特許文献１を参照のこと）。歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反力モーメントとバランスするという「ダランベールの原理」に基づく力学的推論の帰結として、足底接地点と路面の形成する支持多角形の内側にＺＭＰが存在していれば、モーション・パターンの力学的整合性が保証されるから姿勢は安定化する（図１９を参照のこと）また、支持多角形が広くなる姿勢において、ロボットの姿勢はより安定化する。
【０００５】
ここで、支持多角形は、脚式ロボットと平坦路面の接触点集合をすべて包含する最小の凸多角形（Ｃｏｎｖｅｘ Ｈｕｌｌ）であり、衝突検出アルゴリズムにおける接触多角形に類似する概念である。言い換えれば、接触多角形は脚式ロボットの制御にも応用することができ、足平と床面との正確な接触多角形を高速に求めることができれば、リアルタイムで支持多角形を形成して、床面上での脚式ロボットの運動を安定化することが可能となる。
【０００６】
さらに、接触多角形は、平坦な路面上の接地多角形に限定されない。図２０に示すように、ロボットが法線ベクトルの異なる複数の平面に同時に接する場合や、自分自身と物理的に接触している場合（自己干渉）においても、その接触状態を正確に把握して、より一般的な安定化を行なうとともに、複雑な動作計画を立てる必要がある。
【０００７】
例えば、図２１に示すように、２つの立方体が接触しているときは、接触形状を構成するＡ、Ｂ、Ｃ、Ｄの各頂点に反力を作用させる必要がある。２つの立体が接触状態にあるとき、図形ＡＢＣＤのような２立体間の接触部の形状が接触多角形である。
【０００８】
しかしながら、周知の衝突検出アルゴリズムの多くは、２立体間の衝突点の一部しか得ることができず、接触多角形を完全に算出することはできない。例えば、図２２に示すように１衝突当たり代表点Ｐを１点（通常は最も浸透した点、若しくは最も近接した点）、あるいは、図２３に示すように、接触を発生した３角形メッシュＭしか得ることができない。
【０００９】
不十分な衝突検出アルゴリズムの結果を直接用いて力学シミュレーションを実行すると不都合を生じる。ここで、立方体が水平な床上に置かれている場合を例にとって考察してみる。例えば、図２４に示すように代表点Ｐのみに反力を作用させるとき、床反力モデルが復元力を有さない場合には、同図（Ａ）に示すように、Ｐ点の他端が支持されないために、立方体はＰ点回りに回転しながら他端が徐々に床に浸透していってしまう。そして、シミュレーションが１フレームだけ進むとＱ点が最も浸透した点となり、Ｐ点に代わってＱ点に反力が作用するため、Ｑ点を中心として回転し、今度はＰ点がさらに浸透する。以上のような状態を繰り返すと、立方体は際限無く床面に浸透していってしまう。また、床反力モデルが復元力を有している場合には、図２４（Ｂ）に示すように浸透はある一定量以上には及ばないものの、作用点がＰ点とＱ点の間で右往左往し、振動が発生してしまうため、良好なシミュレーション結果が得られ難い。
【００１０】
本発明者らは、このような問題を解消するには、完全な接触多角形を算出し、その頂点群に反力を作用させる必要があると思料する。接触多角形を算出する方法として、干渉を起こしている２立体間の積立体（ｉｎｔｅｒｓｅｃｔｉｏｎ）を求める方法が挙げられる。例えば、２立体間の積立体を基に反力を算出する方法について提案がなされている（例えば、非特許文献２を参照のこと）。また、凸立体間の積立体を算出する方法についても提案がなされている（例えば、非特許文献３又は非特許文献４を参照のこと）。
【００１１】
しかしながら、いずれの方法も、干渉を生じている２立体のすべての頂点を計算に用いるため、計算量が多く、形状が複雑になったりオブジェクト数が増大したりすると、シミュレーションのリアルタイム実行が困難となる可能性がある。
【００１２】
【非特許文献１】ヴコブラトビッチ（Ｍｉｏｍｉｒ Ｖｕｋｏｂｒａｔｏｖｉｃ）著「脚式移動ロボット（ＬＥＧＧＥＤ ＬＯＣＯＭＯＴＩＯＮ ＲＯＢＯＴＳ）」（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社））
【非特許文献２】「力覚インタラクションのための多面体の接触体積に基づく実時間剛体運動シミュレーション」（計測自動制御学会論文集，Ｖｏｌ４０，Ｎｏ．２，２００４）
【非特許文献３】Ｄ．Ｅ．Ｍｕｌｌｅｒ ａｎｄ Ｆ．Ｐ．Ｐｒｅｐａｒａｔａ，“Ｆｉｎｄｉｎｇ ｔｈｅ ｉｎｔｅｒｓｅｃｔｉｏｎ ｏｆ ｔｗｏ ｃｏｎｖｅｘｐｏｌｙｈｅｄｒａ”（ＴｈｅｏｒｅｔｉｃａｌＣｏｍｐｕｔｅｒ Ｓｃｉｅｎｃｅ ７（２），ｐｐ．２１７−２３６，１９８７）
【非特許文献４】Ｓ．Ｈｅｒｔｅｌ， Ｋ．Ｍｅｈｌｈｏｒｎ， Ｍ．Ｍａｎｔｙｌａ ａｎｄ Ｊ．Ｎｉｅｖｅｒｇｅｌｔ，“Ｓｐａｃｅ ｓｗｅｅｐ ｓｏｌｖｅｓｉｎｔｅｒｓｅｃｔｉｏｎ ｏｆ ｔｗｏ ｃｏｎｖｅｘ ｐｏｌｙｈｅｄｒａ ｅｌｅｇａｎｔｌｙ”（Ａｃｔａ Ｉｎｆｏｒｍａｔｉｃａ ２１，ｐｐ．５０１−５１９，１９８４）
【発明の開示】
【発明が解決しようとする課題】
【００１３】
本発明の目的は、複数の３次元立体モデル間の接触を検出し、接触状態にある２立体間の接触形状を好適に算出することができる、優れた接触形状算出装置及び接触形状算出方法、並びにコンピュータ・プログラムを提供することにある。
【００１４】
本発明のさらなる目的は、２つの立体が衝突又は接触している状況において、両者の間に形成される接触多角形を完全に且つ高速に算出することができる、優れた接触形状算出装置及び接触形状算出方法、並びにコンピュータ・プログラムを提供することにある。
【課題を解決するための手段】
【００１５】
本発明は、上記課題を参酌してなされたものであり、その第１の側面は、複数の３次元立体モデル間の接触を検出し、接触状態にある２立体間の接触形状を算出する接触形状算出装置であって、
前記２立体間の衝突又は接触を検出する衝突検出手段と、
前記２立体の共通領域を通過する接触平面を定義する接触平面設定手段と、
前記の各立体と接触平面との接触状態をそれぞれ個別の半接触問題として解く接触問題解決手段と、
前記接触問題解決手段によって求められた立体毎の半接触問題の解を統合して、前記２立体間の接触形状を算出する接触形状算出手段と、
を具備することを特徴とする接触形状算出装置である。
【００１６】
力学シミュレーションの分野では、複数の物体が共存する仮想環境内で、物体間の衝突や接触といった物理的相互作用を計算機上で再現する。ここで、立体同士の衝突が発生したときに、衝突箇所に適切な反力をリアルタイムで作用させるためには、立体間のより正確な接触多角形を高速に求める必要がある。
【００１７】
しかしながら、周知の衝突検出アルゴリズムの多くは、１衝突当たり代表点Ｐを１点（通常は最も浸透した点、若しくは最も近接した点）、あるいは接触を発生した３角形メッシュＭといったように、２立体間の衝突点の一部しか得ることができず、接触多角形を完全に算出することはできない。このように不十分な衝突検出アルゴリズムの結果を直接用いて力学シミュレーションを実行すると、不都合を生じる。
【００１８】
また、衝突又は接触する立体間の完全な接触多角形を算出する方法として、干渉を起こしている２立体間の積立体を求める方法が挙げられるが、干渉を生じている２立体のすべての頂点を計算に用いるため計算量が多い。また、形状が複雑になったりオブジェクト数が増大したりすると、シミュレーションのリアルタイム実行が困難である。
【００１９】
これに対し、本発明は、既存の衝突検出アルゴリズムから得られた衝突代表点対（最浸透点対若しくは最近傍点対）から、その周辺探索のみを行なうことによって、精密な接地多角形を高速に算出するようにした。
【００２０】
衝突又は接触が検出された２立体間の接触状態を直接的に扱うと、さまざまな組み合わせが存在するために問題が複雑となり、計算負荷が高くなる。そこで、本発明では、２立体の共通領域を通過する接触平面を定義し、各立体と接触平面との接触問題を個別に解くことにした。そして、各立体と接触平面との接触問題の解を統合することによって、所望する２立体間の接触形状を算出することができる。
【００２１】
既存の衝突検出アルゴリズムにより衝突又は接触する２立体間の最近傍点又は最浸透点からなる衝突代表点対を検出することができる。本発明では、立体間の衝突代表点対を結ぶベクトルを法線ベクトルとする平面を接触平面と定義することができる。
【００２２】
前記接触問題解決手段は、各立体についての半接触問題の解として、それぞれの立体と接触平面との接触形状を与える半接触多角形を個別に求める。そして、前記接触形状算出手段は、立体毎に得られた半接触多角形の共通領域に基づいて前記２立体間の接触形状を算出することができる。
【００２３】
このとき、半接触多角形を得るために、各立体についてすべての頂点に対して接触平面上に存在するかどうかを探索すると、立体頂点数が多くなるにつれて、頂点探索に要する計算量が増大してしまう。そこで、本発明では、立体毎に頂点の接続関係を表した隣接頂点グラフを用いて半接触多角形を構成する頂点を探索するが、同グラフ中で適当な頂点を起点として、接触平面内に存在しない頂点に到達するまで隣接頂点グラフを再帰的に局所的探索し、半接触多角形内に存在する頂点集合を求めるようにした。全探索に代えて局所的探索を行なうことによって、少ない計算量で高速に半接触多角形を求めることができる。
【００２４】
ここで言う適当な起点として、処理対象とする立体のうち前記法線ベクトル方向に接触相手となる立体に最も浸透した頂点に設定することができる。このような起点は、例えば、前記法線ベクトル（又はその逆）方向へのサポート写像から算出することができる。
【００２５】
このようにして設定された起点から隣接頂点リストの局所探索を開始して、接触平面より接触相手となる立体側にある頂点を半接触多角形内に存在する頂点として順次抽出していけばよい。そして、既に調査済みの頂点や、接触平面より上方に存在する頂点に到達したら、それ以上は隣接頂点グラフを辿る必要はない。そのときまでに得られた、接触平面より下方にある頂点の集を半接触多角形の周上の点及びその内部の点からなる頂点集合として用いる。
【００２６】
また、上述した手順に従って隣接頂点グラフ上を探索して得られた頂点集合には、一般に、半接触多角形の内部に存在する点も含まれてしまう。そこで、前記接触問題解決手段は、抽出された立体の頂点のうち、半接触多角形の内部に存在する頂点などの冗長な点を頂点集合から取り除くようにしている。例えば、半接触多角形内に存在するとして抽出された頂点集合を接触平面ＣＰへ正射影し、正射影した頂点群を包含する最小の凸多角形頂点集合を得るための演算を実行して、接触平面上で内部の点を含まない、半接触多角形の輪郭頂点集合を算出することができる。
【００２７】
また、多くの力学シミュレーションでは、立体が相互に浸透することがないように反力を発生させることが多い。このような非浸透型モデルを扱う場合には、立体を球の半径（ｒ）分だけ膨張させた図形に対して衝突検出を行ない、接触平面を法線ベクトル方向にオフセットさせた平面（オフセット接触平面）と、（膨張前の）原立体との間の半接触多角形を算出し、それぞれの立体から得られた半接触多角形のインターセクションを求めるように構成すればよい。
【００２８】
原立体間が干渉していない場合、既存の衝突検出アルゴリズムにより、衝突代表点対として、最浸透点対の代わりに、２立体上の最近接点対を算出することができる。したがって、非浸透型モデルでは、接触平面を最近接点対の中点を通り、両点を結ぶ直線方向を法線ベクトルとするように定義される。
【００２９】
ここで、オフセット量ｒの決定方法はさまざまであるが、オフセット接触平面と原立体が必ず交点を有するように設定するのが好ましい。このような場合、それぞれの原立体について半接触多角形を算出する際には、当該立体側にｒだけ接触平面をオフセットさせたオフセット接触平面を設定して、上記と同様の手法によりそれぞれの原立体について隣接頂点グラフの探索を実行することができる。
【００３０】
また、本発明の第２の側面は、複数の３次元立体モデル間の接触を検出し、接触状態にある２立体間の接触形状を算出するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記２立体間の衝突又は接触を検出する衝突検出手順と、
前記２立体の共通領域を通過する接触平面を定義する接触平面設定手順と、
前記の各立体と接触平面との接触状態をそれぞれ個別の半接触問題として解く接触問題解決手順と、
前記接触問題解決手順によって求められた立体毎の半接触問題の解を統合して、前記２立体間の接触形状を算出する接触形状算出手順と、
を実行させることを特徴とするコンピュータ・プログラムである。
【００３１】
本発明の第２の側面に係るコンピュータ・プログラムは、コンピュータ・システム上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第２の側面に係るコンピュータ・プログラムをコンピュータ・システムにインストールすることによって、コンピュータ・システム上では協働的作用が発揮され、本発明の第１の側面に係る接触多角形算出装置と同様の作用効果を得ることができる。
【発明の効果】
【００３２】
本発明によれば、複数の３次元立体モデル間の接触を検出し、接触状態にある２立体間の接触形状を好適に算出することができる、優れた接触形状算出装置及び接触形状算出方法、並びにコンピュータ・プログラムを提供することができる。
【００３３】
また、本発明によれば、２つの立体が衝突又は接触している状況において、両者の間に形成される接触多角形を完全に且つ高速に算出することができる、優れた接触形状算出装置及び接触形状算出方法、並びにコンピュータ・プログラムを提供することができる。
【００３４】
本発明によれば、衝突検出アルゴリズムから得られた衝突代表点対（最浸透点対若しくは最近傍点対）から、その周辺探索のみを行なうことによって、精密な接地多角形を高速に算出することができる。そして、得られた接地多角形を利用して、高速な力学シミュレーションや、より複雑な環境下における脚式移動ロボットの制御を実現することができる。
【００３５】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【発明を実施するための最良の形態】
【００３６】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００３７】
Ａ．接触多角形の算出方法
本発明は、衝突又は接触している状況下の凸立体間に形成される接触多角形を高速に求める接触多角形算出方法に関する。まず、簡単のために、図１に示すように、２つの立方体ＡとＢが干渉している場合において、その接触多角形（ｖ１，ｖ２，ｖ３，ｖ４）を求める方法について例示しながら説明する。
【００３８】
本発明では、立体間の接触検出には、従来の衝突検出アルゴリズムを適用することができる。ここでは、衝突検出アルゴリズムとして、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）を想定する。ＧＪＫは凸形図形間距離を算出するための反復的な方法であり、図２に示すように、２立体間の最近傍点や最浸透点といった衝突代表点対を求め、立体間で衝突や接触が発生したことを効率よく検出することができる。
【００３９】
但し、本発明の要旨はＧＪＫアルゴリズムに限定されるものではない。ＧＪＫアルゴリズムの詳細については、例えば、Ｇ．ｖａｎ ｄｅｎ Ｂｅｒｇｅｎ著“ＡＦａｓｔ ａｎｄ Ｒｏｂｕｓｔ ＧＪＫ ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｆｏｒ Ｃｏｌｌｉｓｉｏｎ Ｄｅｔｅｃｔｉｏｎ ｏｆ Ｃｏｎｖｅｘ Ｏｂｊｅｃｔｓ”（Ｊｏｕｒｎａｌｏｆ Ｇｒａｐｈｉｃｓ Ｔｏｏｌｓ，４（２），ｐｐ．７−２５，２００１）を参照されたい。
【００４０】
図３には、立体Ａ及びＢの接触部を横から見た拡大図を示している。上記の衝突検出アルゴリズムにより、接触状態にある２立体間において接触を生じている代表点対（すなわち、相互に最も浸透した点の対）Ｐａ及びＰｂが算出されているとする。本発明に係る接触形状算出方法では、この衝突代表点対に関する情報を出発点とし、接触多角形を構成するすべての頂点ｖ１、ｖ２、ｖ３、ｖ４を求める。
【００４１】
接触状態を知る第１のステップとして、立体Ａ及びＢの接触部位における法線ベクトルｎを用意する。法線ベクトルｎは、通常、衝突代表点対ＰａとＰｂを結ぶ単位ベクトルとして良く近似できることが知られており、下式（１）で求めることができる。
【００４２】
【数１】
2007102394.tif 000003
【００４３】
さらに、これら衝突代表点対Ｐａ及びＰｂ間を結ぶ線分の中点を通り、法線ベクトルをｎとする平面ＣＰを定義し、「接触平面」と呼ぶこととする。
【００４４】
２つの立体ＡとＢの接触状態を直接的に扱うと、さまざまな組合せが存在し、問題が複雑となる。そこで、立体間の接触状態を知る第２ステップとして、図４に示すように、この問題を接触平面ＣＰと一方の立体Ａの接触問題と、及び接触ＣＰと立体Ｂの接触問題という、２つの独立な問題に分割する。
【００４５】
平面と凸立体の接触状態の把握に関しては、立体の頂点のうち、平面上に存在するものをすべて調べるなどの処理により、比較的容易に接触面形状を得ることができる。すなわち、図４に示すように、立体毎に接触平面との接触面形状 （ｖａ１，ｖａ２，ｖａ３，ｖａ４） 及び（ｖｂ１，ｖｂ２，ｖｂ３，ｖｂ４）を得る原理を構成し易い。本明細書では、立体Ａ及びＢのそれぞれと接触平面ＣＰとの接触形状を与える多角形のことを「半接触多角形」と呼ぶことにする。
【００４６】
しかし、実際には、半接触多角形を得るために、上述したように立体Ａ及びＢのそれぞれについてすべての頂点に対して接触平面上に存在するかどうか探索を行なうと、立体の頂点数が多くなるにつれて、頂点探索に要する計算量が増大してしまう。形状が複雑になるにつれて接触多角形の算出速度が低下することは、力学シミュレーション並びらロボットの制御への応用の双方においても実時間処理の障害となるから望ましくない。
【００４７】
そこで、本発明では、局所的探索のみを行なうことによって、少ない計算量で高速に半接触多角形を求める方法を適用することにした。効率的に実装された衝突検出アルゴリズムＧＪＫでは、各立体の衝突代表点Ｐａ及びＰｂの情報とともに、その周辺の立体Ａ及びＢそれぞれについて頂点の接続関係を表すグラフ（すなわち、隣接頂点グラフ）を保持している。Ｐａ及びＰｂから隣接頂点グラフ上の適切な起点を算出し、ここを起点として隣接頂点グラフ上の一部のみを探索することで、半接触多角形を構成する。
【００４８】
衝突検出アルゴリズムによってそれぞれの立体から得られる代表点Ｐａ及びＰｂは、図５に示すように、必ずしも各立体Ａ、Ｂの頂点上に存在するとは限らない。このため、まず、代表点Ｐａ及びＰｂから隣接頂点グラフ上の適切な起点を算出しなければならないが。これは各立体Ａ、Ｂに対して法線ベクトル（又はその逆）方向へのサポート写像を算出することで得ることができる。サポート写像とは、立体の頂点のうち、あるベクトル方向に最も遠のいた頂点（サポート・ポイント）を得る演算である。図６には、ある立体Ｒに対し矢印で示したベクトル方向へのサポート写像を得る様子を示している。図５に示した例では、法線ベクトルと逆方向への立体Ａのサポート写像は例えばｐａ１となる。また、法線ベクトル方向への立体Ｂのサポート写像は例えばｐｂ１となる。そして、サポート写像により得られたこれらの点を立体Ａ、Ｂそれぞれの隣接頂点グラフ探索の起点とすることができる。
【００４９】
サポート写像は衝突検出アルゴリズム（ＧＪＫ）内部でも用いられている。したがって、衝突検出計算の途中結果を用いることにより、上記法線ベクトル方向のサポート写像を高速に得ることができる。なお、サポート写像の算出方法自体は本発明の要旨ではないので、詳細はＧ．ｖａｎｄｅｎ Ｂｅｒｇｅｎ著“Ａ Ｆａｓｔ ａｎｄ Ｒｏｂｕｓｔ ＧＪＫ ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｆｏｒ Ｃｏｌｌｉｓｉｏｎ Ｄｅｔｅｃｔｉｏｎ ｏｆ ＣｏｎｖｅｘＯｂｊｅｃｔｓ”（Ｊｏｕｒｎａｌ ｏｆ Ｇｒａｐｈｉｃｓ Ｔｏｏｌｓ，４（２），ｐｐ．７−２５，２００１）を参照されたい。
【００５０】
続いて、上記のように法線ベクトル（又はその逆）方向へのサポート写像から得られた頂点を起点として、立体Ａ及びＢそれぞれの隣接頂点グラフ上の探索を行なう。図７を参照しながら説明すると、まず、起点ｐａ１に対し、これが接触平面ＣＰの下方にあるかどうか調べる。下方にあれば、頂点ｐａ１は半接触多角形内に存在する点と判断することができる。ここで、ある頂点ｘがある平面（法線ベクトルｎ、ｐを通る平面）の正領域にあるか負領域にあるかは、下式（２）を用いて判定を行なうことができる。
【００５１】
【数２】
2007102394.tif 000004
【００５２】
次に、ｐａ１に隣接した頂点ｐａ２及びｐａ４についても、同様の方法で接触平面の下方に存在するかどうかを調べる。このように再帰的に隣接頂点グラフを辿りながら、接触平面と頂点との位置関係を調査していく。そして、既に調査済みの頂点や、接触平面より上方に存在する頂点に到達したら、それ以上は隣接頂点グラフを辿る必要はない。そのときまでに得られた、接触平面より下方にある頂点の集合が半接触多角形の周上の点及びその内部の点の集合を与える。
【００５３】
このように、法線ベクトル（又はその逆）方向へのサポート写像から得られた頂点を起点とした隣接頂点グラフの局所的探索によれば、隣接頂点グラフ中で接触平面より下方の頂点の調査のみ行なうことから、ごく少数の頂点の調査のみで済み、立体の頂点について全探索を行なう手法に比べて大幅な計算速度の向上を図ることができる。
【００５４】
図７に示す例では、破線以下の４つの点ｐａ１、ｐａ２、ｐａ５、ｐａ６が立体Ａと接触平面との接触形状を与える半接触多角形として最終的に得られる。また、処理対象となる立体Ａがさらに複雑な形状でｐａ３、ｐａ４、ｐａ７、ｐａ８よりも上部に頂点が存在したとしても、上述したアルゴリズムに従って適当な頂点を起点とした隣接頂点グラフの局所的探索を行なうことにより、これ以上の範囲に探索が及ぶことがないことを十分理解されたい。
【００５５】
また、もう一方の接触問題として立体Ｂと接触平面ＣＰの半接触多角形についても同様に、法線方向へのサポート写像により代表点Ｐｂに最も近い頂点ｐｂ１を求めた後、ｐｂ１を起点として接触平面より下方に存在する頂点に到達するまで再帰的に隣接頂点グラフを辿っていくことによって、接触平面ＣＰより上方にある頂点集合を局所探索のみで得ることができる。
【００５６】
上述した手順に従って隣接頂点グラフ上を探索して得られた頂点集合には、一般に、半接触多角形の内部に存在する点も含まれる。例えば、図８に示すように、立方体の面としての多角形を構成する頂点ａ、ｂ、ｃ、ｄの他に、面の中心にもう１点だけ頂点ｅが設定されている場合、当該面が接触平面となるときには、上述した頂点探索の結果として得られる頂点集合は（ａ，ｂ，ｃ，ｄ，ｅ）の５点となり、半接触多角形内部の点ｅを含み、冗長なものとなる。
【００５７】
本発明では、立体間の接触状態を知るための第３のステップとして、このような半接触多角形内部の点を探索結果から取り除くようにしている。具体的には、半接触多角形内に存在するとして抽出された頂点集合を接触平面ＣＰへ正射影し、正射影した頂点群（ａ０，ｂ０，ｃ０，ｄ０，ｅ０）に対して２次元凸包演算を施す。ここでいう凸包演算とは、頂点集合を包含する最小の凸多角形頂点集合を得るための演算であり、Ｇｒａｈａｍ法などを用いることができる。Ｇｒａｈａｍ法については、例えば、ＲｏｎａｌｄＧｒａｈａｍ，“Ａｎ Ｅｆｆｉｃｉｅｎｔ Ａｌｇｏｒｉｔｈｍ ｆｏ Ｄｅｔｅｒｍｉｎｉｎｇ ｔｈｅ Ｃｏｎｖｅｘ Ｈｕｌｌ ｏｆ ａ Ｆｉｎｉｔｅ ＰｌａｎａｒＳｅｔ”（Ｉｎｆｏ．Ｐｒｏｃ．Ｌｅｔｔｅｒｓ，１，ｐｐ．１３２−１３３，１９７２）を参照されたい。
【００５８】
このように半接触多角形内部の点を頂点集合から取り除く処理を行なった結果、図８に示した例では、頂点ｅ’ が取り除かれ、内部の点を含まない、半接触多角形の輪郭（ａ’，ｂ’，ｃ’，ｄ’）を得ることができる。立体Ｂと接触平面ＣＰとの間でも同様にして、接触平面上で内部の点を含まない、半接触多角形の輪郭頂点集合を算出しておく。
【００５９】
そして、最後のステップとして、上述した手順に従って得られた、立体Ａ及びＢそれぞれの半接触多角形を接触平面上で重ね合わせ、そのオーバラップ領域（インターセクション領域、積領域）を求める。このオーバラップ領域が２立体間の接触多角形を与える。
【００６０】
図４に示した例の場合、接触平面上における立体Ａ及びＢそれぞれの半接触多角形は、図９に示すように、（ｖａ１，ｖａ２，ｖａ３，ｖａ４）、並びに（ｖｂ１，ｖｂ２，ｖｂ３，ｖｂ４）のようになる。したがって、そのオーバラップ領域は、（ｖ１，ｖ２，ｖ３，ｖ４）のようになる。２つの２次元多角形のオーバラップ領域を効率良く求めるには、例えば、Ｏ’Ｒｏｕｋｅ，ｅｔ．ａｌ，“Ａｎｅｗ ｌｉｎｅａｒ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｉｎｔｅｒｓｅｃｔｉｎｇ ｃｏｎｖｅｘ ｐｏｌｙｇｏｎｓ”（Ｃｏｍｐｕｔ．Ｇｒａｐｈ．Ｉｍａｇｅ Ｐｒｏｃｅｓｓ．，１９，ｐｐ．３８４−３９１，１９８２）に記載されている方法を用いることができる。
【００６１】
Ｂ．装置構成と処理動作
図１０には、これまで説明してきた、本発明に係る接触多角形算出方法を実現するための装置の機能的構成を模式的に示している。図示の接触多角形算出装置は、衝突検出部１１と、座標系設定部１２と、接触平面決定部１３と、探索起点決定部１４と、半接触多角形決定部１５と、接触多角形決定部１６を備えている。以下、各部について説明する。
【００６２】
衝突検出部１１は、ＧＪＫアルゴリズムを用いて，計算機上に記憶された立体間の衝突を検出する部分であり、シーン・データを保持するとともに、サポート写像算出部１８、ＧＪＫアルゴリズム実行部１７といった機能モジュールを備えている。
【００６３】
シーン・データは、すべての立体に関し、各立体上に設定された座標系のワールド座標系内での位置・姿勢（座標系情報）と、その形状に関する情報を保持している。形状に関する情報の保持の仕方として、頂点や面に関するデータ（３Ｄ形状モデル）とは別に、各頂点がどの頂点と接続されているかをすぐに調べられるように、立体毎の頂点の接続関係を表した隣接頂点グラフを保持しておくようにする。ここで、座標系情報は、各立体の運動を決定する、外部の座標系決定部（例えば、力学シミュレーション・エンジンなど）によって必要に応じて書き変えられる。
【００６４】
サポート写像算出部１８は、図６に示したように、任意の立体の頂点のうちあるベクトル方向に最も遠のく点を求める基本演算を実行する部分である。また、ＧＪＫアルゴリズム実行部１７は、サポート写像算出部１７を繰り返し用い、シーン・データ中の立体のうち接触又は衝突を起こした２立体の組をすべて検出して、各組に対して図３に示したような衝突代表点（最も浸透した点若しくは最も近接した点）を求める。そして、衝突検出部１１は，衝突検出の結果として、すべての衝突に関する衝突代表点のリストを出力する。
【００６５】
接触平面決定部１３は、衝突検出部１１にて得られた衝突代表点対から、図３に示したような、代表点を結ぶベクトルを法線ベクトルとし、代表点の中点を通る平面を接触平面として求める。
【００６６】
探索起点決定部１４は、衝突検出部１１によって衝突が検出された２つの立体それぞれから半接触多角形を求める探索処理の準備として、それぞれの立体の隣接頂点グラフから探索起点となる頂点を求める。探索起点は、接触平面の法線方向（若しくはその逆方向）へのサポート写像として求める。サポート写像の実行は、衝突検出部１１内のサポート写像算出部１８における実行結果を用いることで、より効率的に行なうことができる。
【００６７】
半接触多角形決定部１５は、衝突を起こした２つの立体それぞれに対し、探索起点決定部１４で決定された頂点を起点として、衝突検出部１１内に保持されている隣接頂点グラフ上を局所探索し、接触平面よりも接触相手となる立体側に存在する頂点集合を求める。続いて、図８を参照しながら説明したように、これらの頂点を接触平面上に正射影し、さらに２次元凸包演算を実行して、その輪郭形状を抽出することで、２立体それぞれについて半接触多角形を決定する。
【００６８】
接触多角形決定部１６は、図９を参照しながら説明したように、半接触多角形決定部１５が算出した２立体それぞれについての半接触多角形を接触平面上で重ね合わせ、インターセクション領域を求める。こうして得られた接触平面上の積領域を、２立体間の接触部の形状を表す接触多角形として出力する。
【００６９】
上述した処理を、衝突検出部１１で衝突が検出されたすべての立体の組に対して実行することで、シーン・データ内で発生したすべての立体間衝突について、正確な接触多角形を高速に算出することができる。
【００７０】
図１１には、図１０に示した接触多角形算出装置において実行される、接触多角形を算出するための処理手順をフローチャートの形式で示している。以下、欠く処理ステップについて説明する。
【００７１】
まず、座標系設定ステップでは、シーン・データ部内の各立体の位置及び姿勢情報を設定する（Ｓ１）。
【００７２】
次いで、衝突検出実行ステップでは、ＧＪＫアルゴリズムによりシーン・データ部内で接触又は衝突を起こした２立体の組をすべて求め、各組に対し、衝突代表点（最も浸透した点若しくは最も近接した点）を求める（Ｓ２）。
【００７３】
次いで、接触平面決定ステップでは、上記の衝突代表点を結ぶベクトルを法線ベクトルとし、代表点の中点を通る平面を接触平面と定義し、その平面を規定する方程式を求める（Ｓ３）。
【００７４】
次いで、探索起点決定ステップでは、立体毎に頂点の接続関係を表した隣接頂点グラフから、半接触多角形内の頂点の探索を開始する起点を求める（Ｓ４）。探索起点として、法線ベクトル方向に接触相手となる立体に最も浸透した頂点が適当であり、例えば接触平面の法線方向（若しくはその逆方向）へのサポート写像を算出して求めることができる。
【００７５】
次いで、半接触多角形決定ステップでは、決定した起点から隣接頂点グラフ上を局所探索し、接触平面よりも衝突相手側に存在する頂点集合を求める（Ｓ５）。接触平面内に存在しない頂点に到達するまで隣接頂点グラフを再帰的に局所的探索する。また、これらの頂点を接触平面上に正射影し、ここで２次元凸包演算を実行して、その輪郭形状を抽出することで、冗長な頂点を頂点集合から取り除き、半接触多角形を算出する。
【００７６】
次いで、接触多角形決定ステップでは、２立体からそれぞれ個別に算出された半接触多角形のインターセクション領域を求め、これを接触多角形として出力する（Ｓ６）。
【００７７】
図１２には、接触平面決定処理ステップにおける詳細な処理手順をフローチャートの形式で示している。
【００７８】
まず、上記の衝突代表点を結ぶ線分の中点を算出する（ステップＳ１１）。
【００７９】
次いで、衝突代表点を結ぶ線分のベクトルを求める（ステップＳ１２）。
【００８０】
そして、ステップＳ１１で求めた代表点間の中点を通り、且つステップＳ１２で求めたベクトルを法線ベクトルとする平面の方程式を求め（ステップＳ１３）、これを接触平面として出力する。
【００８１】
力学シミュレーションなどにおいて、衝突又は接触が検出された２つの立体間の接触状態を直接的に扱うと、さまざまな組み合わせが存在するために問題が複雑となり、計算負荷が高くなる。そこで、本発明では、半接触多角形決定ステップにおいて、２立体の共通領域を通過する接触平面を定義し、各立体と接触平面との接触問題を個別に解くことにより、計算負荷の軽減と処理の高速化を図っている。
【００８２】
図１３には、半接触多角形決定ステップにおける詳細な処理手順をフローチャートの形式で示している。
【００８３】
先行する処理ステップＳ４において、接触平面の法線方向（若しくはその逆方向）へのサポート写像を算出することにより、法線ベクトル方向に接触相手となる立体に最も浸透した頂点が求められると、これを隣接頂点グラフ中の局所探索の起点として設定する（ステップＳ２１）。そして、起点として設定された頂点に探索済みフラグを立てて（ステップＳ２２）、候補点リストに追加する（ステップＳ２３）。以降の処理ステップでは、この起点から順に隣接頂点グラフを辿りながら、接触平面と頂点との位置関係を調査していく。
【００８４】
ステップＳ２４では、この起点について、接触平面よりも接触相手となる立体側にあるかどうか領域判定を行なう。この領域判定は、上式（２）を用いて実施することができる。
【００８５】
ここで、起点が接触平面よりも接触相手となる立体側にないことが判明した場合には、隣接頂点グラフを辿る必要はないので、以降の頂点探索処理をスキップして、ステップＳ２８にジャンプする。
【００８６】
一方、領域判定の結果、起点が接触平面よりも接触相手となる立体側にあることが判った場合には、この起点に隣接する頂点が隣接頂点グラフ中にあるかどうかをチェックする（ステップＳ２５）。隣接する頂点がない場合には、隣接頂点グラフを辿る必要はないので、以降の頂点探索処理をスキップして、ステップＳ２８にジャンプする。
【００８７】
また、隣接する頂点が存在する場合には、調査対象を起点から次の隣接頂点に移動し（ステップＳ２６）、別途定義された頂点調査処理を実行する（ステップＳ２７）。頂点調査処理では、処理対象とする頂点が半接触多角形内に存在するかどうかを調査する。具体的には、接触平面内に存在しない頂点に到達するまで隣接頂点グラフを再帰的に局所的探索して、半接触多角形の周上の点及びその内部に存在する頂点の集合を出力する。
【００８８】
頂点探索処理が終了すると、ステップＳ２５に戻る。そして、調査対象とすべき隣接頂点が隣接頂点グラフ中にない場合には、ステップＳ２８に進み、得られた頂点集合中の冗長な頂点を取り除く処理を行なう。
【００８９】
冗長な頂点とは、具体的には、半接触多角形の内部に存在する頂点である（図８を参照のこと）。ステップＳ２８では、半接触多角形内に存在するとして抽出された頂点集合を接触平面ＣＰへ正射影する。
【００９０】
続いて、ステップＳ２９では、接触平面に正射影した頂点群に対して、２次元凸包演算を施すことによって、頂点集合を包含する最小の凸多角形頂点集合を得る。凸包演算として、Ｇｒａｈａｍ法などを用いることができる。
【００９１】
このように半接触多角形内部の点を頂点集合から取り除く処理を行なった結果、冗長な頂点を含まない半接触多角形の輪郭を得ることができる。そして、このような半接触多角形決定処理は、接触する２物体の双方に対しそれぞれ実施される。
【００９２】
図１４には、ステップＳ２７で実行される頂点調査の詳細な処理手順をフローチャートの形式で示している。
【００９３】
まず、調査対象とする頂点に探索済みフラグが設定されているかどうかをチェックする（ステップＳ３１）。
【００９４】
探索済みフラグが既に設定されている場合には、それ以上は隣接頂点グラフを辿る必要はないので、そのときまでに得られた候補リストを出力して、本処理ルーチン全体を終了する。
【００９５】
探索済みフラグがまだ設定されていない場合には、探索済みフラグを当該頂点に設定して（ステップＳ３２）、接触平面よりも接触相手となる立体側にあるかどうか領域判定を行なう（ステップＳ３３）。この領域判定は、上式（２）を用いて実施することができる。
【００９６】
ここで、当該頂点が接触平面よりも接触相手となる立体側にないことが判明した場合には、隣接頂点グラフを辿る必要はないので、それ以上は隣接頂点グラフを辿る必要はないので、そのときまでに得られた候補リストを出力して、本処理ルーチン全体を終了する。
【００９７】
一方、領域判定の結果、当該頂点が接触平面よりも接触相手となる立体側にあることが判った場合には、当該頂点を候補リストに追加する（ステップＳ３４）。
【００９８】
そして、当該頂点に隣接する頂点が隣接頂点グラフ中にあるかどうかをチェックする（ステップＳ３５）。隣接する頂点がない場合には、隣接頂点グラフを辿る必要はないので、そのときまでに得られた候補リストを出力して、本処理ルーチン全体を終了する。
【００９９】
また、隣接する頂点がまだ隣接頂点グラフに残っている場合には、調査対象を次の隣接頂点に移動し（ステップＳ３６）、この隣接頂点に対して上述した頂点調査を再帰的に実行する（ステップＳ３７）。
【０１００】
Ｃ．非浸透型モデルへの適用
これまでは、説明の簡単のため相互に浸透が発生した２立体の組に対して接触多角形を求める方法について述べた。しかし、多くの力学シミュレーションでは、（主に効率面の理由から）立体が相互に浸透することがないように反力を発生させる。このように非浸透型モデルを扱う場合、図１５に示すように、各立体と球のミンコウスキー（Ｍｉｎｋｏｗｓｋｙ）和、すなわち、立体を球の半径（ｒ）分だけ膨張させた図形に対して衝突検出を行ない、接触平面を法線ベクトル方向にオフセットさせた平面（オフセット接触平面）と、（膨張前の）原立体との間の半接触多角形を算出し、インターセクションを求めるように構成する。
【０１０１】
原立体間が干渉していない場合、衝突検出アルゴリズムＧＪＫは、衝突代表点対として、最浸透点対の代わりに、２立体上の最近接点対を算出する。したがって、浸透型モデルでは接触平面は２立体の共通領域を通過する平面として定義されるのに対し、非浸透型モデルでは、接触平面を最近接点対の中点を通り、両点を結ぶ直線方向を法線ベクトルとするように定義される。
【０１０２】
オフセット量ｒの決定方法はさまざまであるが、オフセット接触平面と原立体が必ず交点を有するように設定するのが好ましい。例えば、立体Ａとの半接触多角形を算出する際には、接触平面を立体Ａ側にｒだけオフセットさせたオフセット接触平面を用いれば、上式（２）の判定を、これらのオフセット接触平面と原立体の頂点の間で実行すると、上述と同様に隣接頂点グラフの探索を実行することができる。同様に、立体Ｂとの半接触多角形を算出する際には、上記とは逆方向、すなわち立体Ｂ側にｒだけオフセットさせたオフセット接触平面を用いればよい。また、探索起点となる頂点の選定や、凸包演算による半接触多角形内部の点の除去、インターセクション演算による半接触多角形共通領域の算出など、その他の処理に関しては、浸透型の場合と同一の手続きで実行すればよい。
【０１０３】
非浸透型モデルを用いた場合でも、図１１に示したと同様の処理手順に従って、膨張前の原立体それぞれについて個別に半接触多角形を算出して、原立体間の接触多角形を求めることができる。但し、膨張させていることにより、原立体間が干渉していない場合もあることから、接触平面の決定処理が図１２とは相違する。
【０１０４】
図１６には、非浸透型モデルを用いた場合の接触平面決定処理ステップにおける詳細な処理手順をフローチャートの形式で示している。
【０１０５】
まず、衝突検出実行ステップにより衝突代表点として各立体から求められた最近傍点同士を結ぶ線分の中点を算出する（ステップＳ４１）。
【０１０６】
次いで、これら最近傍点対を結ぶ線分のベクトルを求める（ステップＳ４２）。
【０１０７】
そして、ステップＳ４１で求めた最近傍点間の中点を通り、且つステップＳ４２で求めたベクトルを法線ベクトルとする平面の方程式を求め（ステップＳ４３）、これを接触平面として出力する。
【０１０８】
さらに、それぞれの立体について、ステップＳ４３で求めた接触平面を法線方向にオフセットさせて、オフセット接触平面を決定する（ステップＳ４４）。具体的には、それぞれの立体について、衝突検出実行ステップで用いたオフセット量ｒだけ法線方向で自分側にオフセットさせる。
【０１０９】
Ｄ．その他の形状の立体への適用
これまでの説明では凸多面体間の接触多角形を求める方法について述べたが、本発明の要旨はこれに限定されるものではない。例えば、球や円柱、円錐といった他の形状についても、基本的には同様の手続きで接触多角形を求めることができる。
【０１１０】
立体の形状によって処理が相違する点として、接触平面とこれらの立体間の間で形成される半接触多角形の算出方法を挙げることができる。立体の種類毎に特別な手続きを用意する必要があるからである。
【０１１１】
球の場合、比較的容易に適用することができる。例えば、図１７に示す通り、接触平面の法線方向へのサポート写像を接触平面へ正射影した点を半接触多角形とすればよい。
【０１１２】
その他の形状に関しても初等幾何を用いて比較的容易に求めることができる。また、凹形状に関しても、これらの凸形状の和集合として表現することで扱うことができる。
【０１１３】
Ｅ．力学シミュレーションへの適用
図１８には、衝突又は接触する立体間の接地多角形を上述したアルゴリズムに従って算出し、接地多角形頂点に反力を作用させることで、力学シミュレーションを実行した例を示している。同図において、接地多角形は白線で示されている。
【０１１４】
本発明に係るアルゴリズムによれば、衝突又は接触する２立体間の接地多角形をより正確に算出することができる。したがって、衝突検出アルゴリズムから得られた衝突代表点にのみ力を作用させる場合に比較して，物理的根拠のある複数点に同時に反力が作用することから、振動が発生しにくく、安定な力学シミュレーションを実行することができる。
【０１１５】
また、本発明に係るアルゴリズムによれば、立体を構成する頂点を計算に用いることなく、衝突代表点付近の局所探索のみによって接地多角形の算出が遂行されることから、高速なシミュレーションが可能となる、すなわち、安定性と高速性を兼ね備えた力学シミュレーションが可能となる。
【０１１６】
例えば、脚式ロボットの歩行中における足底と床面との接地多角形をリアルタイムで算出し、ＺＭＰに基づく姿勢安定制御を行なうことができる。さらに、ロボットが異なる複数の平面に同時に接する場合、自分自身と物理的に接触している場合などにおいて、その正確な接触状態をリアルタイムで把握することができ、より一般的な安定化を行なうとともに、複雑な動作計画を立てることが可能になる。
【産業上の利用可能性】
【０１１７】
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１１８】
例えば、移動ロボットのように、周囲の環境との物理的な接触状態が多様に変化し、その状態をリアルタイムで把握する必要があるシステムに対し、本発明を好適に適用することができる。すなわち、画像処理やレンジ・センサによって得られた環境形状と、ロボット自身の３次元形状モデルに対し、本発明を適用することにより、ロボットと環境の間の接触状態を高速に求めることができ、より一般化されたバランス維持機能や、多様な環境下での動作計画が実現可能となる。
【０１１９】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【図面の簡単な説明】
【０１２０】
【図１】図１は、２つの立方体ＡとＢが干渉している様子を示した図である。
【図２】図２は、接触状態にある２立体間の最近傍点や最浸透点といった衝突代表点対を求める様子を示した図である。
【図３】図３は、立体Ａ及びＢの接触部を横から見た拡大図である。
【図４】図４は、立体Ａ及びＢ間の接触問題を接触平面ＣＰと一方の立体Ａの接触問題と、及び接触ＣＰと立体Ｂの接触問題に分割した様子を示した図である。
【図５】図５は、衝突検出アルゴリズムによってそれぞれの立体から得られる代表点Ｐａ及びＰｂを示した図である。
【図６】図６は、ある立体Ｒに対し矢印で示したベクトル方向へのサポート写像を得る様子を示した図である。
【図７】図７は、適当な頂点を起点として隣接頂点グラフを局所探索し、立体Ａと接触平面との最終的な半接触多角形を得た様子を示した図である。
【図８】図８は、立方体の面面の中心に頂点が設定され、頂点探索の結果として含まれる様子を示した図である。
【図９】図９は、接触平面上における立体Ａ及びＢそれぞれの半接触多角形からオーバラップ領域を求める様子を示した図である。
【図１０】図１０は、本発明に係る立体間の接触多角形算出方法を実現するための装置の機能的構成を模式的に示した図である。
【図１１】図１１は、接触多角形を算出するための処理手順を示したフローチャートである。
【図１２】図１２は、接触平面決定処理ステップにおける詳細な処理手順を示したフローチャートである。
【図１３】図１３は、半接触多角形決定ステップにおける詳細な処理手順を示したフローチャートである。
【図１４】図１４は、頂点調査の詳細な処理手順を示したフローチャートである。
【図１５】図１５は、立体を球の半径（ｒ）分だけ膨張させた図形に対して衝突検出を行なう様子を示した図である。
【図１６】図１６は、非浸透型モデルを用いた場合の接触平面決定処理ステップにおける詳細な処理手順を示したフローチャートである。
【図１７】図１７は、３次元立体として球を扱う場合に、接触平面の法線方向へのサポート写像を接触平面へ正射影した点を半接触多角形として算出する様子を示した図である。
【図１８】図１８は、本発明を用いて力学シミュレーションを実行した結果を示した図である。
【図１９】図１９は、脚式ロボットの足底接地点と路面の形成する支持多角形の内側にＺＭＰが存在している様子を示した図である。
【図２０】図２０は、ロボットが法線ベクトルの異なる複数の平面に同時に接する場合や、自分自身と物理的に接触している場合（自己干渉）を示した図である。
【図２１】図２１は、２つの立方体がＡ、Ｂ、Ｃ、Ｄの各点で接触している様子を示した図である。
【図２２】図２２は、１衝突当たり代表点Ｐを１点（通常は最も浸透した点、若しくは最も近接した点）として立体間の衝突又は接触を検出する様子を示した図である。
【図２３】図２３は、立体間の衝突又は接触を発生した３角形メッシュＭで取得する様子を示した図である。
【図２４】図２４は、立方体が水平な床上に置かれている場合において、代表点にのみ半力を作用させたときの力学的シミュレーションの結果を示した図である。
【符号の説明】
【０１２１】
１１…衝突検出部
１２…座標系設定部
１３…接触平面決定部
１４…探索起点決定部
１５…半接触多角形決定部
１６…接触多角形決定部
１７…ＧＪＫアルゴリズム実行部
１８…サポート写像算出部
【図１】
2007102394.tif 000005
【図２】
2007102394.tif 000006
【図３】
2007102394.tif 000007
【図４】
2007102394.tif 000008
【図５】
2007102394.tif 000009
【図６】
2007102394.tif 000010
【図７】
2007102394.tif 000011
【図８】
2007102394.tif 000012
【図９】
2007102394.tif 000013
【図１０】
2007102394.tif 000014
【図１１】
2007102394.tif 000015
【図１２】
2007102394.tif 000016
【図１３】
2007102394.tif 000017
【図１４】
2007102394.tif 000018
【図１５】
2007102394.tif 000019
【図１６】
2007102394.tif 000020
【図１７】
2007102394.tif 000021
【図１８】
2007102394.tif 000022
【図１９】
2007102394.tif 000023
【図２０】
2007102394.tif 000024
【図２１】
2007102394.tif 000025
【図２２】
2007102394.tif 000026
【図２３】
2007102394.tif 000027
【図２４】
2007102394.tif 000028
【手続補正書】
【提出日】平成18年10月18日(2006.10.18)
【手続補正１】
【補正対象書類名】明細書
【補正対象項目名】００３１
【補正方法】変更
【補正の内容】
【００３１】
本発明の第２の側面に係るコンピュータ・プログラムは、コンピュータ・システム上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第２の側面に係るコンピュータ・プログラムをコンピュータ・システムにインストールすることによって、コンピュータ・システム上では協働的作用が発揮され、本発明の第１の側面に係る接触形状算出装置と同様の作用効果を得ることができる。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】００４８
【補正方法】変更
【補正の内容】
【００４８】
衝突検出アルゴリズムによってそれぞれの立体から得られる代表点Ｐａ及びＰｂは、図５に示すように、必ずしも各立体Ａ、Ｂの頂点上に存在するとは限らない。このため、まず、代表点Ｐａ及びＰｂから隣接頂点グラフ上の適切な起点を算出しなければならないが、これは各立体Ａ、Ｂに対して法線ベクトル（又はその逆）方向へのサポート写像を算出することで得ることができる。サポート写像とは、立体の頂点のうち、あるベクトル方向に最も遠のいた頂点（サポート・ポイント）を得る演算である。図６には、ある立体Ｒに対し矢印で示したベクトル方向へのサポート写像を得る様子を示している。図５に示した例では、法線ベクトルと逆方向への立体Ａのサポート写像は例えばｐａ１となる。また、法線ベクトル方向への立体Ｂのサポート写像は例えばｐｂ１となる。そして、サポート写像により得られたこれらの点を立体Ａ、Ｂそれぞれの隣接頂点グラフ探索の起点とすることができる。
【手続補正３】
【補正対象書類名】明細書
【補正対象項目名】００５７
【補正方法】変更
【補正の内容】
【００５７】
本発明では、立体間の接触状態を知るための第３のステップとして、このような半接触多角形内部の点を探索結果から取り除くようにしている。具体的には、半接触多角形内に存在するとして抽出された頂点集合を接触平面ＣＰへ正射影し、正射影した頂点群（ａ０，ｂ０，ｃ０，ｄ０，ｅ０）に対して２次元凸包演算を施す。ここでいう凸包演算とは、頂点集合を包含する最小の凸多角形頂点集合を得るための演算であり、Ｇｒａｈａｍ法などを用いることができる。Ｇｒａｈａｍ法については、例えば、Ｒｏｎａｌｄ  Ｇｒａｈａｍ，“Ａｎ  Ｅｆｆｉｃｉｅｎｔ  Ａｌｇｏｒｉｔｈｍ  ｆｏｒ  Ｄｅｔｅｒｍｉｎｉｎｇ  ｔｈｅ  ＣｏｎｖｅｘＨｕｌｌ  ｏｆ  ａ  Ｆｉｎｉｔｅ  Ｐｌａｎａｒ  Ｓｅｔ”（Ｉｎｆｏ．Ｐｒｏｃ．Ｌｅｔｔｅｒｓ，１，ｐｐ．１３２−１３３，１９７２）を参照されたい。
【手続補正４】
【補正対象書類名】明細書
【補正対象項目名】００６０
【補正方法】変更
【補正の内容】
【００６０】
図４に示した例の場合、接触平面上における立体Ａ及びＢそれぞれの半接触多角形は、図９に示すように、（ｖａ１，ｖａ２，ｖａ３，ｖａ４）、並びに（ｖｂ１，ｖｂ２，ｖｂ３，ｖｂ４）のようになる。したがって、そのオーバラップ領域は、（ｖ１，ｖ２，ｖ３，ｖ４）のようになる。２つの２次元多角形のオーバラップ領域を効率良く求めるには、例えば、Ｏ’Ｒｏｕｒｋｅ，ｅｔ．ａｌ，“Ａ  ｎｅｗ  ｌｉｎｅａｒ  ａｌｇｏｒｉｔｈｍ  ｆｏｒ  ｉｎｔｅｒｓｅｃｔｉｎｇ  ｃｏｎｖｅｘ  ｐｏｌｙｇｏｎｓ”（Ｃｏｍｐｕｔ．Ｇｒａｐｈ．Ｉｍａｇｅ  Ｐｒｏｃｅｓｓ．，１９，ｐｐ．３８４−３９１，１９８２）に記載されている方法を用いることができる。
【手続補正５】
【補正対象書類名】明細書
【補正対象項目名】００６４
【補正方法】変更
【補正の内容】
【００６４】
サポート写像算出部１８は、図６に示したように、任意の立体の頂点のうちあるベクトル方向に最も遠のく点を求める基本演算を実行する部分である。また、ＧＪＫアルゴリズム実行部１７は、サポート写像算出部１８を繰り返し用い、シーン・データ中の立体のうち接触又は衝突を起こした２立体の組をすべて検出して、各組に対して図３に示したような衝突代表点（最も浸透した点若しくは最も近接した点）を求める。そして、衝突検出部１１は，衝突検出の結果として、すべての衝突に関する衝突代表点のリストを出力する。
【手続補正６】
【補正対象書類名】明細書
【補正対象項目名】００９４
【補正方法】変更
【補正の内容】
【００９４】
探索済みフラグが既に設定されている場合には、それ以上は隣接頂点グラフを辿る必要はないので、そのときまでに得られた候補点リストを出力して、本処理ルーチン全体を終了する。
【手続補正７】
【補正対象書類名】明細書
【補正対象項目名】００９６
【補正方法】変更
【補正の内容】
【００９６】
ここで、当該頂点が接触平面よりも接触相手となる立体側にないことが判明した場合には、隣接頂点グラフを辿る必要はないので、それ以上は隣接頂点グラフを辿る必要はないので、そのときまでに得られた候補点リストを出力して、本処理ルーチン全体を終了する。
【手続補正８】
【補正対象書類名】明細書
【補正対象項目名】００９７
【補正方法】変更
【補正の内容】
【００９７】
一方、領域判定の結果、当該頂点が接触平面よりも接触相手となる立体側にあることが判った場合には、当該頂点を候補点リストに追加する（ステップＳ３４）。
【手続補正９】
【補正対象書類名】明細書
【補正対象項目名】００９８
【補正方法】変更
【補正の内容】
【００９８】
そして、当該頂点に隣接する頂点が隣接頂点グラフ中にあるかどうかをチェックする（ステップＳ３５）。隣接する頂点がない場合には、隣接頂点グラフを辿る必要はないので、そのときまでに得られた候補点リストを出力して、本処理ルーチン全体を終了する。
【手続補正１０】
【補正対象書類名】明細書
【補正対象項目名】０１００
【補正方法】変更
【補正の内容】
【０１００】
Ｃ．非浸透型モデルへの適用
これまでは、説明の簡単のため相互に浸透が発生した２立体の組に対して接触多角形を求める方法について述べた。しかし、多くの力学シミュレーションでは、（主に効率面の理由から）立体が相互に浸透することがないように反力を発生させる。このように非浸透型モデルを扱う場合、図１５に示すように、各立体と球のミンコウスキー（Ｍｉｎｋｏｗｓｋｉ）和、すなわち、立体を球の半径（ｒ）分だけ膨張させた図形に対して衝突検出を行ない、接触平面を法線ベクトル方向にオフセットさせた平面（オフセット接触平面）と、（膨張前の）原立体との間の半接触多角形を算出し、インターセクションを求めるように構成する。