""" Some helpful functions for creating geometric transformations. """ import math import numpy as np def get_unit_vector(data, axis=None, out=None): """Return ndarray normalized by length, i.e. Euclidean norm, along axis.""" if out is None: data = np.array(data, dtype=np.float64, copy=True) if data.ndim == 1: data /= math.sqrt(np.dot(data, data)) return data else: if out is not data: out[:] = np.array(data, copy=False) data = out length = np.atleast_1d(np.sum(data * data, axis)) np.sqrt(length, length) if axis is not None: length = np.expand_dims(length, axis) data /= length if out is None: return data def get_translation_matrix(direction): """Return matrix to translate by direction vector.""" M = np.identity(4) M[:3, 3] = direction[:3] return M def get_rotation_matrix(angle, direction, point=None): """Return matrix to rotate about axis defined by point and direction.""" sina = math.sin(angle) cosa = math.cos(angle) direction = get_unit_vector(direction[:3]) # rotation matrix around unit vector R = np.diag([cosa, cosa, cosa]) R += np.outer(direction, direction) * (1.0 - cosa) direction *= sina R += np.array( [[0.0, -direction[2], direction[1]], [direction[2], 0.0, -direction[0]], [-direction[1], direction[0], 0.0]] ) M = np.identity(4) M[:3, :3] = R if point is not None: # rotation not around origin point = np.array(point[:3], dtype=np.float64, copy=False) M[:3, 3] = point - np.dot(R, point) return M