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