def rot_matrix_from_vecs(vec_a, vec_b):
out = Rotation()
vec_a.Normalize()
vec_b.Normalize()
vcross = vec_a * vec_b
vdot = dot(vec_a, vec_b)
# Check if the vectors are in the same direction
if 1.0 - vdot < 0.1:
return out
# Or in the opposite direction
elif 1.0 + vdot < 0.1:
nx = Vector(1, 0, 0)
temp_dot = dot(vec_a, nx)
if -0.9 < abs(temp_dot) < 0.9:
axis = vec_a * nx
out = out.Rot(axis, 3.14)
else:
ny = Vector(0, 1, 0)
axis = vec_a * ny
out = out.Rot(axis, 3.14)
else:
skew_v = skew_mat(vcross)
out = add_mat(
add_mat(Rotation(), skew_v),
scalar_mul(skew_v * skew_v, (1 - vdot) / (vcross.Norm()**2)))
return out
def rot(self, vec, angle):
self.rot_frame = Frame(Rotation.Rot(Vector(*vec),
angle)) * self.rot_frame
