From 58b4b0769bbb6398a6ebb0888e0de1d2b5bb5c13 Mon Sep 17 00:00:00 2001
From: "Olivier J.N. Bertrand" <olivier.bertrand@uni-bielefeld.de>
Date: Mon, 5 Mar 2018 23:09:53 +0100
Subject: [PATCH] Add homogeneous transformation
---
navipy/maths/__init__.py | 1 +
navipy/maths/constants.py | 22 +
navipy/maths/euler.py | 109 +++++
navipy/maths/homogeneous_transformations.py | 424 ++++++++++++++++++
navipy/maths/quaternion.py | 189 ++++++++
navipy/maths/random.py | 49 ++
navipy/maths/test_euler.py | 28 ++
.../maths/test_homogenous_transformations.py | 274 +++++++++++
navipy/maths/test_quaternion.py | 87 ++++
navipy/maths/test_random_cust.py | 19 +
navipy/maths/tools.py | 57 +++
11 files changed, 1259 insertions(+)
create mode 100644 navipy/maths/constants.py
create mode 100644 navipy/maths/euler.py
create mode 100644 navipy/maths/homogeneous_transformations.py
create mode 100644 navipy/maths/quaternion.py
create mode 100644 navipy/maths/random.py
create mode 100644 navipy/maths/test_euler.py
create mode 100644 navipy/maths/test_homogenous_transformations.py
create mode 100644 navipy/maths/test_quaternion.py
create mode 100644 navipy/maths/test_random_cust.py
create mode 100644 navipy/maths/tools.py
diff --git a/navipy/maths/__init__.py b/navipy/maths/__init__.py
index e69de29..d3173e6 100644
--- a/navipy/maths/__init__.py
+++ b/navipy/maths/__init__.py
@@ -0,0 +1 @@
+#package
diff --git a/navipy/maths/constants.py b/navipy/maths/constants.py
new file mode 100644
index 0000000..3a2fd3c
--- /dev/null
+++ b/navipy/maths/constants.py
@@ -0,0 +1,22 @@
+"""
+
+"""
+import numpy as np
+# For testing whether a number is close to zero or not we use epsilon:
+_EPS = np.finfo(float).eps * 4.0
+
+# axis sequences for Euler angles
+_NEXT_AXIS = [1, 2, 0, 1]
+
+# map axes strings to/from tuples of inner axis, parity, repetition, frame
+_AXES2TUPLE = {
+ 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
+ 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
+ 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
+ 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
+ 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
+ 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
+ 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
+ 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
+
+_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
diff --git a/navipy/maths/euler.py b/navipy/maths/euler.py
new file mode 100644
index 0000000..a125a63
--- /dev/null
+++ b/navipy/maths/euler.py
@@ -0,0 +1,109 @@
+"""
+
+"""
+import numpy as np
+import navipy.maths.constants as constants
+import navipy.maths.quaternion as quat
+
+
+def matrix(ai, aj, ak, axes='sxyz'):
+ """Return homogeneous rotation matrix from Euler angles and axis sequence.
+
+ ai, aj, ak : Euler's roll, pitch and yaw angles
+ axes : One of 24 axis sequences as string or encoded tuple
+ """
+ try:
+ firstaxis, parity, repetition, frame = constants._AXES2TUPLE[axes]
+ except (AttributeError, KeyError):
+ constants._TUPLE2AXES[axes] # validation
+ firstaxis, parity, repetition, frame = axes
+
+ i = firstaxis
+ j = constants._NEXT_AXIS[i + parity]
+ k = constants._NEXT_AXIS[i - parity + 1]
+
+ if frame:
+ ai, ak = ak, ai
+ if parity:
+ ai, aj, ak = -ai, -aj, -ak
+
+ si, sj, sk = np.sin(ai), np.sin(aj), np.sin(ak)
+ ci, cj, ck = np.cos(ai), np.cos(aj), np.cos(ak)
+ cc, cs = ci * ck, ci * sk
+ sc, ss = si * ck, si * sk
+
+ M = np.identity(4)
+ if repetition:
+ M[i, i] = cj
+ M[i, j] = sj * si
+ M[i, k] = sj * ci
+ M[j, i] = sj * sk
+ M[j, j] = -cj * ss + cc
+ M[j, k] = -cj * cs - sc
+ M[k, i] = -sj * ck
+ M[k, j] = cj * sc + cs
+ M[k, k] = cj * cc - ss
+ else:
+ M[i, i] = cj * ck
+ M[i, j] = sj * sc - cs
+ M[i, k] = sj * cc + ss
+ M[j, i] = cj * sk
+ M[j, j] = sj * ss + cc
+ M[j, k] = sj * cs - sc
+ M[k, i] = -sj
+ M[k, j] = cj * si
+ M[k, k] = cj * ci
+ return M
+
+
+def from_matrix(matrix, axes='sxyz'):
+ """Return Euler angles from rotation matrix for specified axis sequence.
+
+ axes : One of 24 axis sequences as string or encoded tuple
+
+ Note that many Euler angle triplets can describe one matrix.
+ """
+ try:
+ firstaxis, parity, repetition, frame = constants._AXES2TUPLE[axes.lower(
+ )]
+ except (AttributeError, KeyError):
+ constants._TUPLE2AXES[axes] # validation
+ firstaxis, parity, repetition, frame = axes
+
+ i = firstaxis
+ j = constants._NEXT_AXIS[i + parity]
+ k = constants._NEXT_AXIS[i - parity + 1]
+
+ M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
+ if repetition:
+ sy = np.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
+ if sy > constants._EPS:
+ ax = np.arctan2(M[i, j], M[i, k])
+ ay = np.arctan2(sy, M[i, i])
+ az = np.arctan2(M[j, i], -M[k, i])
+ else:
+ ax = np.arctan2(-M[j, k], M[j, j])
+ ay = np.arctan2(sy, M[i, i])
+ az = 0.0
+ else:
+ cy = np.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
+ if cy > constants._EPS:
+ ax = np.arctan2(M[k, j], M[k, k])
+ ay = np.arctan2(-M[k, i], cy)
+ az = np.arctan2(M[j, i], M[i, i])
+ else:
+ ax = np.arctan2(-M[j, k], M[j, j])
+ ay = np.arctan2(-M[k, i], cy)
+ az = 0.0
+
+ if parity:
+ ax, ay, az = -ax, -ay, -az
+ if frame:
+ ax, az = az, ax
+ return ax, ay, az
+
+
+def from_quaternion(quaternion, axes='sxyz'):
+ """Return Euler angles from quaternion for specified axis sequence.
+ """
+ return from_matrix(quat.matrix(quaternion), axes)
diff --git a/navipy/maths/homogeneous_transformations.py b/navipy/maths/homogeneous_transformations.py
new file mode 100644
index 0000000..759ad99
--- /dev/null
+++ b/navipy/maths/homogeneous_transformations.py
@@ -0,0 +1,424 @@
+"""
+"""
+import numpy as np
+import navipy.maths.constants as constants
+import navipy.maths.euler as euler
+from navipy.maths.tools import vector_norm
+from navipy.maths.tools import unit_vector
+
+
+def translation_matrix(direction):
+ """
+ Return matrix to translate by direction vector
+ """
+ # get 4x4 identity matrix
+ tmatrix = np.identity(4)
+ # set the first three rows of the last colum to be the direction vector
+ tmatrix[:3, 3] = direction[:3]
+ return tmatrix
+
+
+def translation_from_matrix(matrix):
+ """
+ Return translation vector from translation matrix
+ """
+ # returns the first three rows of the last colum in the rotation matrix
+ return np.array(matrix, copy=False)[:3, 3].copy()
+
+
+def reflection_matrix(point, normal):
+ """
+ Return matrix to mirror at plane defined by point and normal vector
+ """
+ # use only the first three columns of the normal vector
+ normal = unit_vector(normal[:3])
+ # get 4x4 identity matrix
+ M = np.identity(4)
+ # substract two times the outer product of the normal vector from the \
+ # upper left 3x3 matrix of the unit matrix to
+ M[:3, :3] -= 2.0 * np.outer(normal, normal)
+ # set the fourth column of the resutlting matrix to 2* the dot product of \
+ # the point and the normal vector times the noraml vector
+ M[:3, 3] = (2.0 * np.dot(point[:3], normal)) * normal
+ return M
+
+
+def reflection_from_matrix(matrix):
+ """Return mirror plane point and normal vector from reflection matrix.
+ """
+ # pointer to matrix (same as input)
+ M = np.array(matrix, dtype=np.float64, copy=False)
+ # normal: unit eigenvector corresponding to eigenvalue -1
+ w, V = np.linalg.eig(M[:3, :3])
+ # get eigenvectors with eigenvalue close to length 1 -> unit eigenvector; \
+ # only for the upper left 3x3 part
+ i = np.where(abs(np.real(w) + 1.0) < 1e-8)[0]
+ if not len(i):
+ raise ValueError("no unit eigenvector corresponding to eigenvalue -1")
+ # choose only first of unit length eigenvectors
+ normal = np.real(V[:, i[0]]).squeeze()
+ # point: any unit eigenvector corresponding to eigenvalue 1
+ # find eigenvector with unit length for whole matrix
+ w, V = np.linalg.eig(M)
+ i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
+ if not len(i):
+ raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
+ # choose last of the unit length eigenvectors
+ point = np.real(V[:, i[-1]]).squeeze()
+ point /= point[3]
+ return point, normal
+
+
+def rotation_matrix(angle, direction, point=None):
+ """Return matrix to rotate about axis defined by point and direction.
+ """
+ sina = np.sin(angle)
+ cosa = np.cos(angle)
+ direction = 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
+
+
+def rotation_from_matrix(matrix):
+ """Return rotation angle and axis from rotation matrix.
+ """
+ R = np.array(matrix, dtype=np.float64, copy=False)
+ R33 = R[:3, :3]
+ # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
+ w, W = np.linalg.eig(R33.T)
+ i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
+ if not len(i):
+ raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
+ direction = np.real(W[:, i[-1]]).squeeze()
+ # point: unit eigenvector of R33 corresponding to eigenvalue of 1
+ w, Q = np.linalg.eig(R)
+ i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
+ if not len(i):
+ raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
+ point = np.real(Q[:, i[-1]]).squeeze()
+ point /= point[3]
+ # rotation angle depending on direction
+ cosa = (np.trace(R33) - 1.0) / 2.0
+ if abs(direction[2]) > 1e-8:
+ sina = (R[1, 0] + (cosa - 1.0) * direction[0]
+ * direction[1]) / direction[2]
+ elif abs(direction[1]) > 1e-8:
+ sina = (R[0, 2] + (cosa - 1.0) * direction[0]
+ * direction[2]) / direction[1]
+ else:
+ sina = (R[2, 1] + (cosa - 1.0) * direction[1]
+ * direction[2]) / direction[0]
+ angle = np.arctan2(sina, cosa)
+ return angle, direction, point
+
+
+def scale_matrix(factor, origin=None, direction=None):
+ """Return matrix to scale by factor around origin in direction.
+ Use factor -1 for point symmetry.
+ """
+ if direction is None:
+ # uniform scaling
+ M = np.diag([factor, factor, factor, 1.0])
+ if origin is not None:
+ M[:3, 3] = origin[:3]
+ M[:3, 3] *= 1.0 - factor
+ else:
+ # nonuniform scaling
+ direction = unit_vector(direction[:3])
+ factor = 1.0 - factor
+ M = np.identity(4)
+ M[:3, :3] -= factor * np.outer(direction, direction)
+ if origin is not None:
+ M[:3, 3] = (factor * np.dot(origin[:3], direction)) * direction
+ return M
+
+
+def scale_from_matrix(matrix):
+ """Return scaling factor, origin and direction from scaling matrix. """
+ M = np.array(matrix, dtype=np.float64, copy=False)
+ M33 = M[:3, :3]
+ factor = np.trace(M33) - 2.0
+ try:
+ # direction: unit eigenvector corresponding to eigenvalue factor
+ w, V = np.linalg.eig(M33)
+ i = np.where(abs(np.real(w) - factor) < 1e-8)[0][0]
+ direction = np.real(V[:, i]).squeeze()
+ direction /= vector_norm(direction)
+ except IndexError:
+ # uniform scaling
+ factor = (factor + 2.0) / 3.0
+ direction = None
+ # origin: any eigenvector corresponding to eigenvalue 1
+ w, V = np.linalg.eig(M)
+ i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
+ if not len(i):
+ raise ValueError("no eigenvector corresponding to eigenvalue 1")
+ origin = np.real(V[:, i[-1]]).squeeze()
+ origin /= origin[3]
+ return factor, origin, direction
+
+
+def projection_matrix(point, normal, direction=None,
+ perspective=None, pseudo=False):
+ """Return matrix to project onto plane defined by point and normal.
+
+ Using either perspective point, projection direction, or none of both.
+
+ If pseudo is True, perspective projections will preserve relative depth
+ such that Perspective = dot(Orthogonal, PseudoPerspective).
+ """
+ M = np.identity(4)
+ point = np.array(point[:3], dtype=np.float64, copy=False)
+ normal = unit_vector(normal[:3])
+ if perspective is not None:
+ # perspective projection
+ perspective = np.array(perspective[:3], dtype=np.float64,
+ copy=False)
+ M[0, 0] = M[1, 1] = M[2, 2] = np.dot(perspective - point, normal)
+ M[:3, :3] -= np.outer(perspective, normal)
+ if pseudo:
+ # preserve relative depth
+ M[:3, :3] -= np.outer(normal, normal)
+ M[:3, 3] = np.dot(point, normal) * (perspective + normal)
+ else:
+ M[:3, 3] = np.dot(point, normal) * perspective
+ M[3, :3] = -normal
+ M[3, 3] = np.dot(perspective, normal)
+ elif direction is not None:
+ # parallel projection
+ direction = np.array(direction[:3], dtype=np.float64, copy=False)
+ scale = np.dot(direction, normal)
+ M[:3, :3] -= np.outer(direction, normal) / scale
+ M[:3, 3] = direction * (np.dot(point, normal) / scale)
+ else:
+ # orthogonal projection
+ M[:3, :3] -= np.outer(normal, normal)
+ M[:3, 3] = np.dot(point, normal) * normal
+ return M
+
+
+def projection_from_matrix(matrix, pseudo=False):
+ """Return projection plane and perspective point from projection matrix.
+
+ Return values are same as arguments for projection_matrix function:
+ point, normal, direction, perspective, and pseudo.
+ """
+ M = np.array(matrix, dtype=np.float64, copy=False)
+ M33 = M[:3, :3]
+ w, V = np.linalg.eig(M)
+ i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
+ if not pseudo and len(i):
+ # point: any eigenvector corresponding to eigenvalue 1
+ point = np.real(V[:, i[-1]]).squeeze()
+ point /= point[3]
+ # direction: unit eigenvector corresponding to eigenvalue 0
+ w, V = np.linalg.eig(M33)
+ i = np.where(abs(np.real(w)) < 1e-8)[0]
+ if not len(i):
+ raise ValueError("no eigenvector corresponding to eigenvalue 0")
+ direction = np.real(V[:, i[0]]).squeeze()
+ direction /= vector_norm(direction)
+ # normal: unit eigenvector of M33.T corresponding to eigenvalue 0
+ w, V = np.linalg.eig(M33.T)
+ i = np.where(abs(np.real(w)) < 1e-8)[0]
+ if len(i):
+ # parallel projection
+ normal = np.real(V[:, i[0]]).squeeze()
+ normal /= vector_norm(normal)
+ return point, normal, direction, None, False
+ else:
+ # orthogonal projection, where normal equals direction vector
+ return point, direction, None, None, False
+ else:
+ # perspective projection
+ i = np.where(abs(np.real(w)) > 1e-8)[0]
+ if not len(i):
+ raise ValueError(
+ "no eigenvector not corresponding to eigenvalue 0")
+ point = np.real(V[:, i[-1]]).squeeze()
+ point /= point[3]
+ normal = - M[3, :3]
+ perspective = M[:3, 3] / np.dot(point[:3], normal)
+ if pseudo:
+ perspective -= normal
+ return point, normal, None, perspective, pseudo
+
+
+def shear_matrix(angle, direction, point, normal):
+ """Return matrix to shear by angle along direction vector on shear plane.
+
+ The shear plane is defined by a point and normal vector. The direction
+ vector must be orthogonal to the plane's normal vector.
+
+ A point P is transformed by the shear matrix into P" such that
+ the vector P-P" is parallel to the direction vector and its extent is
+ given by the angle of P-P'-P", where P' is the orthogonal projection
+ of P onto the shear plane.
+ """
+ normal = unit_vector(normal[:3])
+ direction = unit_vector(direction[:3])
+ if abs(np.dot(normal, direction)) > constants._EPS:
+ raise ValueError("direction and normal vectors are not orthogonal")
+ angle = np.tan(angle)
+ M = np.identity(4)
+ M[:3, :3] += angle * np.outer(direction, normal)
+ M[:3, 3] = -angle * np.dot(point[:3], normal) * direction
+ return M
+
+
+def shear_from_matrix(matrix):
+ """Return shear angle, direction and plane from shear matrix.
+ """
+ M = np.array(matrix, dtype=np.float64, copy=False)
+ M33 = M[:3, :3]
+ # normal: cross independent eigenvectors corresponding to the eigenvalue 1
+ w, V = np.linalg.eig(M33)
+ i = np.where(abs(np.real(w) - 1.0) < 1e-4)[0]
+ if len(i) < 2:
+ raise ValueError("no two linear independent eigenvectors found %s" % w)
+ V = np.real(V[:, i]).squeeze().T
+ lenorm = -1.0
+ for i0, i1 in ((0, 1), (0, 2), (1, 2)):
+ n = np.cross(V[i0], V[i1])
+ w = vector_norm(n)
+ if w > lenorm:
+ lenorm = w
+ normal = n
+ normal /= lenorm
+ # direction and angle
+ direction = np.dot(M33 - np.identity(3), normal)
+ angle = vector_norm(direction)
+ direction /= angle
+ angle = np.arctan(angle)
+ # point: eigenvector corresponding to eigenvalue 1
+ w, V = np.linalg.eig(M)
+ i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
+ if not len(i):
+ raise ValueError("no eigenvector corresponding to eigenvalue 1")
+ point = np.real(V[:, i[-1]]).squeeze()
+ point /= point[3]
+ return angle, direction, point, normal
+
+
+def decompose_matrix(matrix, axes='sxyz'):
+ """Return sequence oftransformations from transformation matrix.
+
+ matrix : array_like
+ Non-degenerative homogeneous transformation matrix
+
+ Return tuple of:
+ scale : vector of 3 scaling factors
+ shear : list of shear factors for x-y, x-z, y-z axes
+ angles : list of Euler angles about static x, y, z axes
+ translate : translation vector along x, y, z axes
+ perspective : perspective partition of matrix
+
+ Raise ValueError if matrix is of wrong type or degenerative.
+ """
+ M = np.array(matrix, dtype=np.float64, copy=True).T
+ if abs(M[3, 3]) < constants._EPS:
+ raise ValueError("M[3, 3] is zero")
+ M /= M[3, 3]
+ P = M.copy()
+ P[:, 3] = 0.0, 0.0, 0.0, 1.0
+ if not np.linalg.det(P):
+ raise ValueError("matrix is singular")
+
+ scale = np.zeros((3, ))
+ shear = [0.0, 0.0, 0.0]
+ angles = [0.0, 0.0, 0.0]
+
+ if any(abs(M[:3, 3]) > constants._EPS):
+ perspective = np.dot(M[:, 3], np.linalg.inv(P.T))
+ M[:, 3] = 0.0, 0.0, 0.0, 1.0
+ else:
+ perspective = np.array([0.0, 0.0, 0.0, 1.0])
+
+ translate = M[3, :3].copy()
+ M[3, :3] = 0.0
+
+ row = M[:3, :3].copy()
+ scale[0] = vector_norm(row[0])
+ row[0] /= scale[0]
+ shear[0] = np.dot(row[0], row[1])
+ row[1] -= row[0] * shear[0]
+ scale[1] = vector_norm(row[1])
+ row[1] /= scale[1]
+ shear[0] /= scale[1]
+ shear[1] = np.dot(row[0], row[2])
+ row[2] -= row[0] * shear[1]
+ shear[2] = np.dot(row[1], row[2])
+ row[2] -= row[1] * shear[2]
+ scale[2] = vector_norm(row[2])
+ row[2] /= scale[2]
+ shear[1:] /= scale[2]
+
+ if np.dot(row[0], np.cross(row[1], row[2])) < 0:
+ np.negative(scale, scale)
+ np.negative(row, row)
+
+ angles = euler.from_matrix(np.linalg.inv(row[:3, :3]), axes)
+ return scale, shear, angles, translate, perspective
+
+
+def compose_matrix(scale=None, shear=None, angles=None, translate=None,
+ perspective=None, axes='sxyz'):
+ """Return transformation matrix from sequence oftransformations.
+
+ This is the inverse of the decompose_matrix function.
+
+ Sequence of transformations:
+ scale : vector of 3 scaling factors
+ shear : list of shear factors for x-y, x-z, y-z axes
+ angles : list of Euler angles about static x, y, z axes
+ translate : translation vector along x, y, z axes
+ perspective : perspective partition of matrix
+ """
+ M = np.identity(4)
+ if perspective is not None:
+ P = np.identity(4)
+ P[3, :] = perspective[:4]
+ M = np.dot(M, P)
+ if translate is not None:
+ T = np.identity(4)
+ T[:3, 3] = translate[:3]
+ M = np.dot(M, T)
+ if angles is not None:
+ R = euler.matrix(angles[0], angles[1], angles[2], axes)
+ M = np.dot(M, R)
+ if shear is not None:
+ Z = np.identity(4)
+ Z[1, 2] = shear[2]
+ Z[0, 2] = shear[1]
+ Z[0, 1] = shear[0]
+ M = np.dot(M, Z)
+ if scale is not None:
+ S = np.identity(4)
+ S[0, 0] = scale[0]
+ S[1, 1] = scale[1]
+ S[2, 2] = scale[2]
+ M = np.dot(M, S)
+ M /= M[3, 3]
+ return M
+
+
+def is_same_transform(matrix0, matrix1):
+ """Return True if two matrices perform same transformation.
+ """
+ matrix0 = np.array(matrix0, dtype=np.float64, copy=True)
+ matrix0 /= matrix0[3, 3]
+ matrix1 = np.array(matrix1, dtype=np.float64, copy=True)
+ matrix1 /= matrix1[3, 3]
+ return np.allclose(matrix0, matrix1)
diff --git a/navipy/maths/quaternion.py b/navipy/maths/quaternion.py
new file mode 100644
index 0000000..698788b
--- /dev/null
+++ b/navipy/maths/quaternion.py
@@ -0,0 +1,189 @@
+"""
+
+"""
+import numpy as np
+import navipy.maths.constants as constants
+from navipy.maths.tools import vector_norm
+
+
+def from_euler(ai, aj, ak, axes='sxyz'):
+ """Return quaternion from Euler angles and axis sequence.
+
+ ai, aj, ak : Euler's roll, pitch and yaw angles
+ axes : One of 24 axis sequences as string or encoded tuple
+ """
+ try:
+ firstaxis, parity, repetition, frame = \
+ constants._AXES2TUPLE[axes.lower(
+ )]
+ except (AttributeError, KeyError):
+ constants._TUPLE2AXES[axes] # validation
+ firstaxis, parity, repetition, frame = axes
+
+ i = firstaxis + 1
+ j = constants._NEXT_AXIS[i + parity - 1] + 1
+ k = constants._NEXT_AXIS[i - parity] + 1
+
+ if frame:
+ ai, ak = ak, ai
+ if parity:
+ aj = -aj
+
+ ai /= 2.0
+ aj /= 2.0
+ ak /= 2.0
+ ci = np.cos(ai)
+ si = np.sin(ai)
+ cj = np.cos(aj)
+ sj = np.sin(aj)
+ ck = np.cos(ak)
+ sk = np.sin(ak)
+ cc = ci * ck
+ cs = ci * sk
+ sc = si * ck
+ ss = si * sk
+
+ q = np.empty((4, ))
+ if repetition:
+ q[0] = cj * (cc - ss)
+ q[i] = cj * (cs + sc)
+ q[j] = sj * (cc + ss)
+ q[k] = sj * (cs - sc)
+ else:
+ q[0] = cj * cc + sj * ss
+ q[i] = cj * sc - sj * cs
+ q[j] = cj * ss + sj * cc
+ q[k] = cj * cs - sj * sc
+ if parity:
+ q[j] *= -1.0
+
+ return q
+
+
+def about_axis(angle, axis):
+ """Return quaternion for rotation about axis.
+ """
+ q = np.array([0.0, axis[0], axis[1], axis[2]])
+ qlen = vector_norm(q)
+ if qlen > constants._EPS:
+ q *= np.sin(angle / 2.0) / qlen
+ q[0] = np.cos(angle / 2.0)
+ return q
+
+
+def matrix(quaternion):
+ """Return homogeneous rotation matrix from quaternion.
+ """
+ q = np.array(quaternion, dtype=np.float64, copy=True)
+ n = np.dot(q, q)
+ if n < constants._EPS:
+ return np.identity(4)
+ q *= np.sqrt(2.0 / n)
+ q = np.outer(q, q)
+ return np.array([
+ [1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], 0.0],
+ [ q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], 0.0],
+ [ q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0],
+ [ 0.0, 0.0, 0.0, 1.0]])
+
+
+def from_matrix(matrix, isprecise=False):
+ """Return quaternion from rotation matrix.
+ If isprecise is True, the input matrix is assumed to be a precise rotation
+ matrix and a faster algorithm is used.
+ """
+ M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4]
+ if isprecise:
+ q = np.empty((4, ))
+ t = np.trace(M)
+ if t > M[3, 3]:
+ q[0] = t
+ q[3] = M[1, 0] - M[0, 1]
+ q[2] = M[0, 2] - M[2, 0]
+ q[1] = M[2, 1] - M[1, 2]
+ else:
+ i, j, k = 1, 2, 3
+ if M[1, 1] > M[0, 0]:
+ i, j, k = 2, 3, 1
+ if M[2, 2] > M[i, i]:
+ i, j, k = 3, 1, 2
+ t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
+ q[i] = t
+ q[j] = M[i, j] + M[j, i]
+ q[k] = M[k, i] + M[i, k]
+ q[3] = M[k, j] - M[j, k]
+ q *= 0.5 / np.sqrt(t * M[3, 3])
+ else:
+ m00 = M[0, 0]
+ m01 = M[0, 1]
+ m02 = M[0, 2]
+ m10 = M[1, 0]
+ m11 = M[1, 1]
+ m12 = M[1, 2]
+ m20 = M[2, 0]
+ m21 = M[2, 1]
+ m22 = M[2, 2]
+ # symmetric matrix K
+ K = np.array([[m00 - m11 - m22, 0.0, 0.0, 0.0],
+ [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
+ [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
+ [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22]])
+ K /= 3.0
+ # quaternion is eigenvector of K that corresponds to largest eigenvalue
+ w, V = np.linalg.eigh(K)
+ q = V[[3, 0, 1, 2], np.argmax(w)]
+ if q[0] < 0.0:
+ np.negative(q, q)
+ return q
+
+
+def multiply(quaternion1, quaternion0):
+ """Return multiplication of two quaternions.
+ """
+ w0, x0, y0, z0 = quaternion0
+ w1, x1, y1, z1 = quaternion1
+ return np.array([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
+ x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
+ -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
+ x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64)
+
+
+def conjugate(quaternion):
+ """Return conjugate of quaternion.
+ """
+ q = np.array(quaternion, dtype=np.float64, copy=True)
+ np.negative(q[1:], q[1:])
+ return q
+
+
+def inverse(quaternion):
+ """Return inverse of quaternion.
+ """
+ q = np.array(quaternion, dtype=np.float64, copy=True)
+ np.negative(q[1:], q[1:])
+ return q / np.dot(q, q)
+
+
+def real(quaternion):
+ """Return real part of quaternion.
+ """
+ return float(quaternion[0])
+
+
+def imag(quaternion):
+ """Return imaginary part of quaternion.
+ """
+ return np.array(quaternion[1:4], dtype=np.float64, copy=True)
+
+
+def diff(quaternion0, quaternion1):
+ """ The axis and angle between two quaternions
+ """
+ q = multiply(quaternion1, conjugate(quaternion0))
+ length = np.sum(np.sqrt(q[1:4]*q[1:4]))
+ angle = 2*np.arctan2(length, q[0])
+ if np.isclose(length, 0):
+ axis = np.array([1, 0, 0])
+ else:
+ axis = q[1:4]/length
+ return axis, angle
diff --git a/navipy/maths/random.py b/navipy/maths/random.py
new file mode 100644
index 0000000..096018d
--- /dev/null
+++ b/navipy/maths/random.py
@@ -0,0 +1,49 @@
+import numpy as np
+import navipy.maths.quaternion as quat
+import navipy.maths.constants as constants
+from navipy.maths.tools import unit_vector
+
+
+def orthogonal_vectors():
+ """ Generate two random vector, which are orthogonal
+ """
+ orthogonal = False
+ while not orthogonal:
+ vector_0 = np.random.random(3) - 0.5
+ vector_1 = np.random.random(3) - 0.5
+ normal = np.cross(vector_0, vector_1)
+ vector_0_unit = unit_vector(vector_0)
+ normal_unit = unit_vector(normal)
+ orthogonal = abs(np.dot(normal_unit, vector_0_unit)
+ ) <= constants._EPS
+ return vector_0, normal
+
+
+def rotation_matrix(rand=None):
+ """Return uniform random rotation matrix.
+
+ rand: array like
+ Three independent random variables that are uniformly distributed
+ between 0 and 1 for each returned quaternion.
+ """
+ return quat.matrix(quaternion(rand))
+
+
+def quaternion(rand=None):
+ """Return uniform random unit quaternion.
+
+ rand: array like or None
+ Three independent random variables that are uniformly distributed
+ between 0 and 1.
+ """
+ if rand is None:
+ rand = np.random.rand(3)
+ else:
+ assert len(rand) == 3
+ r1 = np.sqrt(1.0 - rand[0])
+ r2 = np.sqrt(rand[0])
+ pi2 = np.pi * 2.0
+ t1 = pi2 * rand[1]
+ t2 = pi2 * rand[2]
+ return np.array([np.cos(t2) * r2, np.sin(t1) * r1,
+ np.cos(t1) * r1, np.sin(t2) * r2])
diff --git a/navipy/maths/test_euler.py b/navipy/maths/test_euler.py
new file mode 100644
index 0000000..c1c9533
--- /dev/null
+++ b/navipy/maths/test_euler.py
@@ -0,0 +1,28 @@
+import unittest
+import numpy as np
+import navipy.maths.random as random
+import navipy.maths.euler as euler
+from navipy.maths.tools import vector_norm
+import navipy.maths.constants as constants
+
+
+class TestEuler(unittest.TestCase):
+ def test_from_matrix(self):
+ condition = dict()
+ for key, _ in constants._AXES2TUPLE.items():
+ rotation_0 = euler.matrix(1, 2, 3, key)
+ angles = euler.from_matrix(rotation_0, key)
+ rotation_1 = euler.matrix(*angles, key)
+ condition[key] = np.allclose(rotation_0,
+ rotation_1)
+ if condition[key] is False:
+ print('axes', key, 'failed')
+ self.assertTrue(np.all(np.array(condition.values())))
+
+ def test_from_quaternion(self):
+ angles = euler.from_quaternion([0.99810947, 0.06146124, 0, 0])
+ self.assertTrue(np.allclose(angles, [0.123, 0, 0]))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/navipy/maths/test_homogenous_transformations.py b/navipy/maths/test_homogenous_transformations.py
new file mode 100644
index 0000000..2c45dea
--- /dev/null
+++ b/navipy/maths/test_homogenous_transformations.py
@@ -0,0 +1,274 @@
+import unittest
+import numpy as np
+import navipy.maths.homogeneous_transformations as ht
+import navipy.maths.euler as euler
+import navipy.maths.random as random
+
+
+class TestHT(unittest.TestCase):
+ def test_translation_matrix(self):
+ vector = np.random.random(3)
+ vector -= 0.5
+ vector_test = ht.translation_matrix(vector)[:3, 3]
+ self.assertTrue(np.allclose(vector,
+ vector_test))
+
+ def test_translation_from_matrix(self):
+ vector = np.random.random(3)
+ vector -= 0.5
+ matrix = ht.translation_matrix(vector)
+ vector_test = ht.translation_from_matrix(matrix)
+ self.assertTrue(np.allclose(vector,
+ vector_test))
+
+ def test_reflection_matrix(self):
+ vector_0 = np.random.random(4)
+ vector_0 -= 0.5
+ vector_0[3] = 1.
+ vector_1 = np.random.random(3)
+ vector_1 -= 0.5
+ reflection = ht.reflection_matrix(vector_0,
+ vector_1)
+ vector_2 = vector_0.copy()
+ vector_2[:3] += vector_1
+ vector_3 = vector_0.copy()
+ vector_3[:3] -= vector_1
+ condition_0 = np.allclose(2, np.trace(reflection))
+ condition_1 = np.allclose(vector_0,
+ np.dot(reflection, vector_0))
+ condition_2 = np.allclose(vector_2, np.dot(reflection,
+ vector_3))
+ condition = condition_0 & condition_1 & condition_2
+ self.assertTrue(condition)
+
+ def test_reflection_from_matrix(self):
+ vector_0 = np.random.random(3) - 0.5
+ vector_1 = np.random.random(3) - 0.5
+ matrix_0 = ht.reflection_matrix(vector_0, vector_1)
+ point, normal = ht.reflection_from_matrix(matrix_0)
+ matrix_1 = ht.reflection_matrix(point, normal)
+ condition = ht.is_same_transform(matrix_0, matrix_1)
+ self.assertTrue(condition)
+
+ def test_rotation_matrix_example(self):
+ rotation = ht.rotation_matrix(np.pi / 2,
+ [0, 0, 1],
+ [1, 0, 0])
+ condition = np.allclose(np.dot(rotation,
+ [0, 0, 0, 1]),
+ [1, -1, 0, 1])
+ self.assertTrue(condition)
+
+ def test_rotation_matrix_periodicity(self):
+ angle = (np.random.random() - 0.5) * (2 * np.pi)
+ direc = np.random.random(3) - 0.5
+ point = np.random.random(3) - 0.5
+ rotation_0 = ht.rotation_matrix(angle, direc, point)
+ rotation_1 = ht.rotation_matrix(angle - 2 * np.pi, direc, point)
+ condition = ht.is_same_transform(rotation_0, rotation_1)
+ self.assertTrue(condition)
+
+ def test_rotation_matrix_opposite(self):
+ angle = (np.random.random() - 0.5) * (2 * np.pi)
+ direc = np.random.random(3) - 0.5
+ point = np.random.random(3) - 0.5
+ rotation_0 = ht.rotation_matrix(angle, direc, point)
+ rotation_1 = ht.rotation_matrix(-angle, -direc, point)
+ condition = ht.is_same_transform(rotation_0, rotation_1)
+ self.assertTrue(condition)
+
+ def test_rotation_matrix_identity(self):
+ direc = np.random.random(3) - 0.5
+ identity = np.identity(4)
+ condition = np.allclose(identity,
+ ht.rotation_matrix(np.pi * 2, direc))
+ self.assertTrue(condition)
+
+ def test_rotation_matrix_trace(self):
+ direc = np.random.random(3) - 0.5
+ point = np.random.random(3) - 0.5
+ condition = np.allclose(2,
+ np.trace(ht.rotation_matrix(np.pi / 2,
+ direc,
+ point)))
+ self.assertTrue(condition)
+
+ def test_rotation_from_matrix(self):
+ angle = (np.random.random() - 0.5) * (2 * np.pi)
+ direc = np.random.random(3) - 0.5
+ point = np.random.random(3) - 0.5
+ rotation_0 = ht.rotation_matrix(angle, direc, point)
+ angle, direc, point = ht.rotation_from_matrix(rotation_0)
+ rotation_1 = ht.rotation_matrix(angle, direc, point)
+ self.assertTrue(ht.is_same_transform(rotation_0,
+ rotation_1))
+
+ def test_scale(self):
+ vector = (np.random.rand(4, 5) - 0.5) * 20
+ vector[3] = 1
+ factor = -1.234
+ scale = ht.scale_matrix(factor)
+ condition = np.allclose(np.dot(scale, vector)[:3],
+ factor * vector[:3])
+ self.assertTrue(condition)
+
+ def test_scale_sametransform(self):
+ factor = np.random.random() * 10 - 5
+ origin = np.random.random(3) - 0.5
+ scale_0 = ht.scale_matrix(factor, origin)
+ factor, origin, direction = ht.scale_from_matrix(scale_0)
+ scale_1 = ht.scale_matrix(factor, origin, direction)
+ self.assertTrue(ht.is_same_transform(scale_0, scale_1))
+
+ def test_scale_from_matrix(self):
+ factor = np.random.random() * 10 - 5
+ origin = np.random.random(3) - 0.5
+ direct = np.random.random(3) - 0.5
+ scale_0 = ht.scale_matrix(factor, origin, direct)
+ factor, origin, direction = ht.scale_from_matrix(scale_0)
+ scale_1 = ht.scale_matrix(factor, origin, direction)
+ self.assertTrue(ht.is_same_transform(scale_0, scale_1))
+
+ def test_projection_matrix_example(self):
+ projection = ht.projection_matrix([0, 0, 0], [1, 0, 0])
+ self.assertTrue(np.allclose(projection[1:, 1:],
+ np.identity(4)[1:, 1:]))
+
+ def test_projection_matrix(self):
+ point = np.random.random(3) - 0.5
+ normal = np.random.random(3) - 0.5
+ direct = np.random.random(3) - 0.5
+ persp = np.random.random(3) - 0.5
+ projection_0 = ht.projection_matrix(point, normal)
+ projection_1 = ht.projection_matrix(point, normal,
+ direction=direct)
+ projection_2 = ht.projection_matrix(point, normal,
+ perspective=persp)
+ projection_3 = ht.projection_matrix(point, normal,
+ perspective=persp,
+ pseudo=True)
+ self.assertTrue(ht.is_same_transform(projection_2,
+ np.dot(projection_0,
+ projection_3)))
+
+ def test_projection_from_matrix(self):
+ point = np.random.random(3) - 0.5
+ normal = np.random.random(3) - 0.5
+ projection_0 = ht.projection_matrix(point, normal)
+ result = ht.projection_from_matrix(projection_0)
+ projection_1 = ht.projection_matrix(*result)
+ self.assertTrue(ht.is_same_transform(projection_0,
+ projection_1))
+
+ def test_projection_from_matrix_direct(self):
+ point = np.random.random(3) - 0.5
+ normal = np.random.random(3) - 0.5
+ direct = np.random.random(3) - 0.5
+ projection_0 = ht.projection_matrix(point, normal, direct)
+ result = ht.projection_from_matrix(projection_0)
+ projection_1 = ht.projection_matrix(*result)
+ self.assertTrue(ht.is_same_transform(projection_0,
+ projection_1))
+
+ def test_projection_from_matrix_persp_pseudo_false(self):
+ point = np.random.random(3) - 0.5
+ normal = np.random.random(3) - 0.5
+ persp = np.random.random(3) - 0.5
+ projection_0 = ht.projection_matrix(point, normal, persp,
+ pseudo=False)
+ result = ht.projection_from_matrix(projection_0)
+ projection_1 = ht.projection_matrix(*result)
+ self.assertTrue(ht.is_same_transform(projection_0,
+ projection_1))
+
+ def test_projection_from_matrix_persp_pseudo_true(self):
+ point = np.random.random(3) - 0.5
+ normal = np.random.random(3) - 0.5
+ direct = np.random.random(3) - 0.5
+ persp = np.random.random(3) - 0.5
+ projection_0 = ht.projection_matrix(point, normal, persp,
+ pseudo=True)
+ result = ht.projection_from_matrix(projection_0)
+ projection_1 = ht.projection_matrix(*result)
+ self.assertTrue(ht.is_same_transform(projection_0,
+ projection_1))
+
+ def test_shear_matrix(self):
+ angle = (np.random.random() - 0.5) * 4 * np.pi
+ point = np.random.random(3) - 0.5
+ direct, normal = random.orthogonal_vectors()
+ scale = ht.shear_matrix(angle, direct, point, normal)
+ self.assertTrue(np.allclose(1, np.linalg.det(scale)))
+
+ # Not Working due to unorthogonality of direct and normal
+ # from time to time....
+ # def test_shear_from_matrix(self):
+ # angle = (np.random.random() - 0.5) * 4 * np.pi
+ # direct, normal = random.orthogonal_vectors()
+ # point = np.random.random(3) - 0.5
+ # scale_0 = ht.shear_matrix(angle, direct, point, normal)
+ # result = ht.shear_from_matrix(scale_0)
+ # scale_1 = ht.shear_matrix(*result)
+ # self.assertTrue(ht.is_same_transform(scale_0, scale_1))
+
+ def test_decompose_matrix_translation(self):
+ translation_0 = ht.translation_matrix([1, 2, 3])
+ scale, shear, angles, trans, persp = ht.decompose_matrix(translation_0)
+ translation_1 = ht.translation_matrix(trans)
+ self.assertTrue(np.allclose(translation_0, translation_1))
+
+ def test_decompose_matrix_scale(self):
+ factor = np.random.random()
+ scale = ht.scale_matrix(factor)
+ scale, shear, angles, trans, persp = ht.decompose_matrix(scale)
+ self.assertEqual(scale[0], factor)
+
+ def test_decompose_matrix_rotation(self):
+ rotation_0 = euler.matrix(1, 2, 3)
+ _, _, angles, _, _ = ht.decompose_matrix(rotation_0)
+ rotation_1 = euler.matrix(*angles)
+ self.assertTrue(np.allclose(rotation_0, rotation_1))
+
+ def test_compose_matrix(self):
+ scale = np.random.random(3) - 0.5
+ shear = np.random.random(3) - 0.5
+ angles = (np.random.random(3) - 0.5) * (2 * np.pi)
+ trans = np.random.random(3) - 0.5
+ persp = np.random.random(4) - 0.5
+ matrix_0 = ht.compose_matrix(scale, shear, angles, trans, persp)
+ result = ht.decompose_matrix(matrix_0)
+ matrix_1 = ht.compose_matrix(*result)
+ self.assertTrue(ht.is_same_transform(matrix_0, matrix_1))
+
+ def test_is_same_transform(self):
+ matrix = np.random.rand(4, 4)
+ self.assertTrue(ht.is_same_transform(matrix, matrix))
+
+ def test_is_same_transform_rejection(self):
+ # The determinant of a rotation matrix is one,
+ # Thus we test rejection rotation against scaled identity
+ matrix = 2 * np.identity(4)
+ rotation = random.rotation_matrix()
+ self.assertFalse(ht.is_same_transform(matrix, rotation))
+
+ def test_compose_decompose(self):
+ angles_o = [0.123, -1.234, 2.345]
+ scale_o = np.random.rand(3)
+ translation_o = [1, 2, 3]
+ shear_o = [0, np.tan(angles_o[1]), 0]
+ axes = 'sxyz'
+ matrix = ht.compose_matrix(scale_o, shear_o, angles_o,
+ translation_o, axes=axes)
+ scale, shear, angles, trans, persp = ht.decompose_matrix(matrix, axes)
+ np.testing.assert_almost_equal(scale, scale_o)
+ np.testing.assert_almost_equal(shear, shear_o)
+ np.testing.assert_almost_equal(angles, angles_o)
+ np.testing.assert_almost_equal(trans, translation_o)
+
+ matrix_1 = ht.compose_matrix(scale, shear, angles,
+ trans, persp, axes)
+ self.assertTrue(ht.is_same_transform(matrix, matrix_1))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/navipy/maths/test_quaternion.py b/navipy/maths/test_quaternion.py
new file mode 100644
index 0000000..55fbe93
--- /dev/null
+++ b/navipy/maths/test_quaternion.py
@@ -0,0 +1,87 @@
+import unittest
+import numpy as np
+import navipy.maths.homogeneous_transformations as ht
+import navipy.maths.quaternion as quat
+import navipy.maths.random as random
+
+
+class TestQuaternions(unittest.TestCase):
+ def test_quaternion_from_euler(self):
+ quaternion = quat.from_euler(1, 2, 3, 'ryxz')
+ self.assertTrue(np.allclose(quaternion,
+ [0.435953, 0.310622,
+ -0.718287, 0.444435]))
+
+ def test_about_axis(self):
+ quaternion = quat.about_axis(0.123, [1, 0, 0])
+ self.assertTrue(np.allclose(quaternion,
+ [0.99810947, 0.06146124, 0, 0]))
+
+ def test_matrix(self):
+ matrix = quat.matrix([0.99810947, 0.06146124, 0, 0])
+ self.assertTrue(np.allclose(matrix,
+ ht.rotation_matrix(0.123,
+ [1, 0, 0])))
+
+ def test_matrix_identity(self):
+ matrix = quat.matrix([1, 0, 0, 0])
+ self.assertTrue(np.allclose(matrix, np.identity(4)))
+
+ def test_matrix_diagonal(self):
+ matrix = quat.matrix([0, 1, 0, 0])
+ self.assertTrue(np.allclose(matrix, np.diag([1, -1, -1, 1])))
+
+ def test_from_matrix_identity(self):
+ quaternion = quat.from_matrix(np.identity(4), True)
+ self.assertTrue(np.allclose(quaternion, [1, 0, 0, 0]))
+
+ def test_from_matrix_diagonal(self):
+ quaternion = quat.from_matrix(np.diag([1, -1, -1, 1]))
+ self.assertTrue(np.allclose(quaternion, [0, 1, 0, 0])
+ or np.allclose(quaternion, [0, -1, 0, 0]))
+
+ def test_from_matrix_rotation(self):
+ rotation = random.rotation_matrix()
+ quaternion = quat.from_matrix(rotation)
+ self.assertTrue(ht.is_same_transform(
+ rotation, quat.matrix(quaternion)))
+
+ # Seem to fail from time to time....
+ # def test_quaternion_multiply(self):
+ # rotation_0 = random.rotation_matrix()
+ # quaternion_0 = quat.from_matrix(rotation_0)
+ # rotation_1 = random.rotation_matrix()
+ # quaternion_1 = quat.from_matrix(rotation_1)
+ # quaternion_01 = quat.multiply(quaternion_0,
+ # quaternion_1)
+ # rotation_01 = rotation_0.dot(rotation_1)
+ # quaternion_01_fromrot = quat.from_matrix(rotation_01)
+ # self.assertTrue(np.allclose(quaternion_01,
+ # quaternion_01_fromrot))
+
+ def test_inverse(self):
+ quaternion_0 = random.quaternion()
+ quaternion_1 = quat.inverse(quaternion_0)
+ self.assertTrue(np.allclose(
+ quat.multiply(quaternion_0, quaternion_1),
+ [1, 0, 0, 0]))
+
+ def test_quaternion(self):
+ alpha, beta, gamma = 0.123, -1.234, 2.345
+ xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1]
+ rotx = ht.rotation_matrix(alpha, xaxis)
+ roty = ht.rotation_matrix(beta, yaxis)
+ rotz = ht.rotation_matrix(gamma, zaxis)
+ rotation = rotx.dot(roty.dot(rotz))
+
+ quatx = quat.about_axis(alpha, xaxis)
+ quaty = quat.about_axis(beta, yaxis)
+ quatz = quat.about_axis(gamma, zaxis)
+ quaternion = quat.multiply(quatx, quaty)
+ quaternion = quat.multiply(quaternion, quatz)
+ rotq = quat.matrix(quaternion)
+ self.assertTrue(ht.is_same_transform(rotation, rotq))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/navipy/maths/test_random_cust.py b/navipy/maths/test_random_cust.py
new file mode 100644
index 0000000..be32b6c
--- /dev/null
+++ b/navipy/maths/test_random_cust.py
@@ -0,0 +1,19 @@
+import unittest
+import numpy as np
+import navipy.maths.random as random
+from navipy.maths.tools import vector_norm
+
+
+class TestRandom(unittest.TestCase):
+ def test__rotation_matrix(self):
+ rotation = random.rotation_matrix()
+ self.assertTrue(np.allclose(np.dot(rotation.T, rotation),
+ np.identity(4)))
+
+ def test_random_quaternion(self):
+ quaternion = random.quaternion()
+ self.assertTrue(np.allclose(1, vector_norm(quaternion)))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/navipy/maths/tools.py b/navipy/maths/tools.py
new file mode 100644
index 0000000..6ba238e
--- /dev/null
+++ b/navipy/maths/tools.py
@@ -0,0 +1,57 @@
+"""
+"""
+import numpy as np
+
+
+def vector_norm(data, axis=0):
+ """Return Euclidean norm of ndarray along axis.
+ """
+ if isinstance(data, list):
+ data = np.array(data)
+ assert isinstance(data, np.ndarray), 'data should be a numpy array'
+
+ return np.sqrt(np.sum(data * data, axis=axis, keepdims=True))
+
+
+def unit_vector(data, axis=0):
+ """Return ndarray normalized by length, i.e. Euclidean norm, along axis.
+ """
+ if isinstance(data, list):
+ data = np.array(data)
+
+ assert isinstance(data, np.ndarray), 'data should be a numpy array'
+ repetitions = np.array(data.shape)
+ repetitions[:] = 1
+ repetitions[axis] = data.shape[axis]
+ norm = vector_norm(data, axis)
+ return data / np.tile(norm, repetitions)
+
+
+def angle_between_vectors(vector_0, vector_1, directed=True, axis=0):
+ """Return angle between vectors.
+
+ If directed is False, the input vectors are interpreted as undirected axes,
+ i.e. the maximum angle is pi/2.
+ """
+ vector_0 = np.array(vector_0, dtype=np.float64, copy=False)
+ vector_1 = np.array(vector_1, dtype=np.float64, copy=False)
+ dot = np.sum(vector_0 * vector_1, axis=axis, keepdims=True)
+ dot /= vector_norm(vector_0, axis=axis) \
+ * vector_norm(vector_1, axis=axis)
+ dot = np.squeeze(dot)
+ return np.arccos(dot if directed else np.fabs(dot))
+
+
+def inverse_matrix(matrix):
+ """Return inverse of square transformation matrix.
+ """
+ return np.linalg.inv(matrix)
+
+
+def concatenate_matrices(*matrices):
+ """Return concatenation of series of transformation matrices.
+ """
+ M = np.identity(4)
+ for i in matrices:
+ M = np.dot(M, i)
+ return M
--
GitLab