def getP(self, parenttable):
pos = self.getpos(parenttable.t)
if not parenttable.table.fullyOcc():
dist = euclidist(pos, parenttable.table.ball.getpos())
#print dist, parenttable.perr, normpdf(dist,0,parenttable.perr)
return normpdf(dist,0,parenttable.perr)
else:
# If the particle is in the goal while the ball is covered, it shouldn't be
idx = int((parenttable.t - self.inittime) * 1000)
if idx > len(self.path): return 1e-100
# If the ball is covered, the particle should test vs probability that it should be covered
# NOTE: MUST BE A FASTER WAY OF DOING THIS - use pg.Rect.clip() to get intersection
covmat = np.array([[parenttable.perr,0],[0,parenttable.perr]])
brad = int(parenttable.table.rad)
blocked = []
for w in parenttable.table.walls:
blocked.append(w.r.inflate(2*brad,2*brad))
p = 0
ps = []
# Break up occluders into non-overlapping, then run
uoccs = uniqueOccs(map(lambda x: x.r, parenttable.table.occludes), blocked)
#print map(lambda x: x.r, parenttable.table.occludes)
#print blocked
#print uoccs
for o in uoccs:
lefto = o.left
righto = o.right
topo = o.top
boto = o.bottom
''' OLD WAY: INDIVIDUAL PARAMETER CALC
for x in range(lefto, righto):
for y in range(topo, boto):
pt = (x,y)
good = True
for b in blocked:
if b.r.collidepoint(pt): good = False; continue
if good:
dist = euclidist(pos, pt)
p += normpdf(dist,0,parenttable.perr)
blocked.append(o)
'''
tst = mvnormcdf([lefto,topo],[righto,boto],pos,covmat)
#if tst < 0: print o, pos, tst
p += mvnormcdf([lefto,topo],[righto,boto],pos,covmat)
ps.append(p)
#if p < 0: print p, ps, uoccs
return p
def cdf(self, lower, upper, **kwargs):
cdf = 0.0
for w, m, c in zip(self.weights_, self.means_, self.covars_):
cdf += w*mvnormcdf(lower, upper, m, c, **kwargs)
return cdf
