はじめに
直交していないベクトルからの直交するベクトルの構成
この記事の目的は次の記事の曲率中心（center of curvature）や向心力（centripetal force）の理解と，それらの実用的な計算方法を示すための準備である．式はすべて幾何学的な意味を持ち，内容はできるだけ平易に記述し，図を通してイメージできるようにつとめたが，難しい場合は読み飛ばしても構わない．
また，この記事と合わせて
も見ておくとよいだろう．
図１：物体の運動を速度と曲率中心によって，接線方向と半径方向に分解．
さて，運動する物体の座標系を，曲率中心を原点とする極座標系に設定することで，物体の速度方向（ここでは接線方向と呼ぶ）と，それに直交する物体からみた曲率中心の方向（ここでは半径方向と呼ぶ）に分解できる．
次の記事で述べるが，力学の立場からは，物体の接線方向には物体の速度を決定する力が作用し，半径方向には向心力が作用する．曲率中心を計算することの利点はここにあり，ポイントは，質点の３次元空間運動は，２次元の接線方向と半径方向で構成される平面に落とし込こむことである．つまり剛体の３次元回転が，質点運動では２次元に縮退（次元削減）していることを意味する．
なお，各瞬間でその中心と平面は移動する．しかし，このことは問題とならず，むしろ力学問題を考える上で座標系の並進運動を考慮せず，つねに回転運動を考えるだけで良いことも利点としてある．そして，その移動がまた運動を考える上で意味を持つ．
そこで，運動する物体の曲率中心を計算するうえで，いかに接線方向と半径方向から構成される，互いに直交する座標系を構成するかということがポイントなので，少し回り道になるが，「モーションキャプチャの直交座標系の構成の仕方」について復習し，その後のベクトル三重積（triple product of vectors）の説明につなげ，さらに次の記事での曲率中心や向心力の議論への橋渡しの準備とする内容を示したい．
ここでのテーマは，直交していないベクトルからいかに直交するベクトルを構成するかである．
．
注意：式の表示におけるQiitaの不具合について1
モーションキャプチャの直交座標系の構成の仕方
３点のマーカから構成する座標系
ベクトルとベクトルの直交性を担保するため，一般的に利用されるのはベクトルの外積や内積である．ここでは，外積を利用するもっとも簡単な方法を示す．
図２：物体の装着した３つのマーカと座標系
３点のマーカから，互いに直交する座標系を定める場合（図２参照），まず以下のように２つのベクトル，マーカ$x_0$の位置ベクトル$\boldsymbol{x}_0$からみたマーカ$x_a$の位置ベクトル$\boldsymbol{x}_a$から構成したベクトル$\boldsymbol{a}$と，同様に$\boldsymbol{x}_0$からみたマーカ$x_b$の位置ベクトル$\boldsymbol{x}_b$から構成したベクトル$\boldsymbol{b}$
$$
\begin{aligned}
\boldsymbol{a} &= \boldsymbol{x}_a - \boldsymbol{x}_0
\\
\boldsymbol{b} &= \boldsymbol{x}_b - \boldsymbol{x}_0
\end{aligned}
$$
を構成し，$\boldsymbol{a}$の単位ベクトルを
$$
\begin{aligned}
\boldsymbol{e}_a &= \frac{\boldsymbol{a}}{|\boldsymbol{a}|}
\end{aligned}
$$
のように計算する．$\boldsymbol{a}$方向の単位ベクトルは基準とするベクトルとして，最初に定める単位基底ベクトルとなる．
しかし，たとえ正確にマーカを貼付し，$\boldsymbol{a}, \boldsymbol{b}$が直交するように配慮しても，必ず誤差を含むので，$\boldsymbol{a}$と$\boldsymbol{b}$は直交しない．そこで直交する基底ベクトルを構成したいが，その前に，$\boldsymbol{a}, \boldsymbol{b}$からそれらに直交する単位ベクトル$\boldsymbol{e}_c$を計算し，それをもう二番目の基底ベクトルとしたい．そのため
$$
\begin{aligned}
\boldsymbol{c} &= \boldsymbol{a} \times \boldsymbol{b}
\\
\boldsymbol{e}_c &= \frac{\boldsymbol{c}}{|\boldsymbol{c}|}
\end{aligned}
$$
のように計算する．ここで$\times$はベクトルの外積（cross product, outer product）である2．
外積の計算によって，ベクトル$\boldsymbol{c}$は，ベクトル$\boldsymbol{a}, \boldsymbol{b}$で張られる（構成される）平面に垂直な法線方向のベクトルとなる性質をここで利用している．
これで直交する基底ベクトル$\boldsymbol{e}_a, \boldsymbol{e}_c$を計算できたので，残りの三番目の
$$
\begin{aligned}
\boldsymbol{b}' &= \boldsymbol{c} \times \boldsymbol{a}
\\
\boldsymbol{e}_b &= \frac{\boldsymbol{b}'}{|\boldsymbol{b}'|}
\end{aligned}
$$
を計算することになる．
ここでは，２つのベクトルから運動する剛体の直交する座標系を構成しているが，一旦，$\boldsymbol{c}$を$\boldsymbol{a} \times \boldsymbol{b}$から計算していることに注目していただきたい．
Pythonコード
def calc_rigid_body_frame(marker_positions):
"""
Calculate orthonormal coordinate system of rigid body from 3 marker positions over time
Args:
marker_positions: Array of shape (n_frames, 3, 3) containing 3D positions of 3 markers
First dimension: time frames
Second dimension: 3 markers
Third dimension: x,y,z coordinates
Returns:
Array of shape (n_frames, 3, 3) containing orthonormal basis vectors
Each 3x3 matrix represents coordinate system at that time frame
Columns are unit vectors of local coordinate system
"""
n_frames = marker_positions.shape[0]
basis = np.zeros((n_frames, 3, 3))
for t in range(n_frames):
# Calculate vectors between markers
v1 = marker_positions[t,1] - marker_positions[t,0] # Vector from marker 0 to 1
v2 = marker_positions[t,2] - marker_positions[t,0] # Vector from marker 0 to 2
# First basis vector - normalize v1
# X-axis
e1 = v1 / np.linalg.norm(v1)
# Second basis vector - perpendicular to e1 in plane of v1,v2
# Y-axis
v2_proj = v2 - np.dot(v2,e1)*e1 # Project v2 onto plane perpendicular to e1
e2 = v2_proj / np.linalg.norm(v2_proj) #反射影
# Third basis vector - cross product
# Z-axis
e3 = np.cross(e1, e2)
# Store basis vectors as columns
basis[t] = np.vstack((e1, e2, e3))
return basis
使用例
m1 = # 原点となるマーカのnumpy配列
m2 = # X軸を定めるとなるマーカのnumpy配列（m2-m1がX軸）
m3 = # おおよそY軸を定めるマーカのnumpy配列
marker_positions = np.stack([m1, m2, m3], axis=1)
orth_basis = calc_rigid_body_frame(marker_positions)
このコードは，実は次の話を先取りし，Y軸の計算に反射影を用いている．
ベクトル三重積
図３：ベクトルaとbの外積
外積$\boldsymbol{a} \times \boldsymbol{b}$は，２つのベクトル$\boldsymbol{a},\boldsymbol{b}$が張る面積に直交する方向を表すベクトルとなり，その大きさは，平行四辺形の面積となる（図３）．
ここでは，さらに外積で計算する三つのベクトル$\boldsymbol{a},\boldsymbol{b},\boldsymbol{c}$の積$\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{c})$を考える．この計算結果は同様にベクトルになる3．外積では結合則が成立しないため，$\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{c})$と$(\boldsymbol{a} \times \boldsymbol{b}) \times \boldsymbol{c}$は一般には一致しない．これらの性質からこれらの積をベクトル三重積（vector triple product）と呼び，次の関係
$$
\begin{aligned}
\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{c}) &= (\boldsymbol{a}^T \boldsymbol{c})\boldsymbol{b} - (\boldsymbol{a}^T \boldsymbol{b})\boldsymbol{c}
\\
(\boldsymbol{a} \times \boldsymbol{b}) \times \boldsymbol{c} &= (\boldsymbol{a}^T \boldsymbol{c}) \boldsymbol{b} - (\boldsymbol{b}^T \boldsymbol{c}) \boldsymbol{a}
\end{aligned}
$$
が成り立つ．
図４：ベクトル三重積の特別な場合
ここで，$\boldsymbol{c}=\boldsymbol{a}$という特別な場合を考えよう（図４）．つまり，$\boldsymbol{c}$を$\boldsymbol{a}$にしてしまうことで，一旦$\boldsymbol{n} = \boldsymbol{b} \times \boldsymbol{a}$を計算し，さらに$\boldsymbol{n}$に$\boldsymbol{a}$をかけることになる．つまり，$\boldsymbol{n}$は$\boldsymbol{a}$と$\boldsymbol{b}$に直交するベクトルであるが，$\boldsymbol{a}$を再度それにかけている．
式で示すと，
$$
\begin{aligned}
\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{a}) &= (\boldsymbol{a}^T \boldsymbol{a})\boldsymbol{b} - (\boldsymbol{a}^T \boldsymbol{b})\boldsymbol{a}
\\
&= |\boldsymbol{a}|^2 \boldsymbol{b} - (\boldsymbol{a}^T \boldsymbol{b})\boldsymbol{a}
\end{aligned}
$$
となる．ここでは，$\boldsymbol{a}$と$\boldsymbol{b}$に垂直な$\boldsymbol{n}$を一旦計算し，その$\boldsymbol{a}$と$\boldsymbol{n}$の外積を計算することで，$\boldsymbol{a}$に直交するベクトル$\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{a})$を計算している．
この計算の幾何学的意味は「モーションキャプチャの直交座標系の構成の仕方」と似ている．図２では同様に，$\boldsymbol{a}$と$\boldsymbol{b}$は直交していないので，一旦，$\boldsymbol{a}$に直交する二番目の$\boldsymbol{c} = \boldsymbol{a} \times \boldsymbol{b}$を計算し，それに再度，$\boldsymbol{a}$をかけている．
つまり，この計算は$\boldsymbol{a}$と$\boldsymbol{b}$が張る平面に直交する方向を定め，それに対して再度$\boldsymbol{a}$をかけることで，「直交する３番目の方向」を定めている．３番目の方向とは，もともとの$\boldsymbol{b}$に比較的近い方向となり，$\boldsymbol{a}$に直交する方向を定めることになる．
外積の掛け算の順番の違いはあれど，ベクトル三重積の特別なケース$\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{a})$は，「$\boldsymbol{a}$と$\boldsymbol{b}$で張られる平面内で，$\boldsymbol{a}$に直交するベクトル」を計算している（図４）．
射影と反射影
「$\boldsymbol{a}$と$\boldsymbol{b}$が張る平面の中で，$\boldsymbol{a}$に直交するベクトル」を理解するため，もうひとつ寄り道をする．
ベクトルの直交化は，グラムシュミットの直交化法も密接に関係するが，ここではその基礎となる射影と反射影について説明する．
図５：ベクトルbのベクトルaに対する射影と反射影
3次元空間で，任意のベクトル$\boldsymbol{b}$と，$\boldsymbol{a}$の方向に伸びる直線$l$を考え，その方向の単位ベクトルを$\boldsymbol{n}$とする（図５）．
するとベクトル$\boldsymbol{b}$は，直線$l$に平行な成分$b_{\parallel}$と，直線$l$に対して直交する成分$b_{\perp}$に，分解することができるため，ベクトル$\boldsymbol{b}$に関しては
$$
\boldsymbol{b} = b_{\parallel} + b_{\perp}
$$
のように書くことができ，$b_{\parallel}$を$\boldsymbol{b}$の直線$l$への射影（rejection），$b_{\perp}$を$\boldsymbol{b}$の直線$l$からの反射影（rejection）とよぶ．記号からもわかるように，この場合の射影は直線$l$に対して平行な成分で，反射影は直交する成分である．
幾何学的に$\boldsymbol{b}_{\parallel}$は，ベクトル$\boldsymbol{b}$の$\boldsymbol{n}$への射影で，射影の長さは$\boldsymbol{b}^T \boldsymbol{n}$で，その方向の単位ベクトル$\boldsymbol{n}=\boldsymbol{a}/|\boldsymbol{a}|$なので，
$$
b_{\parallel} = \left(\boldsymbol{b}^T \boldsymbol{n}\right) \boldsymbol{n} = (\boldsymbol{n}^T \boldsymbol{b}) \boldsymbol{n}
$$
となる．
一方，反射影$\boldsymbol{b}_{\perp}$は，
$$
b_{\perp} = \boldsymbol{b} - b_{\parallel}
$$
より計算できる．すると，反射影$\boldsymbol{b}_{\perp}$は，
$$
\begin{aligned}
b_{\perp} &= \boldsymbol{b} - b_{\parallel}
\\
&=
\boldsymbol{b} - (\frac{\boldsymbol{a}^T \boldsymbol{b}}{|\boldsymbol{a}|} ) \frac{\boldsymbol{a}}{|\boldsymbol{a}|}
\\
&= \boldsymbol{b} - (\boldsymbol{n}^T \boldsymbol{b}) \boldsymbol{n}
\end{aligned}
$$
と書くことができる．
繰り返しになるが，ベクトル$\boldsymbol{b}$を，ベクトル$\boldsymbol{a}$を基準に，互いに直交する$b_{\parallel}$と$b_{\perp}$に分解しただけである．また，直交する２つのベクトルに分解する際，まずはベクトル$\boldsymbol{a}$方向の$\boldsymbol{b}$の射影を計算し，その次に反射影$b_{\perp}$を計算する際には，実用上，$\boldsymbol{b}$から$b_{\parallel}$の「引き算」を行えばよいだけである．
この引き算による計算は，次の記事で述べる，曲率中心を計算する際の数値安定化に貢献する．
また，前述の３つのマーカから直交座標系を定義する際，X軸→Z軸→Y軸の順番で軸を計算していたが，反射影を利用すると，「Z軸の計算を経由せずに」，X軸→Y軸を計算でき，X, Y軸の単位ベクトルを$\boldsymbol{e}_a, \boldsymbol{e}_b$とすると，
図６：マーカxaとxbを利用した直交する単位ベクトルの計算方法
図６のようにX,Y,Z軸方向の単位ベクトル$\boldsymbol{e}_a, \boldsymbol{e}_b, \boldsymbol{e}_c$を
反射影$\boldsymbol{b}_{\perp}$を利用し，順番に計算できる．実は前に示したコードは，反射影を用いたアルゴリズムを採用している．
ベクトル三重積と反射影
ベクトル三重積の特別な場合で，算出した式
$$
\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{a}) = |\boldsymbol{a}|^2 \boldsymbol{b} - (\boldsymbol{a}^T \boldsymbol{b})\boldsymbol{a}
$$
を少し変形させる．
$\boldsymbol{a}$を単位ベクトル$\boldsymbol{n}$に置き換えると，
$$
\boldsymbol{n} \times (\boldsymbol{b} \times \boldsymbol{n}) = |\boldsymbol{n}|^2 \boldsymbol{b} - (\boldsymbol{n}^T \boldsymbol{b})\boldsymbol{n}
$$
となるが，ここで，単位ベクトルの大きさは１なので，$|\boldsymbol{n}|^2=1$となり，
$$
\boldsymbol{n} \times (\boldsymbol{b} \times \boldsymbol{n}) = \boldsymbol{b} - (\boldsymbol{n}^T \boldsymbol{b})\boldsymbol{n}
$$
を得る．
そこで，先程の反射影の式
$$
b_{\perp} = \boldsymbol{b} - (\boldsymbol{n}^T \boldsymbol{b}) \boldsymbol{n}
$$
の右辺と見比べると，一致するので
$$
\boldsymbol{n} \times (\boldsymbol{b} \times \boldsymbol{n}) = b_{\perp}
$$
を得る．また，単位ベクトルではなく，一般の$\boldsymbol{a}$についても，
$$
\frac{\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{a})}{|\boldsymbol{a}|^2} = b_{\perp}
$$
が成り立つ．
つまり，ベクトル$\boldsymbol{a}$と$\boldsymbol{b}$を与えて，$\boldsymbol{a}$と$\boldsymbol{b}$の張る平面内で，$\boldsymbol{a}$に直交するベクトルを計算するときは，$b_{\perp}$を計算すればよいわけだが，それは$\boldsymbol{a}$と$\boldsymbol{b}$で構成される，ベクトル三重積で計算できることを示している．
質点の力学
図７：ボールの軌跡と加速度の分解
例えば，図７の質量$m$のボールの軌道$\boldsymbol{x}(t)$を計算し，その速度ベクトルを$\boldsymbol{v}=\dot{\boldsymbol{x}}$，加速度を$\boldsymbol{a}=\ddot{\boldsymbol{x}}$とする．このとき軌道$\boldsymbol{x}(t)$が曲線運動をしているとき，ボールに作用する力を$\boldsymbol{f}$，重力加速度を$\boldsymbol{g}$とすると，ボールの運動方程式は
$$
m \ddot{\boldsymbol{x}} = m(a_{\parallel} + a_{\perp}) = \boldsymbol{f} + m \boldsymbol{g}
$$
となる．ここで，加速度$\boldsymbol{a}$を速度$\boldsymbol{v}$方向に作用する力$a_{\parallel}$と，速度$\boldsymbol{v}$と加速度$\boldsymbol{a}$とが張る平面内にあり$\boldsymbol{v}$に直交する方向の加速度成分$a_{\perp}$に分解する．
つまりボールの加速度のうち，接線方向$\boldsymbol{T}=\boldsymbol{v}/|\boldsymbol{v}|$の成分を
$$
\boldsymbol{a}_{\parallel} = (\boldsymbol{a}^T \boldsymbol{T})\boldsymbol{T} = \left( \boldsymbol{a}^T \frac{\boldsymbol{v}}{|\boldsymbol{v}|} \right) \frac{\boldsymbol{v}}{|\boldsymbol{v}|}
$$
とすると，これはボールを加速する力として機能し，これを積分すれば
$$
\boldsymbol{v}(t) = \int_{t_0}^t \boldsymbol{a}_{\parallel} dt + \boldsymbol{v}(t_0)
$$
となる．すなわち，ボールに作用する加速度の接線方向成分$\boldsymbol{a}_{\parallel}$は，ボール速度に寄与する成分である．
また，このことは接線方向の加速度は，速度ベクトルのノルム（大きさ）の微分
$$
\boldsymbol{a}_{\parallel} = \frac{d}{dt}|\boldsymbol{v}| \boldsymbol{T}
$$
でも計算できることを示している．
次に，ボールに作用する加速度$\boldsymbol{a}$から，この接線方向の加速度成分$\boldsymbol{a}_{\parallel}$を取り除いたものが，
$$
a_{\perp} = \boldsymbol{a} - \boldsymbol{a}_{\parallel}
$$
となるが，これまでの議論から，それは，
$$
a_{\perp} = \frac{\boldsymbol{v} \times (\boldsymbol{a} \times \boldsymbol{v})}{|\boldsymbol{v}|^2} =
\frac{(\boldsymbol{v} \times \boldsymbol{a}) \times \boldsymbol{v}}{|\boldsymbol{v}|^2}
$$
となる．なお，この$a_{\perp}$は，加速度$\boldsymbol{a}$の接線方向$\boldsymbol{a}_{\parallel}$に対して垂直な方向を向いていることまでは明らかだ．
また，質量$m$に$\boldsymbol{a}_{\perp}$をかけた
$$
m \boldsymbol{a}_{\perp} = \frac{m}{|\boldsymbol{v}|^2} \boldsymbol{v} \times (\boldsymbol{a} \times \boldsymbol{v})
= \frac{m}{|\boldsymbol{v}|^2} (\boldsymbol{v} \times \boldsymbol{a}) \times \boldsymbol{v}
$$
は向心力であるが，詳しくは次の記事で述べることとする．
おわりに
ここまでは，少し回り道になったがモーションキャプチャの３つのマーカから，直交する座標系の各軸に張り付く単位ベクトル（正規直交基底ベクトル）をどの用に算出するかという導入から，ベクトル三重積と，射影・反射影への分解方法について述べた．これらは，数式ではなく，幾何学イメージで理解することができる．
次回は，これらと曲率中心や向心力との関係について述べる．これらの幾何学関係がわかれば，割り算による数値的に不安定な曲率中心の計算を，「引き算に置き換える」ことで安定化できることに気がつくはずだ．
一方で，この記事の内容から，原理的には，曲率半径や法線方向の単位ベクトル（主法線ベクトル）のみならず，向心力も速度と加速度だけで計算できることが理解できるだろう．