Pythonによる回転行列と法線ベクトルの実装

0. Pythonによる回転行列と法線ベクトルの実装

色々と図形をぐるぐるまわしたいと思ったので実装した。
図形を回転させたら軌跡は円になるので、回転行列で計算可能なのである。

1. 回転行列

python 3.6を用いた。回転行列の数式は以下を参考にした。
http://1st.geocities.jp/shift486909/program/spin.html

2次元

{\begin{equation}
\begin{vmatrix}
X' \\
Y' \\
\end{vmatrix}
=
\begin{vmatrix}
\cos\theta &-\sin\theta \\
\sin\theta &\cos\theta \\
\end{vmatrix}
\times
\begin{vmatrix}
X \\
Y \\
\end{vmatrix}
\end{equation}
}

3次元 (Z軸周り= x -> y)

{\begin{equation}
\begin{vmatrix}
X' \\
Y' \\
Z' \\
\end{vmatrix}
=
\begin{vmatrix}
\cos\theta z &-\sin\theta z &0 \\
\sin\theta z &\cos\theta z &0 \\
0 &0 &1 \\
\end{vmatrix}
\times
\begin{vmatrix}
X \\
Y \\
Z \\
\end{vmatrix}
\end{equation}
}

今回は2次元で実装

# -*- coding: utf-8 -*-
import numpy as np


if __name__ == '__main__':

    # matrix
    a = np.matrix([2.5, 2.5]).A1
    b = np.matrix([5, 5]).A1
    print("a=" + str(a))
    print("b=" + str(b))
    # AB = AC = r
    pythagoras = b - a
    r = np.sqrt( (pythagoras[0] * pythagoras[0]) + (pythagoras[1] * pythagoras[1] ))
    print("r=" + str(r))

    # tan theta (Inclination)
    tant = (b[1] - a[1]) / (b[0] - a[0])
    # theta
    tan = np.arctan(tant)
    # AD = r*cos α
    # BD = r*sin α
    AD = r * np.cos(tan)
    BD = r * np.sin(tan)
    print("AD=" + str(AD))
    print("BD=" + str(BD))

    theta = np.deg2rad(90)

    rotateMat = np.matrix([
        [np.cos(theta), -np.sin(theta)],
        [np.sin(theta), np.cos(theta)],
    ])

    print(rotateMat)

    xy = np.matrix([[AD], [BD]])

    cxy = (rotateMat * xy)
    cxy = cxy.A1 + a
    print("cxy=" + str(cxy) )

    print("------------------")
    # cx = AD*cos θ-BD*sin θ+ax
    # cy = AD*sin θ+BD*cos θ+ay
    AD2 = b[0] - a[0]
    BD2 = b[1] - a[1]
    cx = ((AD2 * np.cos(theta)) - (BD2 * np.sin(theta))) + a[0]
    print(cx)
    cy = ((AD2 * np.sin(theta)) + (BD2 * np.cos(theta))) + a[1]
    print(cy)

1.1. 回転行列 - 関数化

関数化

def calcRot(_posxy, _orgxy, _rad):
    ADBD = _posxy - _orgxy
    ADBD = np.matrix([[ADBD[0]], [ADBD[1]]])
    rotateMat = np.matrix([
        [np.cos(_rad), -np.sin(_rad)],
        [np.sin(_rad), np.cos(_rad)],
    ])
    cxy = (rotateMat * ADBD).A1 + _orgxy

    return  cxy

こんな感じに使えばおｋ

 _posxy = np.matrix([_x, _y]).A1
 _orgxy = np.matrix([_x, _y]).A1
 _rad = np.deg2rad(90)

 rotatedPos = calcRot(_posxy, _orgxy, _rad)

1.2. 回転行列 - 複素数

コメントで教えて貰いましたが、回転行列は複素数でも表現できるらしい。

イメージ的にはこんな感じ。
https://atarimae.biz/wp-content/uploads/2015/05/fourway.png

確かに。

# -*- coding: utf-8 -*-
import numpy as np

if __name__ == '__main__':

    A = 2.5 + 2.5j
    B = 5.0 + 5.0j
    print('A=' + str(A))
    print('B=' + str(B))

    theta = np.deg2rad(90)
    rotate = np.cos(theta) + 1j * np.sin(theta) # = np.exp(1j * theta)

    AB = B - A
    AC = rotate * AB
    C  = AC + A
    print('C=' + str(C))

2. 法線ベクトル

法線ベクトルは対象とする直線に直交していればよいので、回転行列を用いて90度回転させるだけでよいのである。
もちろん他にもやり方は沢山ある。私は同じ関数(回転行列)を使いまわせるのでこれを使っている。

def calcNorm_vec(_posxy, _orgxy):
    ADBD = _posxy - _orgxy
    ADBD = np.matrix([[ADBD[0]], [ADBD[1]]])
    theta = np.deg2rad(90)
    rotateMat = np.matrix([
        [np.cos(theta), -np.sin(theta)],
        [np.sin(theta), np.cos(theta)],
    ])
    cxy = (rotateMat * ADBD)
    vv = cxy.A1
    nv = vv / np.linalg.norm(vv)
    return nv

以上。