1506 lines
65 KiB
C#
1506 lines
65 KiB
C#
using AgroBase.Models.Operadores;
|
|
using AgroBase.Services;
|
|
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models
|
|
{
|
|
public class TrajetoriaMapaOperacaoModel
|
|
{
|
|
public TrajetoriaMapaOperacaoModel(List<List<GPSModel>> RuasMapa)
|
|
{
|
|
RuasPlantacao = new List<List<GPSModel>>(RuasMapa);
|
|
}
|
|
|
|
#region PARAMETROS
|
|
|
|
public double DistanciaProjecaoRua { get; set; } = 0.8; // Distancia para projetar o primeiro ponto para fora do corredor
|
|
public static double DistanciaEntrePontos { get; set; } = 0.8; // Distancia entre os pontos dentro do corredor
|
|
public static double DistanciaEntrePontosCurva { get; set; } = 0.25; // Distancia entre os pontos durante a curva entre corredores
|
|
private double DistanciaManobraEntreRuas { get; set; } = 3.0; // Distancia máxima para gerar a curva de conexão entre os corredores
|
|
private bool EspacamentoPrimeirosPontosProjecao { get; set; } = false; // Projetar os primeiros pontos com distancia menor entre eles
|
|
private double DistanciaPrimeirosPontosProjecao { get; set; } = 3.0; // Distancia máxima para projetar os primeiros pontos com distancia menor
|
|
public static double LarguraCorredorPadrao { get; set; } = 1.5; // Largura de um corredor padrão
|
|
private int JanelaAtualizacaoDePontos { get; set; } = 20; // Define o tamanho da janela de pontos a atualizar
|
|
|
|
#endregion
|
|
|
|
public DateTime UltimaAtualizacaoDados { get; set; } = DateTime.MinValue;
|
|
[JsonProperty]
|
|
public double TempoEntreLeituras { get; private set; }
|
|
private void AtualizarTempoEntreLeituras()
|
|
{
|
|
double dt = Math.Min((1.0 / GPSService.TaxaAmostragemHz), (UltimaAtualizacaoDados - Variaveis.OperacaoEmAndamento.Sensoriamento.Gps.Momento).TotalSeconds);
|
|
TempoEntreLeituras = dt;
|
|
UltimaAtualizacaoDados = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps.Momento;
|
|
}
|
|
|
|
public List<List<GPSModel>> RuasPlantacao { get; set; }
|
|
public List<List<GPSModel>> Corredores { get; set; }
|
|
public GPSModel PrimeiroPontoRuaMapa
|
|
{
|
|
get
|
|
{
|
|
// Verifica se o índice é válido e se a rua não está vazia
|
|
if (CorredorAtual != null && CorredorAtual.Idx < RuasPlantacao.Count && RuasPlantacao[CorredorAtual.Idx].Count > 0)
|
|
{
|
|
return RuasPlantacao[CorredorAtual.Idx][0]; // Acessa diretamente o primeiro ponto da rua
|
|
}
|
|
else if (CorredorAtual == null && RuasPlantacao.Count > 0)
|
|
{
|
|
return RuasPlantacao[0][0];
|
|
}
|
|
|
|
return new GPSModel(); // Pode ser null se preferir
|
|
}
|
|
}
|
|
public GPSModel UltimoPontoRuaMapa
|
|
{
|
|
get
|
|
{
|
|
// Verifica se o índice é válido e se a rua não está vazia
|
|
if (CorredorAtual != null && CorredorAtual.Idx < RuasPlantacao.Count && RuasPlantacao[CorredorAtual.Idx].Count > 0)
|
|
{
|
|
return RuasPlantacao[CorredorAtual.Idx][RuasPlantacao.Count - 1]; // Acessa diretamente o ultimo ponto da rua
|
|
}
|
|
else if (CorredorAtual == null && RuasPlantacao.Count > 0)
|
|
{
|
|
return RuasPlantacao[0][RuasPlantacao[0].Count - 1];
|
|
}
|
|
|
|
return new GPSModel(); // Pode ser null se preferir
|
|
}
|
|
}
|
|
public int ExtremoMaisProximoMapa
|
|
{
|
|
get
|
|
{
|
|
double distPP = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, PrimeiroPontoRuaMapa);
|
|
double distUP = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, 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 List<PontoTrajetoriaModel> _TrajetoriaFixa { get; set; }
|
|
public bool _TrajetoriaFixaDefinida
|
|
{
|
|
get
|
|
{
|
|
return _TrajetoriaFixa != null && _TrajetoriaFixa.Count > 0;
|
|
}
|
|
}
|
|
[JsonProperty]
|
|
public List<PontoTrajetoriaModel> _TrajetoriaJanela { get; private set; }
|
|
private void AtualizarTrajetoriaJanela()
|
|
{
|
|
_TrajetoriaJanela = new List<PontoTrajetoriaModel>();
|
|
|
|
if (!_TrajetoriaFixaDefinida) return;
|
|
|
|
for (int i = idxInicialJanela; i <= idxFinalJanela; i++)
|
|
{
|
|
_TrajetoriaJanela.Add(_TrajetoriaFixa[i]);
|
|
}
|
|
}
|
|
public List<CorredorTrajetoriaModel> _Corredores { get; set; }
|
|
public List<GPSModel> TrajetoriaFixa => _TrajetoriaFixa.ConvertAll(x => x.Posicao);
|
|
public List<PontoTrajetoriaModel> _TrajetoriaDinamica { get; private set; }
|
|
public List<GPSModel> TrajetoriaDinamica => _TrajetoriaDinamica.ConvertAll(x => x.Posicao);
|
|
public void AtualizarTrajetoriaDinamica()
|
|
{
|
|
// Cria a lista com a última leitura do robô
|
|
var trajetoria = new List<PontoTrajetoriaModel>()
|
|
{
|
|
new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
|
|
{
|
|
Posicao = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps,
|
|
LarguraCorredor = 1.0,
|
|
idxCorredor = CorredorAtual?.Idx ?? 0,
|
|
Visitado = true
|
|
}
|
|
};
|
|
|
|
// Se a trajetória fixa estiver vazia, retorna apenas a leitura atual
|
|
if (!_TrajetoriaFixaDefinida)
|
|
{
|
|
_TrajetoriaDinamica = trajetoria;
|
|
return;
|
|
}
|
|
|
|
// Adiciona os pontos não visitados, já ordenados por idxPonto
|
|
foreach (var ponto in _TrajetoriaFixa)
|
|
{
|
|
if (!ponto.Visitado)
|
|
{
|
|
trajetoria.Add(ponto);
|
|
}
|
|
}
|
|
|
|
_TrajetoriaDinamica = trajetoria;
|
|
}
|
|
|
|
[JsonProperty]
|
|
public static double DistanciaMaximaEntreLeituras { get; private set; }
|
|
public void AtualizarDistanciaMaximaEntreLeituras()
|
|
{
|
|
double velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP);
|
|
double distanciaMaxima = (velocidadeCarroMs / GPSService.TaxaAmostragemHz);
|
|
DistanciaMaximaEntreLeituras = distanciaMaxima;
|
|
}
|
|
|
|
private int idxInicialJanela = 0;
|
|
private int idxFinalJanela = 0;
|
|
private void AtualizarIndicesJanela()
|
|
{
|
|
if (!_TrajetoriaFixaDefinida) return;
|
|
|
|
idxInicialJanela = Math.Max(0, (PontoAtual?.idxPonto ?? 0) - JanelaAtualizacaoDePontos / 2);
|
|
idxFinalJanela = Math.Min(_TrajetoriaFixa.Count - 1, (PontoAtual?.idxPonto ?? 0) + JanelaAtualizacaoDePontos / 2);
|
|
}
|
|
|
|
[JsonProperty]
|
|
public CorredorTrajetoriaModel CorredorAtual { get; private set; }
|
|
private bool _CorredorAtualDefinido
|
|
{
|
|
get
|
|
{
|
|
return (CorredorAtual?.Pontos?.Count ?? 0) > 0;
|
|
}
|
|
}
|
|
private void AtualizarCorredorAtual()
|
|
{
|
|
if (_Corredores == null || _Corredores.Count == 0)
|
|
{
|
|
CorredorAtual = null;
|
|
return;
|
|
}
|
|
|
|
int idxNovoCorredor = PontoAtual?.idxCorredor ?? 0;
|
|
|
|
if ((CorredorAtual?.Idx ?? 0) != idxNovoCorredor)
|
|
{
|
|
CorredorAtual?.AtualizarDados();
|
|
}
|
|
|
|
CorredorAtual = _Corredores[idxNovoCorredor];
|
|
|
|
CorredorAtual.AtualizarDados();
|
|
}
|
|
public DirecaoCarroRua DirecaoCaminho
|
|
{
|
|
get
|
|
{
|
|
return PontoAtual?.Direcao ?? Enums.DirecaoCarroRua.Parado;
|
|
}
|
|
}
|
|
[JsonProperty]
|
|
public double AnguloMedioCorredor { get; private set; }
|
|
private void AtualizarAnguloMedioCorredor()
|
|
{
|
|
AnguloMedioCorredor = GPSUtils.CalcularOrientacao(PontoAtual.Posicao, ProximoPonto.Posicao);
|
|
}
|
|
[JsonProperty]
|
|
public double AnguloCaminho { get; private set; }
|
|
private void AtualizarAnguloCaminho()
|
|
{
|
|
PontoTrajetoriaModel pontoComparar = PontoAtual.Aproximando ? PontoAtual : ProximoPonto != null ? ProximoPonto : PontoAtual;
|
|
AnguloCaminho = GPSUtils.CalcularOrientacao(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, pontoComparar.Posicao);
|
|
}
|
|
[JsonProperty]
|
|
public double DistanciaEsquerda { get; private set; }
|
|
[JsonProperty]
|
|
public double DistanciaDireita { get; private set; }
|
|
private void AtualizarDistanciasLaterais()
|
|
{
|
|
DistanciaEsquerda = CalcularDistanciaLateral(true, Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, CorredorAtual?.idxRuaEsquerda ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
|
|
DistanciaDireita = CalcularDistanciaLateral(false, Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, CorredorAtual?.idxRuaDireita ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
|
|
}
|
|
|
|
public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, DirecaoCarroRua direcaoAtual)
|
|
{
|
|
// Verifica se o corredor atual existe
|
|
if (!_CorredorAtualDefinido) return 0;
|
|
|
|
List<GPSModel> rua = new List<GPSModel>(RuasPlantacao[idxRua]);
|
|
|
|
double d0 = double.MaxValue;
|
|
int idx = -1;
|
|
for (int i = 0; i < rua.Count; i++)
|
|
{
|
|
double dist = GPSUtils.DistanciaEntrePontos(posicaoAtual, rua[i]);
|
|
if (dist < d0)
|
|
{
|
|
d0 = dist;
|
|
idx = i;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
GPSModel p0 = rua[idx - (idx > 0 ? 1 : 0)];
|
|
GPSModel p1 = rua[idx + (idx < (rua.Count - 1) ? 1 : 0)];
|
|
int idxPa = rua.IndexOf(p0);
|
|
int idxPb = rua.IndexOf(p1);
|
|
double anguloAprojetar = GPSUtils.CalcularOrientacao(rua[idxPb], rua[idxPa]);
|
|
double anguloBprojetar = GPSUtils.CalcularOrientacao(rua[idxPa], rua[idxPb]);
|
|
|
|
GPSModel pontoA = GPSUtils.GerarPontoDeslocado(rua[idxPa], anguloAprojetar, DistanciaManobraEntreRuas);
|
|
GPSModel pontoB = GPSUtils.GerarPontoDeslocado(rua[idxPb], anguloBprojetar, DistanciaManobraEntreRuas);
|
|
|
|
List<GPSModel> novaRua = new List<GPSModel>();
|
|
novaRua.Add(pontoA);
|
|
for (int i = idxPa; i <= idxPb; i++)
|
|
{
|
|
novaRua.Add(rua[i]);
|
|
}
|
|
novaRua.Add(pontoB);
|
|
|
|
rua = novaRua;
|
|
|
|
int qtdPontosRua = Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua));
|
|
rua = GPSUtils.InterpolarRota(rua, qtdPontosRua);
|
|
|
|
// Calcula a menor distância entre o robô e a rua
|
|
double distancia = GPSUtils.CalcularMenorDistanciaAteTrecho(posicaoAtual, rua);
|
|
|
|
// Calcula o vetor entre os pontos da rua e o vetor entre o ponto A e o GPS
|
|
double crossProduct =
|
|
(pontoB.Longitude - pontoA.Longitude) * (posicaoAtual.Latitude - pontoA.Latitude) -
|
|
(pontoB.Latitude - pontoA.Latitude) * (posicaoAtual.Longitude - pontoA.Longitude);
|
|
|
|
// Ajusta a distância com base no sentido de deslocamento do robô
|
|
if ((direcaoAtual == DirecaoCarroRua.Volta && ladoEsquerdo) || (direcaoAtual == DirecaoCarroRua.Ida && !ladoEsquerdo))
|
|
{
|
|
distancia *= crossProduct > 0 ? 1 : -1;
|
|
}
|
|
else
|
|
{
|
|
distancia *= crossProduct > 0 ? -1 : 1;
|
|
}
|
|
|
|
// Ajusta a largura do robô no cálculo final
|
|
double larguraAjuste = ladoEsquerdo ? VariaveisEquipamento.LarguraEsquerda : VariaveisEquipamento.LarguraDireita;
|
|
distancia -= (larguraAjuste / 100.0);
|
|
|
|
return distancia;
|
|
}
|
|
[JsonProperty]
|
|
public PontoTrajetoriaModel PontoAtual { get; private set; }
|
|
private void DefinirPontoAtual()
|
|
{
|
|
// ✅ Evita processamento desnecessário se a lista for nula ou vazia
|
|
if (!_TrajetoriaFixaDefinida)
|
|
{
|
|
PontoAtual = null;
|
|
return;
|
|
}
|
|
|
|
// Busca eficiente: percorre do começo ao fim para achar o primeiro não visitado
|
|
for (int i = idxFinalJanela; i >= idxInicialJanela; i--)
|
|
{
|
|
if (_TrajetoriaFixa[i].Visitado)
|
|
{
|
|
PontoAtual = _TrajetoriaFixa[i];
|
|
return;
|
|
}
|
|
}
|
|
|
|
PontoAtual = null;
|
|
}
|
|
[JsonProperty]
|
|
public PontoTrajetoriaModel ProximoPonto { get; private set; }
|
|
private void DefinirProximoPonto()
|
|
{
|
|
if (!_TrajetoriaFixaDefinida)
|
|
{
|
|
ProximoPonto = null;
|
|
return;
|
|
}
|
|
|
|
if (PontoAtual.idxPonto == _TrajetoriaFixa.Count() - 1)
|
|
{
|
|
ProximoPonto = PontoAtual;
|
|
}
|
|
else
|
|
{
|
|
int idx = (_TrajetoriaJanela.IndexOf(PontoAtual) + 1);
|
|
ProximoPonto = _TrajetoriaJanela[idx];
|
|
}
|
|
|
|
/*// Busca eficiente: percorre do começo ao fim para achar o primeiro não visitado
|
|
for (int i = idxInicialJanela; i <= idxFinalJanela; i++)
|
|
{
|
|
if (!_TrajetoriaFixa[i].Visitado)
|
|
{
|
|
ProximoPonto = _TrajetoriaFixa[i];
|
|
return;
|
|
}
|
|
}
|
|
|
|
ProximoPonto = PontoAtual; // Se não houver pontos não visitados, retorna o último ponto visitado
|
|
*/
|
|
}
|
|
[JsonProperty]
|
|
public PontoTrajetoriaModel PontoMaisProximo { get; private set; }
|
|
private void DefinirPontoMaisProximo()
|
|
{
|
|
// ✅ Evita processamento desnecessário se a lista for nula ou vazia
|
|
if (!_TrajetoriaFixaDefinida)
|
|
{
|
|
PontoMaisProximo = null;
|
|
return;
|
|
}
|
|
|
|
double menorDistancia = double.MaxValue;
|
|
PontoTrajetoriaModel melhorPonto = null;
|
|
|
|
// ✅ Busca apenas na janela otimizada
|
|
for (int i = idxInicialJanela; i <= idxFinalJanela && i < _TrajetoriaFixa.Count; i++)
|
|
{
|
|
if (Math.Abs(_TrajetoriaFixa[i].DistanciaTrajeto) < menorDistancia)
|
|
{
|
|
melhorPonto = _TrajetoriaFixa[i];
|
|
menorDistancia = Math.Abs(melhorPonto.DistanciaTrajeto);
|
|
}
|
|
}
|
|
|
|
// ✅ Só atualiza se encontrou um ponto
|
|
if (melhorPonto != null)
|
|
{
|
|
PontoMaisProximo = melhorPonto;
|
|
}
|
|
}
|
|
[JsonProperty]
|
|
public bool NaMargemDoCorredor { get; private set; }
|
|
private void AtualizarNaMargemDoCorredor()
|
|
{
|
|
NaMargemDoCorredor = false;
|
|
foreach (var ponto in _TrajetoriaJanela)
|
|
{
|
|
if (ponto.NaMargem && ponto.PontoBorda)
|
|
{
|
|
NaMargemDoCorredor = true; // Paramos a busca assim que encontramos um ponto válido
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
[JsonProperty]
|
|
public bool ManobrandoEntreRuas { get; private set; }
|
|
private void AtualizarManobrandoEntreRuas()
|
|
{
|
|
//bool proximoAentrada = (ProximoPonto.PontoBorda || ProximoPonto.PontoLigacao) && ProximoPonto.DistanciaAtual < DistanciaManobraEntreRuas;
|
|
//ManobrandoEntreRuas = (proximoAentrada && StatusAtual == StatusCarroMapa.Direcionando) || NaMargemDoCorredor;
|
|
|
|
ManobrandoEntreRuas = StatusAtual == StatusCarroMapa.Manobrando;
|
|
}
|
|
[JsonProperty]
|
|
public StatusCarroMapa StatusAtual { get; private set; }
|
|
private void AtualizarStatusAtual()
|
|
{
|
|
StatusAtual = StatusCarroMapa.Parado;
|
|
if (ProximoPonto != null && CorredorAtual != null && Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.EmAndamento)
|
|
{
|
|
if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas)
|
|
{
|
|
StatusAtual = StatusCarroMapa.Direcionando;
|
|
}
|
|
else if (!CorredorAtual.Dentro && (PontoAtual.PontoBorda || PontoAtual.PontoLigacao || ProximoPonto.Tipo == TipoPontoRua.LigacaoEntrada || PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores))
|
|
{
|
|
StatusAtual = StatusCarroMapa.Manobrando;
|
|
}
|
|
else if (CorredorAtual.Dentro && !NaMargemDoCorredor)
|
|
{
|
|
StatusAtual = StatusCarroMapa.CaminhandoRua;
|
|
}
|
|
else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaEntrada))
|
|
{
|
|
StatusAtual = StatusCarroMapa.EntrandoRua;
|
|
}
|
|
else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaSaida))
|
|
{
|
|
StatusAtual = StatusCarroMapa.SaindoRua;
|
|
}
|
|
else if (!CorredorAtual.Dentro)
|
|
{
|
|
StatusAtual = StatusCarroMapa.Direcionando;
|
|
}
|
|
}
|
|
}
|
|
[JsonProperty]
|
|
public double DistanciaTotal { get; private set; }
|
|
[JsonProperty]
|
|
public double DistanciaPercorrida { get; private set; }
|
|
private void AtualizarDistanciaPercorrida()
|
|
{
|
|
DistanciaPercorrida = _Corredores?.Sum(x => x.DistanciaPercorridaTotal) ?? 0;
|
|
}
|
|
[JsonProperty]
|
|
public double DistanciaRestante { get; private set; }
|
|
private void AtualizarDistanciaRestante()
|
|
{
|
|
var _trajetoria = (_TrajetoriaDinamica ?? new List<PontoTrajetoriaModel>()).Select(x => x.Posicao).ToList();
|
|
DistanciaRestante = GPSUtils.DistanciaDoTrecho(_trajetoria);
|
|
}
|
|
[JsonProperty]
|
|
public double PercentualTrajetoria { get; private set; }
|
|
public void AtualizarPercentualTrajetoria()
|
|
{
|
|
PercentualTrajetoria = DistanciaTotal > 0 ? (DistanciaPercorrida / DistanciaTotal) * 100.0 : 0;
|
|
}
|
|
[JsonProperty]
|
|
public string TempoEstimadoOperacao { get; private set; }
|
|
[JsonProperty]
|
|
public string TempoEstimadoRestante { get; private set; }
|
|
public void AtualizarTempoEstimado(double? velocidadeSemErvasMs = null, double? velocidadeComErvasMs = null)
|
|
{
|
|
if (!velocidadeSemErvasMs.HasValue)
|
|
{
|
|
velocidadeSemErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeMax) * 0.8;
|
|
}
|
|
if (!velocidadeComErvasMs.HasValue)
|
|
{
|
|
velocidadeComErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeMin);
|
|
}
|
|
|
|
// Supondo que metade do percurso tem ervas e a outra metade não tem
|
|
double distanciaComErvas = DistanciaTotal * VariaveisOperacao.PercentualComErvas;
|
|
double distanciaSemErvas = DistanciaTotal * VariaveisOperacao.PercentualSemErvas;
|
|
|
|
// Tempo em segundos para cada parte do percurso
|
|
double tempoSemErvas = velocidadeSemErvasMs.Value > 0 ? distanciaSemErvas / velocidadeSemErvasMs.Value : 0;
|
|
double tempoComErvas = velocidadeComErvasMs.Value > 0 ? distanciaComErvas / velocidadeComErvasMs.Value : 0;
|
|
|
|
// Tempo total em segundos
|
|
double tempoTotalSegundos = tempoSemErvas + tempoComErvas;
|
|
|
|
// Conversão para horas, minutos e segundos
|
|
TimeSpan tempoTotal = TimeSpan.FromSeconds(tempoTotalSegundos);
|
|
DateTime total = new DateTime(tempoTotal.Ticks);
|
|
|
|
TempoEstimadoOperacao = total.ToString("HH:mm:ss");
|
|
|
|
// Distâncias (em metros)
|
|
double distanciaRestante = DistanciaRestante;
|
|
|
|
// Calcular a distância restante sem ervas e com ervas
|
|
double distanciaSemErvasRestante = distanciaRestante * VariaveisOperacao.PercentualSemErvas;
|
|
double distanciaComErvasRestante = distanciaRestante * VariaveisOperacao.PercentualComErvas;
|
|
|
|
// Calcular o tempo restante para as áreas sem ervas e com ervas
|
|
double tempoRestanteSemErvas = distanciaSemErvasRestante / velocidadeSemErvasMs.Value;
|
|
double tempoRestanteComErvas = distanciaComErvasRestante / velocidadeComErvasMs.Value;
|
|
|
|
// Tempo total restante em segundos
|
|
double tempoTotalRestanteSegundos = tempoRestanteSemErvas + tempoRestanteComErvas;
|
|
|
|
// Conversão para horas, minutos e segundos
|
|
TimeSpan tempoTotalRestante = TimeSpan.FromSeconds(tempoTotalRestanteSegundos);
|
|
DateTime totalRestante = new DateTime(tempoTotalRestante.Ticks);
|
|
|
|
TempoEstimadoRestante = totalRestante.ToString("HH:mm:ss");
|
|
}
|
|
|
|
[JsonProperty]
|
|
public double ErroOrientacaoAngular { get; private set; }
|
|
[JsonProperty]
|
|
public double ErroOrientacaoAngularCombinado { get; private set; }
|
|
[JsonProperty]
|
|
public double ErroOrientacaoAngularCaminho { get; private set; }
|
|
[JsonProperty]
|
|
public double ErroOrientacaoAngularProximoPonto { get; private set; }
|
|
[JsonProperty]
|
|
public double ErroLateralAngular { get; private set; }
|
|
|
|
private void AtualizarErroCombinado()
|
|
{
|
|
var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;
|
|
|
|
//double anguloCamera = _Sensoriamento.OperadorVisual.Leitura.obj?.segmentacao?.angulo_rua ?? 0;
|
|
double anguloCorredor = AnguloMedioCorredor;
|
|
double anguloProximoPonto = AnguloCaminho;
|
|
double anguloCarro = _Sensoriamento.Gps.AnguloCarroDefinido;
|
|
|
|
// Erro 1: Direção para onde o robô deve ir (centro do corredor)
|
|
double erroIrParaCentro = GPSUtils.CalcularDiferencaAngulo(anguloCarro, anguloProximoPonto);
|
|
|
|
// Erro 2: Quanto o robô está desalinhado da orientação da rua
|
|
double desalinhamentoComRua = GPSUtils.CalcularDiferencaAngulo(anguloCarro, anguloCorredor);
|
|
|
|
// Erro 3 (secundário): Diferença entre o ângulo do corredor e o ponto (curva futura)
|
|
double curvaDaRua = GPSUtils.CalcularDiferencaAngulo(anguloProximoPonto, anguloCorredor);
|
|
|
|
// Peso do desalinhamento atual (prioriza alinhar o robô ao corredor)
|
|
double pesoAlinhamento = Math.Min(Math.Abs(desalinhamentoComRua) / 30.0, 1.0); // Normaliza até 30°
|
|
|
|
// Peso do centro (prioriza ir pro ponto central do corredor)
|
|
double pesoCentro = 1.0 - pesoAlinhamento;
|
|
|
|
// Peso extra para curvas mais fechadas
|
|
double boostCurva = 1.0 + (Math.Abs(curvaDaRua) / 45.0); // aumenta peso em curvas
|
|
|
|
// Calcule o erro lateral baseado nas distâncias entre as ruas do mapa atraves do GPS
|
|
double erroLateral = !(CorredorAtual?.Dentro ?? false) ? 0 : (DistanciaEsquerda - DistanciaDireita);
|
|
double distanciaProximoPonto = ProximoPonto.DistanciaAtual;
|
|
double erroLateralGraus = Math.Atan2(erroLateral, distanciaProximoPonto) * (180.0 / Math.PI);
|
|
|
|
// Calcule o erro do ultrassom
|
|
double erroUltrassom = UltrasonicA05Helper.CalcularErroUltrassons();
|
|
|
|
// Combina os erros com pesos dinâmicos
|
|
double erroCombinado = (erroIrParaCentro * pesoCentro) + (desalinhamentoComRua * pesoAlinhamento);
|
|
|
|
|
|
ErroOrientacaoAngularProximoPonto = erroIrParaCentro;
|
|
ErroOrientacaoAngularCaminho = desalinhamentoComRua;
|
|
ErroOrientacaoAngular = curvaDaRua;
|
|
ErroOrientacaoAngularCombinado = erroCombinado * boostCurva;
|
|
ErroLateralAngular = erroLateralGraus;
|
|
}
|
|
|
|
private void MarcarPontosIntermediariosNaoVisitados()
|
|
{
|
|
if (!_TrajetoriaFixaDefinida) return;
|
|
|
|
for (int i = PontoAtual.idxPonto; i >= Math.Max(0, PontoAtual.idxPonto - JanelaAtualizacaoDePontos); i--)
|
|
{
|
|
var ponto = _TrajetoriaFixa[i];
|
|
if (!ponto.Visitado)
|
|
{
|
|
ponto.Visitado = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
private void AtualizarPropriedadesPontosTrajetoria()
|
|
{
|
|
if (!_TrajetoriaFixaDefinida) return;
|
|
|
|
// Atualiza apenas os pontos dentro do intervalo definido
|
|
foreach (var ponto in _TrajetoriaJanela)
|
|
{
|
|
ponto.AtualizarPropriedades();
|
|
}
|
|
}
|
|
|
|
private void AtualizarDadosTrajetoria()
|
|
{
|
|
if (!_TrajetoriaFixaDefinida) return;
|
|
|
|
AtualizarTempoEntreLeituras();
|
|
AtualizarDistanciaMaximaEntreLeituras();
|
|
|
|
AtualizarIndicesJanela();
|
|
AtualizarTrajetoriaJanela();
|
|
|
|
DefinirPontoAtual();
|
|
AtualizarCorredorAtual();
|
|
DefinirProximoPonto();
|
|
MarcarPontosIntermediariosNaoVisitados();
|
|
DefinirPontoMaisProximo();
|
|
AtualizarAnguloMedioCorredor();
|
|
AtualizarAnguloCaminho();
|
|
AtualizarDistanciasLaterais();
|
|
AtualizarNaMargemDoCorredor();
|
|
AtualizarManobrandoEntreRuas();
|
|
AtualizarStatusAtual();
|
|
AtualizarDistanciaPercorrida();
|
|
AtualizarDistanciaRestante();
|
|
AtualizarPercentualTrajetoria();
|
|
AtualizarTempoEstimado();
|
|
|
|
AtualizarErroCombinado();
|
|
|
|
AtualizarTrajetoriaDinamica();
|
|
}
|
|
|
|
|
|
|
|
public void LoopAtualizaDados()
|
|
{
|
|
AtualizarPropriedadesPontosTrajetoria();
|
|
|
|
AtualizarDadosTrajetoria();
|
|
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados();
|
|
|
|
//Variaveis.OperacaoEmAndamento.AtualizaInformacoesControleOperacao();
|
|
|
|
RedisService.Publish(CmdKey.ManagerWorkerRx, JsonConvert.SerializeObject(new { cmd = ManagerWorkerCommandType.AtualizarMPC }));
|
|
}
|
|
|
|
|
|
private (List<List<GPSModel>>, List<double>) ProjetarCorredores()
|
|
{
|
|
var corredores = new List<List<GPSModel>>();
|
|
var larguras = new List<double>();
|
|
|
|
// Calcula o deslocamento lateral do GPS
|
|
double larguraTotal = VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita; // Largura total do robô
|
|
double deslocamentoLateralGPSpercentual = ((Math.Abs(VariaveisEquipamento.LarguraEsquerda - VariaveisEquipamento.LarguraDireita) / larguraTotal) / 2.0);
|
|
double deslocamentoLateralGPSmetros = (LarguraCorredorPadrao * deslocamentoLateralGPSpercentual);
|
|
|
|
if (Variaveis.OperacaoEmAndamento.Mapa.TipoMapa == TipoMapaOperacao.Corredores)
|
|
{
|
|
var ruasProjetadas = ProjetarRuasApartirDosCorredores(RuasPlantacao);
|
|
|
|
// Atualiza RuasPlantacao com as ruas projetadas
|
|
RuasPlantacao = ruasProjetadas;
|
|
}
|
|
|
|
for (int i = 0; i < RuasPlantacao.Count - 1; i++)
|
|
{
|
|
var rua1 = RuasPlantacao[i];
|
|
var rua2 = RuasPlantacao[i + 1];
|
|
|
|
// Garantir que as ruas tenham o mesmo número de pontos
|
|
if (rua1.Count != rua2.Count)
|
|
{
|
|
if (rua1.Count > rua2.Count)
|
|
{
|
|
rua2 = GPSUtils.InterpolarRota(rua2, rua1.Count);
|
|
}
|
|
else
|
|
{
|
|
rua1 = GPSUtils.InterpolarRota(rua1, rua2.Count);
|
|
}
|
|
}
|
|
|
|
var corredor = new List<GPSModel>();
|
|
|
|
double primeiraDistancia = 0;
|
|
|
|
// Calcular pontos médios entre as duas ruas e interpolar a cada 25 cm
|
|
for (int j = 0; j < rua1.Count - 1; j++)
|
|
{
|
|
var ponto1Rua1 = rua1[j];
|
|
var ponto2Rua1 = rua1[j + 1];
|
|
var ponto1Rua2 = rua2[j];
|
|
var ponto2Rua2 = rua2[j + 1];
|
|
|
|
//var pontoMedioInicial = GPSUtils.PontoMedio(ponto1Rua1, ponto1Rua2);
|
|
//var pontoMedioFinal = GPSUtils.PontoMedio(ponto2Rua1, ponto2Rua2);
|
|
|
|
var pontoMedioInicial = GPSUtils.PontoMedioComOffset(ponto1Rua1, ponto1Rua2, deslocamentoLateralGPSmetros);
|
|
var pontoMedioFinal = GPSUtils.PontoMedioComOffset(ponto2Rua1, ponto2Rua2, deslocamentoLateralGPSmetros);
|
|
|
|
var distancia = GPSUtils.DistanciaEntrePontos(pontoMedioInicial, pontoMedioFinal);
|
|
if (EspacamentoPrimeirosPontosProjecao && j == 0)
|
|
{
|
|
primeiraDistancia = distancia;
|
|
}
|
|
double distInterpolar = (EspacamentoPrimeirosPontosProjecao && j == 0) ? DistanciaEntrePontosCurva : DistanciaEntrePontos;
|
|
|
|
int numInterpolacoes = (int)(distancia / distInterpolar);
|
|
|
|
for (int k = 0; k <= numInterpolacoes; k++)
|
|
{
|
|
double t = numInterpolacoes == 0 ? 0 : k / (double)numInterpolacoes;
|
|
var pontoInterpolado = GPSUtils.InterpolarPonto(pontoMedioInicial, pontoMedioFinal, t);
|
|
corredor.Add(pontoInterpolado);
|
|
}
|
|
}
|
|
|
|
double distanciaCorredor = GPSUtils.DistanciaDoTrecho(corredor);
|
|
double distanciaRua1 = GPSUtils.DistanciaDoTrecho(rua1);
|
|
double distanciaRua2 = GPSUtils.DistanciaDoTrecho(rua2);
|
|
|
|
if ((distanciaCorredor + DistanciaEntrePontos) < distanciaRua1)
|
|
{
|
|
double dif = Math.Abs(distanciaRua1 - distanciaCorredor);
|
|
double anguloCorredor = GPSUtils.CalcularOrientacao(corredor[corredor.Count - 1], corredor[corredor.Count - 2]);
|
|
GPSModel novoPontoCorredor = GPSUtils.ProjetarPontoDeslocado(corredor[0], dif, anguloCorredor);
|
|
corredor.Insert(0, novoPontoCorredor);
|
|
}
|
|
else if ((distanciaCorredor + DistanciaEntrePontos) < distanciaRua2)
|
|
{
|
|
double dif = Math.Abs(distanciaRua2 - distanciaCorredor);
|
|
double anguloCorredor = GPSUtils.CalcularOrientacao(corredor[corredor.Count - 1], corredor[corredor.Count - 2]);
|
|
GPSModel novoPontoCorredor = GPSUtils.ProjetarPontoDeslocado(corredor[0], dif, anguloCorredor);
|
|
corredor.Insert(0, novoPontoCorredor);
|
|
}
|
|
|
|
// Remover pontos que estão a menos de 1m de distância
|
|
corredor = RemoverPontosMuitoProximos(corredor, DistanciaEntrePontos, primeiraDistancia, DistanciaEntrePontosCurva);
|
|
|
|
// Calcular largura média entre as duas ruas
|
|
double larguraCorredor = CalcularLarguraMediaDoCorredor(rua1, rua2);
|
|
|
|
corredores.Add(corredor);
|
|
larguras.Add(larguraCorredor);
|
|
}
|
|
|
|
|
|
return (corredores, larguras);
|
|
}
|
|
|
|
private List<List<GPSModel>> ProjetarRuasApartirDosCorredores(List<List<GPSModel>> corredores)
|
|
{
|
|
var ruasProjetadas = new List<List<GPSModel>>();
|
|
double larguraCorredor = LarguraCorredorPadrao;
|
|
|
|
if (corredores.Count > 1)
|
|
{
|
|
larguraCorredor = CalcularLarguraMediaDoCorredor(corredores[0], corredores[1]);
|
|
}
|
|
|
|
for (int i = 0; i < corredores.Count; i++)
|
|
{
|
|
var corredor = corredores[i];
|
|
double offset = larguraCorredor / 2.0;
|
|
|
|
// Calcular ruas apenas para o primeiro corredor
|
|
if (i == 0)
|
|
{
|
|
var ruaEsquerda = new List<GPSModel>();
|
|
|
|
for (int j = 0; j < corredor.Count - 1; j++)
|
|
{
|
|
var pontoAtual = corredor[j];
|
|
var pontoProximo = corredor[j + 1];
|
|
|
|
double angulo = GPSUtils.CalcularOrientacao(pontoAtual, pontoProximo);
|
|
var pontoEsquerda = GPSUtils.ProjetarPontoDeslocado(pontoAtual, offset, angulo + 90);
|
|
ruaEsquerda.Add(pontoEsquerda);
|
|
}
|
|
|
|
// Adiciona último ponto da rua esquerda
|
|
var ultimo = corredor[corredor.Count - 1];
|
|
var anterior = corredor[corredor.Count - 2];
|
|
double anguloFinal = GPSUtils.CalcularOrientacao(anterior, ultimo);
|
|
ruaEsquerda.Add(GPSUtils.ProjetarPontoDeslocado(ultimo, offset, anguloFinal + 90));
|
|
|
|
ruasProjetadas.Add(ruaEsquerda);
|
|
}
|
|
|
|
// Sempre criar a rua direita (inclusive para o último corredor)
|
|
var ruaDireita = new List<GPSModel>();
|
|
|
|
for (int j = 0; j < corredor.Count - 1; j++)
|
|
{
|
|
var pontoAtual = corredor[j];
|
|
var pontoProximo = corredor[j + 1];
|
|
|
|
double angulo = GPSUtils.CalcularOrientacao(pontoAtual, pontoProximo);
|
|
var pontoDireita = GPSUtils.ProjetarPontoDeslocado(pontoAtual, offset, angulo - 90);
|
|
ruaDireita.Add(pontoDireita);
|
|
}
|
|
|
|
var ultimoPonto = corredor[corredor.Count - 1];
|
|
var pontoAnterior = corredor[corredor.Count - 2];
|
|
double anguloUltimo = GPSUtils.CalcularOrientacao(pontoAnterior, ultimoPonto);
|
|
ruaDireita.Add(GPSUtils.ProjetarPontoDeslocado(ultimoPonto, offset, anguloUltimo - 90));
|
|
|
|
ruasProjetadas.Add(ruaDireita);
|
|
}
|
|
|
|
return ruasProjetadas;
|
|
}
|
|
|
|
// Método auxiliar para remover pontos muito próximos de forma balanceada
|
|
private List<GPSModel> RemoverPontosMuitoProximos(List<GPSModel> pontos, double distanciaMinima, double apartirDe, double distanciaMenor)
|
|
{
|
|
if (pontos == null || pontos.Count == 0)
|
|
return pontos;
|
|
|
|
pontos = pontos.Where(x => !double.IsNaN(x.Latitude) && !double.IsNaN(x.Longitude)).ToList();
|
|
|
|
var resultado = new List<GPSModel> { pontos[0] }; // Sempre incluir o primeiro ponto
|
|
double distanciaAcumulada = 0.0; // Distância total percorrida
|
|
|
|
for (int i = 1; i < pontos.Count - 1; i++) // Evita verificar o último ponto diretamente
|
|
{
|
|
double distancia = GPSUtils.DistanciaEntrePontos(resultado.Last(), pontos[i]);
|
|
distanciaAcumulada += distancia;
|
|
|
|
// Nos primeiros 2 metros, mantemos um espaçamento mínimo de 25 cm, depois 1 metro
|
|
double distanciaRequerida = distanciaAcumulada <= apartirDe ? distanciaMenor : distanciaMinima;
|
|
|
|
if (distancia >= distanciaRequerida)
|
|
{
|
|
resultado.Add(pontos[i]); // Mantém o ponto se a distância for suficiente
|
|
}
|
|
}
|
|
|
|
// Sempre adiciona o último ponto da lista para garantir fechamento da trajetória
|
|
if (!resultado.Contains(pontos.Last()))
|
|
{
|
|
resultado.Add(pontos.Last());
|
|
}
|
|
|
|
return resultado;
|
|
}
|
|
|
|
public static double CalcularLarguraMediaDoCorredor(List<GPSModel> rua1, List<GPSModel> rua2)
|
|
{
|
|
// 1. Interpolar as duas ruas para garantir pontos uniformemente distribuídos
|
|
List<GPSModel> rua1Interpolada = GPSUtils.InterpolarRota(rua1, Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua1)));
|
|
List<GPSModel> rua2Interpolada = GPSUtils.InterpolarRota(rua2, Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua2)));
|
|
|
|
double somaDistancias = 0;
|
|
int totalPontos = 0;
|
|
|
|
// 2. Para cada ponto da rua1 interpolada, encontrar o mais próximo na rua2 interpolada
|
|
foreach (var ponto1 in rua1Interpolada)
|
|
{
|
|
var pontoMaisProximo = rua2Interpolada.OrderBy(ponto2 => GPSUtils.DistanciaEntrePontos(ponto1, ponto2)).First();
|
|
|
|
double distancia = GPSUtils.DistanciaEntrePontos(ponto1, pontoMaisProximo);
|
|
|
|
somaDistancias += distancia;
|
|
totalPontos++;
|
|
}
|
|
|
|
// 3. Retorna a largura média do corredor
|
|
return totalPontos > 0 ? somaDistancias / totalPontos : 0;
|
|
}
|
|
|
|
public void ProjetarTrajetoriaFixa()
|
|
{
|
|
List<PontoTrajetoriaModel> _trajetoriaFixa = new List<PontoTrajetoriaModel>();
|
|
(List<List<GPSModel>> corredores, List<double> CorredoresLarguras) = ProjetarCorredores();
|
|
Corredores = corredores;
|
|
|
|
Enums.DirecaoCarroRua direcaoAtual = ExtremoMaisProximoMapa == 0 ? Enums.DirecaoCarroRua.Ida : Enums.DirecaoCarroRua.Volta;
|
|
//Enums.DirecaoCarroRua direcaoAtual = Enums.DirecaoCarroRua.Ida;
|
|
|
|
double larguraCorredorMenor = LarguraCorredorPadrao * 0.9;
|
|
|
|
PontoTrajetoriaModel PontoRobo = new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
|
|
{
|
|
idxCorredor = 0,
|
|
idxPonto = 0,
|
|
idxPontoCorredor = 0,
|
|
Posicao = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps,
|
|
Direcao = direcaoAtual,
|
|
Orientacao = 0,
|
|
Visitado = true,
|
|
LarguraCorredor = 1.0
|
|
};
|
|
_trajetoriaFixa.Add(PontoRobo);
|
|
|
|
int idxEsq = 0;
|
|
int idxDir = 0;
|
|
|
|
double anguloAcrescentar = idxDir % 2 == 0 && direcaoAtual == DirecaoCarroRua.Ida ? 20 : -20;
|
|
|
|
for (int idx = 0; idx < Corredores.Count(); idx++)
|
|
{
|
|
double _FatorLarguraCorredor = CorredoresLarguras[idx] / LarguraCorredorPadrao;
|
|
bool _FatorLarguraElevado = _FatorLarguraCorredor > 1.2;
|
|
|
|
if (_trajetoriaFixa.Count() > 1 && idxEsq == 0 && idxDir == 0)
|
|
{
|
|
var pontos = _trajetoriaFixa.Where(x => x.Tipo == TipoPontoRua.Rua).ToList();
|
|
if (pontos.Count >= 2)
|
|
{
|
|
var p0 = pontos[0];
|
|
var p1 = pontos[1];
|
|
|
|
(List<GPSModel> ruaEsq, List<GPSModel> ruaDir) = DeterminarRuasLaterais(p0.Posicao, p1.Posicao, RuasPlantacao[0], RuasPlantacao[1]);
|
|
|
|
idxEsq = RuasPlantacao.IndexOf(ruaEsq);
|
|
idxDir = RuasPlantacao.IndexOf(ruaDir);
|
|
}
|
|
else
|
|
{
|
|
// ⚠️ Caso de erro: não há dois pontos de rua, mesmo com mais de 1 ponto na trajetória.
|
|
Console.WriteLine("Não há ao menos dois pontos do tipo Rua.");
|
|
}
|
|
}
|
|
|
|
bool ultimoCorredor = idx == Corredores.Count() - 1;
|
|
List<GPSModel> CorredorAtual = Corredores[idx];
|
|
|
|
double dP0 = GPSUtils.DistanciaEntrePontos(_trajetoriaFixa.Last().Posicao, CorredorAtual[0]);
|
|
double dP1 = GPSUtils.DistanciaEntrePontos(_trajetoriaFixa.Last().Posicao, CorredorAtual[CorredorAtual.Count() - 1]);
|
|
if (dP1 < dP0)
|
|
{
|
|
CorredorAtual.Reverse();
|
|
}
|
|
|
|
double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).First(), CorredorAtual.First());
|
|
GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.First(), DistanciaProjecaoRua, anguloProjetar1);
|
|
|
|
double anguloProjetar2 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(CorredorAtual.Count() - 2).First(), CorredorAtual.Last());
|
|
// Realiza um acréscimo no angulo à projetar, permitindo que o equipamento faça uma curva mais aberta entre corredores
|
|
if (!ultimoCorredor && !_FatorLarguraElevado)
|
|
{
|
|
anguloProjetar2 += anguloAcrescentar;
|
|
anguloAcrescentar *= -1;
|
|
}
|
|
GPSModel UltimoPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.Last(), !ultimoCorredor ? DistanciaProjecaoRua : (DistanciaProjecaoRua * 2.0), anguloProjetar2);
|
|
|
|
var ultimoPontoTrajetoria = _trajetoriaFixa.LastOrDefault();
|
|
|
|
PontoTrajetoriaModel PontoInicial = new PontoTrajetoriaModel(TipoPontoRua.LigacaoEntrada)
|
|
{
|
|
idxCorredor = idx,
|
|
idxPonto = _trajetoriaFixa.Count(),
|
|
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
|
Posicao = PrimeiroPonto,
|
|
Direcao = direcaoAtual,
|
|
Orientacao = GPSUtils.CalcularOrientacao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto),
|
|
Visitado = false,
|
|
LarguraCorredor = larguraCorredorMenor
|
|
};
|
|
|
|
bool PontoInicialAdicionado = false;
|
|
// Criar curva suave para ligacao dos corredores
|
|
if (idx > 0)
|
|
{
|
|
// 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 = GPSUtils.DistanciaEntrePontos(ultimoPontoTrajetoria.Posicao, PontoInicial.Posicao);
|
|
bool distanciaMinima = distancia < DistanciaManobraEntreRuas;
|
|
|
|
if (distanciaMinima)
|
|
{
|
|
int pontosAdicionr = Convert.ToInt32(CorredoresLarguras[idx] / (DistanciaEntrePontosCurva * _FatorLarguraCorredor));
|
|
|
|
var _penultimoPonto = _trajetoriaFixa[_trajetoriaFixa.Count() - 2].Posicao;
|
|
double anguloProjecao = GPSUtils.CalcularOrientacao(Corredores[idx - 1].First(), Corredores[idx - 1].Last());
|
|
GPSModel _ultimoPonto = GPSUtils.ProjetarPontoDeslocado(Corredores[idx - 1].Last(), DistanciaProjecaoRua, anguloProjecao);
|
|
|
|
GPSModel P0 = CalcularPontoControle(_penultimoPonto, _ultimoPonto, PrimeiroPonto, DistanciaProjecaoRua * 2.0);
|
|
List<GPSModel> CurvaConexao = GerarCurvaConexao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, P0, pontosAdicionr);
|
|
|
|
for (int i = 0; i < CurvaConexao.Count; i++)
|
|
{
|
|
var ponto = CurvaConexao[i];
|
|
bool pontoCorredorAnterior = i < (pontosAdicionr / 2);
|
|
int idxCorredorPontoCurva = pontoCorredorAnterior ? idx - 1 : idx;
|
|
bool pontoLigacao = CurvaConexao.IndexOf(ponto) == CurvaConexao.Count() - 1;
|
|
|
|
bool meiaCurva = false;
|
|
|
|
if ((meiaCurva && pontoCorredorAnterior) || !meiaCurva)
|
|
{
|
|
_trajetoriaFixa.Add(new PontoTrajetoriaModel(pontoLigacao ? TipoPontoRua.LigacaoEntrada : TipoPontoRua.CruvaEntreCorredores)
|
|
{
|
|
idxCorredor = idxCorredorPontoCurva,
|
|
idxPonto = _trajetoriaFixa.Count(),
|
|
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
|
Posicao = ponto,
|
|
Direcao = pontoLigacao ? direcaoAtual : DirecaoCarroRua.Manobra,
|
|
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, ponto),
|
|
Visitado = false,
|
|
LarguraCorredor = larguraCorredorMenor
|
|
});
|
|
}
|
|
|
|
}
|
|
PontoInicialAdicionado = true;
|
|
}
|
|
}
|
|
|
|
if (!PontoInicialAdicionado)
|
|
{
|
|
_trajetoriaFixa.Add(PontoInicial);
|
|
}
|
|
|
|
foreach (GPSModel pontoAtual in CorredorAtual)
|
|
{
|
|
int idxPonto = CorredorAtual.IndexOf(pontoAtual);
|
|
bool bordaEntrada = idxPonto == 0;
|
|
bool bordaSaida = idxPonto == CorredorAtual.Count() - 1;
|
|
double distanciaCorredor = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.idxCorredor == idx).Select(x => x.Posicao).ToList());
|
|
|
|
PontoTrajetoriaModel PontoTrajetoria = new PontoTrajetoriaModel(bordaEntrada ? TipoPontoRua.BordaEntrada : bordaSaida ? TipoPontoRua.BordaSaida : TipoPontoRua.Rua)
|
|
{
|
|
idxCorredor = idx,
|
|
idxPonto = _trajetoriaFixa.Count(),
|
|
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
|
Posicao = pontoAtual,
|
|
Direcao = direcaoAtual,
|
|
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, pontoAtual),
|
|
Visitado = false,
|
|
LarguraCorredor = bordaEntrada || bordaSaida ? larguraCorredorMenor : CorredoresLarguras[idx]
|
|
};
|
|
|
|
_trajetoriaFixa.Add(PontoTrajetoria);
|
|
}
|
|
|
|
PontoTrajetoriaModel PontoFinal = new PontoTrajetoriaModel(TipoPontoRua.LigacaoSaida)
|
|
{
|
|
idxCorredor = idx,
|
|
idxPonto = _trajetoriaFixa.Count(),
|
|
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
|
Posicao = UltimoPonto,
|
|
Direcao = direcaoAtual,
|
|
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, UltimoPonto),
|
|
Visitado = false,
|
|
LarguraCorredor = larguraCorredorMenor
|
|
};
|
|
_trajetoriaFixa.Add(PontoFinal);
|
|
|
|
direcaoAtual = direcaoAtual == Enums.DirecaoCarroRua.Ida ? Enums.DirecaoCarroRua.Volta : Enums.DirecaoCarroRua.Ida;
|
|
}
|
|
|
|
_TrajetoriaFixa = new List<PontoTrajetoriaModel>(_trajetoriaFixa);
|
|
|
|
if (_trajetoriaFixa.Count() > 1 && idxEsq == 0 && idxDir == 0)
|
|
{
|
|
(List<GPSModel> ruaEsq, List<GPSModel> ruaDir) = DeterminarRuasLaterais(_trajetoriaFixa[1].Posicao, _trajetoriaFixa[2].Posicao, RuasPlantacao[0], RuasPlantacao[1]);
|
|
idxEsq = RuasPlantacao.IndexOf(ruaEsq);
|
|
idxDir = RuasPlantacao.IndexOf(ruaDir);
|
|
}
|
|
int incEsq = 0;
|
|
int incDir = 0;
|
|
_Corredores = _TrajetoriaFixa
|
|
.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo)
|
|
.GroupBy(x => x.idxCorredor)
|
|
.Select((x, i) =>
|
|
{
|
|
double distanciaTotal = GPSUtils.DistanciaDoTrecho(x.Select(y => y.Posicao).ToList());
|
|
|
|
(incDir, incEsq) = AtualizaIndiceRuaLateralCorredor(i, idxDir, incDir, incEsq);
|
|
|
|
var obj = new CorredorTrajetoriaModel()
|
|
{
|
|
Idx = x.Key,
|
|
Largura = CorredoresLarguras[x.Key],
|
|
DistanciaTotal = distanciaTotal,
|
|
Pontos = x.ToList(),
|
|
QtdPontos = x.Count(),
|
|
DistanciaPercorridaCorredor = 0,
|
|
DistanciaPercorridaTotal = 0,
|
|
idxRuaDireita = idxDir + incDir,
|
|
idxRuaEsquerda = idxEsq + incEsq,
|
|
FatorLarguraCorredor = CorredoresLarguras[x.Key] / LarguraCorredorPadrao
|
|
};
|
|
|
|
return obj;
|
|
})
|
|
.ToList();
|
|
|
|
_Corredores.ForEach(x => x.Ultimo = x.Idx == _Corredores.Count() - 1);
|
|
|
|
DistanciaTotal = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo).Select(x => x.Posicao).ToList());
|
|
|
|
AtualizarDadosTrajetoria();
|
|
|
|
LoopAtualizaDados();
|
|
|
|
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
|
}
|
|
|
|
private (int, int) AtualizaIndiceRuaLateralCorredor(int i, int idxDir, int incDir, int incEsq)
|
|
{
|
|
bool par = (i % 2 == 0);
|
|
if (i > 0)
|
|
{
|
|
if (idxDir == 0)
|
|
{
|
|
incDir += !par ? 2 : 0;
|
|
incEsq += !par ? 0 : 2;
|
|
}
|
|
else
|
|
{
|
|
incDir += par ? 2 : 0;
|
|
incEsq += par ? 0 : 2;
|
|
}
|
|
}
|
|
return (incDir, incEsq);
|
|
}
|
|
|
|
public static GPSModel CalcularPontoControle(GPSModel penultimoPontoRuaAtual, GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
|
|
{
|
|
double anguloCurva = GPSUtils.CalcularOrientacao(penultimoPontoRuaAtual, ultimoPontoRuaAtual);
|
|
double angulo = GPSUtils.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
|
|
|
|
// Calcular a diferença de ângulo corretamente no espaço de 360 graus
|
|
double diferencaAngulo = ((angulo - anguloCurva + 540) % 360) - 180;
|
|
|
|
bool ParaDentro = diferencaAngulo > 0; // Agora o cálculo é confiável
|
|
|
|
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 ? -90 : 90;
|
|
|
|
// Projetar um ponto a uma certa distância na direção ajustada
|
|
return GPSUtils.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;
|
|
}
|
|
|
|
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 void AjustarTrajetoriaParaDesvio(Obstaculo obstaculo)
|
|
{
|
|
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps;
|
|
|
|
double distanciaObstaculo = ((double)obstaculo.DistanciaMedia_mm / 1000);
|
|
double larguraObstaculo = ((double)obstaculo.Largura_mm / 1000);
|
|
double anguloCaminho = AnguloCaminho;
|
|
double anguloDesvioInicial = anguloCaminho + obstaculo.AnguloParaDesvio;
|
|
double anguloDesvioFinal = anguloCaminho - obstaculo.AnguloParaDesvio;
|
|
|
|
GPSModel ponto1 = GPSUtils.GerarPontoDeslocado(posicaoAtual, anguloDesvioInicial, distanciaObstaculo);
|
|
GPSModel ponto2 = GPSUtils.GerarPontoDeslocado(ponto1, anguloCaminho, larguraObstaculo);
|
|
GPSModel ponto3 = GPSUtils.GerarPontoDeslocado(ponto2, anguloDesvioFinal, distanciaObstaculo);
|
|
List<PontoTrajetoriaModel> lista = new List<PontoTrajetoriaModel>()
|
|
{
|
|
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
|
{
|
|
Posicao = ponto1,
|
|
},
|
|
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
|
{
|
|
Posicao = ponto2,
|
|
},
|
|
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
|
{
|
|
Posicao = ponto3,
|
|
}
|
|
};
|
|
|
|
var idxRemover = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, ponto3, distanciaObstaculo);
|
|
_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 = GPSUtils.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 (List<GPSModel> ruaEsquerda, List<GPSModel> ruaDireita) DeterminarRuasLaterais(GPSModel robo, GPSModel proximoPonto, List<GPSModel> rua1, List<GPSModel> rua2)
|
|
{
|
|
// Determinar qual está à esquerda e qual está à direita
|
|
var r1 = new List<GPSModel>(rua1);
|
|
if (ExtremoMaisProximoMapa == 1)
|
|
{
|
|
r1.Reverse();
|
|
}
|
|
r1 = r1.Take(3).ToList();
|
|
bool rua1EstaEsquerda = EstaEsquerda(robo, proximoPonto, Centroide(r1));
|
|
|
|
return rua1EstaEsquerda ? (rua1, rua2) : (rua2, rua1);
|
|
}
|
|
|
|
// Retorna o ponto central da rua (média das coordenadas)
|
|
private GPSModel Centroide(List<GPSModel> rua)
|
|
{
|
|
double latMedia = rua.Average(p => p.Latitude);
|
|
double lonMedia = rua.Average(p => p.Longitude);
|
|
return new GPSModel { Latitude = latMedia, Longitude = lonMedia };
|
|
}
|
|
|
|
// Determina se um ponto está à esquerda ou à direita do movimento do robô
|
|
private bool EstaEsquerda(GPSModel robo, GPSModel proximoPonto, GPSModel ponto)
|
|
{
|
|
double dxRobo = proximoPonto.Longitude - robo.Longitude;
|
|
double dyRobo = proximoPonto.Latitude - robo.Latitude;
|
|
double dxPonto = ponto.Longitude - robo.Longitude;
|
|
double dyPonto = ponto.Latitude - robo.Latitude;
|
|
|
|
double crossProduct = (dxRobo * dyPonto) - (dyRobo * dxPonto);
|
|
return crossProduct > 0; // Se for positivo, está à esquerda; se for negativo, está à direita.
|
|
}
|
|
|
|
}
|
|
|
|
public class CorredorTrajetoriaModel
|
|
{
|
|
public int Idx { get; set; }
|
|
public double Largura { get; set; }
|
|
public double DistanciaTotal { get; set; }
|
|
public double DistanciaPercorridaCorredor { get; set; }
|
|
public double DistanciaPercorridaTotal { get; set; }
|
|
public List<PontoTrajetoriaModel> Pontos { get; set; }
|
|
public int QtdPontos { get; set; }
|
|
[JsonProperty]
|
|
public bool Dentro { get; private set; }
|
|
[JsonProperty]
|
|
public double DistanciaRestante { get; private set; }
|
|
[JsonProperty]
|
|
public double Progresso { get; private set; }
|
|
[JsonProperty]
|
|
public bool Concluido { get; private set; }
|
|
public bool Ultimo { get; set; }
|
|
public int idxRuaEsquerda { get; set; }
|
|
public int idxRuaDireita { get; set; }
|
|
public double FatorLarguraCorredor { get; set; }
|
|
|
|
public void AtualizarDados()
|
|
{
|
|
Dentro = false;
|
|
// Itera diretamente sem criar uma lista temporária
|
|
foreach (var ponto in Pontos)
|
|
{
|
|
// Precisa estar na margem do ponto e, ser um ponto de rua, que significa que já está dentro, ou então de borda, e que esteja se afastando do centro dele
|
|
if (
|
|
ponto.Visitado &&
|
|
ponto.NaMargem &&
|
|
(ponto.Tipo == Enums.TipoPontoRua.Rua ||
|
|
(
|
|
(ponto.Tipo == TipoPontoRua.BordaEntrada && !ponto.Aproximando) ||
|
|
(ponto.Tipo == TipoPontoRua.BordaSaida && ponto.Aproximando)
|
|
)
|
|
)
|
|
)
|
|
{
|
|
Dentro = true;
|
|
break; // Encontramos um ponto válido, podemos sair
|
|
}
|
|
}
|
|
|
|
DistanciaRestante = DistanciaTotal - DistanciaPercorridaTotal;
|
|
Progresso = (DistanciaTotal > 0) ? FuncoesMatematicas.Clamp((DistanciaPercorridaTotal / DistanciaTotal) * 100.0, 0, 100) : 0;
|
|
Concluido = !Pontos.Any(x => !x.Visitado);
|
|
}
|
|
|
|
public CorredorTrajetoriaModel Clone(bool clonarPontos = true)
|
|
{
|
|
var obj = new CorredorTrajetoriaModel()
|
|
{
|
|
Idx = Idx,
|
|
Largura = Largura,
|
|
DistanciaTotal = DistanciaTotal,
|
|
DistanciaPercorridaCorredor = DistanciaPercorridaCorredor,
|
|
DistanciaPercorridaTotal = DistanciaPercorridaTotal,
|
|
QtdPontos = QtdPontos,
|
|
Dentro = Dentro,
|
|
Concluido = Concluido,
|
|
idxRuaEsquerda = idxRuaEsquerda,
|
|
idxRuaDireita = idxRuaDireita,
|
|
DistanciaRestante = DistanciaRestante,
|
|
Progresso = Progresso,
|
|
Pontos = new List<PontoTrajetoriaModel>(),
|
|
Ultimo = Ultimo,
|
|
FatorLarguraCorredor = FatorLarguraCorredor,
|
|
};
|
|
if (clonarPontos)
|
|
{
|
|
obj.Pontos = new List<PontoTrajetoriaModel>(Pontos);
|
|
}
|
|
|
|
return obj;
|
|
}
|
|
}
|
|
|
|
public class PontoTrajetoriaModel
|
|
{
|
|
public int idxCorredor { get; set; }
|
|
public int idxPonto { get; set; }
|
|
public int idxPontoCorredor { get; set; }
|
|
public GPSModel Posicao { get; set; }
|
|
public double Orientacao { get; set; }
|
|
public DirecaoCarroRua Direcao { get; set; }
|
|
public TipoPontoRua Tipo { get; }
|
|
public double LarguraCorredor { get; set; }
|
|
public double DistanciaMargem { get; private set; }
|
|
public bool Visitado { get; set; } = false;
|
|
|
|
// ✅ Agora, propriedades podem ser atualizadas com `set; private`
|
|
[JsonProperty]
|
|
public double OrientacaoAtual { get; private set; }
|
|
[JsonProperty]
|
|
public double OrientacaoAnterior { get; private set; }
|
|
[JsonProperty]
|
|
public double DistanciaTrajeto { get; private set; }
|
|
[JsonProperty]
|
|
public double DistanciaAtual { get; private set; }
|
|
[JsonProperty]
|
|
public double DistanciaAnterior { get; private set; }
|
|
[JsonProperty]
|
|
public bool Aproximando { get; private set; }
|
|
[JsonProperty]
|
|
public bool NaMargem { get; private set; }
|
|
[JsonProperty]
|
|
public bool PontoBorda { get; private set; }
|
|
[JsonProperty]
|
|
public bool PontoLigacao { get; private set; }
|
|
|
|
// ✅ Construtor: Calcula propriedades fixas uma única vez
|
|
public PontoTrajetoriaModel(TipoPontoRua tipo)
|
|
{
|
|
Tipo = tipo;
|
|
|
|
PontoBorda = Tipo == Enums.TipoPontoRua.BordaEntrada || Tipo == Enums.TipoPontoRua.BordaSaida;
|
|
PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida;
|
|
}
|
|
|
|
public void AtualizarPropriedades()
|
|
{
|
|
AtualizarDistanciaAtual();
|
|
AtualizarDistanciaAnterior();
|
|
AtualizarOrientacaoAtual();
|
|
AtualizarOrientacaoAnterior();
|
|
AtualizarNaMargem();
|
|
AtualizarAproximando();
|
|
AtualizarDistanciaTrajeto();
|
|
AtualizarVisitado();
|
|
}
|
|
|
|
private void AtualizarVisitado()
|
|
{
|
|
if (Visitado) return; // Se já foi visitado, não faz nada
|
|
|
|
double distanciaEntrePontos =
|
|
Tipo == TipoPontoRua.CruvaEntreCorredores ? TrajetoriaMapaOperacaoModel.DistanciaEntrePontosCurva :
|
|
TrajetoriaMapaOperacaoModel.DistanciaEntrePontos;
|
|
|
|
// Define o limite de distância para considerar o ponto como visitado
|
|
double fatorAjuste = Tipo == TipoPontoRua.CruvaEntreCorredores ? 3.0 : 2.0; // Curvas podem ter menos tolerância
|
|
double limiteDistancia = Math.Ceiling(TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras / distanciaEntrePontos) * fatorAjuste * distanciaEntrePontos;
|
|
|
|
// Se o ponto está dentro da margem e dentro do limite de distância, marca como visitado
|
|
if (NaMargem && DistanciaTrajeto <= limiteDistancia)
|
|
{
|
|
Visitado = true;
|
|
}
|
|
}
|
|
|
|
private void AtualizarOrientacaoAtual()
|
|
{
|
|
OrientacaoAtual = GPSUtils.CalcularOrientacao(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, Posicao);
|
|
}
|
|
|
|
private void AtualizarOrientacaoAnterior()
|
|
{
|
|
OrientacaoAnterior = GPSUtils.CalcularOrientacao(GPSService.PenultimaLeitura, Posicao);
|
|
}
|
|
|
|
private void AtualizarDistanciaTrajeto()
|
|
{
|
|
if (!Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixaDefinida)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var pontoAtual = Variaveis.OperacaoEmAndamento.Trajetoria.PontoAtual;
|
|
int idxPontoAtual = pontoAtual.idxPonto + 1;
|
|
int qtdPontos = Math.Abs(idxPonto - idxPontoAtual) + 1; // Sempre positivo
|
|
|
|
if (qtdPontos == 1)
|
|
{
|
|
//DistanciaTrajeto = pontoAtual.Aproximando ? pontoAtual.DistanciaAtual : -pontoAtual.DistanciaAtual;
|
|
DistanciaTrajeto = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaDinamica.First().Posicao, Posicao);
|
|
return;
|
|
}
|
|
|
|
// ✅ Definimos corretamente o intervalo da trajetória
|
|
int idxInicio = Math.Min(idxPontoAtual, idxPonto);
|
|
int idxFim = Math.Max(idxPontoAtual, idxPonto);
|
|
|
|
// ✅ Pega os pontos corretos, independentemente de direção
|
|
List<GPSModel> trecho = Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixa
|
|
.Skip(idxInicio)
|
|
.Take(qtdPontos)
|
|
.ToList()
|
|
.ConvertAll(x => x.Posicao);
|
|
|
|
double distancia = GPSUtils.DistanciaDoTrecho(trecho);
|
|
|
|
// ✅ Agora decidimos se devemos somar ou subtrair a distância do ponto atual
|
|
double distanciaP0 = pontoAtual.Aproximando ? pontoAtual.DistanciaAtual : -pontoAtual.DistanciaAtual;
|
|
|
|
// ✅ Se o ponto está atrás, invertemos a distância final
|
|
if (idxPonto < idxPontoAtual)
|
|
{
|
|
distanciaP0 *= -1;
|
|
distancia *= -1;
|
|
}
|
|
|
|
DistanciaTrajeto = distancia + distanciaP0;
|
|
}
|
|
|
|
private void AtualizarDistanciaAtual()
|
|
{
|
|
DistanciaAtual = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, Posicao);
|
|
}
|
|
|
|
private void AtualizarDistanciaAnterior()
|
|
{
|
|
DistanciaAnterior = GPSUtils.DistanciaEntrePontos(GPSService.PenultimaLeitura, Posicao);
|
|
}
|
|
|
|
private void AtualizarAproximando()
|
|
{
|
|
// ✅ Agora verificamos tudo de uma vez sem variáveis intermediárias
|
|
Aproximando = Math.Abs(OrientacaoAtual - OrientacaoAnterior) <= 10.0
|
|
&& DistanciaAtual < DistanciaAnterior
|
|
&& Math.Abs(OrientacaoAtual - OrientacaoAnterior) < 90.0;
|
|
}
|
|
|
|
private void AtualizarNaMargem()
|
|
{
|
|
DistanciaMargem = LarguraCorredor * 0.8;
|
|
NaMargem = DistanciaAtual < DistanciaMargem;
|
|
}
|
|
|
|
|
|
public PontoTrajetoriaModel Clone()
|
|
{
|
|
return new PontoTrajetoriaModel(Tipo)
|
|
{
|
|
Aproximando = Aproximando,
|
|
Direcao = Direcao,
|
|
DistanciaAnterior = DistanciaAnterior,
|
|
DistanciaAtual = DistanciaAtual,
|
|
DistanciaTrajeto = DistanciaTrajeto,
|
|
idxCorredor = idxCorredor,
|
|
idxPonto = idxPonto,
|
|
idxPontoCorredor = idxPontoCorredor,
|
|
LarguraCorredor = LarguraCorredor,
|
|
NaMargem = NaMargem,
|
|
Orientacao = Orientacao,
|
|
OrientacaoAnterior = OrientacaoAnterior,
|
|
OrientacaoAtual = OrientacaoAtual,
|
|
PontoBorda = PontoBorda,
|
|
PontoLigacao = PontoLigacao,
|
|
Posicao = Posicao?.Clone() ?? new GPSModel(),
|
|
Visitado = Visitado
|
|
};
|
|
}
|
|
}
|
|
|
|
}
|