====== 座標系に関する演算プログラム ======

あらかじめベクトル，回転行列，座標系などのクラスとそれらの演算を用意することで
座標系を用いたロボットの動作生成や作業環境の記述などのプログラムが容易になる．

大規模なライブラリ用にこの手のツールが種々用意されているが
手元で手軽に使える自分用のツールは小回りが利いて実際にいろいろと便利である．

以下の4つのクラスと演算を実装する．
  * ベクトル：和，差，絶対値，内積、スカラー積、外積、行列との積
  * 直交行列：積、ベクトルとの積，列や行のベクトルとしての切り出し，オイラー角，回転軸と回転角の抽出．四元数への変換
  * 四元数：回転変換としての四元数の生成．回転変換としての四元数同士の積．直交行列への変換
  * 座標変換：座標系連鎖（積）、位置ベクトルとの積

これらの実装はpythonで行っているが，他の言語でも同様のクラスを作るのは容易であり，便利である．

ここでは簡単な問題を解きながら使い方を例示する．

===== 補足 =====

  * [[articles:geo_basic|基本的な使い方]]
  * [[articles:matrix_vs_quaternion_1|直交行列と四元数の比較１]]
  * [[articles:matrix_vs_quaternion_2|直交行列と四元数の比較２]]
  * [[articles:numpy_vs_geo|numpyとgeo.pyの速度比較]]

===== ベクトル =====

==== 例題1 ====

a=VECTOR(0.87, 0.5, 0)とb=VECTOR(0.71, 0.71, 0)との間の角度を求める。

[[articles:geo_manual#例題1の解答|例題1の解答]]

==== 例題2 ====

x=VECTOR(1,0,0)を基準に
a=VECTOR(-0.707,-0.707,0)のz軸まわりの回転
角度を求める。

[[articles:geo_manual#例題2の解答|例題2の解答]]


===== 回転行列 =====

==== 例題3 ====
a=VECTOR(1,1,0)をx軸まわりに90度回転させたベ
クトルを求めよ。

ヒント：x軸周りに90度回転の回転行列を作り，a
に左からかける．

[[articles:geo_manual#例題3の解答|例題3の解答]]

==== 例題4 ====

a=VECTOR(1,1,0)を回転させ、b=VECTOR(1,0,1)に
一致させるための、x軸以外の回転軸を1つとその
まわりの回転角度をもとめよ。

ヒント：
  * まずは回転軸を求める．どこにあるか．直観でも良い．
  * 回転軸周りの回転角度は，例題2を参考にする．

[[articles:geo_manual#例題4の解答|例題4の解答]]

==== 例題5 ====
例題4で求めた回転軸、回転角度で
a=VECTOR(1,1,0)を回転させ、 b=VECTOR(1,0,1)に
一致することを確認せよ。

[[articles:geo_manual#例題5の解答|例題5の解答]]

===== 四元数 =====

==== 例題6 ====

a=VECTOR(1,1,0)をx軸まわりに90度回転させたベ
クトルを**四元数を用いて**求めよ。

[[articles:geo_manual#例題6の解答|例題6の解答]]

===== 座標系（FRAME） =====

==== 例題7 ====

座標系$\Sigma_0$を基準座標とする．

  - $\Sigma_0$をz方向に1.0並進移動，z軸まわりにpi/2回転した座標系$\Sigma_1$の座標変換(FRAME．行列ではないがここでは$^0T_1$と表記)を生成せよ．
  - $\Sigma_1$からみてx方向に1.0並進移動，x軸まわりにpi/2回転した座標系$\Sigma_2$の$\Sigma_0$からの座標変換$^0T_2$を求めよ．
  - $\Sigma_2$で，p=VECTOIR(1,1,0)と表現される位置ベクトルの$\Sigma_1$，$\Sigma_0$からの表現を求めよ．

[[articles:geo_manual#例題7の解答|例題7の解答]]

===== 解答例 =====


==== 例題1の解答 ====
[[articles:geo_manual#例題1|例題1]] a=VECTOR(0.87, 0.5, 0)とb=VECTOR(0.71, 0.71, 0)との間の角度を求める。


<code python>
>>> a=VECTOR(0.87,0.5,0)
>>> b=VECTOR(0.71,0.71,0)
>>> co=a.dot(b)/abs(a)/abs(b)
>>> th=acos(co)
>>> th
0.26377986630106198
>>> th*180/pi
15.113473059575982
</code>

==== 例題2の解答 ====

[{{ articles:geo_ex02.png?200|例題2の補足}}]
[[articles:geo_manual#例題2|例題2]]
x=VECTOR(1,0,0)を基準に
a=VECTOR(-0.707,-0.707,0)のz軸まわりの回転
角度を求める。

=== 失敗例 ===

<code python>
>>> x=VECTOR(1,0,0)
>>> a=VECTOR(-0.707,-0.707,0)
>>> co=a.dot(x)/abs(a)
>>> th=acos(co)
>>> th
2.3561944901923448
>>> th*180/pi
135.0
</code>

=== 正解例 ===

<code python>
>>> a=VECTOR(-0.707,-0.707,0)
>>> co=a.dot(VECTOR(1,0,0))
>>> si=a.dot(VECTOR(0,1,0))
>>> th=atan2(si,co)
>>> th*180/pi
-135.0
</code>



==== 例題3の解答 ====
[[articles:geo_manual#例題3|例題3]]
a=VECTOR(1,1,0)をx軸まわりに90度回転させたベ
クトルを求めよ。

<code python>
>>> a=VECTOR(1,1,0)
>>> R=MATRIX(a=pi/2)
>>> b=R*a
>>> b
v:[1.0, 6.123233995736766e-17, 1.0]
</code>

==== 例題4の解答 ====
[[articles:geo_manual#例題4|例題4]]
a=VECTOR(1,1,0)を回転させ，b=VECTOR(1,0,1)に
一致させるための，x軸以外の回転軸を1つとその
回転角度をもとめよ．

=== 回転軸をa,bの外積とした場合 ===

<code python>
>>> c=a*b
>>> c
v:[1.0, -1.0, -1.0]
>>> kco=a.dot(b)
>>> ksi=abs(c)
>>> th=atan2(ksi,kco)
>>> th
1.0471975511965976
>>> th*180/pi
59.999999999999993
</code>

=== 回転軸をa,bの間にした場合 ===

回転軸はa+b, 回転角度はpi,

=== 汎用的な答え ===
回転軸はa+bとa*bが張る平面上のどこでも良い．
したがって，
それぞれの比率をx,yとして関数を書くと，

<code python>
>>> def foo(a,b,x,y) :
...    r1=a+b
...    r2=a*b
...    r=(x*r1+y*r2).normalize()
...    da=(r*a).normalize()
...    db=(r*b).normalize8)
...    dc=r*da
...    kco=da.dot(db)
...    ksi=dc.dot(db)
...    return(r,atan2(ksi,kco))
... 
>>> foo(a,b,1,0)
(v:[0.8164965809277261, 0.4082482904638631, 0.4082482904638631], 3.141592653589793)
>>> foo(a,b,0,1)
(v:[0.5773502691896258, -0.5773502691896258, -0.5773502691896258], 1.0471975511965979)
>>> foo(a,b,0,-1)
(v:[-0.5773502691896258, 0.5773502691896258, 0.5773502691896258], -1.0471975511965979)
>>> foo(a,b,1,1)
(v:[1.0, 0.0, 0.0], 1.5707963267948966)
>>> 

</code>

==== 例題5の解答 ====
[[articles:geo_manual#例題5|例題5]]
例題4で求めた回転軸、回転角度で
a=VECTOR(1,1,0)を回転させ、 b=VECTOR(1,0,1)に
一致することを確認せよ。

<code python>
>>> tmp=foo(a,b,0,1)
>>> R=MATRIX(axis=tmp[0],angle=tmp[1])
>>> R*a
v:[1.0, -2.220446049250313e-16, 1.0000000000000002]
>>> 
</code>

==== 例題6の解答 ====
[[articles:geo_manual#例題6|例題6]]
a=VECTOR(1,1,0)をx軸まわりに90度回転させたベ
クトルを**四元数を用いて**求めよ。

<code python>
>>> a=VECTOR(1,1,0)
>>> qa=QUATERNION(w_v=[0,a])
>>> qr=(QUATERNION(a=pi/2))
>>> qb=qr*qa*qr.conjugate()
>>> qb
q:(0.0, v:[1.0, 2.220446049250313e-16, 1.0])
>>> b=qb.v
>>> b
v:[1.0, 2.220446049250313e-16, 1.0]
>>> 
</code>

==== 例題7の解答 ====
[[articles:geo_manual#例題7|例題7]]
座標系$\Sigma_0$を基準座標とする．[{{ articles:geo_ex07.png?300|例題7のイメージ図}}]

  - $\Sigma_0$をz方向に1.0並進移動，z軸まわりにpi/2回転した座標系$\Sigma_1$の座標変換(FRAME．行列ではないがここでは$^0T_1$と表記)を生成せよ．
  - $\Sigma_1$からみてx方向に1.0並進移動，x軸まわりにpi/2回転した座標系$\Sigma_2$の$\Sigma_0$からの座標変換$^0T_2$を求めよ．
  - $\Sigma_2$で，p=VECTOIR(1,1,0)と表現される位置ベクトルの$\Sigma_1$，$\Sigma_0$からの表現を求めよ．

<code python>
>>> t1=FRAME(xyzabc=[0,0,1,0,0,pi/2])
>>> t2_1=FRAME(xyzabc=[1,0,0,pi/2,0,0])
>>> t2=t1*t2_1
>>> t2
f:[m:[[6.123233995736766e-17, -6.123233995736766e-17,1.0], [1.0, 3.749399456654644e-33, -6.123233995736766e-17], [0.0, 1.0, 6.123233995736766e-17]],v:[6.123233995736766e-17, 1.0, 1.0])
>>> p=VECTOR(1,1,0)
>>> t2_1*p
v:[2.0, 6.123233995736766e-17, 1.0]
>>> t1*_
v:[6.123233995736766e-17, 2.0, 2.0]
>>> t2*p
v:[6.123233995736766e-17, 2.0, 2.0]
</code>