def from_axis_angle(axis, angle, is_normalized=False):
# adapted from
# https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/quaternions.py
axis = Vector(axis) if not isinstance(axis, Vector) else axis
if not is_normalized:
axis.normalize()
x, y, z = axis
c = math.cos(angle)
s = math.sin(angle)
C = 1 - c
xs = x * s
ys = y * s
zs = z * s
xC = x * C
yC = y * C
zC = z * C
xyC = x * yC
yzC = y * zC
zxC = z * xC
return Orientation(np.array([
[x * xC + c, xyC - zs, zxC + ys],
[xyC + zs, y * yC + c, yzC - xs],
[zxC - ys, yzC + xs, z * zC + c],
]))
def from_rotation_vector(v):
if isinstance(v, (np.ndarray, list, tuple)):
v = Vector(*v)
if not isinstance(v, Vector):
raise ValueError("Method take a Vector as argument")
if v.length == 0:
return Orientation(np.identity(3, dtype=np.float32))
u = v.normalized()
idx = (u.data != 0).argmax()
return Orientation.from_axis_angle(u, v[idx] / u[idx], True)
def from_yz(y_vec, z_vec):
"""
Generate a new Orientation from two vectors using y as reference
"""
if not isinstance(y_vec, Vector):
y_vec = Vector(y_vec)
if not isinstance(z_vec, Vector):
z_vec = Vector(z_vec)
y_vec.normalize()
z_vec.normalize()
orient = Orientation()
orient.data[:, 1] = y_vec.data
orient.data[:, 0] = y_vec.cross(z_vec).normalized().data
orient.data[:, 2] = Vector(np.cross(orient.data[:, 0], y_vec.data)).normalized().data
return orient
def from_xy(x_vec, y_vec):
"""
Generate a new Orientation from two vectors using x as reference
"""
if not isinstance(x_vec, Vector):
x_vec = Vector(x_vec)
if not isinstance(y_vec, Vector):
y_vec = Vector(y_vec)
x_vec.normalize()
y_vec.normalize()
orient = Orientation()
orient.data[:, 0] = x_vec.data
orient.data[:, 2] = x_vec.cross(y_vec).normalized().data
orient.data[:, 1] = Vector(np.cross(orient.data[:, 2], x_vec.data)).normalized().data
return orient
def from_xz(x_vec, z_vec):
"""
Generate a new Orientation from two vectors using x as reference
"""
if not isinstance(x_vec, Vector):
x_vec = Vector(x_vec)
if not isinstance(z_vec, Vector):
z_vec = Vector(z_vec)
x_vec.normalize()
z_vec.normalize()
orient = Orientation()
orient._data[:, 0] = x_vec.data
orient._data[:, 1] = z_vec.cross(x_vec).data
orient._data[:, 2] = np.cross(x_vec.data, orient._data[:, 1])
return orient
def __mul__(self, other):
if isinstance(other, Vector):
return Vector(self._data @ other.data)
if isinstance(other, Orientation):
return Orientation(self._data @ other.data)
if isinstance(other, np.ndarray):
if other.shape[0] == 3:
return self.data @ other
if other.shape[1] == 3:
return (self.data @ other.T).T
raise ValueError("Array shape must be 3,x or x,3")
return NotImplemented
def __mul__(self, other):
if isinstance(other, Vector):
data = self.orient.data @ other.data + self.pos.data
return Vector(data)
if isinstance(other, Transform):
return Transform(matrix=self._data @ other.data)
if isinstance(other, np.ndarray):
# This make it easy to support several format of point clouds but might be mathematically wrong
if other.shape[0] == 3:
return (self.orient.data @ other) + self.pos.data.reshape(3, 1)
if other.shape[1] == 3:
return (self.orient.data @ other.T).T + self.pos.data
raise ValueError("Array shape must be 3, x or x, 3")
return NotImplemented
def to_axis_angle(self, unit_thresh=1e-4):
# adapted from
# https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/quaternions.py
M = np.asarray(self._data, dtype=np.float32)
# direction: unit eigenvector of R33 corresponding to eigenvalue of 1
L, W = np.linalg.eig(M.T)
i = np.where(np.abs(L - 1.0) < unit_thresh)[0]
if i.size == 0:
raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
direction = np.real(W[:, i[-1]]).squeeze()
# rotation angle depending on direction
cosa = (np.trace(M) - 1.0) / 2.0
if abs(direction[2]) > 1e-8:
sina = (M[1, 0] + (cosa - 1.0) * direction[0] * direction[1]) / direction[2]
elif abs(direction[1]) > 1e-8:
sina = (M[0, 2] + (cosa - 1.0) * direction[0] * direction[2]) / direction[1]
else:
sina = (M[2, 1] + (cosa - 1.0) * direction[1] * direction[2]) / direction[0]
angle = math.atan2(sina, cosa)
return Vector(direction), angle
def __init__(self,
orientation=None,
vector=None,
matrix=None,
dtype=np.float32):
if matrix is not None:
self._data = matrix
else:
self._data = np.identity(4, dtype=dtype)
if orientation is None:
pass
elif isinstance(orientation, np.ndarray):
if orientation.shape == (3, 3):
self._data[:3, :3] = orientation
else:
raise ValueError()
elif isinstance(orientation, Orientation):
self._data[:3, :3] = orientation.data
else:
raise ValueError(
"orientation argument should be a numpy array, Orientation or None"
)
self._orient = Orientation(self._data[:3, :3], dtype=dtype)
if vector is None:
pass
elif isinstance(vector, np.ndarray):
self._data[:3, 3] = vector
elif isinstance(vector, Vector):
self._data[:3, 3] = vector.data
elif isinstance(vector, (list, tuple)):
self._data[:3, 3] = vector
else:
raise ValueError(
"orientation argument should be a numpy array, Orientation or None"
)
self._pos = Vector(self._data[:3, 3], dtype=dtype)
def pos(self, vector):
if not isinstance(vector, Vector):
raise ValueError()
self._data[:3, 3] = vector.data
self._pos = Vector(
self._data[:3, 3]) # make sure vector data is a view on our data
def from_ros(q, v):
orient = Orientation.from_quaternion(*q)
return Transform(orient, Vector(v))
def from_pose_vector(x, y, z, r1, r2, r3):
o = Orientation.from_rotation_vector(Vector(r1, r2, r3))
return Transform(o, [x, y, z])
def vec_z(self):
return Vector(self._data[:, 2])
def vec_y(self):
return Vector(self._data[:, 1])
def vec_x(self):
return Vector(self._data[:, 0])
def from_xz(x_vec, z_vec, ref='x'):
"""
Generate a new Orientation from two vectors using x as reference
"""
if not isinstance(x_vec, Vector):
x_vec = Vector(x_vec)
if not isinstance(z_vec, Vector):
z_vec = Vector(z_vec)
x_vec.normalize()
z_vec.normalize()
orient = Orientation()
orient.data[:, 1] = z_vec.cross(x_vec).normalized().data
if ref == 'x':
orient.data[:, 0] = x_vec.data
orient.data[:, 2] = Vector(np.cross(x_vec.data, orient.data[:, 1])).normalized().data
elif ref == 'z':
orient.data[:, 2] = z_vec.data
orient.data[:, 0] = Vector(np.cross(orient.data[:, 1], z_vec.data)).normalized().data
else:
raise ValueError('Value of ref can only be x or z')
return orient