class CollisionDetectorTest(TestCase):
def setUp(self):
self.anObstacle = Obstacle((100, 100))
self.anObstacleWall = Obstacle((50, 50))
self.anObstacleInner = Obstacle((370, 370))
self.mapSizeX, self.mapSizeY = 1000, 600
safeMargin = 90
obstacleList = [Obstacle((400, 400))]
self.collisionDetector = CollisionDetector(self.mapSizeX,
self.mapSizeY, safeMargin,
obstacleList)
def test_collisionWithEachCorner(self):
topLeft, topRight, botLeft, botRight = self.collisionDetector.detectCloserObstacleForEachCornerXAxis(
self.anObstacle)
self.assertEqual(topLeft.positionY, 0)
self.assertEqual(topRight.positionY, 0)
self.assertEqual(botLeft.positionY, self.mapSizeY)
self.assertEqual(botRight.positionY, self.mapSizeY)
def test_collisionWithWall(self):
wallFront = self.collisionDetector.isCollidingWithWallFront(
self.anObstacleWall)
wallBack = self.collisionDetector.isCollidingWithWallBack(
self.anObstacleWall)
wallTop = self.collisionDetector.isCollidingWithWallUpper(
self.anObstacleWall)
wallBot = self.collisionDetector.isCollidingWithWallLower(
self.anObstacleWall)
self.assertTrue(wallFront)
self.assertTrue(wallTop)
self.assertTrue(not wallBack)
self.assertTrue(not wallBot)
def test_collisionInnerObstacle(self):
innerFront = self.collisionDetector.hasFrontalInnerCollision(
self.anObstacleInner)
innerBack = self.collisionDetector.hasEndInnerCollision(
self.anObstacleInner)
innerTop = self.collisionDetector.hasUpperInnerCollision(
self.anObstacleInner)
innerBot = self.collisionDetector.hasLowerInnerCollision(
self.anObstacleInner)
self.assertTrue(not innerFront)
self.assertTrue(innerTop)
self.assertTrue(innerBack)
self.assertTrue(innerBot)
class CollisionDetectorTest(TestCase):
def setUp(self):
self.anObstacle = Obstacle((100,100))
self.anObstacleWall = Obstacle((50,50))
self.anObstacleInner = Obstacle((370,370))
self.mapSizeX, self.mapSizeY = 1000,600
safeMargin = 90
obstacleList = [Obstacle((400,400))]
self.collisionDetector = CollisionDetector(self.mapSizeX, self.mapSizeY, safeMargin, obstacleList)
def test_collisionWithEachCorner(self):
topLeft, topRight, botLeft, botRight = self.collisionDetector.detectCloserObstacleForEachCornerXAxis(self.anObstacle)
self.assertEqual(topLeft.positionY, 0)
self.assertEqual(topRight.positionY, 0)
self.assertEqual(botLeft.positionY, self.mapSizeY)
self.assertEqual(botRight.positionY, self.mapSizeY)
def test_collisionWithWall(self):
wallFront = self.collisionDetector.isCollidingWithWallFront(self.anObstacleWall)
wallBack = self.collisionDetector.isCollidingWithWallBack(self.anObstacleWall)
wallTop = self.collisionDetector.isCollidingWithWallUpper(self.anObstacleWall)
wallBot = self.collisionDetector.isCollidingWithWallLower(self.anObstacleWall)
self.assertTrue(wallFront)
self.assertTrue(wallTop)
self.assertTrue(not wallBack)
self.assertTrue(not wallBot)
def test_collisionInnerObstacle(self):
innerFront = self.collisionDetector.hasFrontalInnerCollision(self.anObstacleInner)
innerBack = self.collisionDetector.hasEndInnerCollision(self.anObstacleInner)
innerTop = self.collisionDetector.hasUpperInnerCollision(self.anObstacleInner)
innerBot = self.collisionDetector.hasLowerInnerCollision(self.anObstacleInner)
self.assertTrue(not innerFront)
self.assertTrue(innerTop)
self.assertTrue(innerBack)
self.assertTrue(innerBot)
class GraphGenerator:
SAFE_MARGIN = 85
def __init__(self, obstaclesList, mapSizeX, mapSizeY):
self.MAP_SIZE_X = mapSizeX
self.MAP_SIZE_Y = mapSizeY
self.graph = Graph(obstaclesList, self.SAFE_MARGIN)
self.collisionDetector = CollisionDetector(self.MAP_SIZE_X, self.MAP_SIZE_Y, self.SAFE_MARGIN, obstaclesList)
self.endNodeGenerator = EndNodeGenerator(
self.MAP_SIZE_X, self.MAP_SIZE_Y, self.SAFE_MARGIN, obstaclesList, self.collisionDetector, self.graph
)
self.bottomPathGenerator = BottomPathGenerator(self.SAFE_MARGIN, mapSizeY, self.graph, self.collisionDetector)
self.topPathGenerator = TopPathGenerator(self.SAFE_MARGIN, mapSizeY, self.graph, self.collisionDetector)
self.obstaclesList = obstaclesList
self.obstaclesList.sort(key=lambda obstacle: obstacle.positionX)
def generateGraph(self):
firstObstacle = self.obstaclesList[0]
if self.collisionDetector.isCollidingWithWallFront(firstObstacle) == False:
self.__defaultStart(firstObstacle)
for compteur in range(0, self.obstaclesList.__len__()):
currentObstacle = self.obstaclesList[(compteur)]
startingNode = currentObstacle.startingNode
if compteur < self.obstaclesList.__len__() - 1:
nextObstacle = self.obstaclesList[compteur + 1]
if nextObstacle.startingNode.positionX == 0:
nextObstacle.setStartingNode(currentObstacle.startingNode)
currentOstacleTopLeftCorner = (
currentObstacle.positionX - self.SAFE_MARGIN,
currentObstacle.positionY - self.SAFE_MARGIN,
)
currentOstacleTopRightCorner = (
currentObstacle.positionX + self.SAFE_MARGIN,
currentObstacle.positionY - self.SAFE_MARGIN,
)
currentOstacleBottomRightCorner = (
currentObstacle.positionX + self.SAFE_MARGIN,
currentObstacle.positionY + self.SAFE_MARGIN,
)
currentOstacleBottomLeftCorner = (
currentObstacle.positionX - self.SAFE_MARGIN,
currentObstacle.positionY + self.SAFE_MARGIN,
)
collisionUpperLeftCorner, collisionUpperRightCorner, collisionBottomLeftCorner, collisionBottomRightCorner = self.collisionDetector.detectCloserObstacleForEachCornerXAxis(
currentObstacle
)
borderNodeLeftTop = Node(
(
currentOstacleTopLeftCorner[0],
(currentOstacleTopLeftCorner[1] + collisionUpperLeftCorner.positionY + self.SAFE_MARGIN) / 2,
)
)
borderNodeRightTop = Node(
(
currentOstacleTopRightCorner[0],
(currentOstacleTopRightCorner[1] + collisionUpperRightCorner.positionY + self.SAFE_MARGIN) / 2,
)
)
borderNodeLeftBottom = Node(
(
currentOstacleBottomLeftCorner[0],
(currentOstacleBottomLeftCorner[1] + collisionBottomLeftCorner.positionY - self.SAFE_MARGIN) / 2,
)
)
borderNodeRightBottom = Node(
(
currentOstacleBottomRightCorner[0],
(currentOstacleBottomRightCorner[1] + collisionBottomRightCorner.positionY - self.SAFE_MARGIN) / 2,
)
)
resultTop, collisionInnerTop = self.collisionDetector.hasUpperInnerCollision(currentObstacle)
resultBot, collisionInnerBot = self.collisionDetector.hasLowerInnerCollision(currentObstacle)
if not (
self.collisionDetector.hasEndInnerCollision(currentObstacle)
and (resultTop and collisionInnerTop[-1].positionY > currentObstacle.positionY)
and (resultBot and collisionInnerBot[-1].positionY > currentObstacle.positionY)
):
endNode = self.endNodeGenerator.generateEndNode(
currentObstacle,
currentOstacleTopRightCorner,
currentOstacleBottomRightCorner,
collisionBottomRightCorner,
collisionUpperRightCorner,
compteur,
)
else:
# Might be the Problem
endNode = Node((1000, 1000))
if self.collisionDetector.isCollidingWithWallFront(currentObstacle) == False:
self.__generateFrontPath(borderNodeLeftBottom, borderNodeLeftTop, currentObstacle, startingNode)
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.topPathGenerator.generateTopPath(
collisionUpperLeftCorner,
collisionUpperRightCorner,
currentObstacle,
borderNodeLeftTop,
currentOstacleTopLeftCorner,
borderNodeRightTop,
)
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.bottomPathGenerator.generateBottomPath(
collisionBottomLeftCorner,
collisionBottomRightCorner,
currentObstacle,
borderNodeLeftBottom,
currentOstacleBottomLeftCorner,
borderNodeRightBottom,
)
if self.collisionDetector.isCollidingWithWallBack(currentObstacle) == False:
self.__generateEndPath(borderNodeRightBottom, borderNodeRightTop, currentObstacle, endNode)
return self.graph
def __generateEndPath(self, borderNodeRightBottom, borderNodeRightTop, currentObstacle, endNode):
if self.collisionDetector.hasEndInnerCollision(currentObstacle) == False:
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.graph.connectTwoNodes(borderNodeRightTop, endNode)
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.graph.connectTwoNodes(borderNodeRightBottom, endNode)
else:
result, collidingObstacle = self.collisionDetector.hasLowerInnerCollision(currentObstacle)
if result == False or collidingObstacle[-1].positionX < currentObstacle.positionX:
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.graph.connectTwoNodes(borderNodeRightBottom, endNode)
result, collidingObstacle = self.collisionDetector.hasUpperInnerCollision(currentObstacle)
if result == False or collidingObstacle[-1].positionX < currentObstacle.positionX:
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.graph.connectTwoNodes(borderNodeRightTop, endNode)
def __generateFrontPath(self, borderNodeLeftBottom, borderNodeLeftTop, currentObstacle, startingNode):
if self.collisionDetector.hasFrontalInnerCollision(currentObstacle) == False:
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.graph.connectTwoNodes(startingNode, borderNodeLeftBottom)
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.graph.connectTwoNodes(startingNode, borderNodeLeftTop)
else:
result, collidingObstacle = self.collisionDetector.hasUpperInnerCollision(currentObstacle)
if result == False or collidingObstacle[0].positionX > currentObstacle.positionX:
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.graph.connectTwoNodes(startingNode, borderNodeLeftTop)
result, collidingObstacle = self.collisionDetector.hasLowerInnerCollision(currentObstacle)
if result == False or collidingObstacle[0].positionX > currentObstacle.positionX:
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.graph.connectTwoNodes(startingNode, borderNodeLeftBottom)
def __defaultStart(self, firstObstacle):
safeZoneCornerTopLeft = (0 + (self.SAFE_MARGIN - 20), 0 + (self.SAFE_MARGIN - 20))
safeZoneCornerBotLeft = (0 + (self.SAFE_MARGIN - 20), self.MAP_SIZE_Y - (self.SAFE_MARGIN - 20))
safeZoneCornerTopRight = (firstObstacle.positionX - self.SAFE_MARGIN, safeZoneCornerTopLeft[1])
startingNode = self.graph.generateSafeZone(safeZoneCornerBotLeft, safeZoneCornerTopLeft, safeZoneCornerTopRight)
firstObstacle.setStartingNode(startingNode)
class GraphGenerator:
SAFE_MARGIN = 85
def __init__(self, obstaclesList, mapSizeX, mapSizeY):
self.MAP_SIZE_X = mapSizeX
self.MAP_SIZE_Y = mapSizeY
self.graph = Graph(obstaclesList, self.SAFE_MARGIN)
self.collisionDetector = CollisionDetector(self.MAP_SIZE_X, self.MAP_SIZE_Y, self.SAFE_MARGIN, obstaclesList)
self.endNodeGenerator = EndNodeGenerator(self.MAP_SIZE_X, self.MAP_SIZE_Y, self.SAFE_MARGIN, obstaclesList,
self.collisionDetector, self.graph)
self.bottomPathGenerator = BottomPathGenerator(self.SAFE_MARGIN, mapSizeY, self.graph, self.collisionDetector)
self.topPathGenerator = TopPathGenerator(self.SAFE_MARGIN, mapSizeY, self.graph, self.collisionDetector)
self.obstaclesList = obstaclesList
self.obstaclesList.sort(key=lambda obstacle: obstacle.positionX)
def generateGraph(self):
firstObstacle = self.obstaclesList[0]
if self.collisionDetector.isCollidingWithWallFront(firstObstacle) == False:
self.__defaultStart(firstObstacle)
for compteur in range(0, self.obstaclesList.__len__()):
currentObstacle = self.obstaclesList[(compteur)]
startingNode = currentObstacle.startingNode
if compteur < self.obstaclesList.__len__() - 1:
nextObstacle = self.obstaclesList[compteur + 1]
if nextObstacle.startingNode.positionX == 0:
nextObstacle.setStartingNode(currentObstacle.startingNode)
currentOstacleTopLeftCorner = (currentObstacle.positionX - self.SAFE_MARGIN, currentObstacle.positionY - self.SAFE_MARGIN)
currentOstacleTopRightCorner = (currentObstacle.positionX + self.SAFE_MARGIN, currentObstacle.positionY - self.SAFE_MARGIN)
currentOstacleBottomRightCorner = (currentObstacle.positionX + self.SAFE_MARGIN, currentObstacle.positionY + self.SAFE_MARGIN)
currentOstacleBottomLeftCorner = (currentObstacle.positionX - self.SAFE_MARGIN, currentObstacle.positionY + self.SAFE_MARGIN)
collisionUpperLeftCorner, collisionUpperRightCorner, collisionBottomLeftCorner, collisionBottomRightCorner = self.collisionDetector.detectCloserObstacleForEachCornerXAxis(currentObstacle)
borderNodeLeftTop = Node(
(currentOstacleTopLeftCorner[0], (currentOstacleTopLeftCorner[1] + collisionUpperLeftCorner.positionY + self.SAFE_MARGIN) / 2))
borderNodeRightTop = Node(
(currentOstacleTopRightCorner[0], (currentOstacleTopRightCorner[1] + collisionUpperRightCorner.positionY + self.SAFE_MARGIN) / 2))
borderNodeLeftBottom = Node((currentOstacleBottomLeftCorner[0],
(currentOstacleBottomLeftCorner[1] + collisionBottomLeftCorner.positionY - self.SAFE_MARGIN) / 2))
borderNodeRightBottom = Node((currentOstacleBottomRightCorner[0],
(currentOstacleBottomRightCorner[1] + collisionBottomRightCorner.positionY - self.SAFE_MARGIN) / 2))
resultTop, collisionInnerTop = self.collisionDetector.hasUpperInnerCollision(currentObstacle)
resultBot, collisionInnerBot = self.collisionDetector.hasLowerInnerCollision(currentObstacle)
if not(self.collisionDetector.hasEndInnerCollision(currentObstacle) and (resultTop and collisionInnerTop[-1].positionY > currentObstacle.positionY) and (resultBot and collisionInnerBot[-1].positionY > currentObstacle.positionY)):
endNode = self.endNodeGenerator.generateEndNode(currentObstacle, currentOstacleTopRightCorner, currentOstacleBottomRightCorner,
collisionBottomRightCorner, collisionUpperRightCorner, compteur)
else:
#Might be the Problem
endNode = Node((1000,1000))
if self.collisionDetector.isCollidingWithWallFront(currentObstacle) == False:
self.__generateFrontPath(borderNodeLeftBottom, borderNodeLeftTop, currentObstacle, startingNode)
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.topPathGenerator.generateTopPath(collisionUpperLeftCorner, collisionUpperRightCorner,
currentObstacle, borderNodeLeftTop, currentOstacleTopLeftCorner, borderNodeRightTop)
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.bottomPathGenerator.generateBottomPath(collisionBottomLeftCorner, collisionBottomRightCorner, currentObstacle,
borderNodeLeftBottom, currentOstacleBottomLeftCorner, borderNodeRightBottom)
if self.collisionDetector.isCollidingWithWallBack(currentObstacle) == False:
self.__generateEndPath(borderNodeRightBottom, borderNodeRightTop, currentObstacle, endNode)
return self.graph
def __generateEndPath(self, borderNodeRightBottom, borderNodeRightTop, currentObstacle, endNode):
if self.collisionDetector.hasEndInnerCollision(currentObstacle) == False:
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.graph.connectTwoNodes(borderNodeRightTop, endNode)
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.graph.connectTwoNodes(borderNodeRightBottom, endNode)
else:
result, collidingObstacle = self.collisionDetector.hasLowerInnerCollision(currentObstacle)
if result == False or collidingObstacle[-1].positionX < currentObstacle.positionX:
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.graph.connectTwoNodes(borderNodeRightBottom, endNode)
result, collidingObstacle = self.collisionDetector.hasUpperInnerCollision(currentObstacle)
if result == False or collidingObstacle[-1].positionX < currentObstacle.positionX:
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.graph.connectTwoNodes(borderNodeRightTop, endNode)
def __generateFrontPath(self, borderNodeLeftBottom, borderNodeLeftTop, currentObstacle, startingNode):
if self.collisionDetector.hasFrontalInnerCollision(currentObstacle) == False:
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.graph.connectTwoNodes(startingNode, borderNodeLeftBottom)
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.graph.connectTwoNodes(startingNode, borderNodeLeftTop)
else:
result, collidingObstacle = self.collisionDetector.hasUpperInnerCollision(currentObstacle)
if result == False or collidingObstacle[0].positionX > currentObstacle.positionX:
if self.collisionDetector.isCollidingWithWallUpper(currentObstacle) == False:
self.graph.connectTwoNodes(startingNode, borderNodeLeftTop)
result, collidingObstacle = self.collisionDetector.hasLowerInnerCollision(currentObstacle)
if result == False or collidingObstacle[0].positionX > currentObstacle.positionX:
if self.collisionDetector.isCollidingWithWallLower(currentObstacle) == False:
self.graph.connectTwoNodes(startingNode, borderNodeLeftBottom)
def __defaultStart(self, firstObstacle):
safeZoneCornerTopLeft = (0 + (self.SAFE_MARGIN-20), 0 + (self.SAFE_MARGIN -20))
safeZoneCornerBotLeft = (0 + (self.SAFE_MARGIN-20), self.MAP_SIZE_Y - (self.SAFE_MARGIN-20))
safeZoneCornerTopRight = (firstObstacle.positionX - self.SAFE_MARGIN, safeZoneCornerTopLeft[1])
startingNode = self.graph.generateSafeZone(safeZoneCornerBotLeft, safeZoneCornerTopLeft, safeZoneCornerTopRight)
firstObstacle.setStartingNode(startingNode)