From 28403f83bf59f9b2912896bb0f6333f5efc9371d Mon Sep 17 00:00:00 2001 From: Xeon0X Date: Sat, 20 Apr 2024 22:51:09 +0200 Subject: [PATCH] Add curvature --- main.py | 24 ++++++++- networks/Curve.py | 109 ++++++++++++++++++++++++++----------- networks/Segment.py | 128 ++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 224 insertions(+), 37 deletions(-) diff --git a/main.py b/main.py index f21ea28..8bd89af 100644 --- a/main.py +++ b/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")) diff --git a/networks/Curve.py b/networks/Curve.py index 74bdbcc..99d698a 100644 --- a/networks/Curve.py +++ b/networks/Curve.py @@ -1,42 +1,89 @@ import numpy as np +import networks.Segment as segment from scipy import interpolate -class Curve: - def __init__(self, target_points): - # list of points to [(x1, y1, z1), (...), ...] - self.computed_points = compute_curve(target_points) +def curve(target_points, resolution=40): + """ + Returns a list of spaced points that approximate a smooth curve following target_points. - @staticmethod - def compute_curve(self, target_points, resolution=40): - """ - Fill self.computed_points with a list of points that approximate a smooth curve following self.target_points. + https://stackoverflow.com/questions/18962175/spline-interpolation-coefficients-of-a-line-curve-in-3d-space + """ + # Remove duplicates. Curve can't intersect itself + 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 - """ - # Remove duplicates. Curve can't intersect itself - points = tuple(map(tuple, np.array(target_points))) - points = sorted(set(points), key=points.index) + # Change coordinates structure to (x1, x2, x3, ...), (y1, y2, y3, ...) (z1, z2, z3, ...) + coords = np.array(points, dtype=np.float32) + x = coords[:, 0] + y = coords[:, 1] + z = coords[:, 2] - # Change coordinates structure to (x1, x2, x3, ...), (y1, y2, y3, ...) (z1, z2, z3, ...) - coords = np.array(points, dtype=np.float32) - x = coords[:, 0] - y = coords[:, 1] - z = coords[:, 2] + # Compute + tck, u = interpolate.splprep([x, y, z], s=2, k=2) + x_knots, y_knots, z_knots = interpolate.splev(tck[0], tck) + u_fine = np.linspace(0, 1, resolution) + x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck) - # Compute - tck, u = interpolate.splprep([x, y, z], s=2, k=2) - x_knots, y_knots, z_knots = interpolate.splev(tck[0], tck) - 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) + y_rounded = np.round(y_fine).astype(int) + z_rounded = np.round(z_fine).astype(int) - x_rounded = np.round(x_fine).astype(int) - y_rounded = np.round(y_fine).astype(int) - z_rounded = np.round(z_fine).astype(int) + return [(x, y, z) for x, y, z in zip( + x_rounded, y_rounded, z_rounded)] - return [(x, y, z) for x, y, z in zip( - x_rounded, y_rounded, z_rounded)] - @staticmethod - def offset(self): - pass +def curvature(curve): + """Get the normal vector at each point of the given points representing the direction in wich the curve is turning. + + 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 diff --git a/networks/Segment.py b/networks/Segment.py index a0cd8e6..6da98f5 100644 --- a/networks/Segment.py +++ b/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 = vector / magnitude - orthogonal = np.cross(normalized_vector, normal) + normalized_vector = normalized(vector) + normalized_normal = normalized(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