CODEWITHSUNDEEP
pythonrotation
AlixL
AlixL

Reputation: 372

Find rotation matrix to align two vectors

I try to find the rotation matrix to align two vectors.

I have a vector A = [ax, ay, az] and I would like to align it on the vector B = [1, 0, 0] (x-axis unit vector).

I found the following explanation and tried to implement it: https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d/897677#897677

def align_vectors(a, b):
     v = np.cross(a, b)
     s = np.linalg.norm(v)
     c = np.dot(a, b)

     v1, v2, v3 = v
     h = 1 / (1 + c)

     Vmat = np.array([[0, -v3, v2],
                      [v3, 0, -v1],
                      [-v2, v1, 0]])

     R = np.eye(3, dtype=np.float64) + Vmat + (Vmat.dot(Vmat) * h)
     return R

When I apply it to find the rotation of a point, this is what I have :

x_axis = np.array([1, 0, 0], dtype=np.float64)
direction = np.array([-0.02, 1.004, -0.02], dtype=np.float64)
Ralign = align_vectors(x_axis, direction)
point = 1000 * np.array([-0.02, 1.004, -0.02], dtype=np.float64) # Point in the direction of the unit vector
result = Ralign.dot(point)

The resulting point is not aligned with the unit vector.

Upvotes: 0

Views: 5402

Answers (1)

Aaron
Aaron

Reputation: 1368

If you only want to rotate ONE vector a to align with b, not the entire coordinate contain that vector, use simple vector projection and the length of a:

a_norm = np.linalg.norm(a)
b_norm = np.linalg.norm(b)
result = b * a_norm / b_norm

The following fixes the issue in the question that input are not unit vector by vector normalization.

def align_vectors(a, b):
    b = b / np.linalg.norm(b) # normalize a
    a = a / np.linalg.norm(a) # normalize b
    v = np.cross(a, b)
    # s = np.linalg.norm(v)
    c = np.dot(a, b)
    if np.isclose(c, -1.0):
        return -np.eye(3, dtype=np.float64)

    v1, v2, v3 = v
    h = 1 / (1 + c)

    Vmat = np.array([[0, -v3, v2],
                  [v3, 0, -v1],
                  [-v2, v1, 0]])

    R = np.eye(3, dtype=np.float64) + Vmat + (Vmat.dot(Vmat) * h)
    return R

The algorithm also fixes another edge case: If the function gets called with two parallel vectors with opposite directions h will evalute to inf and R will contain only nans.

align_vectors(np.array([1,0,0]), np.array([-1,0,0]))

This gets caught by comparing c to -1 and returning the negative identity matrix. testing:


def angle(a, b):
    """Angle between vectors"""
    a = a / np.linalg.norm(a)
    b = b / np.linalg.norm(b)
    return np.arccos(a.dot(b))

point = np.array([-0.02, 1.004, -0.02])
direction = np.array([1., 0., 0.])
rotation = align_vectors(point, direction)

# Rotate point in align with direction. The result vector is aligned with direction
result = rotation.dot(point)
print(result)
print('Angle:', angle(direction, point)) # 0.0
print('Length:', np.isclose(np.linalg.norm(point), np.linalg.norm(result))) # True


# Rotate direction by the matrix, result does not align with direction but the angle between the original vector (direction) and the result2 are the same.
result2 = rotation.dot(direction)
print(result2)
print('Same Angle:', np.isclose(angle(point,result), angle(direction,result2))) # True
print('Length:', np.isclose(np.linalg.norm(direction), np.linalg.norm(result2))) # True

Upvotes: 2

Related Questions