Persson-dev/GDMC-2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add curvature

Browse Source
This commit is contained in:
Xeon0X
2024-04-20 22:51:09 +02:00
parent d218a67f42
commit 28403f83bf
3 changed files with 224 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
networks/Curve.py
View File

@@ -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

128
networks/Segment.py
View File

@@ -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