public void nettoyerTrajectoire()
{
int dirX = 0, dirY = 0, oldDirX = 0, oldDirY = 0; // Directions sur les axes X et Y
List <PositionElement> Sortie = new List <PositionElement>();
foreach (PositionElement p in _Positions)
{
if (Sortie.Count == 0)
{
Sortie.Add(p);
}
else
{
dirX = Math.Sign(p.X - Sortie.Last().X);
dirY = Math.Sign(p.Y - Sortie.Last().Y);
if (dirX != oldDirX || dirY != oldDirY)
{
oldDirX = dirX;
oldDirY = dirY;
Sortie.Add(p);
}
else
{
PositionElement p2 = Sortie[Sortie.Count - 1];
p2.X = p.X;
p2.Y = p.Y;
Sortie[Sortie.Count - 1] = p2;
}
}
}
_Positions = Sortie;
}
// Trouve l'objectif le plus près de la position (objectif non effectué )
private Objectif findObjectifNear(PositionElement p)
{
Objectif sortie = null;
double MinDist = -1;
foreach (Objectif ob in _Cubes)
{
// Objectif non fait et non en cours de traitement
if (ob.Done == false && ob.Robot == null)
{
double dist = UtilsMath.DistanceManhattan(p, ob.position);
if (sortie == null) // On a pas encore d'objectif ?
{
sortie = ob;
MinDist = dist;
}
else
{
if (MinDist > dist)
{
sortie = ob;
MinDist = dist;
}
}
}
}
return(sortie);
}
private AStar CreerAstarDepose(ArduinoBotIA robot, Zone Depose)
{
PositionElement PositionCentreZone = UtilsMath.CentreRectangle(Depose.position);
AStar AS = new AStar(PositionCentreZone, robot.Position, _ZoneTravail);
// Ajout des autres cubes en obstacles
foreach (Objectif o in _Cubes)
{
if (o.Robot != null)
{
AS.AddObstacle(o.position);
}
}
// Ajout des Zones en Obstacles
foreach (Zone o in _ZonesDepose)
{
if (o.id != Depose.id)
{
AS.AddObstacle(UtilsMath.CentreRectangle(o.position));
}
}
return(AS);
}
private void btn_test_Click(object sender, EventArgs e)
{
PositionElement pStart = new PositionElement();
pStart.X = 30; pStart.Y = 550;
PositionElement pEnd = new PositionElement();
pEnd.X = 30; pEnd.Y = 30;
PositionZoneTravail pZone = new PositionZoneTravail();
pZone.A.X = 0;
pZone.A.Y = 0;
pZone.B.X = 800;
pZone.B.Y = 600;
//AStar astar = new AStar(pStart, pEnd, pZone);
//Track tr = astar.CalculerTrajectoire();
Bitmap bitmap = new Bitmap(pZone.B.X, pZone.B.Y);
//dessinerTrack(bitmap, tr.Positions);
pictureBox1.Image = bitmap;
/* Point c = new Point(pEnd.X, pEnd.Y);
* dessinerPoint(bitmap, c, Brushes.Red);
*
* c = new Point(pStart.X, pStart.Y);
* dessinerPoint(bitmap, c, Brushes.Green);*/
}
// Verifie si le robot est proche d'un point de l'itinéraire pour changer passer a la suite
private bool checkSuiteItineraire(byte idRobot, out PositionElement NearestPositionTroncon)
{
NearestPositionTroncon = new PositionElement();
// On a bien un robot avec ce numéro
if (_ListArduino.Exists(ArduinoBotIA.ById(idRobot)))
{
ArduinoBotIA Robot = _ListArduino.Find(ArduinoBotIA.ById(idRobot));
// Le robot a bien un tracé définit
if (Robot.Trace != null)
{
Track tr = Robot.Trace;
for (int i = tr.Positions.Count - 1; i >= 0; i--) // On parcours le tracé depuis la fin pour trouver le plus proche de lobjectif
{
// On est assez proche du point, on passe à la suite
if (UtilsMath.DistanceEuclidienne(tr.Positions[i], Robot.Position) < _RadiusNextItineraire)
{
NearestPositionTroncon = tr.Positions[i];
return(true);
}
}
}
}
return(false);
}
// Calcul le centre d'un rectangle de dépose
static public PositionElement CentreRectangle(PositionZone Zone)
{
int X = (Zone.A.X + Zone.C.X) / 2;
int Y = (Zone.A.Y + Zone.C.Y) / 2;
PositionElement p = new PositionElement(X, Y);
return(p);
}
public PositionElement ConvertFromCase(ASCase Case)
{
PositionElement Position = new PositionElement();
Position.X = (int)(Case.Point.X * _UnitByCol);
Position.Y = (int)(Case.Point.Y * _UnitByRow);
return(Position);
}
public ASCase ConvertToCase(PositionElement point)
{
ASCase Case = new ASCase((point.X / (int)_UnitByCol), (point.Y / (int)_UnitByRow));
/* Case.Visited = false;
* Case.Contenu = ASCaseState.NONE;
* Case.X = point.X / _UnitByCol;
* Case.Y = point.Y / _UnitbyRow;*/
return(Case);
}
public System.Drawing.Rectangle CalculerRectangle(PositionElement point)
{
ASCase p = this.ConvertToCase(point);
//ASCase Case = _map.getCase(p.X, p.Y);
PositionElement HautGauche = ConvertFromCase(p);
p.Point.X++;
p.Point.Y++;
PositionElement BasDroite = ConvertFromCase(p);
System.Drawing.Rectangle rect = new System.Drawing.Rectangle(HautGauche.X, HautGauche.Y, BasDroite.X - HautGauche.X, BasDroite.Y - HautGauche.Y);
return(rect);
}
// Calcul la plus petite distance entre un segment et un point
static public double FindDistanceToSegment(PositionElement pt, PositionElement p1, PositionElement p2)
{
PositionElement closest;
int dx = p2.X - p1.X;
int dy = p2.Y - p1.Y;
if ((dx == 0) && (dy == 0))
{
// It's a point not a line segment.
closest = p1;
dx = pt.X - p1.X;
dy = pt.Y - p1.Y;
return(Math.Sqrt(dx * dx + dy * dy));
}
// Calculate the t that minimizes the distance.
float t = ((pt.X - p1.X) * dx + (pt.Y - p1.Y) * dy) / (dx * dx + dy * dy);
// See if this represents one of the segment's
// end points or a point in the middle.
if (t < 0)
{
closest = new PositionElement();
closest.X = p1.X;
closest.Y = p1.Y;
dx = pt.X - p1.X;
dy = pt.Y - p1.Y;
}
else if (t > 1)
{
closest = new PositionElement();
closest.X = p2.X;
closest.Y = p2.Y;
dx = pt.X - p2.X;
dy = pt.Y - p2.Y;
}
else
{
closest = new PositionElement();
closest.X = (int)(p1.X + t * dx);
closest.Y = (int)(p1.Y + t * dy);
//closest = new PointF(p1.X + t * dx, p1.Y + t * dy);
dx = pt.X - closest.X;
dy = pt.Y - closest.Y;
}
return(Math.Sqrt(dx * dx + dy * dy));
}
public AStar(PositionElement Depart, PositionElement Arrivee, PositionZoneTravail ZoneTravail)
{
_open = new SortedNodeList <ASCase>();
_close = new NodeList <ASCase>();
_UnitByCol = (float)Math.Abs(ZoneTravail.A.X - ZoneTravail.B.X) / _NumCol;
_UnitByRow = (float)Math.Abs(ZoneTravail.A.Y - ZoneTravail.B.Y) / _NumRow;
_map = new ASMap(_NumCol, _NumRow);
ASCase Start = ConvertToCase(Depart);
ASCase End = ConvertToCase(Arrivee);
_map.setStart(Start.Point.X, Start.Point.Y);
_map.setEnd(End.Point.X, End.Point.Y);
}
private IEnumerable <Match> InternalParseMatches(byte[] input, int startPosition)
{
foreach (var finder in _finders)
{
finder.PreProcess(input);
}
var history = new PositionElement[input.Length - startPosition + 1];
for (var i = 0; i < history.Length; i++)
{
history[i] = new PositionElement(0, false, null, int.MaxValue);
}
history[0].Price = 0;
ForwardPass(input, startPosition, history);
return(BackwardPass(history).Reverse());
}
public void ConstructeurTest()
{
var position = new Position()
{
X = 23,
Y = 22,
};
var montagne = new Montagne(position);
var positionElement = new PositionElement(montagne);
positionElement.IsMontagne.Should().BeTrue();
positionElement.Montagne.Position.X.Should().Be(position.X);
positionElement.Montagne.Position.Y.Should().Be(position.Y);
positionElement = new PositionElement(position);
positionElement.IsPlaine.Should().BeTrue();
positionElement.Plaine.Position.X.Should().Be(position.X);
positionElement.Plaine.Position.Y.Should().Be(position.Y);
var tresor = new Tresor(position, 2);
positionElement = new PositionElement(tresor);
positionElement.IsTresor.Should().BeTrue();
positionElement.Tresor.Position.X.Should().Be(position.X);
positionElement.Tresor.Position.Y.Should().Be(position.Y);
positionElement.Tresor.NombreTresor.Should().Be(2);
var positionAventurier = new PositionAventurier()
{
X = 15,
Y = 17,
Xmax = 20,
Ymax = 30
};
var aventurier = new Aventurier(positionAventurier, "le nom", "S", "ADADG", 0, 5);
positionElement = new PositionElement(aventurier);
positionElement.Aventurier.Should().BeSameAs(aventurier);
}
public override void positionMulti(int reqId, string account, string modelCode, Contract contract, double pos, double avgCost)
{
Console.WriteLine("Position Multi. Request: " + reqId + ", Account: " + account + ", ModelCode: " + modelCode + ", Symbol: " + contract.Symbol + ", SecType: " + contract.SecType + ", Currency: " + contract.Currency + ", Position: " + pos + ", Avg cost: " + avgCost + "\n");
accountList.TryGetValue(account, out var currentAccount);
currentAccount.Sync.WaitOne();
var accountPositions = currentAccount.PositionContainer;
var positionElement = accountPositions.Positions.FirstOrDefault(e => e.Contract.Symbol == contract.Symbol);
if (positionElement == null)
{
positionElement = new PositionElement();
accountPositions.Positions.Add(positionElement);
}
positionElement.AverageCost = avgCost;
positionElement.Position = pos;
positionElement.Contract = contract;
currentAccount.Sync.ReleaseMutex();
}
// Supprime les point avant le point donne
private void removeBeforePoint(Track tr, PositionElement p)
{
tr.removeBefore(p);
}
public void removeBefore(PositionElement p)
{
this.Positions.RemoveRange(0, this.Positions.IndexOf(p));
}
// Calcul de la distance Manhattan (X+Y)
static public double DistanceManhattan(PositionElement p1, PositionElement p2)
{
return(Math.Abs(p1.X - p2.X) + Math.Abs(p1.Y - p2.Y));
}
// Calcul de la distance Euclidienne SQRT(X²+Y²)
static public double DistanceEuclidienne(PositionElement p1, PositionElement p2)
{
return(Math.Sqrt((p1.X - p2.X) * (p1.X - p2.X) + (p1.Y - p2.Y) * (p1.Y - p2.Y)));
}
public void setPosition(PositionElement pos)
{
_position = pos;
}
public Objectif(int id, PositionElement pos, int idZone)
{
_id = id;
_idZone = idZone;
setPosition(pos);
}
private void pictureBox1_Click(object sender, MouseEventArgs e)
{
int abs = (e.Location.X * ((PictureBox)sender).Image.Width) / ((PictureBox)sender).Width;
int ord = (e.Location.Y * ((PictureBox)sender).Image.Height) / ((PictureBox)sender).Height;
//Logger.GlobalLogger.debug("ABS : " + abs + " ORD : " + ord);
if (e.Button == MouseButtons.Left)
{
pEnd.X = abs;
pEnd.Y = ord;
}
else if (e.Button == MouseButtons.Right)
{
pStart.X = abs;
pStart.Y = ord;
}
else if (e.Button == MouseButtons.Middle)
{
PositionElement pObstacle = new PositionElement();
pObstacle.X = abs;
pObstacle.Y = ord;
pAutre.Add(pObstacle);
}
PositionZoneTravail pZone = new PositionZoneTravail();
pZone.A.X = 0;
pZone.A.Y = 0;
pZone.B.X = 800;
pZone.B.Y = 600;
astar = new AStar(pStart, pEnd, pZone);
astar.AddObstacles(pAutre);
DateTime dt1 = DateTime.Now;
Track tr = astar.CalculerTrajectoire();
DateTime dt2 = DateTime.Now;
Logger.GlobalLogger.info("Temps calcul : " + (dt2 - dt1).Milliseconds);
//Track tr = astar.CalculerTrajectoire();
tr.nettoyerTrajectoire();
List <QuadrillageCoord> quad = astar.CalculerQuadrillage();
Bitmap bitmap = new Bitmap(pZone.B.X, pZone.B.Y);
dessinerTrack(bitmap, tr);
Point c = new Point(pEnd.X, pEnd.Y);
//dessinerPoint(bitmap, c, Brushes.Red);
dessinerRectangle(bitmap, astar.CalculerRectangle(pEnd), Brushes.Red);
c = new Point(pStart.X, pStart.Y);
//dessinerPoint(bitmap, c, Brushes.Green);
dessinerRectangle(bitmap, astar.CalculerRectangle(pStart), Brushes.Green);
foreach (PositionElement obstacle in pAutre)
{
dessinerRectangle(bitmap, astar.CalculerRectangle(obstacle), Brushes.Gray);
//dessinerPoint(bitmap, c, Brushes.Gray);
}
foreach (QuadrillageCoord q in quad)
{
dessinerLigne(bitmap, q.A, q.B, Color.Gray, 1);
}
pictureBox1.Image = bitmap;
}
public void AddObstacle(PositionElement point)
{
ASCase Case = ConvertToCase(point);
_map.AjouterObstacle(Case);
}
public void UpdateImageDebug()
{
if (_Follower.TrackMaker != null && _Follower.TrackMaker.ZoneTravail.B.X != 0)
{
List <PolyligneDessin> ListePoly = new List <PolyligneDessin>(); // Liste envoyé a l'image
Bitmap bitmap = new Bitmap(_Follower.TrackMaker.ZoneTravail.B.X, _Follower.TrackMaker.ZoneTravail.B.Y);
//dessinerTrack(bitmap, tr);
// Dessiner Cubes
// foreach (Objectif obstacle in _Follower.TrackMaker.Cubes)
for (int i = 0; i < _Follower.TrackMaker.Cubes.Count; i++)
{
Objectif obstacle = _Follower.TrackMaker.Cubes[i];
PolyligneDessin p = new PolyligneDessin(Color.LimeGreen);
for (int x = obstacle.position.X - 5; x <= obstacle.position.X + 5; x++)
{
for (int y = obstacle.position.Y - 5; y <= obstacle.position.Y + 5; y++)
{
p.addPoint(new PointDessin(x, y));
}
}
ListePoly.Add(p);
dessinerPoint(bitmap, obstacle.position, Brushes.Gray);
}
foreach (QuadrillageCoord q in _Follower.TrackMaker.CreerAstarQuadriallage().CalculerQuadrillage())
{
PolyligneDessin p = new PolyligneDessin(Color.Gray);
p.addPoint(new PointDessin(q.A.X, q.A.Y));
p.addPoint(new PointDessin(q.B.X, q.B.Y));
ListePoly.Add(p);
dessinerLigne(bitmap, q.A, q.B, Color.Gray, 1);
}
for (int i = 0; i < _Follower.TrackMaker.ZonesDepose.Count; i++)
{
Zone z = _Follower.TrackMaker.ZonesDepose[i];
PositionElement pos = UtilsMath.CentreRectangle(z.position);
PolyligneDessin p = new PolyligneDessin(Color.Pink);
for (int x = pos.X - 5; x <= pos.X + 5; x++)
{
for (int y = pos.Y - 5; y <= pos.Y + 5; y++)
{
p.addPoint(new PointDessin(x, y));
}
}
ListePoly.Add(p);
dessinerPoint(bitmap, pos, Brushes.Pink);
}
for (int ab = 0; ab < _Follower.ListArduino.Count; ab++)
//foreach (ArduinoBotIA robot in _Follower.ListArduino)
{
ArduinoBotIA robot = _Follower.ListArduino[ab];
PolyligneDessin p = new PolyligneDessin(Color.Purple);
PositionElement pos = robot.Position;
for (int x = pos.X - 5; x <= pos.X + 5; x++)
{
for (int y = pos.Y - 5; y <= pos.Y + 5; y++)
{
p.addPoint(new PointDessin(x, y));
}
}
ListePoly.Add(p);
dessinerPoint(bitmap, pos, Brushes.Turquoise);
if (robot.Trace != null)
{
dessinerTrack(bitmap, robot.Trace, _Follower.TrackMaker.CreerAstarQuadriallage());
PolyligneDessin p2 = new PolyligneDessin(Color.Red);
for (int i = 0; i < robot.Trace.Positions.Count; i++)
{
p.addPoint(new PointDessin(robot.Trace.Positions[i].X, robot.Trace.Positions[i].Y));
}
ListePoly.Add(p2);
}
}
imageDebug.Image = bitmap;
if (DrawPolylineEvent != null)
{
DrawPolylineEventArgs args = new DrawPolylineEventArgs(ListePoly);
DrawPolylineEvent(this, args);
}
}
}
public void ajouterPoint(PositionElement point)
{
_Positions.Add(point);
}
