Các phương pháp xoay vector trong không gian 2D và 3D
This post hasn't been updated for 3 years
Lời mở đầu
Chào các bạn, mọi người có khỏe không, hôm nay tôi muốn bàn một chút về việc xoay vector trong không gian 2 chiều và 3 chiều. Do không phải dân chuyên Toán nên các thuật ngữ như Euler, Quaternions, ... tôi cũng chả có cách nào giải thích cho các bạn, nhưng đưa ra vài ví dụ trong lập trình chắc vẫn được nên hôm nay tôi viết bài này cũng muốn tham khảo ý kiến một chút.
Xoay vector trong không gian 2D
Như các bạn đã biết, không gian 2D chỉ chứa trục x và trục y nên ta nhìn qua là một mặt phẳng không có chiều sâu. Để rõ ràng, tôi sẽ minh họa một vector tạo thành từ 2 điểm bất kỳ bằng thư viện matplotlib.
from matplotlib import pyplot as plt
import math
import numpy as np
p1 = [0, 0]
p2 = [40, 40]
plt.axis([-50, 50, -50, 50])
plt.plot([p1[0], p2[0]], [p1[1], p2[1]], label = "line 1")
Tôi đã có 1 vector trong không gian 2 chiều, giờ tôi sẽ xoay vector theo một góc chỉ định, ở đây tôi lấy góc 90 độ. Dựa trên công thức dưới đây sẽ sinh ra x y của vector mới
Nếu code theo python thì ta có
newX = v[0] * math.cos(angle) - v[1] * math.sin(angle)
newY = v[0] * math.sin(angle) + v[1] * math.cos(angle)
Nhưng như thế vector sẽ bị xoay ngược chiều kim đồng hồ nên tôi phải thay đổi một chút
newX = v[0] * math.cos(angle) + v[1] * math.sin(angle)
newY = -v[0] * math.sin(angle) + v[1] * math.cos(angle)
Minh họa dựa theo matplotlib
p1 = [0, 0]
p2 = [40, 40]
plt.axis([-50, 50, -50, 50])
plt.plot([p1[0], p2[0]], [p1[1], p2[1]], label = "line 1")
v = [p2[0] - p1[0], p2[1] - p1[1]]
angle = np.deg2rad(90)
newX = v[0] * math.cos(angle) + v[1] * math.sin(angle)
newY = -v[0] * math.sin(angle) + v[1] * math.cos(angle)
p3 = [p1[0] + newX, p1[1] + newY]
plt.axis([-50, 50, -50, 50])
plt.plot([p1[0], p3[0]], [p1[1], p3[1]], label = "line 2")
plt.xlabel('x')
plt.ylabel('y')
plt.show()
Xoay vector bởi 180 độ
Xoay vector trong không gian 3D
Không gian bao gồm chiều rộng x, chiều cao y và chiều sâu z. Một điểm nằm trong không gian này có giá trị (x, y, z). Cũng như việc xoay vector trong không gian 2 chiều, tôi cần tìm ma trận rotation R rồi nhân với tọa độ của vector sẽ sinh ra một vector mới.
Xoay vector theo trục
Cho một góc 90 độ, chuyển từ độ (degree) thành radian, chọn trục z làm trục quay, tìm rotation vector của trục quay bằng thư viện scipy. Tôi giải thích sơ qua module Rotation của thư viện này chút.
- Đầu tiên tìm unit vector của trục (axis)
- Sau đó cross product của unit vector với ma trận I (Identify matrix).
- Cuối cùng dùng hàm exponential với param là góc và A sẽ trả về ma trận xoay (rotation matrix)
a = axis/norm(axis) #1
A = I x a
exp(theta,A)
Sau khi có ma trận xoay chỉ cần nhân với vector cần xoay sẽ sinh ra một vector mới quay theo trục một góc 90 độ.
from scipy.spatial.transform import Rotation as R
vec = [1,1,1]
rotation_degrees = 90
rotation_radians = np.radians(rotation_degrees)
rotation_axis = np.array([0, 0, 1])
rotation_vector = rotation_radians * rotation_axis
rotation = R.from_rotvec(rotation_vector)
rotated_vec = rotation.apply(vec)
print(rotated_vec)
Euler angle
Định nghĩa: miêu tả hướng của một vật thể trong không gian 3 chiều
Ví dụ: Giả sử tôi có một video quay một máy bay, khi xử lý video cung cấp các frame và tọa độ x y z. Ở frame đầu tiên, máy bay được coi là không dịch chuyển. Từ frame thứ 10 máy bay dịch chuyển và thay đổi vị trí một góc 30 độ. Quá trình biến đổi hướng có thể chia thành 3 giai đoạn:
- Yaw Rotation: chuyển động quay quanh trục z của vector một góc ψ, tạo nên tọa độ mới mà trục z không thay đổi nhưng trục x, y quay theo góc ψ.
- Pitch Rotation
- Roll Rotation
- Ma trận xoay theo góc và trục chỉ định (ux, uy, uz). Với u là unit vector
from math import pi ,sin, cos
def R(theta, u):
return [[cos(theta) + u[0]**2 * (1-cos(theta)),
u[0] * u[1] * (1-cos(theta)) - u[2] * sin(theta),
u[0] * u[2] * (1 - cos(theta)) + u[1] * sin(theta)],
[u[0] * u[1] * (1-cos(theta)) + u[2] * sin(theta),
cos(theta) + u[1]**2 * (1-cos(theta)),
u[1] * u[2] * (1 - cos(theta)) - u[0] * sin(theta)],
[u[0] * u[2] * (1-cos(theta)) - u[1] * sin(theta),
u[1] * u[2] * (1-cos(theta)) + u[0] * sin(theta),
cos(theta) + u[2]**2 * (1-cos(theta))]]
def Rotate(pointToRotate, point1, point2, theta):
u= []
squaredSum = 0
for i,f in zip(point1, point2):
u.append(f-i)
squaredSum += (f-i) **2
u = [i/squaredSum for i in u]
r = R(theta, u)
rotated = []
for i in range(3):
rotated.append(round(sum([r[j][i] * pointToRotate[j] for j in range(3)])))
return rotated
point = [1,0,0]
p1 = [0,0,0]
p2 = [0,1,0]
print Rotate(point, p1, p2, pi)
Công thức Euler–Rodrigues
Định nghĩa: một phương pháp tính vị trí của rotated point.
Một phép xoay quanh trục tọa độ gốc được biểu diễn bởi 4 param a, b, c, d
Khi áp dụng công thức này, một điểm có tọa độ x sẽ xoay về một vị trí mới có tọa độ x'
Code python: hàm rotation_matrix() trả về một ma trận xoay ngược chiều kim đồng hồ theo trục và góc.
import numpy as np
import math
def rotation_matrix(axis, theta):
"""
Return the rotation matrix associated with counterclockwise rotation about
the given axis by theta radians.
"""
axis = np.asarray(axis)
axis = axis / math.sqrt(np.dot(axis, axis))
a = math.cos(theta / 2.0)
b, c, d = -axis * math.sin(theta / 2.0) # bỏ dấu trừ nếu muốn thuận chiều kim đồng hồ
aa, bb, cc, dd = a * a, b * b, c * c, d * d
bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
[2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
[2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])
v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2
print(np.dot(rotation_matrix(axis, theta), v))
# [ 2.74911638 4.77180932 1.91629719]
Quaternions
Cũng như Euler-Rodrigues, quaternion là một vector có 4 phần tử dùng để tính vị trí của rotated point. Ta cũng có thể biến đổi Euler parameters a, b, c, d trên thành các hệ số của quaternion
Một quaternion có thể tính như sau
def angle_axis_quat(theta, axis):
"""
Given an angle and an axis, it returns a quaternion.
"""
axis = np.array(axis) / np.linalg.norm(axis)
return np.append([np.cos(theta/2)],np.sin(theta/2) * axis)
Xoay vector bởi quaternions bằng công thức
Một vector xoay là tích của một quaternion với nghịch đảo của nó. Quaternion này lại là tích của unit vector với liên hợp của nó.
q1:
q2:
Tích của 2 quaternions
def mult_quat(q1, q2):
"""
Quaternion multiplication.
"""
q3 = np.copy(q1)
q3[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]
q3[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]
q3[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]
q3[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]
return q3
Sau khi có được rotated quaternions, áp dụng vào vector cần xoay sẽ sinh ra một vector mới.
def rotate_quat(quat, vect):
"""
Rotate a vector with the rotation defined by a quaternion.
"""
# Transfrom vect into an quaternion
vect = np.append([0],vect)
# Normalize it
norm_vect = np.linalg.norm(vect)
vect = vect/norm_vect
# Computes the conjugate of quat
quat_ = np.append(quat[0],-quat[1:])
# The result is given by: quat * vect * quat_
res = mult_quat(quat, mult_quat(vect,quat_)) * norm_vect
return res[1:]
v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2
print(rotate_quat(angle_axis_quat(theta, axis), v))
# [2.74911638 4.77180932 1.91629719]
Lời kết
Như các bạn thấy thì cả bài này hầu như là một bài tổng hợp các cách xoay vector trong không gian hai chiều và ba chiều, do đó còn nhiều thiếu sót và không đầy đủ, mong các cao nhân có thể giúp đỡ thêm thắt lý thuyết công thức cho tác giả và người đọc hiểu thêm.
References
https://gist.github.com/LyleScott/e36e08bfb23b1f87af68c9051f985302
https://stackoverflow.com/questions/6802577/rotation-of-3d-vector
All Rights Reserved