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 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)