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)