Merge pull request #9 from hsorby/main

Add vector operation functions used by scaffoldmaker here.

Author: hsorby

src/cmlibs/maths/__init__.py

@@ -1 +1 @@
-__version__ = '0.5.0'
+__version__ = '0.6.0'

src/cmlibs/maths/octree.py

@@ -3,12 +3,12 @@
 """
 import math
 
-from cmlibs.maths.vectorops import sub, dot
+from cmlibs.maths.vectorops import sub, dot, add, div
 
 
 class Octree:
     """
-    Octree for searching for objects by coordinates
+    Octree for searching for objects by coordinates.
     """
 
     def __init__(self, tolerance=None):
@@ -104,3 +104,106 @@ def __repr__(self):
             return f'\tleaf {self._coordinates}\n'
 
         return f'{self._coordinates} - \n' + ''.join([f'{c}' for c in self._children])
+
+
+class VolumeOctreeObject:
+    """
+    Interface object for the volume octree.
+    This class defines the required interface for objects
+    given to the VolumeOctree.
+
+    It is not required that you use this object directly.
+    It is required that any objects added to the volume octree
+    follow this interface.
+    """
+
+    def __init__(self, data_object):
+        self._data_object = data_object
+
+    def identifier(self):
+        return self._data_object.identifier()
+
+    def points(self):
+        return self._data_object.points()
+
+    def distance(self, pt, tol):
+        return self._data_object.distance(pt, tol)
+
+
+class VolumeOctree:
+    """
+    An octree for managing objects with a volume.
+    """
+
+    def __init__(self, bounding_box, tolerance=1e-08, depth=0):
+        self._bounding_box = bounding_box
+        self._centre = div(add(bounding_box[0], bounding_box[1]), 2)
+        self._tolerance = tolerance
+        self._objects = {}
+        self._children = {}
+        self._depth = depth
+
+    def _child_index_lookup(self, x):
+        switch = [0 if x[i] < c_i else 1 for i, c_i in enumerate(self._centre)]
+        return sum([2**i * s for i, s in enumerate(switch)])
+
+    def _sub_bounding_box(self, index):
+        masks = [2**i for i in range(len(self._centre))]
+        p = [self._bounding_box[0 if index & m == 0 else 1][i] for i, m in enumerate(masks)]
+        return [[min(i) for i in zip(p, self._centre)], [max(i) for i in zip(p, self._centre)]]
+
+    def insert_object(self, obj):
+        indices = [self._child_index_lookup(pt) for pt in obj.points()]
+        if len(set(indices)) == 1:
+            required_child = indices[0]
+            if required_child not in self._children:
+                self._children[required_child] = VolumeOctree(self._sub_bounding_box(required_child), self._tolerance, depth=self._depth + 1)
+            # if self._children is None:
+            #     self._children = [VolumeOctree(self._sub_bounding_box(i), self._tolerance, depth=self._depth + 1) for i in range(2**len(self._centre))]
+
+            self._children[required_child].insert_object(obj)
+        else:
+            for i in set(indices):
+                if i not in self._objects:
+                    self._objects[i] = []
+                self._objects[i].append(obj)
+
+    def find_object(self, x):
+        """
+        Find the closest existing object with |x - ox| < tolerance.
+        :param x: Coordinates in a list.
+        :return: Nearest object or None.
+        """
+        octant_index = self._child_index_lookup(x)
+        target = self._children[octant_index].find_object(x) if octant_index in self._children else None
+
+        if target is not None:
+            return target
+
+        distances = [obj.distance(x, self._tolerance) for obj in self._objects.get(octant_index, [])]
+        index = next((i for i, x in enumerate(distances) if x < self._tolerance), None)
+        if index is not None:
+            return self._objects[octant_index][index]
+
+        return None  # self._children[octant_index].find_object(x) if octant_index in self._children else None
+
+    def __repr__(self):
+        indent = [' '] * 2 * self._depth
+        indent = ''.join(indent)
+
+        objects_len = ', '.join([f'{o}: {len(self._objects[o])}' for o in self._objects])
+        if self._children is None:
+            return f'{indent}leaf {self._centre} [{objects_len}]\n'
+
+        return f'{indent}{self._centre} [{objects_len}]- \n' + ''.join([f'{self._children[c]}' for c in self._children])
+
+
+def define_bounding_box():
+    """
+    From an initial point define a bounding box that covers everything.
+    Return the top, back, left and bottom, front, right points to define
+    the box.
+
+    :return: A list of two 3D points defining the extent of the bounding box.
+    """
+    return [[math.inf, math.inf, math.inf], [-math.inf, -math.inf, -math.inf]]

src/cmlibs/maths/vectorops.py

@@ -3,38 +3,66 @@
 they were vectors.  A basic implementation to forgo the need
 to use numpy.
 """
-import math
-import sys
-from math import sqrt, cos, sin, fabs, atan2
+from math import acos, sqrt, cos, sin, fabs, atan2
 
 
 def magnitude(v):
-    return sqrt(sum(v[i] * v[i] for i in range(len(v))))
+    """
+    return: scalar magnitude of vector v
+    """
+    return sqrt(sum(c * c for c in v))
+
+
+def set_magnitude(v, mag):
+    """
+    return: Vector v with magnitude set to mag.
+    """
+    scale = mag / magnitude(v)
+    return mult(v, scale)
 
 
 def add(u, v):
-    return [u[i] + v[i] for i in range(len(u))]
+    return [u_i + v_i for u_i, v_i in zip(u, v)]
 
 
 def sub(u, v):
-    return [u[i] - v[i] for i in range(len(u))]
+    return [u_i - v_i for u_i, v_i in zip(u, v)]
+
+
+def mult(v, s):
+    """
+    Calculate s * v
+    :param v: Vector.
+    :param s: Scalar.
+    :return: [s * v_1, s * v_2, ..., s * v_n]
+    """
+    return [c * s for c in v]
+
+
+def div(v, s):
+    """
+    Calculate v / s
+    :param v: Vector.
+    :param s: Scalar.
+    :return: [v_1 / s, v_2 / s, ..., v_n / s]
+    """
+    return [c / s for c in v]
 
 
 def dot(u, v):
-    return sum(u[i] * v[i] for i in range(len(u)))
+    return sum(u_i * v_i for u_i, v_i in zip(u, v))
 
 
 def eldiv(u, v):
-    return [u[i] / v[i] for i in range(len(u))]
+    return [u_i / v_i for u_i, v_i in zip(u, v)]
 
 
 def elmult(u, v):
-    return [u[i] * v[i] for i in range(len(u))]
+    return [u_i * v_i for u_i, v_i in zip(u, v)]
 
 
 def normalize(v):
-    vmag = magnitude(v)
-    return [v[i] / vmag for i in range(len(v))]
+    return div(v, magnitude(v))
 
 
 def cross(u, v):
@@ -45,36 +73,66 @@ def cross(u, v):
     return c
 
 
-def mult(u, c):
-    return [u[i] * c for i in range(len(u))]
+def scalar_projection(v1, v2):
+    """
+    :return: Scalar projection of v1 onto v2.
+    """
+    return dot(v1, normalize(v2))
 
 
-def div(u, c):
-    return [u[i] / c for i in range(len(u))]
+def projection(v1, v2):
+    """
+    Calculate vector projection of v1 on v2
+    :return: A projection vector.
+    """
+    s1 = scalar_projection(v1, v2)
+    return mult(normalize(v2), s1)
 
 
-def quaternion_to_rotation_matrix(quaternion):
+def rejection(v1, v2):
     """
-    This method takes a quaternion representing a rotation
-    and turns it into a rotation matrix.
-    :return: 3x3 rotation matrix suitable for pre-multiplying vector v:
-    i.e. v' = Mv
+    Calculate vector rejection of v1 on v2
+    :return: A rejection vector.
     """
-    mag_q = magnitude(quaternion)
-    norm_q = div(quaternion, mag_q)
-    qw, qx, qy, qz = norm_q
-    ww, xx, yy, zz = qw * qw, qx * qx, qy * qy, qz * qz
-    wx, wy, wz, xy, xz, yz = qw * qx, qw * qy, qw * qz, qx * qy, qx * qz, qy * qz
-    # mx = [[qw * qw + qx * qx - qy * qy - qz * qz, 2 * qx * qy - 2 * qw * qz, 2 * qx * qz + 2 * qw * qy],
-    #       [2 * qx * qy + 2 * qw * qz, qw * qw - qx * qx + qy * qy - qz * qz, 2 * qy * qz - 2 * qw * qx],
-    #       [2 * qx * qz - 2 * qw * qy, 2 * qy * qz + 2 * qw * qx, qw * qw - qx * qx - qy * qy + qz * qz]]
-    # aa, bb, cc, dd = a * a, b * b, c * c, d * d
-    # bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
+    v1p = projection(v1, v2)
+    return add_vectors([v1, v1p], [1.0, -1.0])
 
-    mx = [[ww + xx - yy - zz, 2 * (xy + wz), 2 * (xz - wy)],
-          [2 * (xy - wz), ww + yy - xx - zz, 2 * (yz + wx)],
-          [2 * (xz + wy), 2 * (yz - wx), ww + zz - xx - yy]]
-    return mx
+
+def angle(u, v):
+    """
+    Calculate the angle between two non-zero vectors.
+    :return: The angle between them in radians.
+    """
+    d = magnitude(u) * magnitude(v)
+    return acos(dot(u, v) / d)
+
+
+def add_vectors(vectors, scalars=None):
+    """
+    returns s1*v1+s2*v2+... where scalars = [s1, s2, ...] and vectors=[v1, v2, ...].
+    :return: Resultant vector
+    """
+    if not scalars:
+        scalars = [1] * len(vectors)
+    else:
+        assert len(vectors) == len(scalars)
+
+    vector_dimension = len(vectors[0])
+    resultant = [0] * vector_dimension
+    for i, vector in enumerate(vectors):
+        resultant = [resultant[c] + scalars[i] * vector[c] for c in range(vector_dimension)]
+    return resultant
+
+
+def rotate_vector_around_vector(v, k, a):
+    """
+    Rotate vector v, by an angle a (right-hand rule) in radians around vector k.
+    :return: rotated vector.
+    """
+    k = normalize(k)
+    vperp = add_vectors([v, cross(k, v)], [cos(a), sin(a)])
+    vparal = mult(k, dot(k, v) * (1 - cos(a)))
+    return add_vectors([vperp, vparal])
 
 
 def matrix_constant_mult(m, c):
@@ -100,7 +158,7 @@ def vector_matrix_mult(v, m):
         raise ValueError('vector_matrix_mult mismatched rows')
     columns = len(m[0])
     result = []
-    for c in range(0, columns):
+    for c in range(columns):
         result.append(sum(v[r] * m[r][c] for r in range(rows)))
     return result
 
@@ -132,15 +190,16 @@ def matrix_inv(a):
     """
     Invert a square matrix by compouting the determinant and cofactor matrix.
     """
+    len_a = len(a)
     det = matrix_det(a)
 
-    if len(a) == 2:
+    if len_a == 2:
         return matrix_constant_mult([[a[1][1], -1 * a[0][1]], [-1 * a[1][0], a[0][0]]], 1 / det)
 
     cofactor = []
-    for r in range(len(a)):
+    for r in range(len_a):
         row = []
-        for c in range(len(a)):
+        for c in range(len_a):
             minor = matrix_minor(a, r, c)
             row.append(((-1) ** (r + c)) * matrix_det(minor))
         cofactor.append(row)
@@ -151,7 +210,7 @@ def matrix_inv(a):
 
 def identity_matrix(size):
     """
-    Create an identity matrix of size size.
+    Create an identity matrix of size x size.
     """
     identity = []
     for r in range(size):
@@ -165,18 +224,31 @@ def identity_matrix(size):
 
 
 def transpose(a):
-    if sys.version_info < (3, 0):
-        return map(list, zip(*a))
     return list(map(list, zip(*a)))
 
 
-def angle(u, v):
+def quaternion_to_rotation_matrix(quaternion):
     """
-    Calculate the angle between two non-zero vectors.
-    :return: The angle between them in radians.
+    This method takes a quaternion representing a rotation
+    and turns it into a rotation matrix.
+    :return: 3x3 rotation matrix suitable for pre-multiplying vector v:
+    i.e. v' = Mv
     """
-    d = magnitude(u) * magnitude(v)
-    return math.acos(dot(u, v) / d)
+    mag_q = magnitude(quaternion)
+    norm_q = div(quaternion, mag_q)
+    qw, qx, qy, qz = norm_q
+    ww, xx, yy, zz = qw * qw, qx * qx, qy * qy, qz * qz
+    wx, wy, wz, xy, xz, yz = qw * qx, qw * qy, qw * qz, qx * qy, qx * qz, qy * qz
+    # mx = [[qw * qw + qx * qx - qy * qy - qz * qz, 2 * qx * qy - 2 * qw * qz, 2 * qx * qz + 2 * qw * qy],
+    #       [2 * qx * qy + 2 * qw * qz, qw * qw - qx * qx + qy * qy - qz * qz, 2 * qy * qz - 2 * qw * qx],
+    #       [2 * qx * qz - 2 * qw * qy, 2 * qy * qz + 2 * qw * qx, qw * qw - qx * qx - qy * qy + qz * qz]]
+    # aa, bb, cc, dd = a * a, b * b, c * c, d * d
+    # bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
+
+    mx = [[ww + xx - yy - zz, 2 * (xy + wz), 2 * (xz - wy)],
+          [2 * (xy - wz), ww + yy - xx - zz, 2 * (yz + wx)],
+          [2 * (xz + wy), 2 * (yz - wx), ww + zz - xx - yy]]
+    return mx
 
 
 def euler_to_rotation_matrix(euler_angles):
@@ -198,8 +270,10 @@ def euler_to_rotation_matrix(euler_angles):
     cos_roll = cos(euler_angles[2])
     sin_roll = sin(euler_angles[2])
     mat3x3 = [
-        [cos_azimuth * cos_elevation, cos_azimuth * sin_elevation * sin_roll - sin_azimuth * cos_roll, cos_azimuth * sin_elevation * cos_roll + sin_azimuth * sin_roll],
-        [sin_azimuth * cos_elevation, sin_azimuth * sin_elevation * sin_roll + cos_azimuth * cos_roll, sin_azimuth * sin_elevation * cos_roll - cos_azimuth * sin_roll],
+        [cos_azimuth * cos_elevation, cos_azimuth * sin_elevation * sin_roll - sin_azimuth * cos_roll,
+         cos_azimuth * sin_elevation * cos_roll + sin_azimuth * sin_roll],
+        [sin_azimuth * cos_elevation, sin_azimuth * sin_elevation * sin_roll + cos_azimuth * cos_roll,
+         sin_azimuth * sin_elevation * cos_roll - cos_azimuth * sin_roll],
         [-sin_elevation, cos_elevation * sin_roll, cos_elevation * cos_roll]]
     return mat3x3