def encontra_raio(self, direcaoDesejada):
# descobre se o robo vira para esquerda ou direita
y = Reta.ponto_e_angulo(self.pos, self.orientacao.angulo()).encontra_y(self.posDesejada.x)
theta = self.orientacao.delta_angulo_com(direcaoDesejada)
if (theta < 0 and theta != None):
normal = Vetor.gira(self.orientacao, -math.pi/2) # vira no sentido anti-horario / esquerda
elif (theta > 0):
normal = Vetor.gira(self.orientacao, math.pi/2) # vira no sentido horario / direita
else:
normal = None # linha reta ou vira sem sair do lugar
# encontra raio de curvatura
if (normal == None):
if (direcaoDesejada.tamanho() == 0):
raio = 0 # vira sem sair do lugar
else:
raio = -1 # linha reta
else:
phi = normal.angulo_com(direcaoDesejada)
raio = direcaoDesejada.quadrado()/(2*direcaoDesejada.tamanho() * math.cos(phi))
# encontra centro
centro = Vetor.multiplica(normal, raio)
centro = Vetor.soma(self.pos, centro)
# print "centro.x: %s, centro.y: %s, raio: %s" % (centro.x, centro.y, raio)
# corrige raio comparando com orientacao desejada
# if (raio > 0):
# raio = self.corrige_raio(raio, centro)
return raio
def corrige_raio(self, raio, centro):
# encontra variacao do angulo ate a posicao desejada
centroPos = Vetor.subtrai(self.pos, centro)
centroPosDesejada = Vetor.subtrai(self.posDesejada, centro)
deltaTheta = Vetor.delta_angulo_com(centroPos, centroPosDesejada)
# encontra diferenca entre orientacao final prevista e desejada
orientacaoPrevista = Vetor.gira(self.orientacao, deltaTheta)
erroTheta = orientacaoPrevista.delta_angulo_com(self.orientacaoDesejada)
if (deltaTheta < 0): # sentido anti-horario
raio -= 0.5*raio * ( erroTheta / math.pi ) # fracao do raio * fracao do erro do angulo
else: # sentido horario
raio += 0.5*raio * ( erroTheta / math.pi )
return raio
