292 lines
12 KiB
C#
292 lines
12 KiB
C#
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Windows.Forms;
|
|
|
|
namespace AgroBase.Models
|
|
{
|
|
public class MapaDinamicoModel
|
|
{
|
|
private double _zoom = 1000000.0; // Nível de zoom
|
|
private const double ZoomFactor = 1.1; // Fator de zoom
|
|
private bool _isDragging = false; // Controle de arrasto
|
|
private Point _startPoint = new Point(); // Posição inicial do mouse
|
|
private Point _mapOffset = new Point(); // Deslocamento do mapa
|
|
|
|
private GPSModel _posicaoAtual { get; set; } = new GPSModel();
|
|
private List<GPSModel> TrajetoriaDinamica { get; set; } = new List<GPSModel>();
|
|
private List<GPSModel> TrajetoriaRuaProjetada { get; set; } = new List<GPSModel>();
|
|
private List<GPSModel> TrajetoriaRobo { get; set; } = new List<GPSModel>();
|
|
private Obstaculo obstaculoDetectado { get; set; }
|
|
private float AnguloCaminho { get; set; }
|
|
private float AnguloCarro { get; set; }
|
|
|
|
private Panel pnlZoomMapa;
|
|
private Form parent;
|
|
|
|
public MapaDinamicoModel(Form _parent, Panel pnl)
|
|
{
|
|
this.pnlZoomMapa = pnl;
|
|
|
|
this.pnlZoomMapa.Paint += PnlZoomMapa_Paint;
|
|
this.pnlZoomMapa.MouseWheel += PnlZoomMapa_MouseWheel;
|
|
this.pnlZoomMapa.MouseDown += PnlZoomMapa_MouseDown;
|
|
this.pnlZoomMapa.MouseMove += PnlZoomMapa_MouseMove;
|
|
this.pnlZoomMapa.MouseUp += PnlZoomMapa_MouseUp;
|
|
|
|
parent = _parent;
|
|
|
|
pnlZoomMapa.GetType().GetMethod("SetStyle", System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic).Invoke(pnlZoomMapa, new object[] { ControlStyles.UserPaint | ControlStyles.AllPaintingInWmPaint | ControlStyles.OptimizedDoubleBuffer, true });
|
|
}
|
|
|
|
public void AtualizarDados(Obstaculo obstaculo, float anguloCaminho, float anguloCarro, GPSModel posicaoAtual, List<GPSModel> trajetoriaDinamica, List<GPSModel> trajetoriaRua, List<GPSModel> trajetoriaRobo)
|
|
{
|
|
obstaculoDetectado = obstaculo;
|
|
AnguloCaminho = anguloCaminho;
|
|
AnguloCarro = anguloCarro;
|
|
_posicaoAtual = posicaoAtual;
|
|
TrajetoriaDinamica = trajetoriaDinamica;
|
|
TrajetoriaRuaProjetada = trajetoriaRua;
|
|
TrajetoriaRobo = trajetoriaRobo;
|
|
|
|
pnlZoomMapa.Invalidate();
|
|
}
|
|
|
|
private void PnlZoomMapa_MouseUp(object sender, MouseEventArgs e)
|
|
{
|
|
if (e.Button == MouseButtons.Left)
|
|
{
|
|
_isDragging = false; // Termina o arrasto
|
|
parent.Cursor = Cursors.Default;
|
|
}
|
|
}
|
|
|
|
private void PnlZoomMapa_MouseMove(object sender, MouseEventArgs e)
|
|
{
|
|
Panel panel1 = (Panel)sender;
|
|
|
|
if (_isDragging)
|
|
{
|
|
// Calcula o deslocamento do mapa com base no movimento do mouse
|
|
_mapOffset.X += e.X - _startPoint.X;
|
|
_mapOffset.Y += e.Y - _startPoint.Y;
|
|
|
|
// Atualiza a posição inicial do mouse
|
|
_startPoint = e.Location;
|
|
|
|
// Redesenha o panel para refletir o novo deslocamento
|
|
panel1.Invalidate();
|
|
|
|
parent.Cursor = Cursors.Cross;
|
|
}
|
|
}
|
|
|
|
private void PnlZoomMapa_MouseDown(object sender, MouseEventArgs e)
|
|
{
|
|
if (e.Button == MouseButtons.Left)
|
|
{
|
|
_isDragging = true;
|
|
_startPoint = e.Location; // Salva a posição inicial do mouse
|
|
parent.Cursor = Cursors.Hand;
|
|
}
|
|
}
|
|
|
|
private void PnlZoomMapa_MouseWheel(object sender, MouseEventArgs e)
|
|
{
|
|
Panel panel1 = (Panel)sender;
|
|
|
|
if (e.Delta > 0)
|
|
{
|
|
_zoom *= ZoomFactor; // Aumenta o zoom
|
|
}
|
|
else if (e.Delta < 0)
|
|
{
|
|
_zoom /= ZoomFactor; // Diminui o zoom
|
|
}
|
|
|
|
panel1.Invalidate();
|
|
}
|
|
|
|
private void PnlZoomMapa_Paint(object sender, PaintEventArgs e)
|
|
{
|
|
Panel panel1 = (Panel)sender;
|
|
|
|
Graphics g = e.Graphics;
|
|
|
|
// Defina as dimensões do panel
|
|
int width = panel1.Width;
|
|
int height = panel1.Height;
|
|
|
|
// Coordenadas centrais (onde o robô estará), ajustadas pelo deslocamento do mapa
|
|
float centerX = width / 2 + _mapOffset.X;
|
|
float centerY = height / 2 + _mapOffset.Y;
|
|
|
|
// Desenha os pontos da trajetória projetada
|
|
DrawTrajectory(g, TrajetoriaDinamica, centerX, centerY, Color.Purple, Pens.MediumPurple);
|
|
|
|
// Desenha os pontos da trajetória planejada da rua atual
|
|
DrawTrajectory(g, TrajetoriaRuaProjetada, centerX, centerY, Color.Green, Pens.Blue);
|
|
|
|
// Desenha os pontos da trajetória percorrida
|
|
DrawTrajectory(g, TrajetoriaRobo, centerX, centerY, Color.Orange, Pens.OrangeRed);
|
|
|
|
// Desenha a posição atual do robô (ponto vermelho)
|
|
float PxToCm = DrawRobot(g, centerX, centerY, Color.Red, Color.Red, (float)VariaveisEquipamento.LarguraEsquerda, (float)VariaveisEquipamento.LarguraDireita, (float)VariaveisEquipamento.ComprimentoFrente, (float)VariaveisEquipamento.ComprimentoTras, AnguloCarro);
|
|
|
|
if (obstaculoDetectado != null)
|
|
{
|
|
DrawObstacle(g, centerX, centerY, Color.Cyan, Color.Cyan, AnguloCaminho - (float)obstaculoDetectado.AnguloParaDesvio, obstaculoDetectado, PxToCm);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
private void DrawTrajectory(Graphics g, List<GPSModel> trajectory, float centerX, float centerY, Color pointColor, Pen linePen)
|
|
{
|
|
if (trajectory == null)
|
|
{
|
|
trajectory = new List<GPSModel>();
|
|
}
|
|
|
|
try
|
|
{
|
|
for (int i = 0; i < trajectory.Count; i++)
|
|
{
|
|
var trajPoint = trajectory[i];
|
|
// Calcule a posição no panel
|
|
float x = centerX + (float)((trajPoint.Longitude - _posicaoAtual.Longitude) * _zoom);
|
|
float y = centerY - (float)((trajPoint.Latitude - _posicaoAtual.Latitude) * _zoom);
|
|
|
|
// Desenha o ponto da trajetória
|
|
DrawPoint(g, x, y, pointColor);
|
|
|
|
// Desenha a linha conectando os pontos
|
|
if (i > 0)
|
|
{
|
|
var prevPoint = trajectory[i - 1];
|
|
float prevX = centerX + (float)((prevPoint.Longitude - _posicaoAtual.Longitude) * _zoom);
|
|
float prevY = centerY - (float)((prevPoint.Latitude - _posicaoAtual.Latitude) * _zoom);
|
|
|
|
g.DrawLine(linePen, prevX, prevY, x, y);
|
|
}
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
|
|
}
|
|
}
|
|
|
|
private void DrawPoint(Graphics g, float x, float y, Color color)
|
|
{
|
|
float size = 5;
|
|
using (Brush brush = new SolidBrush(color))
|
|
{
|
|
g.FillEllipse(brush, x - size / 2, y - size / 2, size, size);
|
|
}
|
|
}
|
|
|
|
private float DrawRobot(Graphics g, float x, float y, Color pointColor, Color borderColor, float larguraEsquerda, float larguraDireita, float comprimentoFrente, float comprimentoTras, float angle)
|
|
{
|
|
// Desenhar o ponto GPS
|
|
DrawPoint(g, x, y, pointColor);
|
|
|
|
GPSModel posicaoRobo = GPSService.UltimaLeitura;
|
|
|
|
// Ajustar o ângulo base para cálculo dos pontos
|
|
int agPlus = Convert.ToInt32(135 + angle);
|
|
|
|
// Calcular os pontos laterais considerando zoom em latitude e longitude
|
|
(float xET, float yET) = CalcularPontosLateraisRobo(posicaoRobo, larguraEsquerda, comprimentoTras, x, y, agPlus + 90);
|
|
(float xDT, float yDT) = CalcularPontosLateraisRobo(posicaoRobo, larguraDireita, comprimentoTras, x, y, agPlus + 0);
|
|
(float xEF, float yEF) = CalcularPontosLateraisRobo(posicaoRobo, larguraEsquerda, comprimentoFrente, x, y, agPlus + 180);
|
|
(float xDF, float yDF) = CalcularPontosLateraisRobo(posicaoRobo, larguraDireita, comprimentoFrente, x, y, agPlus + 270);
|
|
|
|
// Calcular os cantos do retângulo usando os pontos calculados
|
|
PointF[] corners = new PointF[4];
|
|
|
|
corners[0] = new PointF(xET, yET); // Superior esquerdo
|
|
corners[1] = new PointF(xDT, yDT); // Superior direito
|
|
corners[2] = new PointF(xDF, yDF); // Inferior direito
|
|
corners[3] = new PointF(xEF, yEF); // Inferior esquerdo
|
|
|
|
// Desenhar o contorno do robô
|
|
using (Pen pen = new Pen(borderColor, 2))
|
|
{
|
|
g.DrawPolygon(pen, corners);
|
|
}
|
|
|
|
// Desenhar a linha indicando a frente do robô (do ponto central para a frente)
|
|
DrawFrontLine(g, x, y, corners[2].X, corners[2].Y, corners[3].X, corners[3].Y, borderColor);
|
|
|
|
// Calcular a relação pixel para cm baseado na largura total
|
|
float PxToCm = Math.Abs(xET - xDT) / (larguraDireita + larguraEsquerda);
|
|
|
|
return PxToCm;
|
|
}
|
|
|
|
private void DrawObstacle(Graphics g, float xRobot, float yRobot, Color pointColor, Color borderColor, float angle, Obstaculo obstaculo, float PxToCm)
|
|
{
|
|
GPSModel posicaoRobo = GPSService.UltimaLeitura;
|
|
GPSModel posicaoObstaculo = Variaveis.OperacaoEmAndamento.Mapa.GerarPontoDeslocado(posicaoRobo, angle, obstaculo.DistanciaMedia_mm / 1000);
|
|
|
|
float x = xRobot + (float)((posicaoObstaculo.Longitude - posicaoRobo.Longitude) * _zoom);
|
|
float y = yRobot - (float)((posicaoObstaculo.Latitude - posicaoRobo.Latitude) * _zoom);
|
|
|
|
// Desenhar o ponto GPS
|
|
DrawPoint(g, x, y, pointColor);
|
|
|
|
|
|
int agPlus = Convert.ToInt32(135 + angle);
|
|
|
|
float largura = obstaculo.Largura_mm / 20;
|
|
|
|
(float xET, float yET) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 90);
|
|
(float xDT, float yDT) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 0);
|
|
(float xEF, float yEF) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 180);
|
|
(float xDF, float yDF) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 270);
|
|
|
|
// Calcular os cantos do retângulo considerando o ângulo
|
|
PointF[] corners = new PointF[4];
|
|
|
|
// Cantos do retângulo com as novas medidas
|
|
corners[0] = new PointF(xET, yET); // Superior esquerdo
|
|
corners[1] = new PointF(xDT, yDT); // Superior direito
|
|
corners[2] = new PointF(xDF, yDF); // Inferior direito
|
|
corners[3] = new PointF(xEF, yEF); // Inferior esquerdo
|
|
|
|
// Desenhar o contorno do robô
|
|
using (Pen pen = new Pen(borderColor, 2))
|
|
{
|
|
g.DrawPolygon(pen, corners);
|
|
}
|
|
}
|
|
|
|
private void DrawFrontLine(Graphics g, float x, float y, float xDF, float yDF, float xEF, float yEF, Color borderColor)
|
|
{
|
|
// Calcular o ponto médio entre DF e EF
|
|
float midX = (xDF + xEF) / 2;
|
|
float midY = (yDF + yEF) / 2;
|
|
|
|
// Desenhar a linha do ponto central para a frente do robô
|
|
using (Pen frontPen = new Pen(borderColor, 2))
|
|
{
|
|
g.DrawLine(frontPen, x, y, midX, midY);
|
|
}
|
|
}
|
|
|
|
private (float, float) CalcularPontosLateraisRobo(GPSModel posicaoRobo, float Largura, float Comprimento, float x, float y, int offAngulo)
|
|
{
|
|
float hP = (float)Math.Sqrt(Math.Pow(Largura, 2) + Math.Pow(Comprimento, 2)) / 100f;
|
|
GPSModel pP = Variaveis.OperacaoEmAndamento.Mapa.GerarPontoDeslocado(posicaoRobo, offAngulo, hP);
|
|
float xP = x + (float)((pP.Longitude - posicaoRobo.Longitude) * _zoom);
|
|
float yP = y - (float)((pP.Latitude - posicaoRobo.Latitude) * _zoom);
|
|
|
|
return (xP, yP);
|
|
}
|
|
|
|
|
|
}
|
|
}
|