def WaypointsAverageDistance(which_waypoints):
#The distance between two adjacent points
waypoints_distances = [
Geom.Distance(which_waypoints[k], which_waypoints[k + 1])
for k in range(len(which_waypoints) - 1)
]
return np.average(waypoints_distances)
def __init__(self, ABC=None, area=None):
self.ABC = []
#transfrom them into np.array
for pos in ABC:
self.ABC.append(np.array(pos))
#three
pos_A, pos_B, pos_C = self.ABC
#Calculate three boundary length
AB = Geom.Distance(pos_A, pos_B)
AC = Geom.Distance(pos_A, pos_C)
CB = Geom.Distance(pos_C, pos_B)
#Helen formula
a, b, c = CB, AC, AB
p = (a + b + c) / 2
#area: traingle area
self.area = np.sqrt(p * (p - a) * (p - b) * (p - c))
def BridgeBetween2Waypoints(waypoints_A, waypoints_B):
#tail of A and head of B
bridge_head = waypoints_A[-1]
bridge_tail = waypoints_B[0]
#distance between bridge_head and bridge_tail
length_bridge = Geom.Distance(bridge_head, bridge_tail)
#step length
step_distance = WaypointsAverageDistance(waypoints_A + waypoints_B)
#amount of bridge way points
num_bridge_waypoints = int(np.floor(length_bridge / step_distance))
#xy diff
xy_diff = (np.array(bridge_tail) -
np.array(bridge_head)) / (length_bridge / step_distance)
return [
np.array(bridge_head) + k * xy_diff
for k in range(1, num_bridge_waypoints)
]
