1696 lines
73 KiB
C#
1696 lines
73 KiB
C#
using AgroBase.Properties;
|
|
using AgroBase.Services;
|
|
using CefSharp.WinForms;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Threading.Tasks;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models
|
|
{
|
|
public class MapasModel
|
|
{
|
|
public ChromiumWebBrowser chromiumWebBrowser { get; set; }
|
|
public Panel pnlMapa { get; set; }
|
|
private PictureBox picLoading { get; set; }
|
|
public Label lblMapa { get; set; }
|
|
public MapasService mapaService { get; set; } = new MapasService();
|
|
public List<List<GPSModel>> TrajetoriaMapa { get; set; } = new List<List<GPSModel>>();
|
|
public List<List<GPSModel>> TrajetoriaProjetada { get; set; } = new List<List<GPSModel>>();
|
|
public List<GPSModel> TrajetoriaDinamica { get; set; } = new List<GPSModel>();
|
|
public readonly object TrajetoriaDinamica_Lock = new object();
|
|
public List<string> RuasPercorrer { get; set; } = new List<string>();
|
|
public List<string> RuasPercorridas { get; set; } = new List<string>();
|
|
public string RuaEmAndamento { get; set; }
|
|
public int IdxRuaAtual { get; set; } = 0;
|
|
public int IdxUltimoPontoTrajetoria { get; set; } = 0;
|
|
public bool AproximandoUltimoPontoTrajetoria { get; set; } = false;
|
|
public int IdxUltimoPontoTrajetoriaDinamica { get; set; } = 0;
|
|
public double DistanciaTotal
|
|
{
|
|
get
|
|
{
|
|
double distancia = 0.0;
|
|
|
|
foreach (var trecho in TrajetoriaMapa)
|
|
{
|
|
distancia += GPSService.DistanciaDoTrecho(trecho);
|
|
}
|
|
|
|
return distancia;
|
|
}
|
|
}
|
|
public DirecaoCarroRua DirecaoCaminho
|
|
{
|
|
get
|
|
{
|
|
DirecaoCarroRua direcaoCaminho = DirecaoCarroRua.Parado;
|
|
lock (TrajetoriaDinamica_Lock)
|
|
{
|
|
if (TrajetoriaDinamica.Any())
|
|
{
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(TrajetoriaDinamica, TrajetoriaTipo.Dinamica);
|
|
int idxPontoMaisProximo = TrajetoriaDinamica.IndexOf(PontoMaisProximo);
|
|
direcaoCaminho = TrajetoriaDinamica[idxPontoMaisProximo].DirecaoTrecho;
|
|
}
|
|
else
|
|
{
|
|
if (ExtremoMaisProximoMapa == 0)
|
|
{
|
|
direcaoCaminho = DirecaoCarroRua.Ida;
|
|
}
|
|
else
|
|
{
|
|
direcaoCaminho = DirecaoCarroRua.Volta;
|
|
}
|
|
}
|
|
}
|
|
return direcaoCaminho;
|
|
}
|
|
}
|
|
public bool DentroDaRua
|
|
{
|
|
get
|
|
{
|
|
if (mapaService.DadosMapa != null && RuasPercorrer.Count() > IdxRuaAtual && TrajetoriaProjetada.Any())
|
|
{
|
|
/*try
|
|
{
|
|
string IDruaAtual = RuasPercorrer[IdxRuaAtual];
|
|
var Rua = mapaService.DadosMapa.features.Where(x => x.geometry.id == IDruaAtual).FirstOrDefault();
|
|
if (Rua != null)
|
|
{
|
|
bool dentro = false;
|
|
List<GPSModel> Trecho = new List<GPSModel>();
|
|
List<GPSModel> PontosRua = new List<GPSModel>();
|
|
Trajetoria[IdxRuaAtual].ToList().ForEach(x => PontosRua.Add(x));
|
|
if (DirecaoCaminho == DirecaoCarroRua.Volta)
|
|
{
|
|
PontosRua.Reverse();
|
|
}
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(PontosRua, TrajetoriaTipo.Rua);
|
|
int idx = PontosRua.IndexOf(PontoMaisProximo);
|
|
foreach (var Ponto in PontosRua)
|
|
{
|
|
// Adiciona os pontos que o robô já passou por eles no trecho, e quando chegar no ponto mais próximo, substitui ele pela posição atual
|
|
int idxPonto = PontosRua.IndexOf(Ponto);
|
|
if (idxPonto < idx)
|
|
{
|
|
Trecho.Add(Ponto);
|
|
}
|
|
else
|
|
{
|
|
if (idx > 0)
|
|
{
|
|
Trecho.Add(GPSService.UltimaLeitura);
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
double distTrecho = GPSService.DistanciaDoTrecho(Trecho);
|
|
dentro = (distTrecho > 0 && distTrecho < Rua.properties.Length) || (idx == 0 && !aproximando);
|
|
|
|
return dentro;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
return false;
|
|
}*/
|
|
|
|
var trajetoria = TrajetoriaProjetada[0];
|
|
for (int i = 0; i < trajetoria.Count - 1; i++)
|
|
{
|
|
var pontoA = trajetoria[i];
|
|
var pontoB = trajetoria[i + 1];
|
|
|
|
var a = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoA);
|
|
var b = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoB);
|
|
|
|
double margem = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS._EtapaAtual == StatusCarroMapa.CaminhandoRua ? 0.00001 : 0.000001;
|
|
|
|
// Verifica se a posição atual está entre os pontos A e B da trajetória
|
|
if (GPSService.EstaEntrePontos(GPSService.UltimaLeitura, pontoA, pontoB, margem))
|
|
{
|
|
return true; // O ponto está dentro da rua
|
|
}
|
|
}
|
|
return false; // O ponto está fora da rua
|
|
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
public double MargemLimiteEntradaRua { get; } = 1.0;
|
|
public double DistanciaProjecaoRua
|
|
{
|
|
get
|
|
{
|
|
return MargemLimiteEntradaRua * 3.0;
|
|
}
|
|
}
|
|
public bool NaMargemEntradaRua
|
|
{
|
|
get
|
|
{
|
|
bool NaMargem = false;
|
|
GPSModel PontoRua = ExtremoMaisProximoRuaProjetada == 0 ? PrimeiroPontoRuaProjetada : UltimoPontoRuaProjetada;
|
|
double Distancia = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PontoRua);
|
|
double DistanciaAnterior = GPSService.DistanciaEntrePontos(GPSService.PenultimaLeitura, PontoRua);
|
|
|
|
double DistanciaFinal = 0;
|
|
if (Distancia > DistanciaAnterior && (Distancia <= (MargemLimiteEntradaRua * 2)))
|
|
{
|
|
DistanciaFinal = Distancia * -1;
|
|
}
|
|
else
|
|
{
|
|
DistanciaFinal = Distancia;
|
|
}
|
|
|
|
if (DistanciaFinal <= MargemLimiteEntradaRua)
|
|
{
|
|
NaMargem = true;
|
|
}
|
|
|
|
return NaMargem;
|
|
}
|
|
}
|
|
public GPSModel PrimeiroPontoRuaProjetada
|
|
{
|
|
get
|
|
{
|
|
if (TrajetoriaProjetada.Any(x => x.Any()))
|
|
{
|
|
var primeiroPontoRua = TrajetoriaProjetada[0].First();
|
|
return primeiroPontoRua;
|
|
}
|
|
else
|
|
{
|
|
return new GPSModel();
|
|
}
|
|
|
|
}
|
|
}
|
|
public GPSModel UltimoPontoRuaProjetada
|
|
{
|
|
get
|
|
{
|
|
if (TrajetoriaProjetada.Any(x => x.Any()))
|
|
{
|
|
var ultimoPontoRua = TrajetoriaProjetada[0].Last();
|
|
return ultimoPontoRua;
|
|
}
|
|
else
|
|
{
|
|
return new GPSModel();
|
|
}
|
|
}
|
|
}
|
|
public GPSModel PrimeiroPontoRuaMapa
|
|
{
|
|
get
|
|
{
|
|
var Trajetoria = TrajetoriaMapa;
|
|
|
|
if (Trajetoria.Any(x => x.Any()) && Trajetoria.Count() > IdxRuaAtual)
|
|
{
|
|
var primeiroPontoRua = Trajetoria.Skip(IdxRuaAtual).First().First();
|
|
return primeiroPontoRua;
|
|
}
|
|
else
|
|
{
|
|
return new GPSModel();
|
|
}
|
|
|
|
}
|
|
}
|
|
public GPSModel UltimoPontoRuaMapa
|
|
{
|
|
get
|
|
{
|
|
var Trajetoria = TrajetoriaMapa;
|
|
|
|
if (Trajetoria.Any(x => x.Any()) && Trajetoria.Count() > IdxRuaAtual)
|
|
{
|
|
var ultimoPontoRua = Trajetoria.Skip(IdxRuaAtual).First().Last();
|
|
return ultimoPontoRua;
|
|
}
|
|
else
|
|
{
|
|
return new GPSModel();
|
|
}
|
|
}
|
|
}
|
|
public int ExtremoMaisProximoMapa
|
|
{
|
|
get
|
|
{
|
|
double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRuaMapa);
|
|
double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRuaMapa);
|
|
|
|
// 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
|
|
// 1 para mais próximo do último ponto da rua (SAINDO)
|
|
return (distPP < distUP) ? 0 : 1;
|
|
}
|
|
}
|
|
public int ExtremoMaisProximoRuaProjetada
|
|
{
|
|
get
|
|
{
|
|
double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRuaProjetada);
|
|
double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRuaProjetada);
|
|
|
|
// 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
|
|
// 1 para mais próximo do último ponto da rua (SAINDO)
|
|
return (distPP < distUP) ? 0 : 1;
|
|
}
|
|
}
|
|
public double AnguloCaminho
|
|
{
|
|
get
|
|
{
|
|
double anguloCaminho = 0;
|
|
try
|
|
{
|
|
lock (TrajetoriaDinamica_Lock)
|
|
{
|
|
if (TrajetoriaDinamica.Any())
|
|
{
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(TrajetoriaDinamica, TrajetoriaTipo.Dinamica);
|
|
int idx = TrajetoriaDinamica.IndexOf(PontoMaisProximo);
|
|
|
|
if ((idx + 1) >= TrajetoriaDinamica.Count())
|
|
{
|
|
return anguloCaminho;
|
|
}
|
|
|
|
double angulo = GPSService.CalcularOrientacao(TrajetoriaDinamica[idx], TrajetoriaDinamica[idx + 1]);
|
|
if (angulo == 0)
|
|
{
|
|
angulo = GPSService.CalcularOrientacao(TrajetoriaDinamica[idx + 1], TrajetoriaDinamica[idx + 2]);
|
|
}
|
|
|
|
anguloCaminho = (!(angulo >= 0 || angulo <= 0)) ? 0 : angulo;
|
|
}
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
anguloCaminho = 0;
|
|
}
|
|
finally
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloRua = anguloCaminho;
|
|
}
|
|
|
|
return anguloCaminho;
|
|
}
|
|
}
|
|
public double AnguloRuaAtual
|
|
{
|
|
get
|
|
{
|
|
try
|
|
{
|
|
if (!TrajetoriaProjetada.Any())
|
|
{
|
|
return 0;
|
|
}
|
|
var RuaAtual = TrajetoriaProjetada[0];
|
|
if (RuaAtual.Any())
|
|
{
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(RuaAtual, TrajetoriaTipo.Rua);
|
|
int idx = RuaAtual.IndexOf(PontoMaisProximo);
|
|
|
|
if ((idx + 1) >= RuaAtual.Count())
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
double angulo = GPSService.CalcularOrientacao(RuaAtual[idx], RuaAtual[idx + 1]);
|
|
if (angulo == 0)
|
|
{
|
|
angulo = GPSService.CalcularOrientacao(RuaAtual[idx + 1], RuaAtual[idx + 2]);
|
|
}
|
|
|
|
return (!(angulo >= 0 || angulo <= 0)) ? 0 : angulo;
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
public double DistanciaEsquerda
|
|
{
|
|
get
|
|
{
|
|
if (IdxRuaAtual >= TrajetoriaMapa.Count())
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
double distancia = 0;
|
|
DirecaoCarroRua direcaoCaminho = DirecaoCaminho;
|
|
|
|
if (direcaoCaminho == DirecaoCarroRua.Ida)
|
|
{
|
|
distancia = DistanciaAteRua(TrajetoriaMapa[IdxRuaAtual], VariaveisEquipamento.LarguraEsquerda);
|
|
}
|
|
else if (direcaoCaminho == DirecaoCarroRua.Volta)
|
|
{
|
|
if (IdxRuaAtual == 0)
|
|
{
|
|
distancia = DistanciaEntreRuas(TrajetoriaMapa) / 2.0;
|
|
}
|
|
else
|
|
{
|
|
distancia = DistanciaAteRua(TrajetoriaMapa[IdxRuaAtual - 1], VariaveisEquipamento.LarguraEsquerda);
|
|
}
|
|
}
|
|
|
|
return distancia;
|
|
}
|
|
}
|
|
public double DistanciaDireita
|
|
{
|
|
get
|
|
{
|
|
if (IdxRuaAtual >= TrajetoriaMapa.Count())
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
double distancia = 0;
|
|
DirecaoCarroRua direcaoCaminho = DirecaoCaminho;
|
|
|
|
if (direcaoCaminho == DirecaoCarroRua.Ida)
|
|
{
|
|
if (IdxRuaAtual == 0)
|
|
{
|
|
distancia = DistanciaEntreRuas(TrajetoriaMapa) / 2.0;
|
|
}
|
|
else
|
|
{
|
|
distancia = DistanciaAteRua(TrajetoriaMapa[IdxRuaAtual - 1], VariaveisEquipamento.LarguraDireita);
|
|
}
|
|
}
|
|
else if (direcaoCaminho == DirecaoCarroRua.Volta)
|
|
{
|
|
distancia = DistanciaAteRua(TrajetoriaMapa[IdxRuaAtual], VariaveisEquipamento.LarguraDireita);
|
|
}
|
|
|
|
return distancia;
|
|
}
|
|
}
|
|
private DateTime UltimaLeituraDistancia;
|
|
private int idxPontoAproximadoRua = 0;
|
|
public int IdxPontoAproximadoRua
|
|
{
|
|
get
|
|
{
|
|
if (!TrajetoriaProjetada.Any())
|
|
{
|
|
return 0;
|
|
}
|
|
try
|
|
{
|
|
if (UltimaLeituraDistancia == null)
|
|
{
|
|
UltimaLeituraDistancia = DateTime.Now.Add(new TimeSpan(0, 0, -10));
|
|
}
|
|
else if (UltimaLeituraDistancia.Add(new TimeSpan(0, 0, 5)) < DateTime.Now)
|
|
{
|
|
int idx = -1;
|
|
double menorDistancia = 9999;
|
|
double distanciaAnterior = 9999;
|
|
|
|
var Rua = TrajetoriaProjetada[0];
|
|
|
|
foreach (var Ponto in Rua)
|
|
{
|
|
double dist = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, Ponto);
|
|
if (dist < menorDistancia)
|
|
{
|
|
menorDistancia = dist;
|
|
idx = Rua.IndexOf(Ponto);
|
|
}
|
|
else if (dist > distanciaAnterior)
|
|
{
|
|
break;
|
|
}
|
|
distanciaAnterior = dist;
|
|
}
|
|
idxPontoAproximadoRua = idx;
|
|
|
|
UltimaLeituraDistancia = DateTime.Now;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
|
|
}
|
|
|
|
|
|
return idxPontoAproximadoRua;
|
|
}
|
|
}
|
|
|
|
public double DistanciaAteProximoPonto
|
|
{
|
|
get
|
|
{
|
|
if (!TrajetoriaProjetada.Any() || (TrajetoriaProjetada[0].Count() - 1) <= IdxUltimoPontoTrajetoria)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
GPSModel ponto = IdxUltimoPontoTrajetoria == 0 ? TrajetoriaProjetada[0][0] : TrajetoriaProjetada[0][IdxUltimoPontoTrajetoria + 1];
|
|
|
|
double distanciaProximoPonto = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, ponto);
|
|
|
|
return distanciaProximoPonto;
|
|
}
|
|
}
|
|
public double DistanciaAtePontoAnterior
|
|
{
|
|
get
|
|
{
|
|
if (!TrajetoriaProjetada.Any() || (TrajetoriaProjetada[0].Count() - 1) <= IdxUltimoPontoTrajetoria || IdxUltimoPontoTrajetoria == 0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
GPSModel ponto = IdxUltimoPontoTrajetoria == TrajetoriaProjetada[0].Count() - 1 ? TrajetoriaProjetada[0].Last() : TrajetoriaProjetada[0][IdxUltimoPontoTrajetoria - 1];
|
|
|
|
double distanciaPontoAnterior = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, ponto);
|
|
|
|
return distanciaPontoAnterior;
|
|
}
|
|
}
|
|
|
|
private double distanciaInicialInicioParaFim = -1;
|
|
private double distanciaInicialFimParaInicio = -1;
|
|
public double DistanciaExtraTrajetoria
|
|
{
|
|
get
|
|
{
|
|
if (Math.Min(distanciaInicialInicioParaFim, distanciaInicialFimParaInicio) < 0)
|
|
{
|
|
return 0;
|
|
}
|
|
return Math.Min(distanciaInicialInicioParaFim, distanciaInicialFimParaInicio);
|
|
}
|
|
}
|
|
private bool GerandoTrajetoria = false;
|
|
private DateTime UltimaAtualizacaoTrajetoriaDinamica = DateTime.MinValue;
|
|
private AsyncTaskTimerModel tmrCarregamento;
|
|
|
|
|
|
public void btnCarregar_Click(object sender, EventArgs e, string caminhoArquivo = "", bool monitoramento = false)
|
|
{
|
|
string script = monitoramento ? PythonService.ScriptMapFollower : PythonService.ScriptMapConverter;
|
|
mapaService.CarregarMapa(script, MapasVariaveisModel.NomeArquivoMapaInterativo, MapasVariaveisModel.NomeArquivoMapaInterativoSaida, caminhoArquivo);
|
|
|
|
if (mapaService.MapaUrl != "")
|
|
{
|
|
if (picLoading == null)
|
|
{
|
|
picLoading = new PictureBox();
|
|
}
|
|
picLoading.Dock = DockStyle.Fill;
|
|
picLoading.BackColor = System.Drawing.Color.Transparent;
|
|
picLoading.Image = Resources.loading;
|
|
picLoading.Name = "picLoading";
|
|
picLoading.SizeMode = PictureBoxSizeMode.Zoom;
|
|
picLoading.Visible = true;
|
|
pnlMapa.Controls.Add(picLoading);
|
|
|
|
tmrCarregamento?.Dispose();
|
|
tmrCarregamento = new AsyncTaskTimerModel("tmrCarregamento", tmrCarregamento_Tick, 500, pnlMapa);
|
|
tmrCarregamento.Start();
|
|
}
|
|
}
|
|
|
|
public void CarregarMapaGPS()
|
|
{
|
|
if (picLoading == null)
|
|
{
|
|
picLoading = new PictureBox()
|
|
{
|
|
SizeMode = PictureBoxSizeMode.Zoom,
|
|
Location = new System.Drawing.Point(0, 0),
|
|
Size = new System.Drawing.Size(pnlMapa.Width, pnlMapa.Height)
|
|
};
|
|
pnlMapa.Controls.Add(picLoading);
|
|
}
|
|
mapaService.CarregarMapaGPS(MapasVariaveisModel.NomeArquivoMapaGPS);
|
|
|
|
tmrCarregamento?.Dispose();
|
|
tmrCarregamento = new AsyncTaskTimerModel("tmrCarregamento", tmrCarregamento_Tick, 500, pnlMapa);
|
|
tmrCarregamento.Start();
|
|
}
|
|
|
|
private async Task tmrCarregamento_Tick()
|
|
{
|
|
try
|
|
{
|
|
lock (mapaService.pythonProcess)
|
|
{
|
|
if (mapaService.pythonProcess == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (mapaService.pythonProcess != null && !mapaService.pythonProcess.HasExited)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Processo finalizado ou não associado
|
|
picLoading.Visible = false;
|
|
mapaService.Carregado = true;
|
|
|
|
if (mapaService.NomeArquivoMapa == MapasVariaveisModel.NomeArquivoMapaInterativo)
|
|
{
|
|
mapaService.CarregarDadosMapa();
|
|
if (lblMapa != null)
|
|
{
|
|
lblMapa.Text = "Mapa carregado: " + mapaService.NomeArquivos;
|
|
}
|
|
|
|
PopularTrajetoriaMapa();
|
|
}
|
|
|
|
IniciarProcessamento();
|
|
|
|
lock (mapaService.pythonProcess)
|
|
{
|
|
mapaService.pythonProcess.Dispose();
|
|
mapaService.pythonProcess = null;
|
|
}
|
|
|
|
tmrCarregamento.Dispose();
|
|
}
|
|
catch (InvalidOperationException ex)
|
|
{
|
|
Console.WriteLine($"Erro ao verificar o estado do processo: {ex.Message}");
|
|
picLoading.Visible = false;
|
|
tmrCarregamento.Dispose();
|
|
}
|
|
}
|
|
|
|
public void PopularTrajetoriaMapa()
|
|
{
|
|
if (mapaService.DadosMapa != null)
|
|
{
|
|
double OrientacaoRuas = -999;
|
|
//DistanciaTotal = 0;
|
|
/*if (!RuasPercorrer.Any())
|
|
{
|
|
RuasPercorrer = mapaService.DadosMapa.features.Select(x => x.geometry.id).ToList();
|
|
}*/
|
|
TrajetoriaMapa = new List<List<GPSModel>>();
|
|
|
|
foreach (var ID in RuasPercorrer)
|
|
{
|
|
var rua = mapaService.DadosMapa.features.FirstOrDefault(x => ID == x.geometry.id);
|
|
if (rua != null)
|
|
{
|
|
TrajetoriaMapa.Add(new List<GPSModel>());
|
|
foreach (var ponto in rua.geometry.coordinates)
|
|
{
|
|
GPSModel item = new GPSModel()
|
|
{
|
|
Longitude = ponto[0],
|
|
Latitude = ponto[1],
|
|
};
|
|
TrajetoriaMapa[TrajetoriaMapa.Count - 1].Add(item);
|
|
/*if (Trajetoria[Trajetoria.Count - 1].Count > 1)
|
|
{
|
|
DistanciaTotal += GPSService.DistanciaEntrePontos(Trajetoria[Trajetoria.Count - 1][Trajetoria[Trajetoria.Count - 1].Count - 2], item);
|
|
}*/
|
|
}
|
|
|
|
var Rua = TrajetoriaMapa[TrajetoriaMapa.Count - 1];
|
|
if (OrientacaoRuas == -999)
|
|
{
|
|
OrientacaoRuas = GPSService.CalcularOrientacao(Rua[Rua.Count - 1], Rua[Rua.Count - 2]);
|
|
}
|
|
else
|
|
{
|
|
double MargemErroAngulo = 45;
|
|
double orientacaoRua = GPSService.CalcularOrientacao(Rua[Rua.Count - 1], Rua[Rua.Count - 2]);
|
|
double anguloOposto = (orientacaoRua + 180) % 360;
|
|
if ((anguloOposto + MargemErroAngulo) >= OrientacaoRuas && (anguloOposto - MargemErroAngulo) <= OrientacaoRuas)
|
|
{
|
|
Rua.Reverse();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void IniciarProcessamento()
|
|
{
|
|
//APIService.Inicializar();
|
|
|
|
chromiumWebBrowser = new ChromiumWebBrowser(mapaService.MapaUrl);
|
|
pnlMapa.Controls.Add(chromiumWebBrowser);
|
|
chromiumWebBrowser.Dock = DockStyle.Fill;
|
|
}
|
|
|
|
public void PararProcessamento()
|
|
{
|
|
//APIService.Parar();
|
|
//GPSService.PararRecepcaoDados();
|
|
}
|
|
|
|
|
|
public double DistanciaEntreRuas(List<List<GPSModel>> ruas)
|
|
{
|
|
double distanciaTotal = 0;
|
|
|
|
if (ruas.Count < 2)
|
|
{
|
|
// Se houver menos de duas ruas, não há distância a calcular
|
|
return distanciaTotal;
|
|
}
|
|
|
|
double menorDistancia = double.MaxValue;
|
|
|
|
for (int i = 0; i < ruas.Count - 1; i++)
|
|
{
|
|
List<GPSModel> ruaAtual = ruas[i];
|
|
List<GPSModel> proximaRua = ruas[i + 1];
|
|
|
|
// Calcule a distância entre as duas ruas (rua atual e próxima rua)
|
|
// Aqui você pode usar o método GPSService.CalcularDistanciaPontos
|
|
// ou uma função personalizada para calcular a distância entre as ruas
|
|
|
|
|
|
foreach (GPSModel pontoAtual in ruaAtual)
|
|
{
|
|
foreach (GPSModel pontoProxima in proximaRua)
|
|
{
|
|
// Calcular a distância entre pontoAtual e pontoProxima
|
|
double distancia = GPSService.DistanciaEntrePontos(pontoAtual, pontoProxima);
|
|
|
|
// Verifique se esta é a menor distância
|
|
if (distancia < menorDistancia)
|
|
{
|
|
menorDistancia = distancia;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Adiciona a menor distância encontrada à distância total
|
|
distanciaTotal += menorDistancia;
|
|
}
|
|
|
|
double distanciaMedia = distanciaTotal / ruas.Count();
|
|
|
|
return menorDistancia;
|
|
}
|
|
|
|
public double DistanciaAteRua(List<GPSModel> Rua, double larguraRobo)
|
|
{
|
|
GPSModel pontoAtual = GPSService.UltimaLeitura;
|
|
|
|
(GPSModel P0, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Rua, TrajetoriaTipo.Geral);
|
|
int idxP0 = Rua.IndexOf(P0);
|
|
bool PontoProximo = idxP0 < Rua.Count() - 1;
|
|
bool PontoAnterior = idxP0 > 0;
|
|
GPSModel P1 = PontoProximo ? Rua[idxP0 + 1] : PontoAnterior ? Rua[idxP0 - 1] : pontoAtual;
|
|
|
|
double a = GPSService.DistanciaEntrePontos(pontoAtual, P0);
|
|
double b = GPSService.DistanciaEntrePontos(pontoAtual, P1);
|
|
double c = GPSService.DistanciaEntrePontos(P0, P1);
|
|
|
|
double distancia = FuncoesMatematicas.CalcularAlturaTriangulo(a, b, c);
|
|
|
|
distancia -= (larguraRobo / 100.0);
|
|
|
|
return distancia;
|
|
}
|
|
|
|
|
|
public void ReiniciarTrajetoriaDinamica()
|
|
{
|
|
distanciaInicialFimParaInicio = -1;
|
|
distanciaInicialInicioParaFim = -1;
|
|
}
|
|
|
|
public void GerarTrajetoriaDinamica()
|
|
{
|
|
if (GerandoTrajetoria)
|
|
{
|
|
return;
|
|
}
|
|
|
|
GerandoTrajetoria = true;
|
|
|
|
double distanciaEntreRuas = DistanciaEntreRuas(TrajetoriaMapa) / 2.0;
|
|
List<GPSModel> Caminho = new List<GPSModel>();
|
|
List<List<GPSModel>> _TrajetoriaProjetada = new List<List<GPSModel>>();
|
|
int _extremoMaisProximoMapa = ExtremoMaisProximoMapa;
|
|
|
|
StatusCarroMapa statusCarro = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.StatusAtual;
|
|
StatusCarroMapa statusCarroAnterior = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS._EtapaAnterior;
|
|
DirecaoCarroRua direcaoRua = DirecaoCarroRua.Parado;
|
|
lock (TrajetoriaDinamica_Lock)
|
|
{
|
|
direcaoRua = TrajetoriaDinamica.Any() && TrajetoriaDinamica[0].DirecaoTrecho != DirecaoCarroRua.Parado ? TrajetoriaDinamica[0].DirecaoTrecho : _extremoMaisProximoMapa == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
|
}
|
|
|
|
for (int i = IdxRuaAtual; i < TrajetoriaMapa.Count(); i++)
|
|
{
|
|
List<GPSModel> RuaAtual = TrajetoriaMapa.Skip(i).First();
|
|
|
|
double anguloProjetar = GPSService.CalcularOrientacao(RuaAtual.First(), RuaAtual.Skip(1).First()) + 90;
|
|
|
|
List<GPSModel> RuaProjetada = ProjetarRua(RuaAtual, distanciaEntreRuas, anguloProjetar);
|
|
_TrajetoriaProjetada.Add(RuaProjetada);
|
|
|
|
if (Caminho.Any())
|
|
{
|
|
double dist0 = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], RuaProjetada[0]);
|
|
double dist1 = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], RuaProjetada[RuaProjetada.Count() - 1]);
|
|
direcaoRua = dist0 < dist1 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
|
}
|
|
|
|
RuaProjetada.ForEach(x =>
|
|
{
|
|
x.idxRua = i;
|
|
x.DirecaoTrecho = direcaoRua;
|
|
});
|
|
if (direcaoRua == DirecaoCarroRua.Volta)
|
|
{
|
|
RuaProjetada.Reverse();
|
|
}
|
|
|
|
double anguloProjetar1 = GPSService.CalcularOrientacao(RuaProjetada.Skip(1).First(), RuaProjetada.First());
|
|
GPSModel PrimeiroPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.First(), DistanciaProjecaoRua, anguloProjetar1);
|
|
double anguloProjetar2 = GPSService.CalcularOrientacao(RuaProjetada.Skip(RuaProjetada.Count() - 2).First(), RuaProjetada.Last());
|
|
GPSModel UltimoPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.Last(), DistanciaProjecaoRua, anguloProjetar2);
|
|
|
|
if (!Caminho.Any())
|
|
{
|
|
bool dentroRua = DentroDaRua;
|
|
|
|
GPSModel inicio = GPSService.UltimaLeitura;
|
|
|
|
List<GPSModel> _caminho = new List<GPSModel>();
|
|
|
|
GPSModel PontoComparar = new GPSModel();
|
|
int idxPontoMaisProximo = 0;
|
|
bool DefinirRotaInicial = false;
|
|
|
|
// O carro esta se direcionando para iniciar a operacao na primeira rua, entao escolhe o caminho mais proximo
|
|
if (DistanciaExtraTrajetoria == 0)
|
|
{
|
|
List<GPSModel> _caminhoDoInicioParaFim = GerarTrajetoLigacaoCarroPontoInicial(inicio, RuaAtual, RuaProjetada, RuaAtual[0], 0, DirecaoCarroRua.Ida, statusCarro);
|
|
distanciaInicialInicioParaFim = GPSService.DistanciaDoTrecho(_caminhoDoInicioParaFim);
|
|
|
|
List<GPSModel> _caminhoDoFimParaInicio = GerarTrajetoLigacaoCarroPontoInicial(inicio, RuaAtual, RuaProjetada, RuaAtual[RuaAtual.Count() - 1], RuaAtual.Count() - 1, DirecaoCarroRua.Volta, statusCarro);
|
|
distanciaInicialFimParaInicio = GPSService.DistanciaDoTrecho(_caminhoDoFimParaInicio);
|
|
|
|
DefinirRotaInicial = true;
|
|
}
|
|
|
|
bool IrPeloInicio = distanciaInicialInicioParaFim < distanciaInicialFimParaInicio;
|
|
double dP0 = GPSService.DistanciaEntrePontos(inicio, RuaAtual[0]);
|
|
double dP1 = GPSService.DistanciaEntrePontos(inicio, RuaAtual[RuaAtual.Count() - 1]);
|
|
|
|
// Define manualmente a direcao para onde o carro deve ir, se orientando pela decisao inicial da melhor trajetoria, impedindo que ocorram mudancas no trajeto
|
|
if (DefinirRotaInicial || ( statusCarro == StatusCarroMapa.Direcionando && statusCarroAnterior == StatusCarroMapa.Parado && IdxRuaAtual == 0 && (IrPeloInicio ? dP0 > dP1 : dP1 > dP0)))
|
|
{
|
|
idxPontoMaisProximo = IrPeloInicio ? 0 : RuaAtual.Count() - 1;
|
|
_extremoMaisProximoMapa = IrPeloInicio ? 0 : 1;
|
|
direcaoRua = IrPeloInicio ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
|
PontoComparar = RuaAtual[idxPontoMaisProximo];
|
|
}
|
|
else
|
|
{
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(RuaProjetada, TrajetoriaTipo.Rua);
|
|
idxPontoMaisProximo = RuaProjetada.IndexOf(PontoMaisProximo);
|
|
|
|
// Se o robo estiver se aproximando do ponto mais proximo, e nao estiver vindo de fora da rua
|
|
if (aproximando && statusCarro != StatusCarroMapa.Direcionando)
|
|
{
|
|
PontoComparar = PontoMaisProximo;
|
|
}
|
|
// Se o robo estiver se aproximando do ponto mais proximo, e estiver vindo de fora da rua
|
|
else if (aproximando && statusCarro == StatusCarroMapa.Direcionando)
|
|
{
|
|
var _a = GPSService.DistanciaEntrePontos(inicio, PrimeiroPonto);
|
|
var _b = GPSService.DistanciaEntrePontos(inicio, PontoMaisProximo);
|
|
bool _c = GPSService.EstaEntrePontos(inicio, PontoMaisProximo, PrimeiroPonto);
|
|
// Verifica se o robo esta na margem da rua projetada e se a distancia do robo ate o ponto mais proximo da rua (0) e menor que a distancia ate o primeiro ponto projetado, ou entao se o robo esta entre ambos
|
|
if ((_b < DistanciaProjecaoRua && _b < _a) || _c)
|
|
{
|
|
PontoComparar = PontoMaisProximo;
|
|
}
|
|
else
|
|
{
|
|
PontoComparar = PrimeiroPonto;
|
|
}
|
|
}
|
|
// Se o robo estiver se afastando do ponto mais proximo, e estiver dentro da rua, e houver mais pontos na trajetoria
|
|
else if (!aproximando && dentroRua && RuaProjetada.Count() > (idxPontoMaisProximo + 1))
|
|
{
|
|
// Interpolar a posicao atual ate o proximo ponto, pois ja passou pelo ponto mais proximo
|
|
idxPontoMaisProximo++;
|
|
PontoComparar = RuaProjetada[idxPontoMaisProximo];
|
|
}
|
|
else if (!aproximando && !dentroRua && RuaProjetada.Count() == (idxPontoMaisProximo + 1))
|
|
{
|
|
PontoComparar = UltimoPonto;
|
|
}
|
|
else
|
|
{
|
|
PontoComparar = PontoMaisProximo;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
List<GPSModel> _caminhoInterpolado = GerarTrajetoLigacaoCarroPontoInicial(inicio, RuaAtual, RuaProjetada, PontoComparar, _extremoMaisProximoMapa, direcaoRua, statusCarro);
|
|
_caminho.AddRange(_caminhoInterpolado);
|
|
|
|
|
|
|
|
|
|
// se a distancia entre a posicao do robo e proximo ponto for menor que 1 metro, significa que o ponto interpolado esta mais adiante que o proximo ponto, entao tambem ignoramos ele
|
|
var a = GPSService.DistanciaEntrePontos(inicio, _caminho.Last());
|
|
var b = GPSService.DistanciaEntrePontos(inicio, PontoComparar);
|
|
|
|
bool Acrescenta = false;
|
|
bool AddUltimoPonto = true;
|
|
if (a == 0 || a > b)
|
|
{
|
|
idxPontoMaisProximo++;
|
|
Acrescenta = true;
|
|
}
|
|
else
|
|
{
|
|
// Verifica se esta se afastando do ultimo ponto gerado
|
|
var a1 = GPSService.DistanciaEntrePontos(GPSService.PenultimaLeitura, _caminho.Last());
|
|
if (a1 < a)
|
|
{
|
|
Acrescenta = true;
|
|
}
|
|
}
|
|
|
|
if (!Acrescenta || (Acrescenta && RuaProjetada.Count() > (idxPontoMaisProximo + 1)))
|
|
{
|
|
_caminho.AddRange(RuaProjetada.Skip(idxPontoMaisProximo));
|
|
}
|
|
else
|
|
{
|
|
AddUltimoPonto = GPSService.EstaEntrePontos(inicio, GPSService.PenultimaLeitura, UltimoPonto);
|
|
}
|
|
|
|
// Se não for adicionar o ultimo ponto, significa que o robô já passou por ele
|
|
if (!AddUltimoPonto)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.VerificaFimRuaAtual();
|
|
if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.Concluido)
|
|
{
|
|
GerandoTrajetoria = false;
|
|
GerarTrajetoriaDinamica();
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (AddUltimoPonto)
|
|
{
|
|
_caminho.Add(UltimoPonto);
|
|
}
|
|
|
|
_caminho.ForEach(x => x.DirecaoTrecho = direcaoRua);
|
|
Caminho.AddRange(_caminho);
|
|
}
|
|
else
|
|
{
|
|
List<GPSModel> _caminho = new List<GPSModel>();
|
|
// Verifica se a distância entre o ultimo ponto da rua anterior e o primeiro ponto da nova rua é curto o bastante para gerar a curva de manobra
|
|
double distancia = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], PrimeiroPonto);
|
|
bool distanciaMinima = distancia < MargemLimiteEntradaRua * 3.0;
|
|
|
|
// Verifica se o angulo formado entre a rua anterior e a rua atual é grande o bastante para gerar a curva de manobra
|
|
double anguloEntreRuas = GPSService.CalcularOrientacao(Caminho.Last(), PrimeiroPonto);
|
|
double anguloFinalRuaAnterior = GPSService.CalcularOrientacao(Caminho.Count() > 1 ? Caminho[Caminho.Count() - 2] : Caminho.First(), Caminho.Last());
|
|
double difAngulo = anguloFinalRuaAnterior - anguloEntreRuas;
|
|
bool anguloMinimo = difAngulo >= -45.0 && difAngulo <= 45.0;
|
|
|
|
/*if (distanciaMinima && !anguloMinimo)
|
|
{
|
|
bool CurvaParaEsquerda = ExtremoMaisProximoMapa == 0;
|
|
GPSModel P0 = CalcularPontoControle(Caminho.Last(), PrimeiroPonto, distanciaEntreRuas * 2, CurvaParaEsquerda);
|
|
List<GPSModel> CurvaConexao = GerarCurvaConexao(Caminho.Last(), PrimeiroPonto, P0, 6);
|
|
|
|
_caminho.AddRange(CurvaConexao);
|
|
_caminho.ForEach(x => x.DirecaoTrecho = DirecaoCarroRua.Manobra);
|
|
}
|
|
else
|
|
{
|
|
_caminho.Add(PrimeiroPonto);
|
|
}*/
|
|
_caminho.Add(PrimeiroPonto);
|
|
|
|
//_caminho.AddRange(GPSService.InterpolarPontos(inicio, PrimeiroPonto, 1.0));
|
|
_caminho.AddRange(RuaProjetada);
|
|
_caminho.Add(UltimoPonto);
|
|
_caminho.ForEach(x => x.DirecaoTrecho = x.DirecaoTrecho != DirecaoCarroRua.Manobra ? direcaoRua : x.DirecaoTrecho);
|
|
Caminho.AddRange(_caminho);
|
|
|
|
//direcaoRua = direcaoRua == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
|
|
}
|
|
}
|
|
|
|
|
|
// Limpa a trajetória dinâmica existente e adiciona os novos pontos
|
|
lock (TrajetoriaDinamica_Lock)
|
|
{
|
|
TrajetoriaProjetada = new List<List<GPSModel>>(_TrajetoriaProjetada);
|
|
TrajetoriaDinamica.Clear();
|
|
Caminho.ForEach(x =>
|
|
{
|
|
// Impede inclusão de pontos duplicados
|
|
if (!TrajetoriaDinamica.Contains(x))
|
|
{
|
|
TrajetoriaDinamica.Add(x);
|
|
}
|
|
});
|
|
Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoriaDinamica = 0;
|
|
|
|
var sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect;
|
|
if (Variaveis.OperacaoEmAndamento.Controle.SonarAtivado && (sonar?.Iniciado ?? false) && (sonar?.DesvioNecessario ?? false))
|
|
{
|
|
if (sonar.DirecaoDesvio == Direcao.Parado || Variaveis.OperacaoEmAndamento.OpMapaGPS.StatusDentroRua.Contains(statusCarro))
|
|
{
|
|
//TrajetoriaDinamica.Clear();
|
|
}
|
|
else
|
|
{
|
|
AjustarTrajetoriaParaDesvio(sonar.ObstaculoCritico);
|
|
}
|
|
}
|
|
}
|
|
|
|
GPSService.AtualizarTrajetoriaDinamica();
|
|
|
|
GerandoTrajetoria = false;
|
|
}
|
|
|
|
public List<GPSModel> ProjetarRua(List<GPSModel> ruaOriginal, double offset, double anguloOffset)
|
|
{
|
|
// Certifique-se de que a lista da rua original não está vazia
|
|
if (ruaOriginal == null || ruaOriginal.Count == 0)
|
|
{
|
|
return new List<GPSModel>();
|
|
}
|
|
|
|
List<GPSModel> RuaProjetada = new List<GPSModel>();
|
|
|
|
foreach (var Ponto in ruaOriginal)
|
|
{
|
|
int idx = ruaOriginal.IndexOf(Ponto);
|
|
GPSModel Ponto2 = idx + 1 < ruaOriginal.Count() ? ruaOriginal[idx + 1] : ruaOriginal[idx - 1];
|
|
double angulo = 0;
|
|
if (idx + 1 < ruaOriginal.Count())
|
|
{
|
|
angulo = GPSService.CalcularOrientacao(Ponto, Ponto2);
|
|
}
|
|
else
|
|
{
|
|
angulo = GPSService.CalcularOrientacao(Ponto2, Ponto);
|
|
}
|
|
RuaProjetada.Add(GPSService.ProjetarPontoDeslocado(Ponto, offset, angulo + 90));
|
|
}
|
|
|
|
return RuaProjetada;
|
|
}
|
|
|
|
|
|
public static GPSModel CalcularBezierQuadratica(GPSModel p0, GPSModel p1, GPSModel p2, double t)
|
|
{
|
|
double x = Math.Pow(1 - t, 2) * p0.Latitude + 2 * (1 - t) * t * p1.Latitude + Math.Pow(t, 2) * p2.Latitude;
|
|
double y = Math.Pow(1 - t, 2) * p0.Longitude + 2 * (1 - t) * t * p1.Longitude + Math.Pow(t, 2) * p2.Longitude;
|
|
return new GPSModel { Latitude = x, Longitude = y };
|
|
}
|
|
|
|
public static GPSModel CalcularPontoControle(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia, bool ParaDentro)
|
|
{
|
|
double angulo = GPSService.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
|
|
double lat = (ultimoPontoRuaAtual.Latitude + primeiroPontoProximaRua.Latitude) / 2;
|
|
double lon = (ultimoPontoRuaAtual.Longitude + primeiroPontoProximaRua.Longitude) / 2;
|
|
GPSModel pontoMedio = new GPSModel()
|
|
{
|
|
Latitude = lat,
|
|
Longitude = lon,
|
|
};
|
|
|
|
// Ajuste o ângulo com base na direção da curva
|
|
double ajusteAngulo = (ParaDentro ? 270 : 90);
|
|
|
|
// Projetar um ponto a uma certa distância na direção ajustada
|
|
return GPSService.ProjetarPontoDeslocado(pontoMedio, distancia, angulo + ajusteAngulo);
|
|
}
|
|
|
|
public static List<GPSModel> GerarCurvaConexao(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, GPSModel pontoControle, int numeroPontos)
|
|
{
|
|
List<GPSModel> pontosCurva = new List<GPSModel>();
|
|
for (int i = 0; i <= numeroPontos; i++)
|
|
{
|
|
double t = i / (double)numeroPontos;
|
|
pontosCurva.Add(CalcularBezierQuadratica(ultimoPontoRuaAtual, pontoControle, primeiroPontoProximaRua, t));
|
|
}
|
|
return pontosCurva;
|
|
}
|
|
|
|
private List<GPSModel> GerarTrajetoLigacaoCarroPontoInicial(GPSModel inicio, List<GPSModel> RuaAtual, List<GPSModel> RuaProjetada, GPSModel PontoComparar, int _extremoMaisProximoMapa, DirecaoCarroRua direcaoRua, StatusCarroMapa statusCarro)
|
|
{
|
|
List<GPSModel> _caminho = new List<GPSModel>();
|
|
|
|
bool CopiarRuasLaterais = true;
|
|
if (CopiarRuasLaterais && new List<StatusCarroMapa>() { StatusCarroMapa.Parado, StatusCarroMapa.Direcionando }.Contains(statusCarro) && IdxRuaAtual == 0 && ((_extremoMaisProximoMapa == 0 && direcaoRua == DirecaoCarroRua.Ida) || (_extremoMaisProximoMapa > 0 && direcaoRua == DirecaoCarroRua.Volta)))
|
|
{
|
|
int idx = 0;
|
|
double distMin = double.MaxValue;
|
|
var DadosMapa = Variaveis.OperacaoEmAndamento.Mapa.mapaService.DadosMapa;
|
|
double anguloCaminho = GPSService.CalcularOrientacao(inicio, PontoComparar);
|
|
|
|
double tolerancia = 90; // Tolerância de 90 graus
|
|
GPSModel pontoAnterior = inicio; // Inicializamos com a posição inicial do robô
|
|
|
|
var PontosOrdenados = DadosMapa.features
|
|
.Select(x => new
|
|
{
|
|
x.geometry.id,
|
|
dist = GPSService.DistanciaEntrePontos(pontoAnterior, new GPSModel()
|
|
{
|
|
Longitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][0],
|
|
Latitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][1]
|
|
}),
|
|
orientacao = GPSService.CalcularOrientacao(pontoAnterior, new GPSModel()
|
|
{
|
|
Longitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][0],
|
|
Latitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][1]
|
|
}),
|
|
coordenadas = new GPSModel()
|
|
{
|
|
Longitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][0],
|
|
Latitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][1],
|
|
},
|
|
distParaPontoFinal = GPSService.DistanciaEntrePontos(new GPSModel()
|
|
{
|
|
Longitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][0],
|
|
Latitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][1]
|
|
}, PontoComparar),
|
|
orientacaoParaPontoFinal = GPSService.CalcularOrientacao(new GPSModel()
|
|
{
|
|
Longitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][0],
|
|
Latitude = x.geometry.coordinates[GetIndexInicial(x.geometry.coordinates, _extremoMaisProximoMapa)][1]
|
|
}, PontoComparar)
|
|
})
|
|
.OrderBy(x => x.dist) // Ordena os pontos pela distância
|
|
.Select((x, i) => new
|
|
{
|
|
x.id,
|
|
idx = i,
|
|
x.dist,
|
|
x.orientacao,
|
|
x.coordenadas,
|
|
x.distParaPontoFinal,
|
|
x.orientacaoParaPontoFinal
|
|
})
|
|
.Where(x =>
|
|
{
|
|
// Verificar se o ponto está se aproximando do ponto final
|
|
if (x.distParaPontoFinal > GPSService.DistanciaEntrePontos(pontoAnterior, PontoComparar))
|
|
{
|
|
return false; // Se o ponto atual está se afastando, desconsiderar
|
|
}
|
|
|
|
// Calcular a orientação entre o ponto atual e o ponto final
|
|
double orientacaoAtualParaPontoFinalNormalizada = (x.orientacaoParaPontoFinal + 360) % 360;
|
|
double orientacaoAnteriorParaPontoFinalNormalizada = GPSService.CalcularOrientacao(pontoAnterior, PontoComparar);
|
|
orientacaoAnteriorParaPontoFinalNormalizada = (orientacaoAnteriorParaPontoFinalNormalizada + 360) % 360;
|
|
|
|
// Verificar se a orientação leva o robô para o ponto final, ou se está se desviando
|
|
double diferencaAnguloFinal = Math.Abs(orientacaoAtualParaPontoFinalNormalizada - orientacaoAnteriorParaPontoFinalNormalizada);
|
|
diferencaAnguloFinal = diferencaAnguloFinal > 180 ? 360 - diferencaAnguloFinal : diferencaAnguloFinal;
|
|
|
|
// Se a diferença de ângulo for maior que a tolerância, desconsiderar o ponto
|
|
if (diferencaAnguloFinal > tolerancia)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Atualizar o ponto anterior para o próximo ponto
|
|
pontoAnterior = new GPSModel() { Longitude = x.coordenadas.Longitude, Latitude = x.coordenadas.Latitude };
|
|
|
|
return true;
|
|
})
|
|
.ToList();
|
|
|
|
foreach (var rua in PontosOrdenados)
|
|
{
|
|
double dist = GPSService.DistanciaEntrePontos(PontoComparar, rua.coordenadas);
|
|
if (dist < distMin)
|
|
{
|
|
distMin = dist;
|
|
idx = PontosOrdenados.IndexOf(rua);
|
|
}
|
|
};
|
|
|
|
double anguloDeslocar = CalcularDeslocamentoDinamico(_extremoMaisProximoMapa == 0 ? 0 : RuaProjetada.Count() - 1, RuaProjetada);
|
|
|
|
_caminho.Add(inicio);
|
|
|
|
double velocidade = FuncoesMatematicas.CalculaVelocidadeRPM(Variaveis.OperacaoEmAndamento.Controle.RPM_SP);
|
|
double velocidadeMs = FuncoesMatematicas.ConverteKmhParaMs(velocidade);
|
|
// Calcula o tempo que passou desde a última atualização
|
|
TimeSpan tempoPassado = UltimaAtualizacaoTrajetoriaDinamica == DateTime.MinValue ? new TimeSpan(10000000) : DateTime.Now - UltimaAtualizacaoTrajetoriaDinamica;
|
|
// Calcula a distância com base no tempo passado (em segundos)
|
|
double distancia = velocidadeMs * tempoPassado.TotalSeconds;
|
|
int pontosPular = 0;
|
|
PontosOrdenados.ForEach(p =>
|
|
{
|
|
if (p.dist <= (distancia + MargemLimiteEntradaRua))
|
|
{
|
|
pontosPular++;
|
|
}
|
|
});
|
|
UltimaAtualizacaoTrajetoriaDinamica = DateTime.Now;
|
|
|
|
for (int iidx = pontosPular; iidx < idx; iidx++)
|
|
{
|
|
GPSModel p0 = GPSService.ProjetarPontoDeslocado(PontosOrdenados[iidx > 0 ? iidx - 1 : iidx].coordenadas, 3.0, anguloDeslocar);
|
|
GPSModel p = GPSService.ProjetarPontoDeslocado(PontosOrdenados[iidx > 0 ? iidx : iidx + 1].coordenadas, 3.0, anguloDeslocar);
|
|
double orientacao = PontosOrdenados[iidx].orientacao;
|
|
double orientacaoA = PontosOrdenados[iidx > 0 ? iidx - 1 : iidx].orientacao;
|
|
|
|
double distP1 = GPSService.DistanciaEntrePontos(inicio, PontoComparar);
|
|
double anguloP1 = GPSService.CalcularOrientacao(inicio, PontoComparar);
|
|
double distP = GPSService.DistanciaEntrePontos(inicio, p);
|
|
double anguloP = GPSService.CalcularOrientacao(inicio, p);
|
|
|
|
//if ((orientacao <= (orientacaoA + 90) && orientacao >= (orientacaoA - 90)))
|
|
if (FuncoesMatematicas.ValorEstaEntre(orientacao, orientacaoA, 90) && distP1 > distP) // && FuncoesMatematicas.ValorEstaEntre(anguloP, anguloP1, 45)
|
|
{
|
|
_caminho.Add(p);
|
|
}
|
|
}
|
|
|
|
if (idx == 1)
|
|
{
|
|
double distPi = GPSService.DistanciaEntrePontos(inicio, RuaProjetada[0]);
|
|
|
|
GPSModel PrimeiroPonto = RuaProjetada[0];
|
|
GPSModel UltimoPonto = RuaProjetada[RuaProjetada.Count() - 1];
|
|
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(RuaProjetada, TrajetoriaTipo.Rua);
|
|
int idxPontoMaisProximo = RuaProjetada.IndexOf(PontoMaisProximo);
|
|
|
|
// Se o robo estiver se aproximando do ponto mais proximo, e estiver vindo de fora da rua
|
|
if (aproximando && statusCarro == StatusCarroMapa.Direcionando)
|
|
{
|
|
var _a = GPSService.DistanciaEntrePontos(inicio, PrimeiroPonto);
|
|
var _b = GPSService.DistanciaEntrePontos(inicio, PontoMaisProximo);
|
|
bool _c = GPSService.EstaEntrePontos(inicio, PontoMaisProximo, PrimeiroPonto);
|
|
// Verifica se o robo esta na margem da rua projetada e se a distancia do robo ate o ponto mais proximo da rua (0) e menor que a distancia ate o primeiro ponto projetado, ou entao se o robo esta entre ambos
|
|
if ((_b < DistanciaProjecaoRua && _b < _a) || _c)
|
|
{
|
|
PontoComparar = PontoMaisProximo;
|
|
}
|
|
else
|
|
{
|
|
PontoComparar = PrimeiroPonto;
|
|
}
|
|
}
|
|
else if (!aproximando && RuaProjetada.Count() == (idxPontoMaisProximo + 1))
|
|
{
|
|
PontoComparar = UltimoPonto;
|
|
}
|
|
else
|
|
{
|
|
PontoComparar = PontoMaisProximo;
|
|
}
|
|
|
|
_caminho.Add(PontoComparar);
|
|
}
|
|
else
|
|
{
|
|
_caminho.Add(PontoComparar);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_caminho.AddRange(GPSService.InterpolarPontos(inicio, PontoComparar, 1.0));
|
|
}
|
|
|
|
return _caminho;
|
|
}
|
|
|
|
public static double CalcularDeslocamentoDinamico(int idxPontoExtremo, List<GPSModel> ruaAtual)
|
|
{
|
|
// Inicializar variáveis de deslocamento
|
|
double anguloDeslocamento = 0;
|
|
double anguloRua = 0;
|
|
bool naHorizontal = false;
|
|
|
|
// Calcular a orientação da rua com base no ponto extremo mais próximo
|
|
if (idxPontoExtremo == 0)
|
|
{
|
|
anguloRua = GPSService.CalcularOrientacao(ruaAtual[0], ruaAtual[1]);
|
|
}
|
|
else
|
|
{
|
|
anguloRua = GPSService.CalcularOrientacao(ruaAtual[idxPontoExtremo], ruaAtual[idxPontoExtremo - 1]);
|
|
}
|
|
|
|
// Normalizar o ângulo para o intervalo [0, 360)
|
|
anguloRua = (anguloRua + 360) % 360;
|
|
|
|
// Definir a tolerância para considerar uma rua como vertical ou horizontal
|
|
double tolerancia = 45.0;
|
|
|
|
// Verificar se a rua está na vertical (próxima de 0° ou 180°)
|
|
if ((anguloRua >= 0 && anguloRua <= tolerancia) || (anguloRua >= 180 - tolerancia && anguloRua <= 180 + tolerancia))
|
|
{
|
|
naHorizontal = false; // A rua está mais alinhada verticalmente
|
|
}
|
|
// Verificar se a rua está na horizontal (próxima de 90° ou 270°)
|
|
else if ((anguloRua >= 90 - tolerancia && anguloRua <= 90 + tolerancia) || (anguloRua >= 270 - tolerancia && anguloRua <= 270 + tolerancia))
|
|
{
|
|
naHorizontal = true; // A rua está mais alinhada horizontalmente
|
|
}
|
|
|
|
// Calcular o deslocamento com base na orientação da rua
|
|
if (naHorizontal)
|
|
{
|
|
if (idxPontoExtremo == 0)
|
|
{
|
|
anguloDeslocamento = 270 - 45; // Deslocar para a esquerda e ligeiramente para baixo
|
|
}
|
|
else
|
|
{
|
|
anguloDeslocamento = 90 + 45; // Deslocar para a direita e ligeiramente para baixo
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (idxPontoExtremo == 0)
|
|
{
|
|
anguloDeslocamento = 0 - 45; // Deslocar para cima e ligeiramente para a esquerda
|
|
}
|
|
else
|
|
{
|
|
anguloDeslocamento = 180 + 45; // Deslocar para baixo e ligeiramente para a esquerda
|
|
}
|
|
}
|
|
|
|
// Normalizar o ângulo de deslocamento para ficar entre 0 e 360 graus
|
|
anguloDeslocamento = (anguloDeslocamento + 360) % 360;
|
|
|
|
// Retornar o ângulo de deslocamento calculado
|
|
return anguloDeslocamento;
|
|
}
|
|
|
|
private static int GetIndexInicial(List<List<double>> coordinates, int extremoProximo)
|
|
{
|
|
return extremoProximo == 0 ? 0 : coordinates.Count() - 1;
|
|
}
|
|
|
|
|
|
|
|
// Método para gerar o caminho entre dois pontos
|
|
public void GerarCaminhoEntreDoisPontos(GPSModel destino)
|
|
{
|
|
//PopularTrajetoriaComOffset(destino);
|
|
//return;
|
|
// Inicialização do caminho com o ponto de início
|
|
GPSModel inicio = GPSService.UltimaLeitura;
|
|
|
|
// Gera waypoints intermediários
|
|
var caminho = GPSService.InterpolarPontos(inicio, destino, 1.0); // 1 metro de distância entre os pontos
|
|
|
|
// Limpa a trajetória dinâmica existente e adiciona os novos pontos
|
|
TrajetoriaDinamica.Clear();
|
|
caminho.ForEach(x => TrajetoriaDinamica.Add(x));
|
|
|
|
if (KinectService.Leitura.DesvioNecessario)
|
|
{
|
|
AjustarTrajetoriaParaDesvio(KinectService.Leitura.ObstaculoCritico);
|
|
}
|
|
|
|
GPSService.AtualizarTrajetoriaDinamica();
|
|
}
|
|
|
|
public void AjustarTrajetoriaParaDesvio(Obstaculo obstaculo)
|
|
{
|
|
GPSModel posicaoAtual = GPSService.UltimaLeitura;
|
|
|
|
double distanciaObstaculo = ((double)obstaculo.DistanciaMedia_mm / 1000);
|
|
double larguraObstaculo = ((double)obstaculo.Largura_mm / 1000);
|
|
double anguloCaminho = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
|
|
double anguloDesvioInicial = anguloCaminho + obstaculo.AnguloParaDesvio;
|
|
double anguloDesvioFinal = anguloCaminho - obstaculo.AnguloParaDesvio;
|
|
|
|
GPSModel ponto1 = GerarPontoDeslocado(posicaoAtual, anguloDesvioInicial, distanciaObstaculo);
|
|
GPSModel ponto2 = GerarPontoDeslocado(ponto1, anguloCaminho, larguraObstaculo);
|
|
GPSModel ponto3 = GerarPontoDeslocado(ponto2, anguloDesvioFinal, distanciaObstaculo);
|
|
List<GPSModel> lista = new List<GPSModel>()
|
|
{
|
|
ponto1,
|
|
ponto2,
|
|
ponto3
|
|
};
|
|
|
|
var idxRemover = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, ponto3, distanciaObstaculo);
|
|
lock (TrajetoriaDinamica_Lock)
|
|
{
|
|
TrajetoriaDinamica.RemoveRange(1, idxRemover);
|
|
TrajetoriaDinamica.InsertRange(1, lista);
|
|
}
|
|
}
|
|
|
|
private int EncontrarIndiceFinalDesvio(List<GPSModel> trajetoria, GPSModel ultimoWaypointDesvio, double distanciaSeguranca)
|
|
{
|
|
// Encontrar o índice do waypoint que está imediatamente após o retorno do desvio
|
|
// Este é um método simplificado. A lógica exata pode variar dependendo da precisão necessária.
|
|
double distanciaAnterior = 0;
|
|
for (int i = 0; i < trajetoria.Count; i++)
|
|
{
|
|
double distancia = GPSService.DistanciaEntrePontos(trajetoria[i], ultimoWaypointDesvio);
|
|
bool aumentando = distancia > distanciaAnterior;
|
|
distanciaAnterior = distancia;
|
|
if (distancia < distanciaSeguranca)
|
|
{
|
|
if (aumentando)
|
|
{
|
|
return i;
|
|
}
|
|
}
|
|
}
|
|
return 0; // Se não encontrar, assuma o último waypoint
|
|
}
|
|
|
|
private int EncontrarIndiceInicioDesvio(List<GPSModel> trajetoria, GPSModel posicaoAtual, double distanciaObstaculo)
|
|
{
|
|
|
|
foreach (var wp in trajetoria)
|
|
{
|
|
double distancia = GPSService.DistanciaEntrePontos(wp, posicaoAtual);
|
|
if (distancia < distanciaObstaculo)
|
|
{
|
|
return trajetoria.IndexOf(wp);
|
|
}
|
|
}
|
|
|
|
// Este é um método simplificado. Você precisará implementar a lógica para encontrar o waypoint mais adequado.
|
|
return trajetoria.FindIndex(wp => GPSService.DistanciaEntrePontos(wp, posicaoAtual) < distanciaObstaculo);
|
|
}
|
|
|
|
private List<GPSModel> CalcularWaypointsDesvio(GPSModel posicaoAtual, Obstaculo obstaculo, int indiceInicioDesvio)
|
|
{
|
|
List<GPSModel> waypointsDesvio = new List<GPSModel>();
|
|
|
|
double anguloCaminho = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
|
|
|
|
// Calcular o ângulo e distância para o desvio
|
|
double anguloDesvio = anguloCaminho + ((obstaculo.DirecaoDesvio == Direcao.Direita ? 90 : 270) + obstaculo.AnguloParaDesvio);
|
|
double deslocamento = (double)obstaculo.DistanciaMedia_mm / 1000; // Math.Sqrt(Math.Pow(((double)obstaculo.DeslocamentoNecessarioDesvio / 100), 2) + Math.Pow(((double)obstaculo.DistanciaMedia / 100), 2));
|
|
|
|
// Waypoint de início do desvio
|
|
GPSModel waypointInicioDesvio = CalcularNovoWaypoint(posicaoAtual, anguloDesvio, deslocamento);
|
|
waypointsDesvio.Add(waypointInicioDesvio);
|
|
|
|
// Waypoint de contorno do obstáculo
|
|
double anguloContorno = 270 - anguloCaminho; // Ajustar conforme a orientação do desvio
|
|
GPSModel waypointContornoObstaculo = CalcularNovoWaypoint(waypointInicioDesvio, anguloContorno, ((double)obstaculo.Largura_mm / 1000)); //((double)obstaculo.Largura / 100)
|
|
waypointsDesvio.Add(waypointContornoObstaculo);
|
|
|
|
// Waypoint de retorno à trajetória
|
|
double anguloRetorno = anguloCaminho + ((obstaculo.DirecaoDesvio == Direcao.Direita ? 90 : 270) - obstaculo.AnguloParaDesvio); //anguloDesvio - obstaculo.AnguloParaDesvio - 180;// Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + (obstaculo.AnguloParaDesvio); // Pode precisar ajustar baseado na orientação do desvio
|
|
GPSModel waypointRetorno = CalcularNovoWaypoint(waypointContornoObstaculo, anguloRetorno, deslocamento);
|
|
waypointsDesvio.Add(waypointRetorno);
|
|
|
|
return waypointsDesvio;
|
|
}
|
|
|
|
private GPSModel CalcularNovoWaypoint(GPSModel posicaoAtual, double angulo, double distancia)
|
|
{
|
|
double anguloRad = FuncoesMatematicas.ConvertDegreesToRadians(angulo);
|
|
double deltaLatitude = Math.Cos(anguloRad) * distancia / 111111; // Conversão para graus
|
|
double deltaLongitude = Math.Sin(anguloRad) * distancia / (111111 * Math.Cos(FuncoesMatematicas.ConvertDegreesToRadians(posicaoAtual.Latitude)));
|
|
|
|
return new GPSModel
|
|
{
|
|
Latitude = posicaoAtual.Latitude + deltaLatitude,
|
|
Longitude = posicaoAtual.Longitude + deltaLongitude
|
|
};
|
|
}
|
|
|
|
|
|
|
|
public void PopularTrajetoriaComOffset(GPSModel destino)
|
|
{
|
|
if (mapaService.DadosMapa != null)
|
|
{
|
|
GPSModel inicio = GPSService.UltimaLeitura;
|
|
|
|
// Coleta os primeiros pontos de cada rua com base na orientação e posição atual do robô
|
|
var pontosExtremosComOffset = new List<GPSModel>();
|
|
|
|
foreach (var rua in mapaService.DadosMapa.features)
|
|
{
|
|
var primeiroPonto = new GPSModel()
|
|
{
|
|
Latitude = rua.geometry.coordinates.First()[1],
|
|
Longitude = rua.geometry.coordinates.First()[0]
|
|
};
|
|
var ultimoPonto = new GPSModel()
|
|
{
|
|
Latitude = rua.geometry.coordinates.Last()[1],
|
|
Longitude = rua.geometry.coordinates.Last()[0]
|
|
};
|
|
GPSModel pontoInicial = new GPSModel();
|
|
GPSModel pontoSecundario = new GPSModel();
|
|
|
|
if (GPSService.DistanciaEntrePontos(inicio, primeiroPonto) < GPSService.DistanciaEntrePontos(inicio, ultimoPonto))
|
|
{
|
|
pontoInicial = primeiroPonto;
|
|
pontoSecundario = new GPSModel()
|
|
{
|
|
Latitude = rua.geometry.coordinates.Skip(1).First()[1],
|
|
Longitude = rua.geometry.coordinates.Skip(1).First()[0]
|
|
};
|
|
}
|
|
else
|
|
{
|
|
pontoInicial = ultimoPonto;
|
|
pontoSecundario = new GPSModel()
|
|
{
|
|
Latitude = rua.geometry.coordinates.Skip(rua.geometry.coordinates.Count - 2).First()[1],
|
|
Longitude = rua.geometry.coordinates.Skip(rua.geometry.coordinates.Count - 2).First()[0]
|
|
};
|
|
}
|
|
|
|
var orientacaoRua = GPSService.CalcularOrientacao(pontoInicial, pontoSecundario);
|
|
|
|
// Calcula o ponto com offset baseado no ângulo oposto à orientação da rua
|
|
var anguloOffset = (orientacaoRua + 180) % 360;
|
|
var distanciaOffset = 10; // Defina o offset desejado aqui
|
|
|
|
// Adiciona o ponto com offset à lista
|
|
pontosExtremosComOffset.Add(new GPSModel
|
|
{
|
|
Latitude = pontoInicial.Latitude,// + (Math.Cos(anguloOffset * (Math.PI / 180)) * distanciaOffset),
|
|
Longitude = pontoInicial.Longitude// + (Math.Sin(anguloOffset * (Math.PI / 180)) * distanciaOffset),
|
|
});
|
|
}
|
|
|
|
var pontosRelevantes = FiltrarPontosRelevantes(inicio, destino, pontosExtremosComOffset);
|
|
|
|
// Ordena os pontos com offset pela proximidade com a posição atual
|
|
var pontosOrdenados = pontosRelevantes.OrderBy(p => GPSService.DistanciaEntrePontos(inicio, p)).ToList();
|
|
|
|
lock (TrajetoriaDinamica_Lock)
|
|
{
|
|
// Agora, conecta os pontos ordenados para formar a trajetória
|
|
TrajetoriaDinamica.Clear();
|
|
GPSModel pontoAnterior = inicio;
|
|
foreach (var ponto in pontosOrdenados)
|
|
{
|
|
//var caminho = GPSService.InterpolarPontos(pontoAnterior, ponto, 1.0);
|
|
//caminho.ForEach(x => TrajetoriaDinamica.Add(x));
|
|
//pontoAnterior = ponto;
|
|
TrajetoriaDinamica.Add(ponto);
|
|
}
|
|
}
|
|
|
|
// Atualiza a trajetória dinâmica
|
|
GPSService.AtualizarTrajetoriaDinamica();
|
|
}
|
|
}
|
|
private List<GPSModel> FiltrarPontosRelevantes(GPSModel inicio, GPSModel destino, List<GPSModel> todasRuas)
|
|
{
|
|
var ruasRelevantes = new List<GPSModel>();
|
|
|
|
// Define os limites do quadrante
|
|
double minLatitude = Math.Min(inicio.Latitude, destino.Latitude);
|
|
double maxLatitude = Math.Max(inicio.Latitude, destino.Latitude);
|
|
double minLongitude = Math.Min(inicio.Longitude, destino.Longitude);
|
|
double maxLongitude = Math.Max(inicio.Longitude, destino.Longitude);
|
|
|
|
// Filtra previamente os pontos que estão dentro do quadrante definido
|
|
var pontosNoQuadrante = todasRuas.Where(ponto => ponto.Latitude >= minLatitude && ponto.Latitude <= maxLatitude
|
|
&& ponto.Longitude >= minLongitude && ponto.Longitude <= maxLongitude).ToList();
|
|
|
|
// Continua a filtragem dos pontos relevantes como antes, mas agora apenas com os pontos dentro do quadrante
|
|
GPSModel pontoMaisProximoInicio = IdentificarPontoMaisProximo(inicio, pontosNoQuadrante);
|
|
if (pontoMaisProximoInicio != null && !ruasRelevantes.Contains(pontoMaisProximoInicio))
|
|
{
|
|
ruasRelevantes.Add(pontoMaisProximoInicio);
|
|
}
|
|
|
|
GPSModel ultimoPontoRelevante = pontoMaisProximoInicio;
|
|
|
|
foreach (var ponto in pontosNoQuadrante)
|
|
{
|
|
if (ponto.Equals(inicio) || ponto.Equals(pontoMaisProximoInicio)) continue;
|
|
|
|
double margem = 20;
|
|
double distanciaPontoAnterior = GPSService.DistanciaEntrePontos(ultimoPontoRelevante, destino);
|
|
double distanciaPontoAtual = GPSService.DistanciaEntrePontos(ponto, destino);
|
|
double diferenca = (distanciaPontoAnterior - distanciaPontoAtual);
|
|
bool relevante = diferenca > 0 && diferenca < margem;
|
|
|
|
if (relevante || distanciaPontoAtual == 0 || true)
|
|
{
|
|
ruasRelevantes.Add(ponto);
|
|
ultimoPontoRelevante = ponto;
|
|
}
|
|
|
|
if (distanciaPontoAtual == 0)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return ruasRelevantes;
|
|
}
|
|
|
|
// Método auxiliar para identificar o ponto mais próximo a um dado ponto de referência
|
|
private GPSModel IdentificarPontoMaisProximo(GPSModel referencia, List<GPSModel> pontos)
|
|
{
|
|
GPSModel pontoMaisProximo = null;
|
|
double distanciaMinima = double.MaxValue;
|
|
|
|
foreach (var ponto in pontos)
|
|
{
|
|
double distancia = GPSService.DistanciaEntrePontos(referencia, ponto);
|
|
if (distancia < distanciaMinima)
|
|
{
|
|
distanciaMinima = distancia;
|
|
pontoMaisProximo = ponto;
|
|
}
|
|
}
|
|
|
|
return pontoMaisProximo;
|
|
}
|
|
|
|
|
|
|
|
public GPSModel GerarPontoDeslocado(GPSModel pontoInicial, double angulo, double distancia)
|
|
{
|
|
double latitude = pontoInicial.Latitude;
|
|
double longitude = pontoInicial.Longitude;
|
|
|
|
// Conversão de ângulo de graus para radianos
|
|
double anguloRad = angulo * (Math.PI / 180);
|
|
|
|
// Raio da Terra em metros
|
|
double raioTerra = GPSService.RaioDaTerra;
|
|
|
|
// Calcular deslocamento de latitude em radianos
|
|
double deltaLat = distancia * Math.Cos(anguloRad) / raioTerra;
|
|
// Converter de radianos para graus
|
|
double latAtt = latitude + deltaLat * (180 / Math.PI);
|
|
|
|
// Calcular deslocamento de longitude em radianos
|
|
double deltaLong = distancia * Math.Sin(anguloRad) / (raioTerra * Math.Cos(latitude * (Math.PI / 180)));
|
|
// Converter de radianos para graus
|
|
double longAtt = longitude + deltaLong * (180 / Math.PI);
|
|
|
|
return new GPSModel()
|
|
{
|
|
Latitude = latAtt,
|
|
Longitude = longAtt,
|
|
};
|
|
}
|
|
|
|
|
|
}
|
|
|
|
public class MapaFeatureCollectionModel
|
|
{
|
|
public string type { get; set; }
|
|
public List<MapaFeatureModel> features { get; set; }
|
|
}
|
|
|
|
public class MapaFeatureModel
|
|
{
|
|
public string type { get; set; }
|
|
public MapaFeaturePropertiesModel properties { get; set; }
|
|
public MapaFeatureGeometryModel geometry { get; set; }
|
|
}
|
|
|
|
public class MapaFeaturePropertiesModel
|
|
{
|
|
public string Id { get; set; }
|
|
public string Name { get; set; }
|
|
public double Length { get; set; }
|
|
public double Dist1 { get; set; }
|
|
public double Dist2 { get; set; }
|
|
}
|
|
|
|
public class MapaFeatureGeometryModel
|
|
{
|
|
public string id { get; set; }
|
|
public string type { get; set; }
|
|
public List<List<double>> coordinates { get; set; }
|
|
}
|
|
|
|
|
|
|
|
}
|