def SIVIA(X0, data, eps):
stack = deque([IntervalVector(X0)])
lf = LargestFirst(eps/2.0)
k = 0
rbox = []
while len(stack) > 0:
X = stack.popleft()
k = k+1
t = IBOOL(testR(X,data[0],float(data[1]),data[2]))
if (t == IBOOL.IN):
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[r]' )
#rbox.append((X[0][0],X[0][1], X[1][0], X[1][1]))
elif (t == IBOOL.OUT):
# r = 1;
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[b]' )
elif (t == IBOOL.MAYBE):
# r = 1;
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[orange]' )
elif (t != IBOOL.OUT and t != IBOOL.MAYBE):
if (X.max_diam() > eps):
(X1, X2) = lf.bisect(X)
stack.append(X1)
stack.append(X2)
else :
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[y]' )
#vibes.drawBoxesUnion(rbox,'[r]')
vibes.axisEqual()
def SIVIA(X0, test, test3, test2, eps):
stack = deque([IntervalVector(X0)])
lf = LargestFirst(eps/2.0)
k = 0
boatBoxesNP = []
while len(stack) > 0:
X = stack.popleft()
k = k+1
t = test.test(X)
t2 = test2.test(X)
t3 = test3.test(X)
# if (t == IBOOL.IN):
# vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[g]' )
if (t==IBOOL.IN or t2 == IBOOL.IN or t3 == IBOOL.IN):
i,j = test.toPixels(X[0].lb(),X[1].ub())
i1,j1 = test.toPixels(X[0].ub(),X[1].ub())
i2,j2 = test.toPixels(X[0].ub(),X[1].lb())
i3,j3 = test.toPixels(X[0].lb(),X[1].lb())
boatBoxesNP.append([[i,j],[i1,j1],[i2,j2],[i3,j3]])
if (t==IBOOL.IN): # Gascogne
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[g]' )
elif (t2 == IBOOL.IN): # Robots
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[r]' )
elif (t3 == IBOOL.IN): # Trail
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[r]' )
elif (t == IBOOL.OUT and t2 == IBOOL.OUT and t3 == IBOOL.OUT):
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[b]' )
elif (t2 == IBOOL.MAYBE):
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[orange]' )
else:
try:
if (X.max_diam() > eps):
(X1, X2) = lf.bisect(X)
stack.append(X1)
stack.append(X2)
else :
vibes.drawBox(X[0][0],X[0][1], X[1][0], X[1][1], '[y]' )
except Exception:
print(type(lf),lf)
vibes.axisEqual()
return boatBoxesNP
(y / 500)**2)) + 6 * np.exp(-((x + 600) / 300)**2 -
((y - 400) / 300)**2) - 20
plt.subplot(121)
cont = plt.contour(x, y, h, 10)
plt.clabel(cont, inline=1, fontsize=8)
plt.axis('equal')
h0 = -17
dh = 0.3
### generate the slice image for an altitude h0 ###
hBin = ((h < h0 + dh) * (h > h0 - dh)).astype(int)
plt.subplot(122)
plt.imshow(hBin, extent=[-1000, 1000, -1000, 1000], origin='lower')
plt.title("h0 = " + str(h0) + ", dh = " + str(dh))
plt.show()
### create the contractor associated with the slice
hBin = np.flipud(
hBin) # flip the slice (the origin for an image is the top-left corner)
hOut = hBin.cumsum(0).cumsum(1) # compute the integral image
ctc = CtcRaster(hOut.T, -1000, 1000, 4, -4)
### results on Vibes ###
vibes.beginDrawing()
vibes.newFigure('CtcImage')
P = IntervalVector(2, [-1000, 1000])
pySIVIA(P, ctc, 30)
vibes.axisEqual()
def post_visit(self):
vibes.axisEqual()
sep = SepQInterProjF(seps)
sep.q = 0
# init drawing area
vibes.beginDrawing()
vibes.newFigure('Result')
vibes.setFigureProperties({'x': 0, 'y': 0, 'width': 1000, 'height': 1000})
#configure pySIVIA output
params = {'color_in': '#888888[#444444]', 'color_out':
'#888888[#DDDDDD]', 'color_maybe': '#888888[w]', 'use_patch' : True}
# create the initial box X0 = [-10, 10] x [-10, 10]
X0 = IntervalVector([[-12, 11], [-6, 17]]) # '#888888[#DDDDDD]'
# run SIVIA
(res_in, res_out, res_y) = pySIVIA(X0, sep, 0.1, **params)
vibes.drawAUV(robot[0], robot[1], 1, np.rad2deg(0.3))
for (x, y), d in zip(landmarks, dist):
vibes.drawCircle(x,y, 0.1, "[k]")
vibes.drawCircle(x,y, d.lb(), "k")
vibes.drawCircle(x,y, d.ub(), "k")
#equalize axis lenght
vibes.axisEqual()
vibes.endDrawing()
def post_visit(self):
vibes.axisEqual()
def post_visit(self, paving):
vibes.axisEqual()
