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