def plane_horizontalling_rotation(p: np.ndarray) -> Optional[np.ndarray]:
"""Compute a rotation that brings p to z=0
>>> p = [1.0, 2.0, 3.0]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0, 0, np.linalg.norm(p)])
True
>>> p = [0, 0, 1.0]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0, 0, np.linalg.norm(p)])
True
>>> p = [0, 0, -1.0]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0, 0, np.linalg.norm(p)])
True
>>> p = [1e-14, 1e-14, -1.0]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0, 0, np.linalg.norm(p)])
True
"""
v0 = p[:3]
v1 = np.array([0.0, 0.0, 1.0])
angle = tf.angle_between_vectors(v0, v1)
axis = tf.vector_product(v0, v1)
if np.linalg.norm(axis) > 0:
return tf.rotation_matrix(angle, axis)[:3, :3]
elif angle < 1.0:
return np.eye(3)
elif angle > 3.0:
return np.diag([1, -1, -1])
return None
def plane_horizontalling_rotation(p):
'''Compute a rotation that brings p to z=0
>>> p = [1.0, 2.0, 3.0]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0, 0, np.linalg.norm(p)])
True
>>> p = [0, 0, 1.0]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0, 0, np.linalg.norm(p)])
True
>>> p = [0, 0, -1.0]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0, 0, np.linalg.norm(p)])
True
>>> p = [1e-14, 1e-14, -1.0]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0, 0, np.linalg.norm(p)])
True
'''
v0 = p[:3]
v1 = [0.0, 0.0, 1.0]
angle = tf.angle_between_vectors(v0, v1)
axis = tf.vector_product(v0, v1)
if np.linalg.norm(axis) > 0:
return tf.rotation_matrix(angle, axis)[:3, :3]
elif angle < 1.0:
return np.eye(3)
elif angle > 3.0:
return np.diag([1, -1, -1])
def vector_angle_many(u, v):
"""Angles between to lists of vectors.
>>> u = [[0.99500417, -0.33333333, -0.09983342], [0, -1, 0], [0, 1, 0]]
>>> v = [[0.99500417, -0.33333333, -0.09983342], [0, +1, 0], [0, 0, 1]]
>>> angles = vector_angle_many(u, v)
>>> np.allclose(angles, [0., 3.1416, 1.5708])
True
"""
ua = np.array(u, dtype=np.float64, copy=False).reshape(-1, 3)
va = np.array(v, dtype=np.float64, copy=False).reshape(-1, 3)
return tf.angle_between_vectors(ua, va, axis=1)
def vector_angle_many(u, v):
"""Angles between to lists of vectors.
>>> u = [[0.99500417, -0.33333333, -0.09983342], [0, -1, 0], [0, 1, 0]]
>>> v = [[0.99500417, -0.33333333, -0.09983342], [0, +1, 0], [0, 0, 1]]
>>> angles = vector_angle_many(u, v)
>>> np.allclose(angles, [0., 3.1416, 1.5708])
True
"""
ua = np.array(u, dtype=np.float64, copy=False).reshape(-1, 3)
va = np.array(v, dtype=np.float64, copy=False).reshape(-1, 3)
return tf.angle_between_vectors(ua, va, axis=1)
def plane_horizontalling_rotation(p):
'''Compute a rotation that brings p to z=0
>>> p = [1.,2.,3.]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0,0,np.linalg.norm(p)])
True
'''
v0 = p[:3]
v1 = [0,0,1.0]
return tf.rotation_matrix(tf.angle_between_vectors(v0, v1),
tf.vector_product(v0, v1)
)[:3,:3]
def plane_horizontalling_rotation(p):
'''Compute a rotation that brings p to z=0
>>> p = [1.,2.,3.]
>>> R = plane_horizontalling_rotation(p)
>>> np.allclose(R.dot(p), [0,0,np.linalg.norm(p)])
True
'''
v0 = p[:3]
v1 = [0,0,1.0]
angle = tf.angle_between_vectors(v0, v1)
if angle > 0:
return tf.rotation_matrix(angle,
tf.vector_product(v0, v1)
)[:3,:3]
else:
return np.eye(3)
