Нормализовать все векторы на оси Z

Question

Я написал код, который вычисляет угол между двумя векторами. Однако для этого нужно начать с двух векторов, повернуть каждый в соответствии с некоторыми углами Эйлера, рассчитанными в отдельной программе, а затем вычислить угол между векторами.

До сих пор я работал с сценарий использования, который означает, что оба начальных вектора равны (0,0,1), что делает жизнь супер легкой. Я мог бы просто взять один набор эйлеровых углов от другого и затем вычислить угол между 0,0,1 и вектором, который был повернут на разницу. Это означало, что я мог построить хорошие графики распределения и векторные диаграммы, потому что все было нормализовано до 0,0,1. (У меня есть тысячи этих векторов для записи).

Нет. Я пытаюсь написать функцию, которая позволила бы использовать случай, когда два начальных вектора не равны 0,0,1. Я подумал, что самый простой способ сделать это - вычислить направление вектора относительно 0,0,1, а после вычисления положения вектора просто повернуть на предварительно рассчитанные смещения. (это может быть глупый способ сделать это, если, пожалуйста, скажите мне).

Мой текущий код работает для случая, когда вектор равен 0,1,0, но затем ломается, если я начинаю вводить случайный номера.

import numpy as np
import math

def RotationMatrix(axis, rotang):
    """
    This uses Euler-Rodrigues formula.
    """
    #Input taken in degrees, here we change it to radians
    theta = rotang * 0.0174532925
    axis = np.asarray(axis)
    #Ensure axis is a unit vector
    axis = axis/math.sqrt(np.dot(axis, axis))
    #calclating a, b, c and d according to euler-rodrigues forumla requirments
    a = math.cos(theta/2)
    b, c, d = axis*math.sin(theta/2)
    a2, b2, c2, d2 = 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 the rotation matrix
    return np.array([[a2+b2-c2-d2, 2*(bc-ad), 2*(bd+ac)],
                     [2*(bc+ad), a2+c2-b2-d2, 2*(cd-ab)],
                     [2*(bd-ac), 2*(cd+ab), a2+d2-b2-c2]])



def ApplyRotationMatrix(vector, rotationmatrix):
    """
    This function take the output from the RotationMatrix function and
    uses that to apply the rotation to an input vector
    """
    a1 = (vector[0] * rotationmatrix[0, 0]) + (vector[1] * rotationmatrix[0, 1]) + (vector[2] * rotationmatrix[0, 2])
    b1 = (vector[0] * rotationmatrix[1, 0]) + (vector[1] * rotationmatrix[1, 1]) + (vector[2] * rotationmatrix[1, 2])
    c1 = (vector[0] * rotationmatrix[2, 0]) + (vector[1] * rotationmatrix[2, 1]) + (vector[2] * rotationmatrix[2, 2])

    return np.array((a1, b1, c1)


'''
Functions for Calculating the angles of 3D vectors relative to one another
'''

def CalculateAngleBetweenVector(vector, vector2):
    """
    Does what it says on the tin, outputs an angle in degrees between two input vectors.
    """
    dp = np.dot(vector, vector2)

    maga = math.sqrt((vector[0] ** 2) + (vector[1] ** 2) + (vector[2] ** 2))
    magb = math.sqrt((vector2[0] ** 2) + (vector2[1] ** 2) + (vector2[2] ** 2))
    magc = maga * magb

    dpmag = dp / magc

    #These if statements deal with rounding errors of floating point operations
    if dpmag > 1:
        error = dpmag - 1
        print('error = {}, do not worry if this number is very small'.format(error))
        dpmag = 1
    elif dpmag < -1:
        error = 1 + dpmag
        print('error = {}, do not worry if this number is very small'.format(error))
        dpmag = -1


    angleindeg = ((math.acos(dpmag)) * 180) / math.pi

    return angleindeg


def CalculateAngleAroundZ(Vector):
    X,Y,Z = Vector[0], Vector[1], Vector[2]
    AngleAroundZ = math.atan2(Y, X)
    AngleAroundZdeg = (AngleAroundZ*180)/math.pi
    return AngleAroundZdeg

def CalculateAngleAroundX(Vector):
    X,Y,Z = Vector[0], Vector[1], Vector[2]
    AngleAroundZ = math.atan2(Y, Z)
    AngleAroundZdeg = (AngleAroundZ*180)/math.pi
    return AngleAroundZdeg

def CalculateAngleAroundY(Vector):
    X,Y,Z = Vector[0], Vector[1], Vector[2]
    AngleAroundZ = math.atan2(X, Z)
    AngleAroundZdeg = (AngleAroundZ*180)/math.pi
    return AngleAroundZdeg


V1 = (0,0,1)
V2 = (3,5,4)

Xoffset = (CalculateAngleAroundX(V2))
Yoffset = (CalculateAngleAroundY(V2))
Zoffset = (CalculateAngleAroundZ(V2))

XRM = RotationMatrix((1,0,0), (Xoffset * 1))
YRM = RotationMatrix((0,1,0), (Yoffset * 1))
ZRM = RotationMatrix((0,0,1), (Zoffset * 1))

V2 = V2 / np.linalg.norm(V2)
V2X = ApplyRotationMatrix(V2, XRM)
V2XY = ApplyRotationMatrix(V2X, YRM)
V2XYZ = ApplyRotationMatrix(V2XY, ZRM)
print(V2XYZ)
print(CalculateAngleBetweenVector(V1, V2XYZ))

Буду очень признателен за любые советы по решению этой проблемы.

Answer 1

Я идиот. Мне просто нужно было сделать перекрестное произведение и точечное произведение и повернуть его на точечное произведение * -1 вокруг перекрестного произведения.

Answer 2

Я не уверен, что полностью понимаю, что вам нужно, но если вы хотите вычислить угол между двумя векторами в пространстве, вы можете использовать формулу:

где ab - скалярное произведение, а theta - угол между векторами.

, таким образом, ваша функция CalculateAngleBetweenVector становится:

def CalculateAngleBetweenVector(vector, vector2):
    return math.acos(np.dot(vector,vector2)/(np.linalg.norm(vector)* np.linalg.norm(vector2))) * 180 /math.pi

Вы также можете упростить вашу функцию ApplyRotationMatrix :

def ApplyRotationMatrix(vector, rotationmatrix):
"""
This function take the output from the RotationMatrix function and
uses that to apply the rotation to an input vector
"""
return rotationmatrix @ vector

символ @ - это матричное произведение

Надеюсь, это поможет вам. Не стесняйтесь уточнять свой запрос, если это не поможет.