def createWallOld(world, num_case, num_line):
'''
return :
list of pylygon object corresponding to the walls of a World
arguments :
num_case : number of case
num_line : number of line
'''
polyObs1 = [(0, 0), ((num_case) * world.elementSize, 0),
((num_case) * world.elementSize, world.elementSize),
(0, world.elementSize)]
polyObs2 = [(0, world.elementSize), (world.elementSize, world.elementSize),
(world.elementSize, (num_line) * world.elementSize),
(0, (num_line) * world.elementSize)]
polyObs3 = [((num_case - 1) * world.elementSize, world.elementSize),
((num_case) * world.elementSize, world.elementSize),
((num_case) * world.elementSize,
(num_line - 1) * world.elementSize),
(((num_case - 1) * world.elementSize,
(num_line - 1) * world.elementSize))]
polyObs4 = [(0, (num_line - 1) * world.elementSize),
(num_case * world.elementSize,
(num_line - 1) * world.elementSize),
(num_case * world.elementSize, num_line * world.elementSize),
(0, num_line * world.elementSize)]
polyObs5 = [(18 * world.elementSize, 0), (19 * world.elementSize, 0),
(19 * world.elementSize, 12 * world.elementSize),
(18 * world.elementSize, 12 * world.elementSize)]
polyObs6 = [(31 * world.elementSize, 11 * world.elementSize),
(32 * world.elementSize, 11 * world.elementSize),
(32 * world.elementSize, (num_line - 1) * world.elementSize),
(31 * world.elementSize, (num_line - 1) * world.elementSize)]
walls = [
pylygon.Polygon(polyObs1),
pylygon.Polygon(polyObs2),
pylygon.Polygon(polyObs3),
pylygon.Polygon(polyObs4)
]
walls = [
pylygon.Polygon(polyObs1),
pylygon.Polygon(polyObs2),
pylygon.Polygon(polyObs3),
pylygon.Polygon(polyObs4),
pylygon.Polygon(polyObs5),
pylygon.Polygon(polyObs6)
]
return walls
def has_collision(self, other):
"""Check if the object has collided with *other*.
Return True or False"""
self_poly = pylygon.Polygon(self.get_world_envelope())
other_poly = pylygon.Polygon(other.get_world_envelope())
# TODO: use distance() for performance
#print("Dist:", self_poly.distance(other_poly))
collision = self_poly.collidepoly(other_poly)
if isinstance(collision, bool):
if not collision: return False
# Test code - print out collisions
#print("Collision between {} and {}".format(self, other))
# end of test code
return True
def elementWall(ix0, iy0, ix1, iy1, elementSize):
polyObs = [(ix0 * elementSize, iy0 * elementSize),
(ix1 * elementSize, iy0 * elementSize),
(ix1 * elementSize, iy1 * elementSize),
(ix0 * elementSize, iy1 * elementSize)]
#print polyObs
ply = pylygon.Polygon(polyObs)
#print ply
return ply
def createRobot(daarrt):
'''
return :
list pylygon object corresponding to the daarrt (wheel )
arguments :
daarrt object
'''
rotationCenter = [-daarrt.robotLenght / 2, -daarrt.robotWidth / 2]
body = createBody(daarrt, rotationCenter[0], rotationCenter[1])
frontId = createFrontId(daarrt, rotationCenter[0], rotationCenter[1])
wheel1 = createWheel(daarrt, -daarrt.wheelLenght / 2 + rotationCenter[0],
rotationCenter[1])
wheel2 = createWheel(
daarrt,
daarrt.robotLenght - daarrt.wheelLenght / 2 + rotationCenter[0],
rotationCenter[1])
wheel3 = createWheel(
daarrt,
daarrt.robotLenght - daarrt.wheelLenght / 2 + rotationCenter[0],
daarrt.robotWidth + daarrt.wheelWidth + rotationCenter[1])
wheel4 = createWheel(
daarrt, -daarrt.wheelLenght / 2 + rotationCenter[0],
daarrt.robotWidth + daarrt.wheelWidth + rotationCenter[1])
clawBase = createClawBase(
daarrt, daarrt.robotLenght + rotationCenter[0],
daarrt.robotWidth / 2 - daarrt.clawWidthOpened / 2 + rotationCenter[1])
claw1 = createClaw(
daarrt, daarrt.robotLenght + daarrt.clawBase + rotationCenter[0],
daarrt.robotWidth / 2 - daarrt.clawWidthClosed / 2 -
daarrt.clawWidthOpened * daarrt.clawOpening / 180.0 +
rotationCenter[1])
claw2 = createClaw(
daarrt, daarrt.robotLenght + daarrt.clawBase + rotationCenter[0],
daarrt.robotWidth / 2 +
daarrt.clawWidthOpened * daarrt.clawOpening / 180.0 +
rotationCenter[1])
#robotTmp = [pylygon.Polygon(body) , pylygon.Polygon(clawBase) , pylygon.Polygon(wheel1) , pylygon.Polygon(wheel2) , pylygon.Polygon(wheel3) , pylygon.Polygon(wheel4) , pylygon.Polygon(claw1) , pylygon.Polygon(claw2)]
#robot = [poly_translate(poly_rotate(element,daarrt.robotCap),daarrt.posX,daarrt.posY) for element in robotTmp]
robotTmp = [
pylygon.Polygon(body),
pylygon.Polygon(frontId),
pylygon.Polygon(wheel1),
pylygon.Polygon(wheel2),
pylygon.Polygon(wheel3),
pylygon.Polygon(wheel4)
]
robot = [
poly_translate(poly_rotate(element, daarrt.robotCap), daarrt.posX,
daarrt.posY) for element in robotTmp
]
return robot
def createObstacle(world, x, y):
'''
return :
pylygon object corresponding to an obstacle
arguments :
x : position on x Axis
y : position on y Axis
'''
obs = pylygon.Polygon([(x, y), (x + world.elementSize, y),
(x + world.elementSize, y + world.elementSize),
(x, y + world.elementSize)])
return obs
def poly_translate(p, tx, ty):
'''
return :
pylygon object corresponding to the argument translated
arguments :
p : pylygon object to translate
tx : translation on X axis
ty : translation on Y axis
'''
points = p.P
pointsTranslates = array([(x + tx, y + ty) for (x, y) in points])
p.P = pointsTranslates
return pylygon.Polygon(p)
def sonarCollideOld(self,world):
finalDist=[]
j=0
ib=0
for beam in self.sonar:
dist=[]
#self.intersect[ib][1] = self.intersect[ib][0]
#print "nb obstacles ",len(world.obstacle)
iobs=0
for obstacle in world.obstacle :
if not beam.collidepoly(obstacle) is False :
print "------------------------------"
print iobs,ib
print beam.collidepoly(obstacle)
print obstacle
tmp=5000
i=0
for beamPoint in beam.P :
distance=(math.sqrt((beamPoint[0]-self.posX)*(beamPoint[0]-self.posX) + (beamPoint[1]-self.posY)*(beamPoint[1]-self.posY)))
if distance<tmp :
index=i
tmp=distance
i+=1
x0=beam.P[index][0]
y0=beam.P[index][1]
ply=pylygon.Polygon([(x0,y0)])
tmp1=ply.distance(obstacle)
print tmp
#print "[%d],dist %d = %f"%(ib,iobs,math.sqrt(tmp[0]*tmp[0] + tmp[1]*tmp[1]))
dist.append(math.sqrt(tmp1[0]*tmp1[0] + tmp1[1]*tmp1[1]))
iobs+=1
try :
finalDist.append(min(dist))
#finalDist.append(10*j)
#j+=1
except :
finalDist.append(5000)
ib += 1
# convert sonarDist to world coordinates
print finalDist
for i in range(len(finalDist)):
finalDist[i] *= elementScale
self.sonarDist[i]=finalDist[i]
return finalDist
def poly_rotate(p, theta):
'''
return :
pylygon object corresponding to the argument rotated
arguments :
p : pylygon object to rotate
theta : rotation angle in degrees
'''
theta = theta * math.pi / 180.0
points = p.P
pointsTournes = array([(x * math.cos(theta) - y * math.sin(theta),
x * math.sin(theta) + y * math.cos(theta))
for (x, y) in points])
p.P = pointsTournes
return pylygon.Polygon(p)
def get_contact_points(self, other):
"""Get a list of contact points with other object.
Returns a list of (x, y)"""
self_poly = pylygon.Polygon(self.get_world_envelope())
other_poly = pylygon.Polygon(other.get_world_envelope())
return self_poly.intersection_points(other_poly)
def createSonar(daarrt, sonitv):
'''
return :
list of pylygon objects containing each sonar of the DAARRT
arguments :
daarrt object
'''
sonar = []
sonarLoc = []
theta = daarrt.sonarAngle * math.pi / 180.0
for i in range(daarrt.nFrontSonar):
if daarrt.nFrontSonar > 1:
x0 = daarrt.robotLenght / 2 + 2
y0 = daarrt.robotWidth * i / (daarrt.nFrontSonar -
1) - daarrt.robotWidth / 2
else:
x0 = daarrt.robotLenght / 2 + 2
y0 = 0
ply = pylygon.Polygon([(x0, y0),
(x0 + sonarLenght * math.cos(theta),
y0 + sonarLenght * math.sin(theta)),
(x0 + sonarLenght * math.cos(-theta),
y0 + sonarLenght * math.sin(-theta))])
ivtx = pylygon.Polygon([(x0, y0)])
sonarLoc.append(ivtx)
sonar.append(ply)
for i in range(daarrt.nBackSonar):
if daarrt.nBackSonar > 1:
x0 = -daarrt.robotLenght / 2 + 2
y0 = daarrt.robotWidth * i / (daarrt.nBackSonar -
1) - daarrt.robotWidth / 2
else:
x0 = -daarrt.robotLenght / 2 + 2
y0 = 0
ply = pylygon.Polygon([(x0, y0),
(-x0 - sonarLenght * math.cos(theta),
y0 + sonarLenght * math.sin(theta)),
(-x0 - sonarLenght * math.cos(-theta),
y0 + sonarLenght * math.sin(-theta))])
ivtx = pylygon.Polygon([(x0, y0)])
sonarLoc.append(ivtx)
sonar.append(ply)
for i in range(daarrt.nLeftSonar):
if daarrt.nLeftSonar > 1:
x0 = -daarrt.robotLenght / 2 + daarrt.wheelLenght / 2 + (
daarrt.robotLenght -
daarrt.robotWidth) * i / (daarrt.nLeftSonar - 1)
y0 = -daarrt.robotWidth / 2 + 2
else:
x0 = 0
y0 = -daarrt.robotWidth / 2 + 2
ply = pylygon.Polygon([(x0, y0),
(x0 + sonarLenght * math.sin(theta),
-y0 - sonarLenght * math.cos(theta)),
(x0 + sonarLenght * math.sin(-theta),
-y0 - sonarLenght * math.cos(-theta))])
ivtx = pylygon.Polygon([(x0, y0)])
sonarLoc.append(ivtx)
sonar.append(ply)
for i in range(daarrt.nRightSonar):
if daarrt.nRightSonar > 1:
x0 = -daarrt.robotLenght / 2 + daarrt.wheelLenght / 2 + (
daarrt.robotLenght -
daarrt.robotWidth) * i / (daarrt.nLeftSonar - 1)
y0 = daarrt.robotWidth / 2 + 2
else:
x0 = 0
y0 = daarrt.robotWidth / 2 + 2
ply = pylygon.Polygon([(x0, y0),
(x0 + sonarLenght * math.sin(theta),
+y0 + sonarLenght * math.cos(theta)),
(x0 + sonarLenght * math.sin(-theta),
+y0 + sonarLenght * math.cos(-theta))])
ivtx = pylygon.Polygon([(x0, y0)])
sonarLoc.append(ivtx)
sonar.append(ply)
sonar = [
poly_translate(poly_rotate(element, daarrt.robotCap), daarrt.posX,
daarrt.posY) for element in sonar
]
sonarLoc = [
poly_translate(poly_rotate(element, daarrt.robotCap), daarrt.posX,
daarrt.posY) for element in sonarLoc
]
if sonitv == 0:
return sonar
else:
return sonarLoc
def createSonarIntersect(daarrt):
'''
return :
list of pylygon objects containing each sonar of the DAARRT
arguments :
daarrt object
'''
intersect = []
theta = daarrt.sonarAngle * math.pi / 180.0
for i in range(daarrt.nFrontSonar):
if daarrt.nFrontSonar > 1:
x0 = daarrt.robotLenght / 2 + 2
y0 = daarrt.robotWidth * i / (daarrt.nFrontSonar -
1) - daarrt.robotWidth / 2
else:
x0 = daarrt.robotLenght / 2 + 2
y0 = 0
intersect.append(
pylygon.Polygon([(x0, y0 - 1), (x0, y0 + 1), (x0 + 50, y0)]))
for i in range(daarrt.nBackSonar):
if daarrt.nBackSonar > 1:
x0 = -daarrt.robotLenght / 2 + 2
y0 = daarrt.robotWidth * i / (daarrt.nBackSonar -
1) - daarrt.robotWidth / 2
else:
x0 = -daarrt.robotLenght / 2 + 2
y0 = 0
intersect.append(
pylygon.Polygon([(x0, y0 - 1), (x0, y0 + 1), (-x0 - 50, y0)]))
for i in range(daarrt.nLeftSonar):
if daarrt.nLeftSonar > 1:
x0 = -daarrt.robotLenght / 2 + daarrt.wheelLenght / 2 + (
daarrt.robotLenght -
daarrt.robotWidth) * i / (daarrt.nLeftSonar - 1)
y0 = -daarrt.robotWidth / 2 + 2
else:
x0 = 0
y0 = -daarrt.robotWidth / 2 + 2
intersect.append(
pylygon.Polygon([(x0 - 1, y0), (x0 + 1, y0), (x0, -y0 - 50)]))
for i in range(daarrt.nRightSonar):
if daarrt.nRightSonar > 1:
x0 = -daarrt.robotLenght / 2 + daarrt.wheelLenght / 2 + (
daarrt.robotLenght -
daarrt.robotWidth) * i / (daarrt.nLeftSonar - 1)
y0 = daarrt.robotWidth / 2 + 2
else:
x0 = 0
y0 = daarrt.robotWidth / 2 + 2
intersect.append(
pylygon.Polygon([(x0 - 1, y0), (x0 + 1, y0), (x0, y0 + 50)]))
intersect = [
poly_translate(poly_rotate(element, daarrt.robotCap), daarrt.posX,
daarrt.posY) for element in intersect
]
return intersect
