Rotation of arbitrary plane around a given axis results in inconsistent results

296 Views Asked by At

I am trying to rotate and translate arbitrary planes around some arbitrary axis. For testing purposes I have written a simple python program that rotates a random plane around the X axis in enter image description here degrees.

Unfortunately when checking the angle between the planes I get inconsistent results. This is the code:

def angle_between_planes(plane1, plane2):
    plane1 = (plane1 / np.linalg.norm(plane1))[:3]
    plane2 = (plane2/ np.linalg.norm(plane2))[:3]
    cos_a = np.dot(plane1.T, plane2) / (np.linalg.norm(plane1) * np.linalg.norm(plane2))
    print(np.arccos(cos_a)[0, 0])


def test():
    axis = np.array([1, 0, 0])
    theta = np.pi / 2
    translation = np.array([0, 0, 0])
    T = get_transformation(translation, axis * theta)
    for i in range(1, 10):
        source = np.append(np.random.randint(1, 20, size=3), 0).reshape(4, 1)
        target = np.dot(T, source)
        angle_between_planes(source, target)

It prints:

1.21297144225
1.1614420953
1.48042948278
1.10098697889
0.992418096794
1.16954303911
1.04180591409
1.08015300394
1.51949177153

When debugging this code I see that the transformation matrix is correct, as it shows that it is enter image description here

I'm not sure what's wrong and would love any assistance here.

* The code that generates the transformation matrix is:

def get_transformation(translation_vec, rotation_vec):
    r_4 = np.array([0, 0, 0, 1]).reshape(1, 4)
    rotation_vec= rotation_vec.reshape(3, 1)
    theta = np.linalg.norm(rotation_vec)
    axis = rotation_vec/ theta
    R = get_rotation_mat_from_axis_and_angle(axis, theta)
    T = translation_vec.reshape(3, 1)
    R_T = np.append(R, T, axis = 1)
    return np.append(R_T, r_4, axis=0)



def get_rotation_mat_from_axis_and_angle(axis, theta):
    axis = axis / np.linalg.norm(axis)
    a, b, c = axis
    omct = 1 - np.cos(theta)
    ct = np.cos(theta)
    st = np.sin(theta)
    rotation_matrix =  np.array([a * a * omct + ct,  a * b * omct - c * st,  a * c * omct + b * st,
                                 a * b * omct + c * st, b * b * omct + ct,  b * c * omct - a * st,
                                 a * c * omct - b * st, b * c * omct + a * st, c * c * omct + ct]).reshape(3, 3)
    rotation_matrix[abs(rotation_matrix) < 1e-8] = 0
    return rotation_matrix
1

There are 1 best solutions below

4
On

The source you generate is not a vector. In order to be one, it should have its fourth coordinate equal to zero.

You could generate valid ones with:

source = np.append(np.random.randint(1, 20, size=3), 0).reshape(4, 1)

Note that your code can't be tested as you pasted it in your question: for example, vec = vec.reshape(3, 1) in get_transformation uses vec that hasn't been defined anywhere before...