1156 lines
35 KiB
Python
1156 lines
35 KiB
Python
from math import sqrt
|
||
from math import pi
|
||
from math import cos, sin
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
from scipy import interpolate
|
||
|
||
|
||
def line(xyz1, xyz2, pixelPerfect=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.
|
||
pixelPerfect (bool, optional): Blocks will be placed diagonally,
|
||
not side by side if pixelPerfect is True. Defaults to True.
|
||
|
||
Returns:
|
||
list: List of blocks.
|
||
"""
|
||
(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),
|
||
)
|
||
|
||
ListOfPoints = []
|
||
ListOfPoints.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
|
||
ListOfPoints.append((x1, y1, z1))
|
||
if p1 >= 0:
|
||
y1 += ys
|
||
if not pixelPerfect:
|
||
if ListOfPoints[-1][1] != y1:
|
||
ListOfPoints.append((x1, y1, z1))
|
||
p1 -= 2 * dx
|
||
if p2 >= 0:
|
||
z1 += zs
|
||
if not pixelPerfect:
|
||
if ListOfPoints[-1][2] != z1:
|
||
ListOfPoints.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
|
||
ListOfPoints.append((x1, y1, z1))
|
||
if p1 >= 0:
|
||
x1 += xs
|
||
if not pixelPerfect:
|
||
if ListOfPoints[-1][0] != x1:
|
||
ListOfPoints.append((x1, y1, z1))
|
||
p1 -= 2 * dy
|
||
if p2 >= 0:
|
||
z1 += zs
|
||
if not pixelPerfect:
|
||
if ListOfPoints[-1][2] != z1:
|
||
ListOfPoints.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
|
||
ListOfPoints.append((x1, y1, z1))
|
||
if p1 >= 0:
|
||
y1 += ys
|
||
if not pixelPerfect:
|
||
if ListOfPoints[-1][1] != y1:
|
||
ListOfPoints.append((x1, y1, z1))
|
||
p1 -= 2 * dz
|
||
if p2 >= 0:
|
||
x1 += xs
|
||
if not pixelPerfect:
|
||
if ListOfPoints[-1][0] != x1:
|
||
ListOfPoints.append((x1, y1, z1))
|
||
p2 -= 2 * dz
|
||
p1 += 2 * dy
|
||
p2 += 2 * dx
|
||
return ListOfPoints
|
||
|
||
|
||
def offset(distance, xy1, xy2):
|
||
"""
|
||
Compute the coordinates of perpendicular points from two points. 2D
|
||
only.
|
||
|
||
Args:
|
||
distance (int): Distance from the line[xy1;xy2] of the
|
||
perpendicular points.
|
||
xy1 (tuple): First position.
|
||
xy2 (tuple): Second position.
|
||
|
||
Returns:
|
||
tuple: The coordinates of perpendicular points.
|
||
A: Perpendicular from [xy1;xy2] at distance from pos1.
|
||
B: perpendicular from [xy1;xy2] at -distance from pos1.
|
||
C: perpendicular from [xy2;xy1] at distance from pos2.
|
||
D: perpendicular from [xy2;xy1] at -distance from pos2.
|
||
"""
|
||
A, B = perpendicular(distance * 2, xy1, xy2)
|
||
C, D = perpendicular(distance * 2, xy2, xy1)
|
||
return ([A, D], [B, C])
|
||
|
||
|
||
def perpendicular(distance, xy1, xy2):
|
||
"""
|
||
Return a tuple of the perpendicular coordinates.
|
||
|
||
Args:
|
||
distance (int): Distance from the line[xy1;xy2].
|
||
xy1 (tuple): First coordinates.
|
||
xy2 (tuple): Second coordinates.
|
||
|
||
Returns:
|
||
tuple: Coordinates of the line length distance, perpendicular
|
||
to [xy1; xy2] at xy1.
|
||
"""
|
||
(x1, y1) = xy1
|
||
(x2, y2) = xy2
|
||
dx = x1 - x2
|
||
dy = y1 - y2
|
||
dist = sqrt(dx * dx + dy * dy)
|
||
dx /= dist
|
||
dy /= dist
|
||
x3 = x1 + (distance / 2) * dy
|
||
y3 = y1 - (distance / 2) * dx
|
||
x4 = x1 - (distance / 2) * dy
|
||
y4 = y1 + (distance / 2) * dx
|
||
return ((round(x3), round(y3)), (round(x4), round(y4)))
|
||
|
||
|
||
def curve(points, number_true_pts=40, debug=False):
|
||
"""
|
||
Returns a 3d curve.
|
||
|
||
https://stackoverflow.com/questions/18962175/spline-interpolation-coefficients-of-a-line-curve-in-3d-space
|
||
|
||
Args:
|
||
points (np.array): Points where the curves should pass.
|
||
number_true_pts (int, optional): Number of points to compute. Defaults to 40.
|
||
debug (bool, optional): Just a visual graphic. Defaults to False.
|
||
|
||
Returns:
|
||
tuple: Tuple of list of each coordinate.
|
||
"""
|
||
# Remove duplicates.
|
||
points = tuple(map(tuple, points))
|
||
points = sorted(set(points), key=points.index)
|
||
|
||
x_sample = []
|
||
y_sample = []
|
||
z_sample = []
|
||
|
||
for i in range(len(points)):
|
||
x_sample.append(points[i][0])
|
||
z_sample.append(points[i][1])
|
||
y_sample.append(points[i][2])
|
||
|
||
x_sample = np.array(x_sample)
|
||
y_sample = np.array(y_sample)
|
||
z_sample = np.array(z_sample)
|
||
|
||
tck, u = interpolate.splprep([x_sample, y_sample, z_sample], s=2, k=2)
|
||
x_knots, y_knots, z_knots = interpolate.splev(tck[0], tck)
|
||
u_fine = np.linspace(0, 1, number_true_pts)
|
||
x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck)
|
||
|
||
if debug:
|
||
fig2 = plt.figure(2)
|
||
ax3d = fig2.add_subplot(111, projection="3d")
|
||
ax3d.plot(x_sample, y_sample, z_sample, "r*")
|
||
ax3d.plot(x_knots, y_knots, z_knots, "go")
|
||
ax3d.plot(x_fine, y_fine, z_fine, "r")
|
||
fig2.show()
|
||
plt.show()
|
||
|
||
x = x_fine.tolist()
|
||
z = y_fine.tolist()
|
||
y = z_fine.tolist()
|
||
|
||
for i in x:
|
||
i = round(i)
|
||
for i in y:
|
||
i = round(i)
|
||
for i in z:
|
||
i = round(i)
|
||
|
||
return x, y, z
|
||
|
||
|
||
def curveOffset(x, y, z, distance=5):
|
||
"""
|
||
Offset a curve.
|
||
|
||
Args:
|
||
x (list): List of x coordinates.
|
||
y (list): List of y coordinates.
|
||
z (list): List of z coordinates.
|
||
distance (int, optional): Distance of offsetting. Defaults to 5.
|
||
|
||
Returns:
|
||
tuple: Lists of points from the upper curve and the lower curve.
|
||
|
||
TODO:
|
||
The accuracy can be improved by finding the inner and outer arc:
|
||
connect the points of the arc and not calculate their
|
||
middle.
|
||
"""
|
||
lineA = []
|
||
lineB = []
|
||
line0 = []
|
||
line1 = []
|
||
|
||
# Offsetting
|
||
for i in range(len(x) - 1):
|
||
parallel = offset(distance, (x[i], z[i]), (x[i + 1], z[i + 1]))
|
||
lineA.append(
|
||
(
|
||
(parallel[0][0][0], y[i], parallel[0][0][1]),
|
||
(parallel[0][1][0], y[i + 1], parallel[0][1][1]),
|
||
)
|
||
)
|
||
lineB.append(
|
||
(
|
||
(parallel[1][0][0], y[i], parallel[1][0][1]),
|
||
(parallel[1][1][0], y[i + 1], parallel[1][1][1]),
|
||
)
|
||
)
|
||
|
||
# First points
|
||
# print(x, y, z, distance)
|
||
# print(x, len(x))
|
||
# print("lineA:", lineA)
|
||
# print("parallel:", parallel)
|
||
line0.append(
|
||
(
|
||
round(lineA[0][0][0]),
|
||
round(lineA[0][0][1]),
|
||
round(lineA[0][0][2]),
|
||
)
|
||
)
|
||
line1.append(
|
||
(
|
||
round(lineB[0][0][0]),
|
||
round(lineB[0][0][1]),
|
||
round(lineB[0][0][2]),
|
||
)
|
||
)
|
||
|
||
# Middle of between segments
|
||
for i in range(len(lineA) - 1):
|
||
line0.append(
|
||
(
|
||
round((lineA[i][1][0] + lineA[i + 1][0][0]) / 2),
|
||
round((lineA[i][1][1] + lineA[i + 1][0][1]) / 2),
|
||
round((lineA[i][1][2] + lineA[i + 1][0][2]) / 2),
|
||
)
|
||
)
|
||
line1.append(
|
||
(
|
||
round((lineB[i][1][0] + lineB[i + 1][0][0]) / 2),
|
||
round((lineB[i][1][1] + lineB[i + 1][0][1]) / 2),
|
||
round((lineB[i][1][2] + lineB[i + 1][0][2]) / 2),
|
||
)
|
||
)
|
||
|
||
# Last points
|
||
line0.append(
|
||
(
|
||
round(lineA[-1][1][0]),
|
||
round(lineA[-1][1][1]),
|
||
round(lineA[-1][1][2]),
|
||
)
|
||
)
|
||
line1.append(
|
||
(
|
||
round(lineB[-1][1][0]),
|
||
round(lineB[-1][1][1]),
|
||
round(lineB[-1][1][2]),
|
||
)
|
||
)
|
||
|
||
return line0, line1
|
||
|
||
|
||
def pixelPerfect(path):
|
||
"""
|
||
Remove blocks that are side by side in the path. Keep the blocks
|
||
that are in diagonal.
|
||
|
||
Args:
|
||
path (list): List of coordinates from a path.
|
||
|
||
Returns:
|
||
list: List cleaned.
|
||
|
||
TODO:
|
||
Add 3D.
|
||
"""
|
||
# NotPixelPerfect detection
|
||
if len(path) == 1 or len(path) == 0:
|
||
return path
|
||
else:
|
||
notPixelPerfect = []
|
||
c = 0
|
||
while c < len(path):
|
||
if c > 0 and c + 1 < len(path):
|
||
if (
|
||
(
|
||
path[c - 1][0] == path[c][0]
|
||
or path[c - 1][1] == path[c][1]
|
||
)
|
||
and (
|
||
path[c + 1][0] == path[c][0]
|
||
or path[c + 1][1] == path[c][1]
|
||
)
|
||
and path[c - 1][1] != path[c + 1][1]
|
||
and path[c - 1][0] != path[c + 1][0]
|
||
):
|
||
notPixelPerfect.append(path[c])
|
||
c += 1
|
||
|
||
# Double notPixelPerfect detection
|
||
if len(notPixelPerfect) == 1 or len(notPixelPerfect) == 0:
|
||
return notPixelPerfect
|
||
else:
|
||
d = 0
|
||
while d < len(notPixelPerfect):
|
||
if d + 1 < len(notPixelPerfect):
|
||
if (
|
||
notPixelPerfect[d][0] == notPixelPerfect[d + 1][0]
|
||
and (notPixelPerfect[d][1] - notPixelPerfect[d + 1][1])
|
||
in {1, -1}
|
||
) or (
|
||
notPixelPerfect[d][1] == notPixelPerfect[d + 1][1]
|
||
and (notPixelPerfect[d][0] - notPixelPerfect[d + 1][0])
|
||
in {1, -1}
|
||
):
|
||
notPixelPerfect.remove(notPixelPerfect[d + 1])
|
||
d += 1
|
||
|
||
# Remove notPixelPerfect from path
|
||
for i in range(len(notPixelPerfect)):
|
||
path.remove(notPixelPerfect[i])
|
||
|
||
return path
|
||
|
||
|
||
def cleanLine(path): # HERE
|
||
"""
|
||
Clean and smooth a list of blocks. Works in 2d but supports 3d.
|
||
|
||
Args:
|
||
path (list): List of blocks.
|
||
|
||
Returns:
|
||
list: List cleaned.
|
||
|
||
TODO:
|
||
Do not work perfectly since 16/04/2021.
|
||
Add new patterns.
|
||
Problem with i -= 10 : solved but not understand why.
|
||
16/04/2021.
|
||
"""
|
||
|
||
pathTemp = []
|
||
for i in path:
|
||
if i not in pathTemp:
|
||
pathTemp.append(i)
|
||
path = pathTemp
|
||
|
||
i = 0
|
||
|
||
while i < len(path):
|
||
|
||
# 2 blocks, 90 degrees, 2 blocks = 1 block, 1 block, 1 block
|
||
if i + 3 < len(path):
|
||
if (
|
||
path[i][0] == path[i + 1][0]
|
||
and path[i + 2][-1] == path[i + 3][-1]
|
||
):
|
||
if len(path[i + 1]) == 3:
|
||
path.insert(
|
||
(i + 1),
|
||
(path[i + 2][0], path[i + 2][1], path[i + 1][-1]),
|
||
)
|
||
else:
|
||
path.insert((i + 1), (path[i + 2][0], path[i + 1][-1]))
|
||
del path[i + 2] # 2nd block
|
||
del path[i + 2] # 3rd block
|
||
i -= 1
|
||
continue
|
||
elif (
|
||
path[i][-1] == path[i + 1][-1]
|
||
and path[i + 2][0] == path[i + 3][0]
|
||
):
|
||
if len(path[i + 1]) == 3:
|
||
path.insert(
|
||
(i + 1),
|
||
(path[i + 1][0], path[i + 1][1], path[i + 2][-1]),
|
||
)
|
||
else:
|
||
path.insert(
|
||
(i + 1),
|
||
(path[i + 1][0], path[i + 2][-1]),
|
||
)
|
||
del path[i + 2] # 2nd block
|
||
del path[i + 2] # 3rd block
|
||
i -= 1
|
||
continue
|
||
|
||
# 1 block, 3 blocks, 1 block = 1 block, 2 blocks, 2 blocks
|
||
if i - 1 >= 0 and i + 5 <= len(path):
|
||
if (
|
||
(
|
||
path[i + 1][-1] == path[i + 2][-1]
|
||
and path[i + 2][-1] == path[i + 3][-1]
|
||
)
|
||
and (
|
||
path[i + 1][-1] != path[i][-1]
|
||
and path[i + 3][-1] != path[i + 4][-1]
|
||
)
|
||
and (
|
||
path[i - 1][-1] != path[i][-1]
|
||
and path[i + 4][-1] != path[i + 5][-1]
|
||
)
|
||
):
|
||
if len(path[i]) == 3:
|
||
path.insert(
|
||
(i + 1), (path[i + 1][0], path[i + 1][1], path[i][-1])
|
||
)
|
||
else:
|
||
path.insert((i + 1), (path[i + 1][0], path[i][-1]))
|
||
del path[i + 2] # 2nd block
|
||
i -= 1
|
||
continue
|
||
elif (
|
||
(
|
||
path[i + 1][0] == path[i + 2][0]
|
||
and path[i + 2][0] == path[i + 3][0]
|
||
)
|
||
and (
|
||
path[i + 1][0] != path[i][0]
|
||
and path[i + 3][0] != path[i + 4][0]
|
||
)
|
||
and (
|
||
path[i - 1][0] != path[i][0]
|
||
and path[i + 4][0] != path[i + 5][0]
|
||
)
|
||
):
|
||
if len(path[i]) == 3:
|
||
path.insert(
|
||
(i + 1), (path[i][0], path[i][1], path[i + 1][-1])
|
||
)
|
||
else:
|
||
path.insert((i + 1), (path[i][0], path[i + 1][-1]))
|
||
del path[i + 2] # 2nd block
|
||
i -= 1
|
||
continue
|
||
|
||
i += 1
|
||
|
||
return path
|
||
|
||
|
||
def distance2D(A, B): # TODO : Can be better.
|
||
return sqrt((B[0] - A[0]) ** 2 + (B[1] - A[1]) ** 2)
|
||
|
||
|
||
def curveSurface(
|
||
points,
|
||
distance,
|
||
resolution=7,
|
||
pixelPerfect=False,
|
||
factor=2,
|
||
start=0,
|
||
returnLine=True,
|
||
): # HERE
|
||
"""
|
||
Create a curve with a thickness.
|
||
|
||
Args:
|
||
points (numpy.ndarray): Points where the curve should go.
|
||
distance (int): Thickness.
|
||
resolution (int, optional): Number of blocks that separate each
|
||
point to calculate parallel curves. 0 to use the points
|
||
calculated to create the curve. Defaults to 7.
|
||
pixelPerfect (bool, optional): True to avoid heaps. Defaults to
|
||
False.
|
||
factor (int, optional): Number of sub-line that will be
|
||
calculated to avoid hole with coordinates. Defaults to 2.
|
||
|
||
Returns:
|
||
dict: Key 0 is the list of coordinates of the center line.
|
||
Positive keys are lists of coordinates of lines on the right
|
||
side, negative keys are for the left side.
|
||
|
||
>>> curveSurface(
|
||
np.array(
|
||
[
|
||
[12, 248, -103],
|
||
[-5, 219, -85],
|
||
[-22, 205, -128],
|
||
[-51, 70, -240],
|
||
[40, 198, -166],
|
||
[19, 241, -102],
|
||
[-6, 62, -223],
|
||
]
|
||
),
|
||
5,
|
||
resolution=7,
|
||
)
|
||
"""
|
||
if len((points)) >= 3:
|
||
# Calculate resolution of the main curve depending of the total curve length.
|
||
lenCurve = 0
|
||
for i in range(len(points) - 1):
|
||
lenCurve += sqrt(
|
||
((points[i][0] - points[i + 1][0]) ** 2)
|
||
+ ((points[i][1] - points[i + 1][1]) ** 2)
|
||
+ ((points[i][2] - points[i + 1][2]) ** 2)
|
||
)
|
||
number_true_pts = round(lenCurve / 6)
|
||
|
||
# Calculate the main line.
|
||
X, Y, Z = curve(points, number_true_pts)
|
||
if len(X) < 2:
|
||
X, Y, Z = (
|
||
(points[0][0], points[1][0]),
|
||
(points[0][1], points[1][1]),
|
||
(points[0][2], points[1][2]),
|
||
)
|
||
else:
|
||
X, Y, Z = (
|
||
(points[0][0], points[1][0]),
|
||
(points[0][1], points[1][1]),
|
||
(points[0][2], points[1][2]),
|
||
)
|
||
|
||
centerLineTemp = []
|
||
for i in range(len(X) - 1):
|
||
xyz0 = X[i], Y[i], Z[i]
|
||
xyz1 = (X[i + 1], Y[i + 1], Z[i + 1])
|
||
centerLineTemp.extend(line(xyz0, xyz1))
|
||
|
||
if not returnLine:
|
||
returnPoints = []
|
||
for i in range(len(X)):
|
||
returnPoints.append((round(X[i]), round(Y[i]), round(Z[i])))
|
||
|
||
# Clean the main line.
|
||
# centerLine = cleanLine(centerLineTemp)
|
||
centerLine = centerLineTemp
|
||
|
||
# Offset.
|
||
centerPoints = []
|
||
|
||
if resolution != 0:
|
||
for i in range(0, len(centerLine), resolution):
|
||
centerPoints.append(centerLine[i])
|
||
else:
|
||
for i in range(len(X)):
|
||
centerPoints.append((X[i], Y[i], Z[i]))
|
||
|
||
X = [centerPoints[i][0] for i in range(len(centerPoints))]
|
||
Y = [centerPoints[i][1] for i in range(len(centerPoints))]
|
||
Z = [centerPoints[i][2] for i in range(len(centerPoints))]
|
||
|
||
rightPoints = []
|
||
leftPoints = []
|
||
|
||
# print(centerLine, "XYZ centerPoint to offset")
|
||
# print(cleanLine(centerLineTemp), "with cleanLine")
|
||
for i in range(start * factor, distance * factor):
|
||
rightPoint, leftPoint = curveOffset(X, Y, Z, i / factor)
|
||
rightPoints.append(rightPoint)
|
||
leftPoints.append(leftPoint)
|
||
|
||
rightLine = []
|
||
leftLine = []
|
||
rightSide = []
|
||
leftSide = []
|
||
|
||
if returnLine == True: # Creating lines on each side between each point.
|
||
for i in range(len(rightPoints)):
|
||
for j in range(len(rightPoints[i]) - 1):
|
||
rightLine.extend(
|
||
line(
|
||
rightPoints[i][j],
|
||
rightPoints[i][j + 1],
|
||
pixelPerfect,
|
||
)
|
||
)
|
||
rightSide.append(rightLine)
|
||
rightLine = []
|
||
|
||
for i in range(len(leftPoints)):
|
||
for j in range(len(leftPoints[i]) - 1):
|
||
leftLine.extend(
|
||
line(
|
||
leftPoints[i][j],
|
||
leftPoints[i][j + 1],
|
||
pixelPerfect,
|
||
)
|
||
)
|
||
leftSide.append(leftLine)
|
||
leftLine = []
|
||
|
||
else: # Do not create lines. Points instead.
|
||
for i in range(len(rightPoints)):
|
||
for j in range(len(rightPoints[i])):
|
||
rightLine.append(rightPoints[i][j])
|
||
rightSide.append(rightLine)
|
||
rightLine = []
|
||
|
||
for i in range(len(leftPoints)):
|
||
for j in range(len(leftPoints[i])):
|
||
leftLine.append(leftPoints[i][j])
|
||
leftSide.append(leftLine)
|
||
leftLine = []
|
||
|
||
# Returns. 0 is the center line, positive values are lines on the
|
||
# right, negative values are lines on the left.
|
||
smoothCurveSurfaceDict = {}
|
||
if returnLine:
|
||
smoothCurveSurfaceDict[0] = centerLine
|
||
else:
|
||
smoothCurveSurfaceDict[0] = returnPoints
|
||
|
||
countLine = 0
|
||
for l in rightSide:
|
||
# l = cleanLine(l)
|
||
countLine += 1
|
||
smoothCurveSurfaceDict[countLine] = l
|
||
countLine = 0
|
||
for l in leftSide:
|
||
# l = cleanLine(l)
|
||
countLine -= 1
|
||
smoothCurveSurfaceDict[countLine] = l
|
||
|
||
return smoothCurveSurfaceDict
|
||
|
||
|
||
def getAngle(xy0, xy1, xy2):
|
||
"""
|
||
Compute angle (in degrees) for xy0, xy1, xy2 corner.
|
||
|
||
https://stackoverflow.com/questions/13226038/calculating-angle-between-two-vectors-in-python
|
||
|
||
Args:
|
||
xy0 (numpy.ndarray): Points in the form of [x,y].
|
||
xy1 (numpy.ndarray): Points in the form of [x,y].
|
||
xy2 (numpy.ndarray): Points in the form of [x,y].
|
||
|
||
Returns:
|
||
float: Angle negative for counterclockwise angle, angle positive
|
||
for counterclockwise angle.
|
||
"""
|
||
if xy2 is None:
|
||
xy2 = xy1 + np.array([1, 0])
|
||
v0 = np.array(xy0) - np.array(xy1)
|
||
v1 = np.array(xy2) - np.array(xy1)
|
||
|
||
angle = np.math.atan2(np.linalg.det([v0, v1]), np.dot(v0, v1))
|
||
return np.degrees(angle)
|
||
|
||
|
||
def lineIntersection(line0, line1, fullLine=True):
|
||
"""
|
||
Find (or not) intersection between two lines. Works in 2d but
|
||
supports 3d.
|
||
|
||
https://stackoverflow.com/questions/20677795/how-do-i-compute-the-intersection-point-of-two-lines
|
||
|
||
Args:
|
||
line0 (tuple): Tuple of tuple of coordinates.
|
||
line1 (tuple): Tuple of tuple of coordinates.
|
||
fullLine (bool, optional): True to find intersections along
|
||
full line - not just in the segment.
|
||
|
||
Returns:
|
||
tuple: Coordinates (2d).
|
||
|
||
>>> lineIntersection(((0, 0), (0, 5)), ((2.5, 2.5), (-2.5, 2.5)))
|
||
"""
|
||
xdiff = (line0[0][0] - line0[1][0], line1[0][0] - line1[1][0])
|
||
ydiff = (line0[0][-1] - line0[1][-1], line1[0][-1] - line1[1][-1])
|
||
|
||
def det(a, b):
|
||
return a[0] * b[-1] - a[-1] * b[0]
|
||
|
||
div = det(xdiff, ydiff)
|
||
if div == 0:
|
||
return None
|
||
|
||
d = (det(*line0), det(*line1))
|
||
x = det(d, xdiff) / div
|
||
y = det(d, ydiff) / div
|
||
|
||
if not fullLine:
|
||
if (
|
||
min(line0[0][0], line0[1][0]) <= x <= max(line0[0][0], line0[1][0])
|
||
and min(line1[0][0], line1[1][0])
|
||
<= x
|
||
<= max(line1[0][0], line1[1][0])
|
||
and min(line0[0][-1], line0[1][-1])
|
||
<= y
|
||
<= max(line0[0][-1], line0[1][-1])
|
||
and min(line1[0][-1], line1[1][-1])
|
||
<= y
|
||
<= max(line1[0][-1], line1[1][-1])
|
||
):
|
||
return x, y
|
||
else:
|
||
return None
|
||
else:
|
||
return x, y
|
||
|
||
|
||
def circleLineSegmentIntersection(
|
||
circleCenter, circleRadius, xy0, xy1, fullLine=True, tangentTol=1e-9
|
||
):
|
||
"""
|
||
Find the points at which a circle intersects a line-segment. This
|
||
can happen at 0, 1, or 2 points. Works in 2d but supports 3d.
|
||
|
||
https://stackoverflow.com/questions/30844482/what-is-most-efficient-way-to-find-the-intersection-of-a-line-and-a-circle-in-py
|
||
Note: We follow: http://mathworld.wolfram.com/Circle-LineIntersection.html
|
||
|
||
Args:
|
||
circleCenter (tuple): The (x, y) location of the circle center.
|
||
circleRadius (int): The radius of the circle.
|
||
xy0 (tuple): The (x, y) location of the first point of the
|
||
segment.
|
||
xy1 ([tuple]): The (x, y) location of the second point of the
|
||
segment.
|
||
fullLine (bool, optional): True to find intersections along
|
||
full line - not just in the segment. False will just return
|
||
intersections within the segment. Defaults to True.
|
||
tangentTol (float, optional): Numerical tolerance at which we
|
||
decide the intersections are close enough to consider it a
|
||
tangent. Defaults to 1e-9.
|
||
|
||
Returns:
|
||
list: A list of length 0, 1, or 2, where each element is a point
|
||
at which the circle intercepts a line segment (2d).
|
||
"""
|
||
|
||
(p1x, p1y), (p2x, p2y), (cx, cy) = (
|
||
(xy0[0], xy0[-1]),
|
||
(xy1[0], xy1[-1]),
|
||
(circleCenter[0], circleCenter[1]),
|
||
)
|
||
(x1, y1), (x2, y2) = (p1x - cx, p1y - cy), (p2x - cx, p2y - cy)
|
||
dx, dy = (x2 - x1), (y2 - y1)
|
||
dr = (dx ** 2 + dy ** 2) ** 0.5
|
||
big_d = x1 * y2 - x2 * y1
|
||
discriminant = circleRadius ** 2 * dr ** 2 - big_d ** 2
|
||
|
||
if discriminant < 0: # No intersection between circle and line
|
||
return []
|
||
else: # There may be 0, 1, or 2 intersections with the segment
|
||
intersections = [
|
||
(
|
||
cx
|
||
+ (
|
||
big_d * dy
|
||
+ sign * (-1 if dy < 0 else 1) * dx * discriminant ** 0.5
|
||
)
|
||
/ dr ** 2,
|
||
cy
|
||
+ (-big_d * dx + sign * abs(dy) * discriminant ** 0.5)
|
||
/ dr ** 2,
|
||
)
|
||
for sign in ((1, -1) if dy < 0 else (-1, 1))
|
||
] # This makes sure the order along the segment is correct
|
||
if (
|
||
not fullLine
|
||
): # If only considering the segment, filter out intersections that do not fall within the segment
|
||
fraction_along_segment = [
|
||
(xi - p1x) / dx if abs(dx) > abs(dy) else (yi - p1y) / dy
|
||
for xi, yi in intersections
|
||
]
|
||
intersections = [
|
||
pt
|
||
for pt, frac in zip(intersections, fraction_along_segment)
|
||
if 0 <= frac <= 1
|
||
]
|
||
if (
|
||
len(intersections) == 2 and abs(discriminant) <= tangentTol
|
||
): # If line is tangent to circle, return just one point (as both intersections have same location)
|
||
return [intersections[0]]
|
||
else:
|
||
return intersections
|
||
|
||
|
||
def circle(xyC, r):
|
||
"""
|
||
Can be used for circle or disc.
|
||
|
||
Args:
|
||
xyC (tuple): Coordinates of the center.
|
||
r (int): Radius of the circle.
|
||
|
||
Returns:
|
||
dict: Keys are distance from the circle. Value is a list of all
|
||
coordinates at this distance. 0 for a circle. Negative values
|
||
for a disc, positive values for a hole.
|
||
"""
|
||
area = (
|
||
(round(xyC[0]) - round(r), round(xyC[1]) - round(r)),
|
||
(round(xyC[0]) + round(r) + 1, round(xyC[1]) + round(r) + 1),
|
||
)
|
||
|
||
circle = {}
|
||
for x in range(area[0][0], area[1][0]):
|
||
for y in range(area[0][1], area[1][1]):
|
||
d = round(distance2D((x, y), (xyC))) - r
|
||
if circle.get(d) == None:
|
||
circle[d] = []
|
||
circle[d].append((x, y))
|
||
return circle
|
||
|
||
|
||
def circleIntersections(xy0, r0, xy1, r1):
|
||
# https://stackoverflow.com/questions/55816902/finding-the-intersection-of-two-circles
|
||
|
||
x0, y0 = xy0
|
||
x1, y1 = xy1
|
||
d = sqrt((x1 - x0) ** 2 + (y1 - y0) ** 2)
|
||
|
||
# Non intersecting.
|
||
if d > r0 + r1:
|
||
return None
|
||
# One circle within other.
|
||
if d < abs(r0 - r1):
|
||
return None
|
||
# Coincident circles.
|
||
if d == 0 and r0 == r1:
|
||
return None
|
||
else:
|
||
a = (r0 ** 2 - r1 ** 2 + d ** 2) / (2 * d)
|
||
h = sqrt(r0 ** 2 - a ** 2)
|
||
x2 = x0 + a * (x1 - x0) / d
|
||
y2 = y0 + a * (y1 - y0) / d
|
||
x3 = x2 + h * (y1 - y0) / d
|
||
y3 = y2 - h * (x1 - x0) / d
|
||
|
||
x4 = x2 - h * (y1 - y0) / d
|
||
y4 = y2 + h * (x1 - x0) / d
|
||
|
||
return ((x3, y3), (x4, y4))
|
||
|
||
|
||
def InTriangle(point, xy0, xy1, xy2):
|
||
# https://stackoverflow.com/questions/2049582/how-to-determine-if-a-point-is-in-a-2d-triangle#:~:text=A%20simple%20way%20is%20to,point%20is%20inside%20the%20triangle.
|
||
dX = point[0] - xy0[0]
|
||
dY = point[1] - xy0[1]
|
||
dX20 = xy2[0] - xy0[0]
|
||
dY20 = xy2[1] - xy0[1]
|
||
dX10 = xy1[0] - xy0[0]
|
||
dY10 = xy1[1] - xy0[1]
|
||
|
||
s_p = (dY20 * dX) - (dX20 * dY)
|
||
t_p = (dX10 * dY) - (dY10 * dX)
|
||
D = (dX10 * dY20) - (dY10 * dX20)
|
||
|
||
if D > 0:
|
||
return (s_p >= 0) and (t_p >= 0) and (s_p + t_p) <= D
|
||
else:
|
||
return (s_p <= 0) and (t_p <= 0) and (s_p + t_p) >= D
|
||
|
||
|
||
def circlePoints(xyC, r, n=100):
|
||
# https://stackoverflow.com/questions/8487893/generate-all-the-points-on-the-circumference-of-a-circle
|
||
points = [
|
||
(cos(2 * pi / n * x) * r, sin(2 * pi / n * x) * r)
|
||
for x in range(0, n + 1)
|
||
]
|
||
|
||
for i in range(len(points)):
|
||
points[i] = (
|
||
points[i][0] + xyC[0],
|
||
points[i][1] + xyC[1],
|
||
)
|
||
|
||
return points
|
||
|
||
|
||
def optimizedPath(coords, start=None):
|
||
# https://stackoverflow.com/questions/45829155/sort-points-in-order-to-have-a-continuous-curve-using-python
|
||
if start is None:
|
||
start = coords[0]
|
||
pass_by = coords
|
||
path = [start]
|
||
pass_by.remove(start)
|
||
while pass_by:
|
||
nearest = min(pass_by, key=lambda x: distance2D(path[-1], x))
|
||
path.append(nearest)
|
||
pass_by.remove(nearest)
|
||
return path
|
||
|
||
|
||
def nearest(points, start):
|
||
return min(points, key=lambda x: distance2D(start, x))
|
||
|
||
|
||
def sortRotation(points):
|
||
"""
|
||
Sort point in a rotation order. Works in 2d but supports 3d.
|
||
|
||
https://stackoverflow.com/questions/58377015/counterclockwise-sorting-of-x-y-data
|
||
|
||
Args:
|
||
points: List of points to sort in the form of [(x, y, z), (x, y,
|
||
z)] or [(x, y), (x, y), (x, y), (x, y)]...
|
||
|
||
Returns:
|
||
list: List of tuples of coordinates sorted (2d or 3d).
|
||
|
||
>>> sortRotation([(0, 45, 100), (4, -5, 5),(-5, 36, -2)])
|
||
[(0, 45, 100), (-5, 36, -2), (4, -5, 5)]
|
||
"""
|
||
x, y = [], []
|
||
for i in range(len(points)):
|
||
x.append(points[i][0])
|
||
y.append(points[i][-1])
|
||
x, y = np.array(x), np.array(y)
|
||
|
||
x0 = np.mean(x)
|
||
y0 = np.mean(y)
|
||
|
||
r = np.sqrt((x - x0) ** 2 + (y - y0) ** 2)
|
||
|
||
angles = np.where(
|
||
(y - y0) > 0,
|
||
np.arccos((x - x0) / r),
|
||
2 * np.pi - np.arccos((x - x0) / r),
|
||
)
|
||
|
||
mask = np.argsort(angles)
|
||
|
||
x_sorted = list(x[mask])
|
||
y_sorted = list(y[mask])
|
||
|
||
# Rearrange tuples to get the right coordinates.
|
||
sortedPoints = []
|
||
for i in range(len(points)):
|
||
j = 0
|
||
while (x_sorted[i] != points[j][0]) and (y_sorted[i] != points[j][-1]):
|
||
j += 1
|
||
else:
|
||
if len(points[0]) == 3:
|
||
sortedPoints.append((x_sorted[i], points[j][1], y_sorted[i]))
|
||
else:
|
||
sortedPoints.append((x_sorted[i], y_sorted[i]))
|
||
|
||
return sortedPoints
|
||
|
||
|
||
def curveCornerIntersectionPoints(
|
||
line0, line1, startDistance, angleAdaptation=False
|
||
):
|
||
"""
|
||
Create points between the two lines to smooth the intersection.
|
||
|
||
Args:
|
||
line0 (tuple): Tuple of tuple. Line coordinates. Order matters.
|
||
line1 (tuple): Tuple of tuple. Line coordinates. Order matters.
|
||
startDistance (int): distance from the intersection where the
|
||
curve should starts.
|
||
angleAdaptation (bool, optional): True will adapt the
|
||
startDistance depending of the angle between the two lines.
|
||
False will force the distance to be startDistance. Defaults to
|
||
False.
|
||
|
||
Returns:
|
||
[list]: List of tuple of coordinates (2d) that forms the curve.
|
||
Starts on the line and end on the other line.
|
||
|
||
>>> curveCornerIntersectionPoints(((0, 0), (50, 20)), ((-5, 50), (25, -5)), 10)
|
||
"""
|
||
intersection = lineIntersection(line0, line1, fullLine=True)
|
||
|
||
if intersection == None:
|
||
return None
|
||
|
||
# Define automatically the distance from the intersection, where the curve
|
||
# starts.
|
||
if angleAdaptation:
|
||
angle = getAngle(
|
||
(line0[0][0], line0[0][-1]),
|
||
intersection,
|
||
(line1[0][0], line1[0][-1]),
|
||
)
|
||
# Set here the radius of the circle for a square angle.
|
||
startDistance = startDistance * abs(1 / (angle / 90))
|
||
|
||
startCurvePoint = circleLineSegmentIntersection(
|
||
intersection, startDistance, line0[0], intersection, fullLine=True
|
||
)[0]
|
||
endCurvePoint = circleLineSegmentIntersection(
|
||
intersection, startDistance, line1[0], intersection, fullLine=True
|
||
)[0]
|
||
# Higher value for better precision
|
||
perpendicular0 = perpendicular(10e3, startCurvePoint, intersection)[0]
|
||
perpendicular1 = perpendicular(10e3, endCurvePoint, intersection)[1]
|
||
|
||
center = lineIntersection(
|
||
(perpendicular0, startCurvePoint), (perpendicular1, endCurvePoint)
|
||
)
|
||
|
||
# Distance with startCurvePoint and endCurvePoint from the center are the
|
||
# same.
|
||
radius = distance2D(startCurvePoint, center)
|
||
|
||
circle = circlePoints(
|
||
center, round(radius), 32
|
||
) # n=round((2 * pi * radius) / 32)
|
||
|
||
# Find the correct point on the circle.
|
||
curveCornerPointsTemp = [startCurvePoint]
|
||
for point in circle:
|
||
if InTriangle(point, intersection, startCurvePoint, endCurvePoint):
|
||
curveCornerPointsTemp.append(point)
|
||
curveCornerPointsTemp.append(endCurvePoint)
|
||
|
||
# Be sure that all the points are in correct order.
|
||
curveCornerPoints = optimizedPath(curveCornerPointsTemp, startCurvePoint)
|
||
return curveCornerPoints
|
||
|
||
|
||
def curveCornerIntersectionLine(
|
||
line0, line1, startDistance, angleAdaptation=False, center=()
|
||
):
|
||
"""
|
||
Create a continuous circular line between the two lines to smooth
|
||
the intersection.
|
||
|
||
Args:
|
||
line0 (tuple): Tuple of tuple. Line coordinates. Order matters.
|
||
line1 (tuple): Tuple of tuple. Line coordinates. Order matters.
|
||
startDistance (int): distance from the intersection where the
|
||
curve should starts.
|
||
angleAdaptation (bool, optional): True will adapt the
|
||
startDistance depending of the angle between the two lines.
|
||
False will force the distance to be startDistance. Defaults to
|
||
False.
|
||
|
||
Returns:
|
||
[list]: List of tuple of coordinates (2d) that forms the curve.
|
||
Starts on the line and end on the other line.
|
||
|
||
TODO:
|
||
angleAdaptation : Set circle radius and not startDistance.
|
||
Polar coordinates / Unit circle instead of InTriangle.
|
||
|
||
>>> curveCornerIntersectionLine(((0, 0), (50, 20)), ((-5, 50), (25, -5)), 10)
|
||
"""
|
||
intersection = lineIntersection(line0, line1, fullLine=True)
|
||
|
||
if intersection == None:
|
||
return None
|
||
|
||
# Define automatically the distance from the intersection, where the curve
|
||
# starts.
|
||
if angleAdaptation:
|
||
angle = getAngle(
|
||
(line0[0][0], line0[0][-1]),
|
||
intersection,
|
||
(line1[0][0], line1[0][-1]),
|
||
)
|
||
# Set here the radius of the circle for a square angle.
|
||
startDistance = startDistance * abs(1 / (angle / 90))
|
||
|
||
startCurvePoint = circleLineSegmentIntersection(
|
||
intersection, startDistance, line0[0], intersection, fullLine=True
|
||
)[0]
|
||
endCurvePoint = circleLineSegmentIntersection(
|
||
intersection, startDistance, line1[0], intersection, fullLine=True
|
||
)[0]
|
||
# Higher value for better precision
|
||
perpendicular0 = perpendicular(10e3, startCurvePoint, intersection)[0]
|
||
perpendicular1 = perpendicular(10e3, endCurvePoint, intersection)[1]
|
||
|
||
if center == ():
|
||
center = lineIntersection(
|
||
(perpendicular0, startCurvePoint), (perpendicular1, endCurvePoint)
|
||
)
|
||
|
||
# Distance with startCurvePoint and endCurvePoint from the center
|
||
# are almost the same.
|
||
radius = distance2D(startCurvePoint, center)
|
||
|
||
circleArc = circle(center, round(radius))[0]
|
||
|
||
# Find the correct point on the circle.
|
||
curveCornerPointsTemp = [startCurvePoint]
|
||
for point in circleArc:
|
||
if InTriangle(point, intersection, startCurvePoint, endCurvePoint):
|
||
curveCornerPointsTemp.append(point)
|
||
# curveCornerPointsTemp.append(endCurvePoint)
|
||
|
||
# Be sure that all the points are in correct order.
|
||
curveCornerPoints = optimizedPath(curveCornerPointsTemp, startCurvePoint)
|
||
return curveCornerPoints, center
|
||
|
||
|
||
def middleLine(xyz0, xyz1):
|
||
x = (xyz0[0] + xyz1[0]) / 2
|
||
z = (xyz0[-1] + xyz1[-1]) / 2
|
||
if len(xyz0) <= 2:
|
||
return (x, z)
|
||
else:
|
||
y = (xyz0[1] + xyz1[1]) / 2
|
||
return (round(x), round(y), round(z))
|