幾何学関数

実数ベクトル(float-vector)

float-vectorは、要素が実数である1次元ベクトルである。 float-vectorは、どんなサイズでも良い。 resultが引き数リストで指定されているとき、 そのresultはfloat-vectorであるべきである。

float-vector &rest numbers

を要素とするfloat-vectorを新しく作る。 (float-vector 1 2 3)と#F(1 2 3)の違いに注意すること。 前者は、呼ばれたときはいつでもベクトルが生成されるが、 後者は読み込まれたときのみ生成される。

float-vector-p obj

がfloat-vectorであるならば、Tを返す。

v+ fltvec1 fltvec2 &optional result

2つのfloat-vectorを加える。

v- fltvec1 &optional fltvec2 result

2つのfloat-vectorを差し引く。もし、fltvec2が省略されているならば、 fltvec1の符号が反転される。

v. fltvec1 fltvec2

2つのfloat-vectorの内積を計算する。

v* fltvec1 fltvec2 &optional result

2つのfloat-vectorの外積を計算する。

v.* fltvec1 fltvec2 fltvec3

スカラー3重積を計算する。(v.* A B C)=(V. A (V* B C))=(V. (V* A B) C)

v:math:`=` fltvec1 fltvec2

もし、fltvec1の要素がfltvec2の対応する要素とすべて等しいとき、 Tを返す。

v:math:`<` fltvec1 fltvec2

もし、fltvec1の要素がfltvec2の対応する要素よりすべて小さいとき、 Tを返す。

v:math:`>` fltvec1 fltvec2

もし、fltvec1の要素がfltvec2の対応する要素よりすべて大きいとき、 Tを返す。

vmin &rest fltvec

の中のそれぞれの次元における最小値を捜し、 その値でfloat-vectorを新しく作る。vminvmaxは、 頂点の座標から最小のminimal-boxを見つけるために使用される。

vmax &rest fltvec

の中のそれぞれの次元における最大値を捜し、 その値でfloat-vectorを新しく作る。

minimal-box v-list minvec maxvec &optional err

与えられたv-listに対してminimal bounding boxを計算し、 その結果をminvecmaxvecに蓄積する。 もし、実数errが指定されているならば、minimal boxはその比率によって 成長する。すなわち、もしerrが0.01のとき、minvecのそれぞれの 要素はminvecmaxvecとの距離の1%減少する。 そして、maxvecのそれぞれの要素は1%増加する。 minimal-boxは、minvecmaxvecとの距離を返す。

scale number fltvec &optional result

のすべての要素をスカラーnumber倍する。

norm fltvec

のノルムを求める。\(\Vert fltvec\Vert\)

norm2 fltvec

のノルムの2乗を求める。 \(\Vert fltvec\Vert^2\)=(v. fltvec fltvec)

normalize-vector fltvec &optional result

のノルムが1.0となるように正規化する。

distance fltvec1 fltvec2

2つのfloat-vectorの距離を返す。\(|fltvec-fltvec2|\)

distance2 fltvec1 fltvec2

2つのfloat-vectorの距離の2乗を返す。\(|fltvec-fltvec2|^2\)

homo2normal homovec &optional normalvec

同次ベクトルhomovecを正規表現に変換する。

homogenize normalvec &optional homovec

正規ベクトルnormalvecを同次表現に変換する。

midpoint p p1 p2 &optional result

は実数で、p1,p2は同次元のfloat-vectorである。 p1:math:`-`p2\(p:(1-p)\)の比率で等分した点\((1-p)\cdot p1 + p\cdot p2\) を返す。

rotate-vector fltvec theta axis &optional result

2次元あるいは3次元のfltvecaxis回りにthetaラジアン 回転する。 axisは、:x, :y, :z, 0, 1, 2, または NILの内の一つである。 axisがNILのとき、fltvecは2次元として扱われる。 3次元空間の任意の軸の回りにベクトルを回転するためには、 rotation-matrixで回転行列を作り、そのベクトルにかければよい。

行列と変換

行列は、要素がすべて実数の2次元の配列である。 ほとんどの関数において行列はどんなサイズでもよいが、 v*, v.*, euler-angle, rpy-angle関数では3次元の行列のみ 扱うことができる。 transform, m*transposeは、行列を正方行列に限定せず、 一般のn*m行列に対して処理を行う。

resultパラメータを受けた関数は、計算結果をそこに置く。 そのため、ヒープは使用しない。 すべての行列関数は、正規座標系における変換を考慮しており、 同次座標系は考慮していない。

rpy-angle関数は、回転行列をワールド座標系におけるz,y,x軸回りの 3つの回転角に分解する。 euler-angle関数はrpy-angleと同様に分解するが、 回転軸がローカル座標系のz,y,z軸となっている。 角度が反対方向にも得られるため、これらの関数は2つの解を返す。

; Mat is a 3X3 rotation matrix.
(setq rots (rpy-angle mat))
(setq r (unit-matrix 3))
(rotate-matrix r (car rots) :x t r)
(rotate-matrix r (cadr rots) :y t r)
(rotate-matrix r (caddr rots) :z t r)
;--> resulted r is equivalent to mat

3次元空間の位置と方向の組みを保つために、[Coordinates]節に記載されている coordinatescascaded-coordsクラスを使用すること。

matrix &rest elements

から行列を新しく作る。 Row x Col = (elementsの数) x (最初のelementの長さ) elementsは、どの型の列((list 1 2 3)や(vector 1 2 3)や (float-vector 1 2 3))でもよい。 それぞれの列は行列の行ベクトルとしてならべられる。

make-matrix rowsize columnsize &optional init

\(rowsize \times columnsize\)の大きさの行列を作る。

matrixp obj

もし、objが行列のとき、すなわち、objが2次元の配列で その要素が実数であるとき、Tを返す。

matrix-row mat row-index

行列matからrow-indexで示される行ベクトルを抽出する。 matrix-rowは、setfを使用することにより 行列の特定の行にベクトルを設定することにも使用される。

matrix-column mat column-index

行列matからcoloumn-indexで示される列ベクトルを抽出する。 matrix-columnは、setfを使用することにより 行列の特定の列にベクトルを設定することにも使用される。

m* matrix1 matrix2 &optional result

matrix2の積を返す。

transpose matrix &optional result

の転置行列を返す。すなわち、matrixの列と行を入れ替える。

unit-matrix dim

\(\times\) dimの単位行列を作る。

replace-matrix dest src

行列destのすべての要素を同一な行列srcで置き換える。

scale-matrix scalar mat

のすべての要素にscalerを掛ける。

copy-matrix matrix

のコピーを作る。

transform matrix fltvector &optional result

行列matrixをベクトルfltvectorの左から掛ける。

transform fltvector matrix &optional result

行列matrixをベクトルfltvectorの右から掛ける。

rotate-matrix matrix theta axis &optional world-p result

で行列matrixを回転させるとき、 回転軸(:x, :y, :zまたは0,1,2)はワールド座標系あるいは ローカル座標系のどちらかを与えられる。 もし、world-pにNILが指定されているとき、 ローカル座標系の軸に沿った回転を意味し、回転行列を左から掛ける。 もし、world-pがnon-NILのとき、 ワールド座標系に対する回転行列を作り、回転行列を右から掛ける。 もし、axisにNILが与えられたとき、行列matrixは2次元と仮定され、 world-pの如何にかかわらず2次元空間の回転が与えられる。

rotation-matrix theta axis &optional result

軸回りの2次元あるいは3次元の回転行列を作る。 軸は:x,:y,:z,0,1,2,3次元ベクトルあるいはNILのどれかである。 2次元回転行列を作るとき、axisはNILでなければならない。

rotation-angle rotation-matrix

から等価な回転軸と角度を抽出し、 実数とfloat-vectorのリストを返す。 rotation-matrixが単位行列のとき、NILが返される。 また、回転角が小さいとき、結果がエラーとなる。 rotation-matrixが2次元のとき、1つの角度値が返される。

rpy-matrix ang-z ang-y ang-x

ロール、ピッチ、ヨー角で定義される回転行列を作る。 最初に、単位行列をx軸回りにang-xラジアン回転させる。 次に、y軸回りにang-yラジアン、最後にz軸回りにang-z ラジアン回転させる。 すべての回転軸はワールド座標系で与えられる。

rpy-angle matrix

の2組のロール、ピッチ、ヨー角を抽出する。

Euler-matrix ang-z ang-y ang2-z

3つのオイラー角で定義される回転行列を作る。 最初に単位行列をz軸回りにang-z回転させ、次にy軸回りにang-y 回転させ、最後にz軸回りにang2-z回転させる。 すべての回転軸はローカル座標系で与えられる。

Euler-angle matrix

から2組のオイラー角を抽出する。

LU分解

lu-decomposelu-solveは、線形の連立方程式を 解くために用意されている。 最初に、lu-decomposeは行列を下三角行列を上三角行列に分解する。 もし、行列が特異値なら、lu-decomposeはNILを返す。 そうでなければ、lu-solveに与えるべき順列ベクトルを返す。 lu-solveは、与えられた定数ベクトルの解をLU行列で計算する。 この手法は、同じ係数行列と異なった定数ベクトルのたくさんの組に対して 解を求めたいときに効果的である。 simultaneous-equationは、1つの解だけを求めたいときにもっとも 手軽な関数である。 lu-determinantは、LU分解された行列の行列式を計算する。 inverse-matrix関数は、lu-decomposeを1回とlu-solveをn回 使って逆行列を求める。 3*3行列での計算時間は約4msである。

lu-decompose matrix &optional result

にLU分解を実行する。

lu-solve lu-mat perm-vector bvector &optional result

LU分解された1次連立方程式を解く。 perm-vectorは、lu-decomposeで返された結果でなければならない。

lu-determinant lu-mat perm-vector

LU分解された行列の行列式を求める。

simultaneous-equation mat vec

係数がmatで、定数がvecで記述される1次連立方程式を解く。

inverse-matrix mat

正方行列matの逆行列を求める。

pseudo-inverse mat

特異値分解を用いて擬似逆行列を求める。

座標系

座標系と座標変換は、coordinatesクラスで表現される。 4*4の同次行列表現の代わりに、 Euslisp内で座標系は、高速性と一般性のために 3*3の回転行列と3次元位置ベクトルの組で表現される。

coordinates

:super = ** propertied-object**
:slots (pos :type float-vector
rot :type array)

位置ベクトルと3x3の回転行列の組みで座標系を定義する。

coordinates-p obj

coordinatesクラスかまたはそのサブクラスのインスタンス のとき、Tを返す。

:rot **

この座標系の3x3回転行列を返す。

:pos **

この座標系の3次元位置ベクトルを返す。

:newcoords newrot &optional newpos

newposでこの座標系を更新する。 newposが省略された時はnewrotにはcoordinatesのインスタンスを 与える。 この座標系の状態が変化するときはいつでも、このメソッドを用いて新しい 回転行列と位置ベクトルに更新するべきである。 このメッセージはイベントを伝えて他の:updateメソッドを呼び出す。

:replace-coords newrot &optional newpos

メソッドを呼び出さずにrotとposスロットを変更する。 newposが省略された時はnewrotにはcoordinatesのインスタンスを与える。

:coords **

:copy-coords &optional dest

もしdestが与えられなかったとき、:copy-coordsは同じrotとpos スロットを持つ座標系オブジェクトを作る。 もし、destが与えられたとき、dest座標系にこの座標系のrotとposを コピーする。

:reset-coords **

この座標系の回転行列を単位行列にし、位置ベクトルをすべてゼロにする。

:worldpos **

:worldrot **

:worldcoords **

このオブジェクトのワールド座標系における位置ベクトル・回転行列・座標系 を計算する。 その座標系は、いつもワールド座標系で表現されていると仮定され、 これらのメソッドは簡単にposとrotとselfを返すことができる。 これらのメソッドはcascaded-coordsクラスと互換性がとられている。 cascaded-coordsではワールド座標系での表現と仮定していない。

:copy-worldcoords &optional dest

最初に、ワールド座標系が計算され、destにコピーされる。 もし、destが指定されてないとき、新たにcoordinates オブジェクトを作る。

:rotate-vector vec

この座標系の回転行列によってvecを回転させる。すなわち、 この座標系で表現される方向ベクトルをワールド座標系における表現 に変換する。 この座標系の位置は、回転に影響を与えない。

:transform-vector vec

この座標系で表現されるvecをワールド座標系の表現に変換する。

:inverse-transform-vector vec

ワールド座標系におけるvecをローカル座標系の表現に逆変換する。

:transform trans &optional (wrt :local)

座標系で表現されるtransによってこの座標系を変換する。 transは座標系の型でなければならないし、wrtは :local, :parent, :worldのキーワードあるいはcoordinatesの インスタンスでなければならない。 もしwrtが:localのとき、transをこの座標系の右 から適用する。もしwrtが:world, :parentのとき、 transを左から掛ける。 もしwrtcoordinatesの型であるとき、wrt座標系で表現される transは最初にワールド座標系の表現に変換され、左から掛ける。

:move-to trans &optional (wrt :local)

で表現されるtransでこの座標系のrotとpos を置き換える。

:translate p &optional (wrt :local)

このオブジェクトの位置をwrt座標系で相対的に変更する。

:locate p &optional (wrt :local)

この座標系の位置をwrt座標系で絶対的に変更する。 もし、wrtが:localのとき、:translateと同一な 効果を生む。

:rotate theta axis &optional (wrt :local)

軸回りにthetaラジアンだけ相対的にこの座標系を回転させる。 axisは、軸キーワード(:x, :y, :z)あるいは任意のfloat-vector である。 axiswrt座標系で表現されていると考える。 よって、もしwrtが:localでaxisが:zであるとき、 座標系はローカル座標系のz軸回りに回転される。 もしwrtが:world, :parentであるとき、ワールド座標系のz軸回りに 回転される。 言い換えると、もしwrtが:localのとき、 回転行列はこの座標系の右から掛けられる。 そして、もしwrtが:worldあるいは:parentのとき、 回転行列は左から掛けられる。 wrtが:worldあるいは:parentでさえ、 この座標系のposベクトルは変化しない。 本当にワールド座標系の軸回りに回転するためには、 回転を表現するcoordinatesクラスのインスタンスを :transformメソッドに与えなければならない。

:orient theta axis &optional (wrt :local)

を強制的に変更する。:rotateメソッドの絶対値版である。

:inverse-transformation **

この座標系の逆変換を持つ座標系を新しく作る。

:transformation coords (wrt :local)

この座標系と引き数で与えられるcoordsとの間の変換を作る。 もし、wrtが:localであるとき、ローカル座標系で表現される。 すなわち、もしこの:transformationの結果を:transformの引き数として wrt=:localと一緒に 与えたとき、この座標系はcoordsと同一な座標系に変換される。

:Euler az1 ay az2

オイラー角(az1, ay, az2)で表現される回転行列をrotに 設定する。

:roll-pitch-yaw roll pitch yaw

ロール・ピッチ・ヨー角で表現される回転行列をrotに設定する。

:4x4 &optional mat44

もし、mat44として4x4行列が与えられるとき、3x3回転行列と3次元位置ベクトル の座標表現に変換される。 もし、mat44が与えられないとき、この座標系の表現を 4x4の同次行列表現に変換して返す。

:init = `[メソッド]
&key = (pos #f(0 0 0)) = (rot #2f((1 0 0) (0 1 0) (0 0 1))) rpy euler axis angle 4X4 coords properties name

このcoordinatesオブジェクトを初期化し、rotとposを設定する。 それぞれのキーワードの意味は、以下に示す通りである。

:dimension
2あるいは3 (デフォルトは 3)
:pos
位置ベクトルを指定する (デフォルトは #f(0 0 0))
:rot
回転行列を指定する (デフォルトは単位行列)
:euler
オイラー角として3つの要素の列を与える
:rpy
ロール・ピッチ・ヨー角として3つの要素の列を与える
:axis
回転軸 (:x,:y,:z あるいは任意のfloat-vector)
:angle
回転角 (:axisと一緒に使用)
:wrt
回転軸を示す座標系 (デフォルトは :local)
:4X4
4X4 行列(posとrotを同時に指定)
:coords
coordsからrotとposをコピーする
:name
:name値を設定する。

:angle:axisと組みで唯一使用することができる。 その軸は:wrt座標系で決定される。 :wrtと関係なしに:Eulerはローカル座標系で定義されるオイラー角 (az1, ayaz2)をいつも指定する。 また、:rpyはワールド座標系のz, yx軸回りの角度 を指定する。 :rot, :Euler, :rpy, :axis, :4X4の中から2つ以上を連続で指定することは できない。しかしながら、指定してもエラーは返さない。 axis:angleパラメータには列を指定することができる。 その意味は、与えられた軸回りの回転を連続的に処理する。 属性とその値の組みのリストを:propertiesの引き数として 与えることができる。これらの組みは、この座標系のplistにコピーされる。

連結座標系

cascaded-coords

:super = ** coordinates**
:slots (parent descendants worldcoords manager changed)

連結された座標系を定義する。cascaded-coordsは、 しばしばcascoordsと略す。

:inheritance **

このcascaded-coordsの子孫をすべて記述した継承treeリストを返す。 もし、aとbがこの座標系の直下の子孫でcがaの 子孫であるとき、((a (c)) (b))を返す。

:assoc childcoords &optional relative-coords

は、この座標系の子孫として関係している。 もし、childcoordsが既に他のcascaded-coordsassocされて いるとき、childcoordsはそれぞれのcascaded-coordsが1つの親しか 持っていないならdissocされる。 ワールド座標系におけるchildcoordsの方向あるいは位置は変更されない。

:dissoc childcoords

この座標系の子孫リストからchildcoordsを外す。 ワールド座標系におけるchildcoordsの方向あるいは位置は変更されない。

:changed **

この座標系の親座標系が変更されていることを通知する。 また、もっとあとでワールド座標系が要求されたとき、ワールド座標系を再計算する 必要がある。

:update **

現在のワールド座標系を再計算するために:worldcoordsメソッド を呼び出す。

:worldcoords **

ルートの座標系からこの座標系までの全ての座標系を連結させることにより、 この座標系をワールド座標系で表現したcoordinatesオブジェクトで返す。 その結果は、このオブジェクトが持ち、後に再利用される。 よって、この結果の座標系を変更すべきでない。

:worldpos **

ワールド座標系で表現したこの座標系のrotを返す。

:worldrot **

ワールド座標系で表現したこの座標系のposを返す。

:transform-vector vec

をこのローカル座標系での表現とみなして、ワールド座標系での 表現に変換する。

:inverse-transform-vector vec

ワールド座標系で表現されるvecをこのローカル座標系の表現に逆変換する。

:inverse-transformation **

この座標系の逆変換を表現するcoordinatesのインスタンスを作る。

:transform trans &optional (wrt :local)

:translate fltvec &optional (wrt :local)

:locate fltvec &optional (wrt :local)

:rotate theta axis &optional (wrt :local)

:orient theta axis &optional (wrt :local)

クラスの記述を参照すること。

make-coords &key pos rot rpy Euler angle axis 4X4 coords name

make-cascoords &key pos rot rpy Euler angle axis 4X4 coords name

coords &key pos rot rpy Euler angle axis 4X4 coords name

cascoords &key pos rot rpy Euler angle axis 4X4 coords name

これらの関数は、すべてcoordinatesあるいはcascaded-coordsを 新しく作る。 キーワードパラメータについては、coordinatesクラスの:initメソッドを 見ること。

transform-coords coords1 coords2 &optional (coords3 (coords))

coords2に左から適用(乗算)される。 その積はcoords3に蓄積される。

transform-coords* &rest coords

にリスト表現されている変換を連結させる。 連結された変換で表現されるcoordinatesのインスタンスを返す。

wrt coords vec

coordsにおける表現に変換する。 その結果は(send coords :transform-vector vec)と同一である。

変換行列とcoordinates classとの関係

変換行列Tを\(4\times4\)の同次行列表現として以下のように表したした時の、 coordinatesとの関係を説明する。

\[\begin{split}T = \begin{pmatrix} \mathbf{R}_T & \mathbf{p}_T \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

\(\mathbf{R}_T\)\(3\times3\)行列、\(\mathbf{p}_T\)\(3\times1\)行列(eus内部処理 では要素数3のfloat-vector)であって、 EusLispにおけるcoordinatesクラスは\(\mathbf{R}\)\(3\times3\) の回転行列と 3 次元位置ベクトルの組で表現されており、 スロット変数のrotとposは、それぞれ、\(\mathbf{R}_T\)\(\mathbf{p}_T\)に対応する。

回転行列と3次元位置の取り出し

coordinates classのメソッドを用いて、以下のように\(\mathbf{R}\)\(\mathbf{p}\)を取り出せる。

以下、Tはcoordinateクラスのインスタンスである。

(send T :rot)

\(\Rightarrow\) \(\mathbf{R}_T\)

(send T :pos)

\(\Rightarrow\) \(\mathbf{p}_T\)

ベクトルを変換するメソッド

以下、\(\mathbf{v}\)は3次元ベクトルである。

(send T :rotate-vector :math:`mathbf{v}`)

\(\Rightarrow\) \(\mathbf{R}_T \mathbf{v}\)

(send T :inverse-rotate-vector :math:`mathbf{v}`)

\(\Rightarrow\) \(\mathbf{v}^T \mathbf{R}_T\)

(send T :transform-vector :math:`mathbf{v}`)

\(\Rightarrow\) \(\mathbf{R}_T\mathbf{v} + \mathbf{p}_T\)

ローカル座標系Tで表されたベクトルをワールド座標系で表されたベクトルに変換する

(send T :inverse-transform-vector :math:`mathbf{v}`)

\(\Rightarrow\) \(\mathbf{R}_T^{-1}\left( \mathbf{v} - \mathbf{p}_T \right)\)

ワールド座標系で表されたベクトルをローカル座標系Tで表されたベクトルに変換する

座標系を返すメソッド (座標系を変更しない)

(send T :inverse-transformation)

\(\Rightarrow\) \(T^{-1}\)

逆行列を返す。

\[\begin{split}T^{-1} = \begin{pmatrix} \mathbf{R}_T^{-1} & -\mathbf{R}_T^{-1}\mathbf{p}_T \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

(send T :transformation A (&optional (wrt :local)))

wrt == :local のとき、\(T^{-1}A\) を返す

wrt == :world のとき、\(AT^{-1}\) を返す

wrt == W (coordinates class) のとき、\(W^{-1}AT^{-1}W\) を返す

座標系を変更するメソッド

以下、A は coordinatesクラスのインスタンスである。

\(\Leftrightarrow\) はスロット変数と与えられたインスタンス(行列またはベクトル)が同一になることを表す。 一方の変更が、他方の変更に反映されるため、注意して使用すること。

\(\leftarrow\) は代入を表す。

(send T :newcoords A)

\(\mathbf{R}_T \Leftrightarrow \mathbf{R}_A\)

\(\mathbf{p}_T \Leftrightarrow \mathbf{p}_A\)

(send T :newcoords :math:`mathbf{R}` :math:`mathbf{p}`)

\(\mathbf{R}_T \Leftrightarrow \mathbf{R}\)

\(\mathbf{p}_T \Leftrightarrow \mathbf{p}\)

(send T :move-to A (&optional (wrt :local)))

wrt == :local のとき、\(T \leftarrow TA\)

wrt == :world のとき、\(T \Leftrightarrow A\)

wrt == W (coordinates class) のとき、\(T \leftarrow WA\)

(send T :translate :math:`mathbf{v}` (&optional (wrt :local)))

wrt == :local のとき、 \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{T} + \mathbf{R}_{T}\mathbf{v}\)

wrt == :world のとき、 \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{T} + \mathbf{v}\)

wrt == W (coordinates class) のとき、 \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{T} + \mathbf{R}_{W}\mathbf{v}\)

(send T :locate :math:`mathbf{v}` (&optional (wrt :local)))

wrt == :local のとき、 \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{T} + \mathbf{R}_{T} \mathbf{v}\)

wrt == :world のとき、 \(\mathbf{p}_{T} \leftarrow \mathbf{v}\)

wrt == W (coordinates class) のとき、 \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{W} + \mathbf{R}_{W}\mathbf{v}\)

(send T :transform A (&optional (wrt :local)))

wrt == :local のとき、 \(T \leftarrow TA\)

wrt == :world のとき、 \(T \leftarrow AT\)

wrt == W (coordinates class) のとき、\(T \leftarrow\) \(\left( WAW \right)^{-1} T\)