2D
在右手坐标系 X O Y XOY XOY下,点 P ( x , y ) P(x,y) P(x,y)绕任意点 O ′ ( x o , y o ) O^\prime(x_{o},y_{o}) O′(xo,yo) 旋转 θ \theta θ 角度(逆时针)得到点 P ′ ( x ′ , y ′ ) P^\prime(x^\prime,y^\prime) P′(x′,y′)
令
∣
O
′
P
∣
=
r
|O^\prime P|=r
∣O′P∣=r,
O
′
P
→
\overrightarrow{O^\prime P}
O′P与
x
x
x轴的夹角为
α
\alpha
α
则有
{
x
=
r
c
o
s
α
+
x
o
y
=
r
s
i
n
α
+
y
o
(1)
\left\{ \begin{matrix} x=rcos\alpha + x_{o} \\ y=rsin\alpha + y_{o} \\ \end{matrix} \right. \tag{1}
{x=rcosα+xoy=rsinα+yo(1)
{ x ′ = r c o s ( α + θ ) + x o y ′ = r s i n ( α + θ ) + y o (2) \left\{ \begin{matrix} x^\prime=rcos(\alpha+\theta) + x_{o} \\ y^\prime=rsin(\alpha+\theta) + y_{o} \\ \end{matrix} \right. \tag{2} {x′=rcos(α+θ)+xoy′=rsin(α+θ)+yo(2)
将(1)代入(2)消去
r
r
r并展开,得
{
x
′
=
c
o
s
θ
x
−
s
i
n
θ
y
+
x
o
(
1
−
c
o
s
θ
)
+
y
o
s
i
n
θ
y
′
=
s
i
n
θ
x
+
c
o
s
θ
y
+
y
o
(
1
−
c
o
s
θ
)
−
x
o
s
i
n
θ
\left\{ \begin{matrix} x^\prime=cos\theta x - sin\theta y + x_{o}(1-cos\theta)+y_{o}sin\theta \\ y^\prime=sin\theta x + cos\theta y + y_{o}(1-cos\theta)-x_{o}sin\theta \\ \end{matrix} \right.
{x′=cosθx−sinθy+xo(1−cosθ)+yosinθy′=sinθx+cosθy+yo(1−cosθ)−xosinθ
齐次旋转矩阵为
[
c
o
s
θ
−
s
i
n
θ
x
o
(
1
−
c
o
s
θ
)
+
y
o
s
i
n
θ
s
i
n
θ
c
o
s
θ
y
o
(
1
−
c
o
s
θ
)
−
x
o
s
i
n
θ
0
0
1
]
\begin{bmatrix} cos\theta&-sin\theta&x_{o}(1-cos\theta)+y_{o}sin\theta \\ sin\theta&cos\theta&y_{o}(1-cos\theta)-x_{o}sin\theta\\ 0&0&1\\ \end{bmatrix}
⎣⎡cosθsinθ0−sinθcosθ0xo(1−cosθ)+yosinθyo(1−cosθ)−xosinθ1⎦⎤
即
[ x ′ y ′ 1 ] = [ c o s θ − s i n θ x o ( 1 − c o s θ ) + y o s i n θ s i n θ c o s θ y o ( 1 − c o s θ ) − x o s i n θ 0 0 1 ] [ x y 1 ] \begin{bmatrix} x^\prime\\ y^\prime\\ 1\\ \end{bmatrix}= \begin{bmatrix} cos\theta&-sin\theta&x_{o}(1-cos\theta)+y_{o}sin\theta \\ sin\theta&cos\theta&y_{o}(1-cos\theta)-x_{o}sin\theta\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ 1\\ \end{bmatrix} ⎣⎡x′y′1⎦⎤=⎣⎡cosθsinθ0−sinθcosθ0xo(1−cosθ)+yosinθyo(1−cosθ)−xosinθ1⎦⎤⎣⎡xy1⎦⎤
3D
点 P ( x , y , z ) P(x,y,z) P(x,y,z)绕任意点 O ′ ( x o , y o , z o ) O^\prime(x_{o},y_{o},z_{o}) O′(xo,yo,zo)旋转矩阵 R R R可分解为 R x ( θ x ) R y ( θ y ) R z ( θ z ) R_{x}(\theta_{x})R_{y}(\theta_{y})R_{z}(\theta_{z}) Rx(θx)Ry(θy)Rz(θz)
由二维可以推广
R
x
(
θ
x
)
R_{x}(\theta_{x})
Rx(θx)在右手坐标系
Y
O
Z
YOZ
YOZ下进行,则线性变化为
[
x
′
y
′
z
′
1
]
=
[
1
0
0
0
0
c
o
s
θ
x
−
s
i
n
θ
x
y
o
(
1
−
c
o
s
θ
x
)
+
z
o
s
i
n
θ
x
0
s
i
n
θ
x
c
o
s
θ
x
z
o
(
1
−
c
o
s
θ
x
)
−
y
o
s
i
n
θ
x
0
0
0
1
]
[
x
y
z
1
]
\begin{bmatrix} x^\prime\\ y^\prime\\ z^\prime\\ 1\\ \end{bmatrix}= \begin{bmatrix} 1&0&0&0\\ 0&cos\theta_{x}&-sin\theta_{x}&y_{o}(1-cos\theta_{x})+z_{o}sin\theta_{x} \\ 0&sin\theta_{x}&cos\theta_{x}&z_{o}(1-cos\theta_{x})-y_{o}sin\theta_{x}\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}
⎣⎢⎢⎡x′y′z′1⎦⎥⎥⎤=⎣⎢⎢⎡10000cosθxsinθx00−sinθxcosθx00yo(1−cosθx)+zosinθxzo(1−cosθx)−yosinθx1⎦⎥⎥⎤⎣⎢⎢⎡xyz1⎦⎥⎥⎤
同理可以推广
R
y
(
θ
y
)
R_{y}(\theta_{y})
Ry(θy)在右手坐标系
Z
O
X
ZOX
ZOX下进行,则线性变化为
[
x
′
y
′
z
′
1
]
=
[
c
o
s
θ
y
0
s
i
n
θ
y
x
o
(
1
−
c
o
s
θ
y
)
−
z
o
s
i
n
θ
y
0
1
0
0
−
s
i
n
θ
y
0
c
o
s
θ
y
z
o
(
1
−
c
o
s
θ
y
)
+
x
o
s
i
n
θ
y
0
0
0
1
]
[
x
y
z
1
]
\begin{bmatrix} x^\prime\\ y^\prime\\ z^\prime\\ 1\\ \end{bmatrix}= \begin{bmatrix} cos\theta_{y}&0&sin\theta_{y}&x_{o}(1-cos\theta_{y})-z_{o}sin\theta_{y}\\ 0&1&0&0 \\ -sin\theta_{y}&0&cos\theta_{y}&z_{o}(1-cos\theta_{y})+x_{o}sin\theta_{y}\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}
⎣⎢⎢⎡x′y′z′1⎦⎥⎥⎤=⎣⎢⎢⎡cosθy0−sinθy00100sinθy0cosθy0xo(1−cosθy)−zosinθy0zo(1−cosθy)+xosinθy1⎦⎥⎥⎤⎣⎢⎢⎡xyz1⎦⎥⎥⎤
同理可以推广
R
z
(
θ
z
)
R_{z}(\theta_{z})
Rz(θz)在右手坐标系
X
O
Y
XOY
XOY下进行,则线性变化为
[
x
′
y
′
z
′
1
]
=
[
c
o
s
θ
z
−
s
i
n
θ
z
0
x
o
(
1
−
c
o
s
θ
z
)
+
y
o
s
i
n
θ
z
s
i
n
θ
z
c
o
s
θ
z
0
y
o
(
1
−
c
o
s
θ
z
)
−
x
o
s
i
n
θ
z
0
0
1
0
0
0
0
1
]
[
x
y
z
1
]
\begin{bmatrix} x^\prime\\ y^\prime\\ z^\prime\\ 1\\ \end{bmatrix}= \begin{bmatrix} cos\theta_{z}&-sin\theta_{z}&0&x_{o}(1-cos\theta_{z})+y_{o}sin\theta_{z}\\ sin\theta_{z}&cos\theta_{z}&0&y_{o}(1-cos\theta_{z})-x_{o}sin\theta_{z}\\ 0&0&1&0\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1\\ \end{bmatrix}
⎣⎢⎢⎡x′y′z′1⎦⎥⎥⎤=⎣⎢⎢⎡cosθzsinθz00−sinθzcosθz000010xo(1−cosθz)+yosinθzyo(1−cosθz)−xosinθz01⎦⎥⎥⎤⎣⎢⎢⎡xyz1⎦⎥⎥⎤
import numpy as np
from math import cos, sin
def Rx(x, y, z, theta):
return np.array([
[1, 0, 0, 0],
[0, cos(theta), -sin(theta), y*(1-cos(theta))+z*sin(theta)],
[0, sin(theta), cos(theta), z*(1-cos(theta))-y*sin(theta)],
[0, 0, 0, 1]
])
def Ry(x, y, z, theta):
return np.array([
[cos(theta), 0, sin(theta), x*(1-cos(theta))-z*sin(theta)],
[0, 1, 0, 0],
[-sin(theta), 0, cos(theta), z*(1-cos(theta))+x*sin(theta)],
[0, 0, 0, 1]
])
def Rz(x, y, z, theta):
return np.array([
[cos(theta), -sin(theta), 0, x*(1-cos(theta))+y*sin(theta)],
[sin(theta), cos(theta), 0, y*(1-cos(theta))-x*sin(theta)],
[0, 0, 1, 0, ],
[0, 0, 0, 1]
])
def rotate_matrix(points, theta):
rx = Rx(points[0],points[1],points[2], theta[0])
ry = Ry(points[0],points[1],points[2], theta[1])
rz = Rz(points[0],points[1],points[2], theta[2])
R = rx@ry@rz
return R