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) ]