Поворот отрезка в пространстве
На вход приходит какой-то случайны отрезок в пространстве в виде координат двух его концов p1(x1,y1,z1) и p2(x2,y2,z2).
Задача заключается в следующем - необходимо выполнить такие преобразования его координат, чтобы он был строго вертикален, при чем сохранив свою длину. Демонстрация на картинке (зеленый - исходный, синий - повернутый).
была идея взять любую точку на исходном отрезке (обозначим A1), и достроить вторую точку (A2) которая выше точки А1 на длину исходного отрезка. Но так не пойдет, потому что преобразования которые мы применяем к отрезку чтобы его повернуть, будут применены и к другим 3D объектам.
также была идея вычислить угол между отрезком и всеми осями (x,y,z), и последовательно домножить все координаты отрезка на матрицу поворота по всем осям. Но новый отрезок после выполнения этой операции не удовлетворил ожиданиям (смотрел не строго вверх).
Здесь последовательно домножаю координаты на вот эти 3 матрицы
угол альфа,бета,гамма заранее известен 45 градусов, можете проверить что угол взят верно, на картинке (зеленый отрезок) для примера взят отрезок с координатами p1(0,0,0) p2(1,1,1)
попытка реализации алгоритм для получение матрицы поворота:
import numpy as np
# нормализовать вектор (привести к единичной длине)
def normalize_vector(vector):
norm_v = np.linalg.norm(vector)
if norm_v != 0:
return vector / norm_v
else:
return vector
# Исходные данные задачи (2 конца отрезка)
x = np.array([0,0,0])
y = np.array([1,1,1])
#нормализуем вектора
x = normalize_vector(x)
y = normalize_vector(y)
# необходимые вектора для решения задачи
s = np.array([y[0] - x[0],y[1] - x[1],y[2] - x[2]])
t = np.array([0,1,0])
a = np.cross(s,t)
#нормализуем вектора
s = normalize_vector(s)
t = normalize_vector(t)
a = normalize_vector(a)
# матрицы
S = np.array([s, a, np.cross(s,a)])
T = np.array([t, a, np.cross(t,a)])
#транспонированная S
S_t = np.transpose(S)
# получаем решение задачи
solution = np.dot(S_t, T)
# тестируем, пробуем получить повернутый отрезок
x = np.array([[0,0,0]])
y = np.array([[1,1,1]])
x = np.dot(x, solution)
y = np.dot(y, solution)
x = np.round(x, decimals=2)
y = np.round(y, decimals=2)
print(x, y)
Ответы (2 шт):
Применяем матрицу переноса на (-A1.x, -A1.y, -A1.z)
Теперь A1 в начале координат
Применяем матрицу поворота вокруг оси OZ на угол f=atan2(A1.y-A2.y,A1.x-A2.x)
Теперь вектор в плоскости OXZ
Применяем матрицу поворота вокруг оси OY на угол t=acos((A1.z-A2.z)/Len(A))
Теперь вектор коллинеарен OZ
Если нужно, перенос обратно (A1.x, A1.y, A1.z)
(знаки углов проверяйте, мог ошибиться)
Все вектора далее приводятся к единичной длине. Это важно.
Введём вектор s = (x2 - x1, y2 - y1, z2 - z1).
Вектор t = (0, 1, 0). Вектор a = s × t - векторное произведение (это будет ось поворота).
Из векторов соберем две матрицы: S = (s, a, s × a) и T = (t, a, t × a). "Соберем" означает что первый вектор становится первым столбцом матрицы, второй - вторым, третий - третьим.
Нужная вам матрица поворота есть S-1·T.
P.S. Алгебра выписана в предположении что вектора — вектора-строки, то есть умножение вектора на матрицу вычисляется как v·M. Если вектора — вектора-столбцы, то "сборка" матрицы меняется со столбцов на строки и последнее произведением имеет вид: T·S-1.
P.P.S. Если длина вектора a до нормализации близка к нулю, вектор s уже почти вертикальный. Можно не нормализовать, сразу вернуть единичную матрицу.
P.P.P.S. Общий метод решения подобных задач: для исходного и для конечного положений придумать два ортонормальных базиса. Из базисов составить две матрицы. Первую обратить и умножить на вторую - получится матрица перехода между базисами, что вам и нужно.
P.P.P.P.S. Задача может решена множеством способов, это решение - кратчайший поворот.
P.P.P.P.P.S. Демонстрация:
import numpy as np
def normalized(v):
return v / np.linalg.norm(v)
def get_rotation(x, y):
s = normalized(y - x)
t = np.array([0, 1, 0])
a = np.cross(s, t)
an = np.linalg.norm(a)
if an == 0:
return np.identity(3)
a /= an
sm = np.array([s, a, np.cross(s, a)])
tm = np.array([t, a, np.cross(t, a)])
return np.dot(np.transpose(sm), tm)
def test(x, y):
print()
print('x', x)
print('y', y)
r = get_rotation(x, y)
# чистый поворот, но он смещает x
# xm = np.dot(x, r)
# ym = np.dot(y, r)
# поворот вокруг x, x остаётся на месте
xm = x
ym = x + np.dot(y - x, r)
print('xm', xm)
print('ym', ym)
print('ym - xm', ym - xm)
print('norm(y - x)', np.linalg.norm(y - x))
test(np.array([0, 0, 0]), np.array([1, 1, 1]))
for _ in range(5):
test(np.random.random(3), np.random.random(3))
$ python motion.py x [0 0 0] y [1 1 1] xm [0 0 0] ym [8.32667268e-17 1.73205081e+00 1.11022302e-16] ym - xm [8.32667268e-17 1.73205081e+00 1.11022302e-16] norm(y - x) 1.7320508075688772 x [0.09054937 0.25113354 0.84059327] y [0.66784923 0.45813704 0.97364723] xm [0.09054937 0.25113354 0.84059327] ym [0.09054937 0.87869146 0.84059327] ym - xm [-1.38777878e-17 6.27557916e-01 0.00000000e+00] norm(y - x) 0.627557915509163 x [0.45851044 0.61577351 0.9463866 ] y [0.79713568 0.81397556 0.91020798] xm [0.45851044 0.61577351 0.9463866 ] ym [0.45851044 1.00980396 0.9463866 ] ym - xm [0. 0.39403046 0. ] norm(y - x) 0.3940304551397485 x [0.19882745 0.59956245 0.47065679] y [0.94670724 0.47282411 0.67784346] xm [0.19882745 0.59956245 0.47065679] ym [0.19882745 1.38589132 0.47065679] ym - xm [0. 0.78632888 0. ] norm(y - x) 0.7863288771527435 x [0.70422901 0.08023806 0.26255832] y [0.63219791 0.35879577 0.96748029] xm [0.70422901 0.08023806 0.26255832] ym [0.70422901 0.84161698 0.26255832] ym - xm [0. 0.76137892 0. ] norm(y - x) 0.7613789207408871 x [0.73489304 0.54577026 0.95083131] y [0.82219623 0.42600696 0.34361226] xm [0.73489304 0.54577026 0.95083131] ym [0.73489304 1.17081432 0.95083131] ym - xm [0. 0.62504406 0. ] norm(y - x) 0.6250440595015537