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
def encontra_tragetoria(self): direcaoDesejada = Vetor.subtrai(self.posDesejada, self.pos) #vetor que liga a posicao atual a posicao desejada sentido = self.encontra_sentido(direcaoDesejada); raio = self.encontra_raio(direcaoDesejada); return (sentido, raio)