Your Name
108 lines
2.5 KiB
Python
108 lines
2.5 KiB
Python
import math
|
|
|
|
|
|
# Distance * length_of_the_arms to get units
|
|
# 0.03 seems a good distance
|
|
def checkDistance(coord1, coord2, distance):
|
|
|
|
# Check if the line point are in the outer edge
|
|
if coord1[1] > 0.97 and coord2[1] > 0.97:
|
|
return False
|
|
|
|
# Changing polar coords to X and Y
|
|
x1, y1 = polarToXY(coord1[0], coord1[1])
|
|
x2, y2 = polarToXY(coord2[0], coord2[1])
|
|
|
|
# Getting the change of coords
|
|
deltaX = x2 - x1
|
|
deltaY = y2 - y1
|
|
|
|
# Getting the distance between the points
|
|
d = math.sqrt(deltaX**2 + deltaY**2)
|
|
|
|
# If distance is over twice the wanted distance
|
|
if d > distance * 2:
|
|
return True
|
|
else:
|
|
return False
|
|
|
|
|
|
# Change from polar to cartesian
|
|
def polarToXY(theta, r):
|
|
x = r * math.cos(theta)
|
|
y = r * math.sin(theta)
|
|
|
|
return x, y
|
|
|
|
|
|
# Changes from cartesian to polar
|
|
def xyToPolar(x, y):
|
|
r = round(math.sqrt(x**2 + y**2), 5)
|
|
th = round(math.atan2(y, x), 5)
|
|
|
|
return th, r
|
|
|
|
|
|
def divideBy(coord1, coord2, distance):
|
|
|
|
# Changing polar coords to X and Y
|
|
x1, y1 = polarToXY(coord1[0], coord1[1])
|
|
x2, y2 = polarToXY(coord2[0], coord2[1])
|
|
|
|
# Getting the change of coords
|
|
deltaX = x2 - x1
|
|
deltaY = y2 - y1
|
|
|
|
# Getting the distance between the points
|
|
d = math.sqrt(deltaX**2 + deltaY**2)
|
|
|
|
# Calculating how many points it divides to
|
|
num_points = int(d / distance)
|
|
|
|
# If number of point is 0 then exit
|
|
if num_points == 0:
|
|
return False
|
|
|
|
# Calculating the distance between points
|
|
r = d / num_points
|
|
|
|
# Getting the angle from point A to point B
|
|
theta = math.atan2(deltaY, deltaX)
|
|
|
|
# Starting the coord list with point A
|
|
new_coords = [[x1, y1]]
|
|
|
|
# Calculating all the points between
|
|
for i in range(0, num_points):
|
|
x_new = new_coords[-1][0] + r * math.cos(theta)
|
|
y_new = new_coords[-1][1] + r * math.sin(theta)
|
|
|
|
new_coords.append([x_new, y_new])
|
|
|
|
# And adding the point B
|
|
new_coords.append([x2, y2])
|
|
|
|
coords_polar = []
|
|
|
|
# Changing the points to polar
|
|
for i in new_coords:
|
|
th, r_new = xyToPolar(i[0], i[1])
|
|
coords_polar.append([th, r_new])
|
|
|
|
if th == coord2[0]:
|
|
coords_polar.append(coord2)
|
|
break
|
|
|
|
return coords_polar
|
|
|
|
|
|
# Writing list to a file to test
|
|
def writeToFile(coords_polar):
|
|
with open('test.thr', 'a') as f:
|
|
for i in coords_polar:
|
|
f.write(str(i[0]) + ' ' + str(i[1]) + '\n')
|
|
|
|
|
|
if __name__ == '__main__':
|
|
coords = divideBy([math.pi /2, 1], [0, 1], 0.01)
|
|
writeToFile(coords) |