class CSpaceWorld:
g_obcColor = [ 150, 150, 150 ]
g_obcThickness = 0;
g_spaceColor = [ 0, 150, 0 ]
g_spaceThickness = 3;
def __init__( self, robot ):
self.mRobot = robot;
self.mRatio = 100;
self.mScaledWidth = 900;
self.mScaledHeight = 900;
self.mCollisionMgr = CollisionManager( robot );
def renderCSpace( self, ifsave = True ):
imgSurface = pygame.display.set_mode((self.mScaledHeight, self.mScaledWidth));
imgSurface.fill((255,255,255));
ObstacleColor = ( 50, 50, 50 );
obstaclePoints = [];
inObstacle = 0;
# Randomly sample and draw in the image
for i in range( 0, 200000 ):
scaledAlpha = random.randint( 0, self.mScaledWidth );
scaledPhi = random.randint( 0, self.mScaledHeight );
alpha, phi = self.map2UnscaledSpace( scaledAlpha, scaledPhi );
collide = self.mCollisionMgr.ifCollide( (alpha, phi) );
if collide:
inObstacle += 1;
obstaclePoints.append( (scaledAlpha, scaledPhi) );
pygame.draw.line( imgSurface, ObstacleColor, (scaledAlpha, scaledPhi), (scaledAlpha, scaledPhi));
print "{0} / {1} obstacles".format( inObstacle, 200000 );
# Save Data
if ifsave:
worldFile = open( "CSpace.txt", 'w' );
worldFile.write( "WIDTH\t{0}\nHEIGHT\t{1}\n".format( self.mScaledWidth, self.mScaledHeight ) );
for pair in obstaclePoints:
worldFile.write( "{0}\t{1}\n".format(pair[0], pair[1]) );
return imgSurface;
def map2UnscaledSpace( self, alpha, phi ):
"""The given parameters are scaled, we want to map them to -1.5~1.5PI"""
retAlpha = ((alpha-self.mScaledWidth/2) / float(self.mScaledWidth)) * math.pi * 2;
retPhi = ((phi-self.mScaledHeight/2) / float(self.mScaledHeight)) * math.pi * 2;
return retAlpha, retPhi;
def map2ScaledSpace( self, alpha, phi ):
"""Given two angles, alpha, phi, in the unscaled real world, map them to scaled space."""
retXcoord = alpha * self.mScaledWidth / (2.0*math.pi) + self.mScaledWidth/2.0;
retYcoord = phi * self.mScaledHeight / (2.0*math.pi) + self.mScaledHeight/2.0;
return retXcoord, retYcoord;
def loadCSpace(self, filename):
"""Load C space info from file. Return rendered image"""
imgSurface = None;
ObstacleColor = ( 50, 50, 50 );
obstaclePoints = [];
CSpaceFile = open(filename, 'r');
lineNum = 0;
for line in CSpaceFile:
lineNum += 1;
info = line.split('\t');
if(info[0]=="WIDTH"):
self.mScaledWidth = int(info[1]);
elif info[0] == "HEIGHT":
self.mScaledHeight = int(info[1]);
elif len(info)==2:
if( imgSurface == None ):
imgSurface = pygame.display.set_mode((self.mScaledHeight, self.mScaledWidth));
imgSurface.fill((255,255,255));
pygame.draw.line( imgSurface, ObstacleColor, (int(info[0]),int(info[1])), (int(info[0]),int(info[1])));
print "{0} / {1} obstacles".format( lineNum, 200000 );
return imgSurface;
def mapPath2UnscaledSpace(self, start, goal):
"""Map a path between two configurations in scaled space to unscaled space"""
dx = ( goal[0]-start[0] );
dy = ( goal[1]-start[1] );
if( dx > 0 and self.mScaledWidth - dx < dx ):
dx = -(self.mScaledWidth - dx);
elif( dx < 0 and self.mScaledWidth - (-dx) < (-dx)):
dx = self.mScaledWidth - (-dx);
if( dy > 0 and self.mScaledHeight - dy < dy ):
dy = -(self.mScaledHeight - dy);
elif( dy < 0 and self.mScaledHeight-(-dy)<(-dy) ):
dy = self.mScaledHeight-(-dy);
newgoal = (start[0] + dx, start[1] + dy);
start_alpha, start_phi = self.map2UnscaledSpace( start[0], start[1] );
goal_alpha, gloal_phi = self.map2UnscaledSpace( newgoal[0], newgoal[1] );
return (start_alpha, start_phi), (goal_alpha, gloal_phi);
