def testSynthetic(): '''Test against a synthetic set of pedestrians''' SIZE = 10.0 # define the domain minCorner = Vector2( -SIZE / 2.0, -SIZE / 2.0 ) domainSize = Vector2( SIZE, SIZE ) RES = int( SIZE / CELL_SIZE ) resolution = Vector2( RES, RES ) # define the signal ( a single point, moving from the origin to the corner traj, STEP_COUNT = syntheticPedestrians( SIZE ) print "Maximum kernel value:", kernel.data.max() print "Kernel sum: ", kernel.data.sum() grids = [] pedDomain = Grid.RectDomain( minCorner, domainSize ) sig = Signals.DiracSignal( pedDomain ) for i in xrange( STEP_COUNT ): sig.setData( traj[:, :, i] ) grid = Grid.DataGrid( minCorner, domainSize, resolution ) kernel.convolve( sig, grid ) ## grid.cells /= ( CELL_SIZE * CELL_SIZE ) grids.append( grid ) visGrids( grids )
def __init__(self, minCorner=Vector2(0.0, 0.0), size=Vector2(1.0, 1.0), resolution=(1, 1), cellSize=None): '''Grid constructor. @param minCorner A 2-tuple-like instace of floats. The position, in world space, of the "bottom-left" corner of the domain. (Minimum x- and y- values. @param size A 2-tuple-like instace of floats. The span of the domain (in world space.) The maximum values of the domain are minCorner[0] + size[0] and minCorner[1] + size[1], respectively. @param resolution A 2-tuple like instance of ints. The number of cells in the domain in both the x- and y-directions. This will imply a cell size. ''' RectDomain.__init__(self, minCorner, size) self.resolution = resolution # tuple (x, y) - int # size of each cell in the world grid if (cellSize is None): self.cellSize = Vector2(size[0] / float(resolution[0]), size[1] / float(resolution[1])) else: self.cellSize = cellSize assert (np.abs(self.cellSize[0] * self.resolution[0] - self.size[0]) < 0.0001 and np.abs(self.cellSize[1] * self.resolution[1] - self.size[1]) < 0.0001)
def makeDomain(domainX, domainY, cellSize=None): '''Defines a rectangular domain from the given specifications. If cellSize is not specified, the domain is a RectDomain, otherwise, it is an Abstract Grid. @param domainX A two-tuple like object of floats. The range of the test region on the x-axis as [minX, maxX] @param domainY A two-tuple like object of floats. The range of the test region on the y-axis as [minY, maxY] @param cellSize A float. The size of the cells for the discretized domain. @returns An instance of RectDomain or AbstractGrid, depending on cellSize. If cellSize is not defined, then a RectDomain is constructed, otherwise an AbstractGrid. ''' minCorner = Vector2(domainX[0], domainY[0]) size = Vector2(domainX[1] - domainX[0], domainY[1] - domainY[0]) if (cellSize is None): return RectDomain(minCorner, size) else: rX = int(np.floor(size[0] / cellSize)) # explicit truncation rY = int(np.floor(size[1] / cellSize)) # explicit truncation size = Vector2(rX * cellSize, rY * cellSize) return AbstractGrid(minCorner, size, (rX, rY))
def main(): '''Test the functionality''' pygame.init() map = FlameMap() SAMPLES = 300 samples = np.linspace(-5, 5, SAMPLES) X, Y = np.meshgrid(samples, samples) grid = np.dstack((X, Y)) if (True): print "Single segment" p1 = Vector2(-1.0, 1.0) p2 = Vector2(1.0, 0.0) seg = Segment(p1, p2) s = time.clock() dist = computeSegmentDistance(seg, grid) e = time.clock() print "\tTook %f seconds to compute %d distances" % (e - s, grid.size / 2) surface = map.colorOnSurface((dist.min(), dist.max()), dist.T[:, ::-1]) imgSeg = imgSpaceSegment(seg, (-5.0, 5.0), (-5.0, 5.0), SAMPLES, SAMPLES) pygame.draw.line(surface, (255, 255, 255), (imgSeg.p1.x, imgSeg.p1.y), (imgSeg.p2.x, imgSeg.p2.y)) pygame.image.save(surface, 'distFieldSeg.png') if (True): print "Obstacle" o = Obstacle() o.closed = True # create a hexagonal obstacle RADIUS = 2.0 RAD_SAMPLE = 12 for i in xrange(RAD_SAMPLE): theta = 2.0 * np.pi / RAD_SAMPLE * i x = np.cos(theta) * RADIUS y = np.sin(theta) * RADIUS o.vertices.append(Vector3(x, y, 0)) s = time.clock() dist = computeObstacleDistance(o, grid) e = time.clock() print "\tTook %f seconds to compute %d distances" % (e - s, grid.size / 2) ## print dist surface = map.colorOnSurface((dist.min(), dist.max()), dist.T[:, ::-1]) for seg in o.segments: imgSeg = imgSpaceSegment(seg, (-5.0, 5.0), (-5.0, 5.0), SAMPLES, SAMPLES) pygame.draw.line(surface, (255, 255, 255), (imgSeg.p1.x, imgSeg.p1.y), (imgSeg.p2.x, imgSeg.p2.y)) pygame.image.save(surface, 'distFieldObst.png')
def intersection(self, domain): '''Computes the intersection of two domains and reports it as a domain.minCorner @param domain An instance of RectDomain. The domain to intersect. @returns A RectDomain instance covering the region of intersection. Returns None if there is no intersection. ''' if (not self.intersects(domain)): return None # there is an intersection X = [ self.minCorner[0], domain.minCorner[0], self.minCorner[0] + self.size[0], domain.minCorner[0] + domain.size[0] ] X.sort() Y = [ self.minCorner[1], domain.minCorner[1], self.minCorner[1] + self.size[1], domain.minCorner[1] + domain.size[1] ] Y.sort() minCorner = Vector2(X[1], Y[1]) maxCorner = Vector2(X[2], Y[2]) return RectDomain(minCorner, maxCorner - minCorner)
def convolveDirac(self, signal, grid): '''Convolve this kernel with the dirac signal provided, placing the result on the provided grid. @param signal An instance of Signal class (See Signal.py ) @param grid An instance of grid (see Grid.py). Convolution is computed over the domain represented by the grid. The grid's values will be changed as a result of this operation. ''' for i, pos in enumerate(signal.impulses): k, sigma = self.getImpulseKernel(i, signal, grid) halfK = k / 2 try: self.splatTruncatedKernel(pos, grid, halfK, sigma) except KernelDomainError: pass if (self.reflectBoundaries): # compute domain in which the reflection matters w = (halfK + 0.5) * self._cellSize corner = Vector2(grid.minCorner[0] - w, grid.minCorner[1] - w) size = Vector2(grid.size[0] + 2 * w, grid.size[1] + 2 * w) tgtDomain = domains.RectDomain(corner, size) # compute reflection reflection = signal.reflectPoint(pos) for p in reflection: if (tgtDomain.pointInside(p)): self.splatTruncatedKernel(p, grid, halfK, sigma)
def test(): import sceneXML as XML import obstacles obst, bb = obstacles.readObstacles('junkObstacles.xml') expParam = XML.ExpParams() agtParam = XML.AgentSetParams() R = 0.18 agtParam.set('Common', 'r', R) noise = (0.1, 0.03) if (False): pos = fillRectangle(R, bb.min.x, bb.min.y, bb.max.x, bb.max.y, 4.5, COLUMN_RANK, noise) elif (False): p0 = Vector2(-0, 1) p1 = Vector2(3, -4) pos = corridorMob(R, p1, p0, 2.0, COLUMN_RANK, 200, None) elif (True): center = np.array((0.0, 0.0), dtype=np.float32) #Vector2( -3.0, 1.0 ) pos = rectMob(R, center, 0.0, 0.0, 2.0, 3.0, COLUMN_RANK, 60, None, 25) xml = XML.sceneXML(expParam, (pos, ), (agtParam, ), obst) f = open('junk.xml', 'w') f.write(xml) f.close()
def addLine(self): '''Causes the context to go into new line mode. Returning the new name.''' self.canDraw = True self.editState = self.ADD self.activeID = -1 self.names.append('Line %d' % len(self.names)) self.lines.append(GLFlowSegment(Vector2(0, 0), Vector2(0, 0))) self.activeLine = self.lines[-1] return self.names[-1]
def __init__(self, minCorner=Vector2(0.0, 0.0), size=Vector2(1.0, 1.0), resolution=(1, 1), cellSize=None, initVal=0.0, arrayType=np.float32, leaveEmpty=False): AbstractGrid.__init__(self, minCorner, size, resolution, cellSize) self.initVal = initVal self.clear(arrayType, leaveEmpty)
def readHeader(self): '''Reads the header of the open file.''' corner = struct.unpack('ff', self.file.read(8)) self.corner = Vector2(corner[0], corner[1]) size = struct.unpack('ff', self.file.read(8)) self.size = Vector2(size[0], size[1]) self.w, self.h = struct.unpack('ii', self.file.read(8)) self.arrayType = np.dtype(NP_TYPES[struct.unpack( 'i', self.file.read(4))[0]]) self.count = struct.unpack('i', self.file.read(4))[0] self.range = struct.unpack(self.arrayType.char * 2, self.file.read(self.arrayType.itemsize * 2)) self.headerSize = 32 + self.arrayType.itemsize * 2
def imgSpaceSegment(segment, xRange, yRange, xCount, yCount): '''Given the segment defined in world space, and the definition of the image space (specified by the xrange of values, the yrange of values and the number of cells in each direction) Computes the equivalent segment in that image space.''' xMin = xRange[0] xScale = xRange[1] - xRange[0] yMin = yRange[0] yScale = yRange[1] - yRange[0] col0 = int((segment.p1.x - xMin) / xScale * xCount) row0 = int((segment.p1.y - yMin) / yScale * yCount) col1 = int((segment.p2.x - xMin) / xScale * xCount) row1 = int((segment.p2.y - yMin) / yScale * yCount) return Segment(Vector2(col0, row0), Vector2(col1, row1))
def setDomain(self, minX, minY, maxX, maxY): '''Defines the analysis domain. @param minX A float. The minimum point of the rectangular domain along the x-axis. @param minY A float. The minimum point of the rectangular domain along the y-axis. @param maxX A float. The maximum point of the rectangular domain along the x-axis. @param maxY A float. The maximum point of the rectangular domain along the y-axis. ''' self.domainX = Vector2(minX, maxX) self.domainY = Vector2(minY, maxY)
def test(): from trajectory import loadTrajectory import os ## obstPath = r'/projects/crowd/fund_diag/paper/pre_density/experiment/Inputs/Corridor_oneway/c240_obstacles.xml' ## path = r'/projects/crowd/fund_diag/paper/pre_density/experiment/results/density/gaussian_S1.5/uo-065-240-240_combined_MB.density' ## outPath = r'/projects/crowd/fund_diag/paper/pre_density/experiment/results/density/gaussian_S1.5/uo-065-240-240_combined_MB_density/' pedFile = r'/projects/crowd/fund_diag/paper/pre_density/experiment/Inputs/Corridor_onewayDB/uo-065-240-240_combined_MB.txt' try: frameSet = loadTrajectory(pedFile) except ValueError: print "Unable to recognize the data in the file: %s" % (pedFile) domain = AbstractGrid(Vector2(0.0, -6), Vector2(2.4, 12), (10, 100)) gfs = GridFileSequence('sequence', arrayType=np.float32) gfs.computeVoronoiDensity(domain, frameSet, None)
def visGrids( grids, frames=None ): '''Given a set of grids, visualizes the grids''' # define limits of grid pygame.init() minVal = 0 maxVal = 0 for g in grids: M = g.maxVal() if ( M > maxVal ): maxVal = M print "Vis grids" print "\t(min, max):", minVal, maxVal RADIUS = int( np.ceil( 0.19 / CELL_SIZE ) ) for i, grid in enumerate( grids ): s = grid.surface( cMap, minVal, maxVal ) # memory leak in writing png. if ( frames ): try: frame, frameID = frames.next() except StopIteration: break for impulse in frame: x, y = grid.getCenter( Vector2( impulse[0], impulse[1] ) ) y = int( grid.resolution[1] ) - y pygame.draw.circle( s, ( 50, 50, 255 ), (x,y), RADIUS ) pygame.image.save( s, os.path.join( PATH, '%03d.png' % i ) ) pygame.image.save( cMap.lastMapBar(7), os.path.join( PATH, 'bar.png' ) )
def lineIntersectionTest(self, line1, line2): """ A function to perform lin intersection test in 2D @ param line1: a list of 2 points: starting and ending points (line1[0] is starting, line1[1] is ending) @ param line2: a list of 2 poitns: starting and ending points (line2[0] is starting, line2[1] is ending) @ return if there is an intersection between two line segment then return an intersection point which is a list of Vector2 else return an empty list""" x1 = line1.p1[0] x2 = line1.p2[0] x3 = line2.p1[0] x4 = line2.p2[0] y1 = line1.p1[1] y2 = line1.p2[1] y3 = line2.p1[1] y4 = line2.p2[1] denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1) num1 = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3) num2 = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3) if (denom == 0): return [] ua = num1 / denom ub = num2 / denom if ((ua > 0 and ua < 1) and (ub > 0 and ub < 1)): intersectX = x1 + ua * (x2 - x1) intersectY = y1 + ua * (y2 - y1) intersectPt = Vector2(intersectX, intersectY) return intersectPt return None
def next(self, stride=1, newInstance=True): """Returns the next frame in sequence from current point""" if (self.currFrameIndex >= self.maxFrames - 1): raise StopIteration self.currFrameIndex += stride if (newInstance or self.currFrame == None): self.currFrame = Frame(self.readAgtCount) for i in range(0, self.readAgtCount, self.readAgtStride): data = self.file.read(self.singleAgentRead) if (data == ''): self.currFrame = None raise StopIteration else: try: if (self.agentByteSize == 12): x, y, o = struct.unpack('fff', data) elif (self.agentByteSize == 16): x, y, o, s = struct.unpack('ffff', data) self.currFrame.agents[i].pos = Vector2(x, y) self.currFrame.agents[i].state = s except struct.error: self.currFrame = None raise StopIteration # seek forward based on skipping self.file.seek(self.agentDelta, 1) # advance to next frame self.file.seek(self.strideDelta, 1) # advance according to stride return self.currFrame, self.currFrameIndex
def splatKernel(self, pos, halfW, grid, value): '''Used by the dirac convolution. Splats the kernel at the given position. Splats the kernel into the given grid. @param pos A numpy array of shape (2,1). The position of the kernel @param halfW An int. The width of the kernel / 2. It should be true that halfW = kernelData.shape[0] / 2 @param kernelData A kxk numpy array of the kernel data.max @param value A float. The value of the kernel (in all locations) ''' center = grid.getCenter(Vector2(pos[0], pos[1])) gW = int(grid.resolution[0]) gH = int(grid.resolution[1]) l = max(0, center[0] - halfW) r = min(gW, center[0] + halfW + 1) b = max(0, center[1] - halfW) t = min(gH, center[1] + halfW + 1) if (l >= gW or r < 0 or b >= gH or t < 0): return if (l < r and b < t): # Convolution grid.cells[l:r, b:t] += value
def rasterizePosition2( self, frame, distFunc, maxRad ): """Given a frame of agents, rasterizes the whole frame""" kernel = Kernel2( maxRad, self.cellSize ) w, h = kernel.data.shape w /= 2 h /= 2 for agt in frame.agents: center = self.getCenter( agt.pos ) centerWorld = Vector2( center[0] * self.cellSize[0] + self.minCorner[0], center[1] * self.cellSize[1] + self.minCorner[1] ) kernel.instance( distFunc, centerWorld, agt.pos ) l = center[0] - w r = center[0] + w + 1 b = center[1] - h t = center[1] + h + 1 kl = 0 kb = 0 kr, kt = kernel.data.shape if ( l < 0 ): kl -= l l = 0 if ( b < 0 ): kb -= b b = 0 if ( r >= self.resolution[0] ): kr -= r - self.resolution[0] r = self.resolution[0] if ( t >= self.resolution[1] ): kt -= t - self.resolution[1] t = self.resolution[1] self.cells[ l:r, b:t ] += kernel.data[ kl:kr, kb:kt ]
def cellSegmentDistance(self, minima, maxima, p1, p2): '''Returns a 2D array of distances from a line segment: (p1, p2). @param minima: a 2-tuple-like object of ints. The indices (i, j) represent the smallest indices of the region to compute. @param maxima: a 2-tuple-like object of ints. The indices (I, J) represent the largest indices of the region to compute. The region is defined as field[ i:I, j:J ]. @param p1: a 2-tuple of floats. The first end point of the segment. @param p2: a 2-tuple of floats. The second end point of the segment. @return: a (I-i) x (J-j) array of floats. The distances from each cell center to point. ''' centers = self.cellCenters(minima, maxima) # create the implicit equation of the line A = p1[1] - p2[1] B = p2[0] - p1[0] mag = np.sqrt(A * A + B * B) A /= mag B /= mag C = (p1[0] * p2[1] - p2[0] * p1[1]) / mag # create a vector in the direction of the line. Use this to determine which points are near the # line segment and which are near the end points dir = Vector2(p2[0], p2[1]) - Vector2(p1[0], p1[1]) mag = dir.magnitude() dir = dir / mag dir = np.array((dir[0], dir[1])) relCenters = centers - np.array(p1) projection = np.sum(relCenters * dir, axis=2) nearP1 = projection < 0 nearP2 = projection > mag nearSeg = ~(nearP1 | nearP2) segDist = np.abs(A * centers[nearSeg, 0] + B * centers[nearSeg, 1] + C) dX = centers[nearP1, 0] - p1[0] dY = centers[nearP1, 1] - p1[1] p1Dist = np.sqrt(dX * dX + dY * dY) dX = centers[nearP2, 0] - p2[0] dY = centers[nearP2, 1] - p2[1] p2Dist = np.sqrt(dX * dX + dY * dY) distances = np.zeros((centers.shape[:-1])) distances[nearSeg] = segDist distances[nearP1] = p1Dist distances[nearP2] = p2Dist return distances
def constructKDTree(obstacles, domainX=Vector2(0.0, 3.2), domainY=Vector2(-6.0, 6.0)): # TODO : modify to work with line -> check intersection points with boundary """ Construct KD Tree from the list of obstacles; domainX and domainY specify size of the world in x-axis and y-axis @param obstacles: list of obstacles in the world @param domainX: range of the world in x-axis stored in form Vector2 @param domainY: range of the world in y-axis stored in form Vector2 @return Node of the Tree or None if we can construct the Tree""" if not obstacles: # obstacle in the scene return None if len(obstacles) <= 1: return Node(obstacles[0], None, None) sizeX = domainX[1] - domainX[0] sizeY = domainY[1] - domainY[0] # devide along the longer axis between the two midAxis = Vector2(0.0, 0.0) sortedList = [] axis = 0 if (sizeX > sizeY): sortedList = sort(obstacles, 0) axis = 0 midAxis = Vector2(domainX[0] + sizeX / 2., 0.0) else: sortedList = sort(obstacles, 1) axis = 1 midAxis = Vector2(0.0, domainY[0] + sizeY / 2.) left = [] right = [] for obstacle in sortedList: # partition the sortedList according to the axis if (obstacle[axis] < midAxis[axis]): left.append(obstacle) else: right.append(obstacle) leftNode = None rightNode = None if (axis == 0): leftNode = constructKDTree(left, Vector2(domainX[0], midAxis[0]), domainY) rightNode = constructKDTree(right, Vector2(midAxis[0], domainX[1]), domainY) else: leftNode = constructKDTree(left, domainX, Vector2(domainY[0], midAxis[1])) rightNode = constructKDTree(right, domainX, Vector2(midAxis[1], domainY[1])) return Node(midAxis, leftNode, rightNode)
def rasterizeProgress( self, f2, initFrame, prevProgress, excludeStates=(), callBack=None ): '''Given the current frame and the initial frame, computes the fraction of the circle that each agent has travelled around the kaabah.''' # TODO: don't let this be periodic. TWO_PI = 2.0 * np.pi for i in range( len( f2.agents ) ): # first compute progress based on angle between start and current position ag2 = f2.agents[ i ] if ( ag2.state in excludeStates ): continue ag1 = initFrame.agents[ i ] dir2 = ag2.pos.normalize() dir1 = ag1.pos.normalize() angle = np.arccos( dir2.dot( dir1 ) ) cross = dir1.det( dir2 ) if ( cross < 0 ): angle = TWO_PI - angle progress = angle / TWO_PI # now determine direction from best progress so far if ( progress > prevProgress[i,2] ): best = Vector2( prevProgress[i,0], prevProgress[i,1] ) cross = best.det( dir2 ) if ( cross < 0 ): # if I'm moving backwards from best progress BUT I'm apparently improving progress # I've backed over the 100% line. progress = 0.0 else: prevProgress[ i, 2 ] = progress prevProgress[ i, 0 ] = dir2[0] prevProgress[ i, 1 ] = dir2[1] center = self.getCenter( ag2.pos ) INFLATE = True # causes the agents to inflate more than a single cell if ( INFLATE ): l = center[0] - 1 r = l + 3 b = center[1] - 1 t = b + 3 else: l = center[0] r = l + 1 b = center[1] t = b + 1 if ( l < 0 ): l = 0 if ( b < 0 ): b = 0 if ( r >= self.resolution[0] ): r = self.resolution[0] if ( t >= self.resolution[1] ): t = self.resolution[1] self.cells[ l:r, b:t ] = progress if ( callBack ): callBack( progress )
def computeProgress(self, minCorner, size, resolution, maxRad, frameSet, timeStep, excludeStates, timeWindow=1): """Computes the progress from one frame to the next - progress is measured in the fraction of the circle traversed from the initial position""" print "Computing progress:" print "\tminCorner: ", minCorner print "\tsize: ", size print "\tresolution: ", resolution print "\tmaxRad: ", maxRad print "\ttime step: ", timeStep print "\ttime window:", timeWindow outFile = open(self.outFileName + '.progress', 'wb') outFile.write(self.header(minCorner, size, resolution)) maxVal = -1e6 minVal = 1e6 gridCount = 0 gridSize = resolution[0] * resolution[1] cellSize = Vector2(size.x / float(resolution[0]), size.y / float(resolution[1])) frameSet.setNext(0) data = [frameSet.next() for i in range(timeWindow + 1)] stats = StatRecord(frameSet.agentCount()) initFrame, initIndex = data[0] progress = self.initProgress(initFrame) while (data[-1][0]): print '.', f1, i1 = data.pop(0) f2, i2 = data[-1] g = RasterGrid(minCorner, size, resolution, 100.0) g.rasterizeProgress(f2, initFrame, progress, excludeStates, stats) m = g.minVal() if (m < minVal): minVal = m g.swapValues(100.0, -100.0) M = g.maxVal() if (M > maxVal): maxVal = M outFile.write(g.binaryString()) gridCount += 1 data.append(frameSet.next()) stats.nextFrame() print # add the additional information about grid count and maximum values self.fillInHeader(outFile, gridCount, minVal, maxVal) outFile.close() return stats
def test(): print "Testing signals!" if (True): print "\n\tTesting dirac signals" data = np.random.rand(10, 2) pedDomain = domains.RectDomain((-2.0, -2.0), (4.0, 4.0)) s = DiracSignal(pedDomain, data) for i, pos in enumerate(s.impulses): print '\t\t', i, pos, data[i, :] if (True): print "\n\tTesting field signal" data = np.array(((1, 2, 3), (10, 20, 30)), dtype=np.float32) grid = Grid.DataGrid((0.0, 0.0), (2.0, 3.0), (2, 3)) grid.cells[:, :] = data s = FieldSignal(grid) convolve = Grid.DataGrid(Vector2(1.0, 1.0), Vector2(1.0, 1.0), (1, 1)) domSig = s.getDomainSignal(convolve, 2, False) expected = np.array( ((0, 0, 0, 0, 0), (0, 1, 2, 3, 0), (0, 10, 20, 30, 0), (0, 0, 0, 0, 0), (0, 0, 0, 0, 0)), dtype=np.float32) ## print data print "\nNO REFLECTION" different = expected != domSig if (different.sum() != 0): print "\t\tFAILED" else: print "\t\tPASSED" ## print domSig domSig = s.getDomainSignal(convolve, 2, True) print "WITH REFLECTION" expected = np.array( ((1, 1, 2, 3, 3), (1, 1, 2, 3, 3), (10, 10, 20, 30, 30), (10, 10, 20, 30, 30), (1, 1, 2, 3, 3)), dtype=np.float32) ## different = expected != domSig if (different.sum() != 0): print "\t\tFAILED" else: print "\t\tPASSED"
def __init__( self ): # polygon self.poly = None # the obj face for this polygon # the explicit definition of the 3D plane for this polygon self.A = 0.0 self.B = 0.0 self.C = 0.0 self.center = Vector2(0.0, 0.0) self.edges = [] self.obstacles = []
def splatTruncatedKernel(self, pos, grid, halfK, smoothParm): '''Splats the kernel onto the domain -- computing only that portion of it required to fit onto the domain. @param pos An 2-tuple-like object of floats. The x,y position of the signal in world space. @param grid An instance of DataGrid. The kernel will be added into this kernel. @param halfK An int. The kernel half size (where the kernel is 2k + 1 cells wide. @param smoothParam A float. The smoothing parameter used in the kernel function. ''' center = grid.getCenter(Vector2(pos[0], pos[1])) l = center[0] - halfK r = center[0] + halfK + 1 b = center[1] - halfK t = center[1] + halfK + 1 gW = int(grid.resolution[0]) gH = int(grid.resolution[1]) if (l > gW or r < 0 or b > gH or t < 0): raise KernelDomainError kl = 0 kb = 0 kr = kt = 2 * halfK + 1 if (l < 0): kl -= l l = 0 if (b < 0): kb -= b b = 0 if (r >= gW): kr -= r - gW r = gW if (t >= gH): kt -= t - gH t = gH # compute the minimum 1D kernel section rangeMin = min(kl, kb) rangeMax = max(kr, kt) x = np.arange(rangeMin - halfK, rangeMax - halfK, dtype=np.float32) * self._cellSize data1D = self.FUNC(x, smoothParm) X = data1D[kl - rangeMin:kr - rangeMin] X.shape = (-1, 1) Y = data1D[kb - rangeMin:kt - rangeMin] Y.shape = (1, -1) kernel = np.dot(X, Y) grid.cells[l:r, b:t] += kernel
def drawObstacles( obstacles, surface, grid ): '''Draws an obstacle on the a surface using the grid as the translation from world to image coordsinates. @param obstacles An instance of ObstacleSet (see obstacles.py) @param surface An instance of a pygame surface. The obstacles will be drawn on this surface. @param grid An instance of AbstractGrid (see Grid.py). Used to map from world to image coordinates. ''' OBST_COLOR = np.array( (128,128,128), dtype=np.uint8 ) OBST_WIDTH = 1 for obst in obstacles.polys: if ( obst.closed ): verts = map( lambda x: grid.getCenter( Vector2( x[0], x[1] ) ), obst.vertices ) pygame.draw.polygon( surface, OBST_COLOR, verts ) else: for seg in obst.segments: p0 = grid.getCenter( Vector2( seg.p1[0], seg.p1[1] ) ) p1 = grid.getCenter( Vector2( seg.p2[0], seg.p2[1] ) ) pygame.draw.line( surface, OBST_COLOR, (p0[0],p0[1]), (p1[0], p1[1]), OBST_WIDTH )
def testPedestrian(): '''Test against legitimate pedestrian data''' # pedestrian domain minCorner = Vector2( 0.0, -6 ) domainSize = Vector2( 2.4, 12 ) pedDomain = Grid.RectDomain( minCorner, domainSize ) # grid domain minCorner = Vector2( 0.0, -2 ) domainSize = Vector2( 2.4, 4 ) resolution = Vector2( domainSize.x / CELL_SIZE, domainSize.y / CELL_SIZE) gridDomain = Grid.AbstractGrid( minCorner, domainSize, resolution ) # load pedestrian data pedFile = '/projects/crowd/fund_diag/paper/pre_density/experiment/Inputs/Corridor_onewayDB/uo-065-240-240_combined_MB.txt' try: data = loadTrajectory ( pedFile ) except ValueError: print "Unable to recognize the data in the file: %s" % ( pedFile ) return grids = [] sig = Signals.PedestrianSignal( pedDomain ) print gridDomain while ( True ): try: sig.setData( data ) except StopIteration: break grid = gridDomain.getDataGrid() kernel.convolve( sig, grid ) ## grid.cells /= ( CELL_SIZE * CELL_SIZE ) print "Frame %d has min/max values: %f, %f" % ( sig.index, grid.minVal(), grid.maxVal() ) grids.append( grid ) ## break data.setNext( 0 ) visGrids( grids, data )
def drawSites(sites, surface, grid, radius=3): '''Draws dots at the site locations onto the provided surface. @param sites An Nx2 numpy array of locations in world coordinates. @param surface An instance of a pygame Surface. The sites will be drawn onto this surface. @param grid An instance of AbstractGrid (see Grid.py) Used to map from world to image coordinates. @param radius A float. The radius of the sites (in pixels) ''' RADIUS = 3 for site in sites: x, y = grid.getCenter(Vector2(site[0], site[1])) y = grid.resolution[1] - y pygame.draw.circle(surface, (0, 0, 0), (x, y), radius + 2) pygame.draw.circle(surface, (255, 255, 255), (x, y), radius)
def rasterizeSpeedBlit( self, kernel, f2, f1, distFunc, maxRad, timeStep, excludeStates=(), callBack=None, maxSpeed=2.5 ): """Given two frames of agents, computes per-agent displacement and rasterizes the whole frame""" invDT = 1.0 / timeStep # compute speeds disp = f2[:, :2] - f1[:,:2] speed = np.sqrt( np.sum( disp * disp, axis = 1 ) ) * invDT tooFast = speed > maxSpeed if ( np.sum( tooFast ) > 0 ): topSpeed = speed[ ~tooFast ] speed[ tooFast ] = topSpeed for i in xrange( f2.shape[0] ): if ( excludeStates ): pass p = Vector2( f2[i, 0], f2[i, 1] ) center = self.getCenter( p ) INFLATE = True # causes the agents to inflate more than a single cell if ( INFLATE ): l = center[0] - 1 r = l + 3 b = center[1] - 1 t = b + 3 else: l = center[0] r = l + 1 b = center[1] t = b + 1 # outside if ( l >= self.resolution[0] or r < 0 or b >= self.resolution[1] or t < 0 ): continue # clip if ( l < 0 ): l = 0 if ( b < 0 ): b = 0 if ( r >= self.resolution[0] ): r = self.resolution[0] if ( t >= self.resolution[1] ): t = self.resolution[1] self.cells[ l:r, b:t ] = speed[i] if ( callBack ): callBack( speed[i] )
def convolveDirac(self, signal, grid): '''Convolve the kernel against a dirac signal. It is "fast" because it approximates the agent's position by the nearest grid cell center. @param signal An instance of DiracSignal. The kernel will be copied centered at each position in the signal. @param grid The grid onto which the kernel is splatted. It is assumed that the grid has been initialized to zero. ''' w = self.data1D.size w /= 2 expandDist = Vector2(grid.cellSize[0] * w, grid.cellSize[1] * w) minPt = grid.minCorner - expandDist size = grid.size + (2 * expandDist) domain = domains.RectDomain(minPt, size) impulses = signal.getDomainSignal(grid, domain, self.reflectBoundaries) kernelValue = 1.0 / (self._smoothParam * self._smoothParam) for pos in impulses: self.splatKernel(pos, w, grid, kernelValue)