def get_velocity(drone):
vx = drone.velocity.x + drone.accelerationX
vy = drone.velocity.y + drone.accelerationY
vz = drone.velocity.z + drone.accelerationZ
velocity = node(vx,vy,vz)
return velocity
def get_random_nodes(point,radius, obstacles):
nodes = []
for i in range(100):
nodes.append(get_random_node(goalSampleRate=0, goalNode=node(0,0,0), obstacles = obstacles,
minRandx=int(point.x) - radius, maxRandx= int(point.x) + radius,
minRandy= int(point.y) - radius, maxRandy=int(point.y) + radius,
minRandz = int(point.z) - radius, maxRandz = int(point.z) + radius))
return nodes
def make_step(nodef, nodet, stepx, stepy, stepz):
dx = nodet.x - nodef.x
dy = nodet.y - nodef.y
dz = nodet.z - nodef.z
r = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
xx = dx * (stepx / r)
yy = dy * (stepy / r)
zz = dz * (stepz / r)
newnode = node(nodef.x + xx, nodef.y + yy, nodef.z + zz)
return newnode
def get_random_node(drone):
a = True
goalProbability = random.randint(1,100)
if drone.goalSampleRate <= goalProbability:
x = random.randint(drone.minRandx, drone.maxRandx)
y = random.randint(drone.minRandy, drone.maxRandy)
z = random.randint(drone.minRandz, drone.maxRandz)
randomNode = node(x, y, z)
return randomNode
else:
randomNode = drone.finish
return randomNode
def __init__(self, start, finish, initialVelocityX, initialVelocityY,
initialVelocityZ, maxAccelerationX, maxAccelerationY,
maxAccelerationZ, size, mass, obstacles, annimation, ID):
self.start = start
self.finish = finish
self.velocityX = initialVelocityX
self.velocityY = initialVelocityY
self.velocityZ = initialVelocityZ
self.velocity = node(self.velocityX, self.velocityY, self.velocityZ)
self.accelerationX = 0
self.accelerationY = 0
self.accelerationZ = 0
self.size = size
self.mass = mass
self.obstacles = obstacles
self.position = start
self.nodeList = [start]
self.path = None
self.RRTfinished = False
self.annimation = annimation
self.recipents = []
self.intercaptors = []
self.intercaptionPoint = []
self.ID = ID
self.velocity = node(self.velocityX, self.velocityY, self.velocityZ)
self.velocities = []
self.maxAccelerationY = maxAccelerationY
self.maxAccelerationX = maxAccelerationX
self.maxAccelerationZ = maxAccelerationZ
self.minRandx = min(start.x, finish.x)
self.maxRandx = max(start.x, finish.x)
self.minRandy = min(start.y, finish.y)
self.maxRandy = max(start.y, finish.y)
self.minRandz = min(start.z, finish.z)
self.maxRandz = max(start.z, finish.z)
self.goalSampleRate = 100
def get_interception_point(line1, line2, size1, size2):
for j in range(len(line1)):
for k in range(len(line2)):
if node_distance(node1 = line1[j], node2 = line2[k]) <= size1 + size2 : return line1[j]
return node(-666,-500,-500)
droneSize = 10 # set all drones sizing
initialVelocityX = 0 # set initial drone velocity in x
initialVelocityY = 0 # set initial drone velocity in y
initialVelocityZ = 0 # set initial drone velocity in z
maxAccelerationX = 10 # set max drone acceleration in x
maxAccelerationY = 10 # set max drone acceleration in y
maxAccelerationZ = 10 # set max drone acceleration in z
step = 1 # set drone maximum step distance used for PRM(in later version will be fixed)
droneMass = 1 # set drone mass, used for intertia calculations, will be improved in later version
annimation = True # True to observe path building process, False to observe only path
colors = ['r', 'g', 'y', 'k', 'b', 'm']
startNodes = []
goalNodes = []
for i in range(len(start)):
startNodes.append(node(x=start[i][0], y=start[i][1], z=start[i][2]))
goalNodes.append(node(x=goal[i][0], y=goal[i][1], z=goal[i][2]))
obstacles = []
for i in range(len(obstacleCoordinates)):
obstacles.append([
node(x=obstacleCoordinates[i][0],
y=obstacleCoordinates[i][1],
z=obstacleCoordinates[i][2]), obstacleCoordinates[i][3]
])
drones = []
for i in range(len(start)):
drones.append(
drone(start=startNodes[i],
finish=goalNodes[i],