Normalise all vectors on the z axis

Question

I have written code that calculates the angle between two vectors. However the way in which is does this is to start with two vectors, rotate each according to some euler angles calculated in a separate program, then calculate the angle between the vectors.

Up until now I have been working with a use case that means both starting vectors are (0,0,1) that makes life super easy. I could just take one set of euler angles away from the other and then calculate the angle between 0,0,1 and the vector that had been rotated by the difference. It meant I could plot nice distribution plots and vector diagrams because everything was normalised to 0,0,1. (I have 1000s of these vectors for the record).

No I am trying to write in a function that would allow for a use case where the two starting vectors are not on 0,0,1. I figured the easiest way to do this would be to calculate direction of the vector relative to 0,0,1 and after calculating the position of the vector just rotate by the precalculated offsets. (this might be a stupid way to do it, if it is please tell me).

MY current code works for a case where a vector is 0,1,0 but then breaks down if i start entering random numbers.

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))

Any advice to fix this problem will be much appreciated.

Answer 1

I'm not sure to fully understand what you need but if it is to compute the angle between two vectors in space you can use the formula:

where ab is the scalar product and theta is the angle between vectors.

thus your function CalculateAngleBetweenVector becomes:

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

You can also simplify your ApplyRotationMatrix function:

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

the @ symbol is the matrix product

Hope this will help you. Feel free to precise your request if this is not helpfull.

Answer 2

Im an idiot I just needed to do the cross product and the dot product and rotate by the dot product *-1 around the cross product.