Add curvature

main.py

@@ -22,5 +22,25 @@ editor = Editor(buffering=True)
 #     print(point)
 #     editor.placeBlock(point, Block("stone"))
 
-print(segment.parrallel(((0, 0, 0), (0, 0, 10)), 10))
+
-print(segment.orthogonal((0, 0, 0), (1, 0, 0), 10))
+# print(segment.parallel(((0, 0, 0), (0, 0, 10)), 10))
+# print(segment.orthogonal((0, 0, 0), (1, 0, 0), 10))
+# print(curve.curvature(np.array(([0, 0, 0], [0, 1, 1], [1, 0, 1]))))
+
+
+curve_points = curve.curve(
+    [(390, 150, 788), (368, 155, 803), (377, 160, 836)], resolution=5)
+offset = curve.offset(curve_points, 10)
+
+for coordinate in offset:
+    editor.placeBlock(coordinate, Block("blue_concrete"))
+
+curve_points = curve.curve(
+    [(390, 150, 788), (368, 155, 803), (377, 160, 836)], resolution=5)
+offset = curve.offset(curve_points, -10)
+
+for coordinate in offset:
+    editor.placeBlock(coordinate, Block("red_concrete"))
+
+for coordinate in curve_points:
+    editor.placeBlock(coordinate, Block("white_concrete"))

networks/Curve.py

@@ -1,42 +1,89 @@
 import numpy as np
+import networks.Segment as segment
 from scipy import interpolate
 
 
-class Curve:
+def curve(target_points, resolution=40):
-    def __init__(self, target_points):
+    """
-        # list of points to [(x1, y1, z1), (...), ...]
+    Returns a list of spaced points that approximate a smooth curve following target_points.
-        self.computed_points = compute_curve(target_points)
 
-    @staticmethod
+    https://stackoverflow.com/questions/18962175/spline-interpolation-coefficients-of-a-line-curve-in-3d-space
-    def compute_curve(self, target_points, resolution=40):
+    """
-        """
+    # Remove duplicates. Curve can't intersect itself
-        Fill self.computed_points with a list of points that approximate a smooth curve following self.target_points.
+    points = tuple(map(tuple, np.array(target_points)))
+    points = sorted(set(points), key=points.index)
 
-        https://stackoverflow.com/questions/18962175/spline-interpolation-coefficients-of-a-line-curve-in-3d-space
+    # Change coordinates structure to (x1, x2, x3, ...), (y1, y2, y3, ...) (z1, z2, z3, ...)
-        """
+    coords = np.array(points, dtype=np.float32)
-        # Remove duplicates. Curve can't intersect itself
+    x = coords[:, 0]
-        points = tuple(map(tuple, np.array(target_points)))
+    y = coords[:, 1]
-        points = sorted(set(points), key=points.index)
+    z = coords[:, 2]
 
-        # Change coordinates structure to (x1, x2, x3, ...), (y1, y2, y3, ...) (z1, z2, z3, ...)
+    # Compute
-        coords = np.array(points, dtype=np.float32)
+    tck, u = interpolate.splprep([x, y, z], s=2, k=2)
-        x = coords[:, 0]
+    x_knots, y_knots, z_knots = interpolate.splev(tck[0], tck)
-        y = coords[:, 1]
+    u_fine = np.linspace(0, 1, resolution)
-        z = coords[:, 2]
+    x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck)
 
-        # Compute
+    x_rounded = np.round(x_fine).astype(int)
-        tck, u = interpolate.splprep([x, y, z], s=2, k=2)
+    y_rounded = np.round(y_fine).astype(int)
-        x_knots, y_knots, z_knots = interpolate.splev(tck[0], tck)
+    z_rounded = np.round(z_fine).astype(int)
-        u_fine = np.linspace(0, 1, resolution)
-        x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck)
 
-        x_rounded = np.round(x_fine).astype(int)
+    return [(x, y, z) for x, y, z in zip(
-        y_rounded = np.round(y_fine).astype(int)
+        x_rounded, y_rounded, z_rounded)]
-        z_rounded = np.round(z_fine).astype(int)
 
-        return [(x, y, z) for x, y, z in zip(
-            x_rounded, y_rounded, z_rounded)]
 
-    @staticmethod
+def curvature(curve):
-    def offset(self):
+    """Get the normal vector at each point of the given points representing the direction in wich the curve is turning.
-        pass
+
+    https://stackoverflow.com/questions/28269379/curve-curvature-in-numpy
+
+    Args:
+        curve (np.array): array of points representing the curve
+
+    Returns:
+        np.array: array of points representing the normal vector at each point in curve array
+
+    >>> curvature(np.array(([0, 0, 0], [0, 0, 1], [1, 0, 1])))
+    [[ 0.92387953 0. -0.38268343]
+    [ 0.70710678 0. -0.70710678]
+    [ 0.38268343 0. -0.92387953]]
+    """
+    curve_points = np.array(curve)
+    dx_dt = np.gradient(curve_points[:, 0])
+    dy_dt = np.gradient(curve_points[:, 1])
+    dz_dt = np.gradient(curve_points[:, 2])
+    velocity = np.array([[dx_dt[i], dy_dt[i], dz_dt[i]]
+                        for i in range(dx_dt.size)])
+
+    ds_dt = np.sqrt(dx_dt * dx_dt + dy_dt * dy_dt + dz_dt * dz_dt)
+
+    tangent = np.array([1/ds_dt]).transpose() * velocity
+    tangent_x = tangent[:, 0]
+    tangent_y = tangent[:, 1]
+    tangent_z = tangent[:, 2]
+
+    deriv_tangent_x = np.gradient(tangent_x)
+    deriv_tangent_y = np.gradient(tangent_y)
+    deriv_tangent_z = np.gradient(tangent_z)
+
+    dT_dt = np.array([[deriv_tangent_x[i], deriv_tangent_y[i], deriv_tangent_z[i]]
+                     for i in range(deriv_tangent_x.size)])
+    length_dT_dt = np.sqrt(
+        deriv_tangent_x * deriv_tangent_x + deriv_tangent_y * deriv_tangent_y + deriv_tangent_z * deriv_tangent_z)
+
+    normal = np.array([1/length_dT_dt]).transpose() * dT_dt
+    return normal
+
+
+def offset(curve, distance):
+    curvature_values = curvature(curve)
+
+    # Offsetting
+    offset_curve = [segment.parallel(
+        (curve[i], curve[i+1]), distance) for i in range(len(curve) - 1)]
+
+    return offset_curve
+
+    # for i in range(1, len(offset_curve)-1):
+    #     pass

networks/Segment.py

@@ -10,10 +10,19 @@ def parallel(segment, distance, normal=np.array([0, 1, 0])):
 
     Returns:
         (np.array(), np.array()): parallel segment.
+
+    >>> parrallel(((0, 0, 0), (0, 0, 10)), 10))
+    (array([-10.,   0.,   0.]), array([-10.,   0.,  10.]))
     """
     return (orthogonal(segment[0], segment[1], distance, normal), orthogonal(segment[1], segment[0], -distance, normal))
 
 
+def normalized(vector):
+    magnitude = np.linalg.norm(vector)
+    normalized_vector = vector / magnitude
+    return normalized_vector
+
+
 def orthogonal(origin, point, distance, normal=np.array([0, 1, 0])):
     """Get orthogonal point from a given one at the specified distance in 3D space with normal direction.
 
@@ -29,16 +38,127 @@ def orthogonal(origin, point, distance, normal=np.array([0, 1, 0])):
     Returns:
         np.array: (x y z)
 
-    >>>orthogonal((5, 5, 5), (150, 5, 5), 10)
+    >>> orthogonal((5, 5, 5), (150, 5, 5), 10)
     [ 5.  5. 15.]
     """
     vector = np.subtract(point, origin)
-    magnitude = np.linalg.norm(vector)
+    normalized_vector = normalized(vector)
-    normalized_vector = vector / magnitude
+    normalized_normal = normalized(normal)
-    orthogonal = np.cross(normalized_vector, normal)
+    orthogonal = np.cross(normalized_vector, normalized_normal)
 
     if np.array_equal(orthogonal, np.zeros((3,))):
         raise ValueError("The input vectors are not linearly independent.")
 
     orthogonal = np.add(np.multiply(orthogonal, distance), origin)
     return orthogonal
+
+
+def discrete_segment(xyz1, xyz2, pixel_perfect=True):
+    """
+    Calculate a line between two points in 3D space.
+
+    https://www.geeksforgeeks.org/bresenhams-algorithm-for-3-d-line-drawing/
+
+    Args:
+        xyz1 (tuple): First coordinates.
+        xyz2 (tuple): Second coordinates.
+        pixel_perfect (bool, optional): If true, remove unnecessary coordinates connecting to other coordinates side by side, leaving only a diagonal connection. Defaults to True.
+
+    Returns:
+        list: List of coordinates.
+    """
+    (x1, y1, z1) = xyz1
+    (x2, y2, z2) = xyz2
+    x1, y1, z1, x2, y2, z2 = (
+        round(x1),
+        round(y1),
+        round(z1),
+        round(x2),
+        round(y2),
+        round(z2),
+    )
+
+    points = []
+    points.append((x1, y1, z1))
+    dx = abs(x2 - x1)
+    dy = abs(y2 - y1)
+    dz = abs(z2 - z1)
+    if x2 > x1:
+        xs = 1
+    else:
+        xs = -1
+    if y2 > y1:
+        ys = 1
+    else:
+        ys = -1
+    if z2 > z1:
+        zs = 1
+    else:
+        zs = -1
+
+    # Driving axis is X-axis
+    if dx >= dy and dx >= dz:
+        p1 = 2 * dy - dx
+        p2 = 2 * dz - dx
+        while x1 != x2:
+            x1 += xs
+            points.append((x1, y1, z1))
+            if p1 >= 0:
+                y1 += ys
+                if not pixel_perfect:
+                    if points[-1][1] != y1:
+                        points.append((x1, y1, z1))
+                p1 -= 2 * dx
+            if p2 >= 0:
+                z1 += zs
+                if not pixel_perfect:
+                    if points[-1][2] != z1:
+                        points.append((x1, y1, z1))
+                p2 -= 2 * dx
+            p1 += 2 * dy
+            p2 += 2 * dz
+
+    # Driving axis is Y-axis
+    elif dy >= dx and dy >= dz:
+        p1 = 2 * dx - dy
+        p2 = 2 * dz - dy
+        while y1 != y2:
+            y1 += ys
+            points.append((x1, y1, z1))
+            if p1 >= 0:
+                x1 += xs
+                if not pixel_perfect:
+                    if points[-1][0] != x1:
+                        points.append((x1, y1, z1))
+                p1 -= 2 * dy
+            if p2 >= 0:
+                z1 += zs
+                if not pixel_perfect:
+                    if points[-1][2] != z1:
+                        points.append((x1, y1, z1))
+                p2 -= 2 * dy
+            p1 += 2 * dx
+            p2 += 2 * dz
+
+    # Driving axis is Z-axis
+    else:
+        p1 = 2 * dy - dz
+        p2 = 2 * dx - dz
+        while z1 != z2:
+            z1 += zs
+            points.append((x1, y1, z1))
+            if p1 >= 0:
+                y1 += ys
+                if not pixel_perfect:
+                    if points[-1][1] != y1:
+                        points.append((x1, y1, z1))
+                p1 -= 2 * dz
+            if p2 >= 0:
+                x1 += xs
+                if not pixel_perfect:
+                    if points[-1][0] != x1:
+                        points.append((x1, y1, z1))
+                p2 -= 2 * dz
+            p1 += 2 * dy
+            p2 += 2 * dx
+    return points