public Segment(Position start, Position end, int radius)
{
this.Start = start;
this.End = end;
this.Radius = radius;
this.droite = DroiteFactory.Build(start, end);
//new ConsoleHelper().Debug($"DROITE for start={start} end={end} {droite}");
}
public static Vector3 _Intersection3D_DroiteCoupantUnCercle(Cercle c, Droite d, float angle)
{
Intersections3D i12 = _Intersections3D_DroiteCoupantUnCercle(c, d);
//i1 ou i2 ==> celui qui est le plus proche du point sans marge ??
float Bx = c.r * Mathf.Sin(angle / 180 * Mathf.PI);
float By = c.r * Mathf.Cos(angle / 180 * Mathf.PI);
Vector3 b = new Vector3(Bx, 0, By);
if (Vector3.Distance(b, i12.intersection_1) < Vector3.Distance(b, i12.intersection_2))
{
return(i12.intersection_1);
}
else
{
return(i12.intersection_2);
}
}
//Equation d'un cercle (1) : (x - Ox)² + (y - Oy)² = c.r ²
//Equation d'une droite (2) : y = a * x + b
//(2) dans (1) : (x - Ox)² + (a * x + b - Oy)² = r²
//g = (a * x + b - Oy)² = (a * x + b - Oy) * (a * x + b - Oy)
//g = a * a * x * x + a * x * b - a * x * Oy + b * a * x + b * b - b * Oy - Oy * a * x - Oy * b + Oy * Oy;
//g = x * x * (a * a) + x * (a * b - a * Oy + b * a - Oy * a) + (b * b - b * Oy - Oy * b + Oy * Oy);
//g = x * x * (a * a) + x * 2 * a * (b - Oy) + (b - Oy) * (b - Oy);
//(2) dans (1) : x * x + Ox * Ox - 2 * x * Ox + x * x * (a * a) + x * 2 * a * (b - Oy) + (b - Oy) * (b - Oy) = r * r
//r * r = x * x + x * (- 2 * Ox) + Ox * Ox + x * x * (a * a) + x * 2 * a * (b - Oy) + (b - Oy) * (b - Oy);
//r * r= x * x * (1 + a * a) + x * (-2 * Ox + 2 * a * (b - Oy)) + Ox * Ox + (b - Oy) * (b - Oy);
//x * x * (1 + a * a) + x * (-2 * Ox + 2 * a * (b - Oy)) + Ox * Ox + (b - Oy) * (b - Oy) - r * r= 0;
//Equation de la forme a * x²+ b * x + c = 0
public static Intersections2D _Intersections2D_DroiteCoupantUnCercle(Cercle c, Droite d)
{
if (d.a != float.MinValue && d.a != float.MaxValue)
{
float a = d.a;
float b = d.b;
float Ox = c.O.x;
float Oy = c.O.y;
float r = c.r;
float _a = (1 + a * a);
float _b = (-2 * Ox + 2 * a * (b - Oy));
float _c = Ox * Ox + (b - Oy) * (b - Oy) - r * r;
Vector2?X12 = _TrinomeDu2ndDegres(_a, _b, _c);
if (X12 == null)
{
Debug.Log($"Aucun point de la droite y = {d.a} * x + {d.b} ne coupe le cercle de rayon {c.r} et de centre {c.O}");
return(null);
}
Vector2 x12 = (Vector2)X12;
//trouve les Y avec l'équation de la droite
float Y1 = a * x12.x + b;
float Y2 = a * x12.y + b;
return(new Intersections2D()
{
intersection_1 = new Vector2(x12.x, Y1),
intersection_2 = new Vector2(x12.y, Y2)
});
}
else
{
return(new Intersections2D()
{
intersection_1 = new Vector2(0, c.r),
intersection_2 = new Vector2(0, -c.r)
});
}
}
public static Intersections3D _Intersections3D_DroiteCoupantUnCercle(Cercle c, Droite d)
{
if (d.a != float.MinValue && d.a != float.MaxValue)
{
float a = d.a;
float b = d.b;
float Ox = c.O.x;
float Oy = c.O.y;
float r = c.r;
float _a = (1 + a * a);
float _b = (-2 * Ox + 2 * a * (b - Oy));
float _c = Ox * Ox + (b - Oy) * (b - Oy) - r;
Vector2?X12 = _TrinomeDu2ndDegres(_a, _b, _c);
if (X12 == null)
{
return(null);
}
Vector2 x12 = (Vector2)X12;
//trouve les Y avec l'équation de la droite
float Y1 = a * x12.x + b;
float Y2 = a * x12.y + b;
return(new Intersections3D()
{
intersection_1 = new Vector3(x12.x, 0, Y1),
intersection_2 = new Vector3(x12.y, 0, Y2)
});
}
else
{
return(new Intersections3D()
{
intersection_1 = new Vector3(0, 0, c.r),
intersection_2 = new Vector3(0, 0, -c.r)
});
}
}
public Angle CalculAngle(Segment segmentPointsProches, double distanceMaxSegment, int nombreMesures)
{
double angleSomme = 0;
int nb = 0;
for (int i = 0; i < nombreMesures; i++)
{
List<PointReel> points = GetMesure();
if (points.Count > 0)
{
List<PointReel> pointsBordure = points.Where(p => p.Distance(segmentPointsProches) < distanceMaxSegment).ToList();
Droite interpol = new Droite(pointsBordure);
Plateau.ObstaclesPlateau.Add(interpol);
Console.WriteLine(new Angle(Math.Atan(interpol.A), AnglyeType.Radian).AngleDegresPositif - 270);
angleSomme += Math.Atan(interpol.A);
nb++;
}
}
return new Angle(angleSomme / nb, AnglyeType.Radian);
}
/// <summary>
/// Réception d'un message envoyé par la carte de la balise
/// </summary>
/// <param name="trame">Message reçu</param>
public void connexion_NouvelleTrame(Trame trame)
{
if (trame == null)
return;
try
{
// On ne traite que les messages qui nous sont adressés
if (trame[0] != (byte)0xB1)
return;
if (trame[1] == (byte)FonctionBalise.DetectionRapide)
{
if (Position != null)
{
int capteur = trame[2];
double debut = 360 - ((trame[3] * 256 + trame[4]) / 100.0) + Config.CurrentConfig.GetOffsetBalise(capteur);
double fin = 360 - ((trame[5] * 256 + trame[6]) / 100.0) + Config.CurrentConfig.GetOffsetBalise(capteur);
debut = debut + Position.Angle;
fin = fin + Position.Angle;
if (DetectionsRapides == null)
DetectionsRapides = new List<DetectionBalise>();
DetectionBalise detect = new DetectionBalise(this, debut, fin);
bool recalcul = false;
if (capteur == 2 && detect.Distance > 1250)
{
recalcul = true;
detect.Distance = 1250;
}
else if (capteur == 2 && detect.Distance < 480)
{
recalcul = true;
detect.Distance = 480;
}
else if (capteur == 3 && detect.Distance > 600)
{
recalcul = true;
detect.Distance = 600;
}
else if (capteur == 3 && detect.Distance < 300)
{
recalcul = true;
detect.Distance = 300;
}
if (recalcul)
{
// Un peu de trigo pas bien compliquée
double xPoint = Position.Coordonnees.X + Math.Cos(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance;
double yPoint = Position.Coordonnees.Y + Math.Sin(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance;
detect.Position = new PointReel(xPoint, yPoint);
}
DetectionsRapides.Add(detect);
nbDetectionsRapides++;
PositionsChange();
}
}
if (trame[1] == (byte)FonctionBalise.Detection)
{
if (Position != null)
{
if (DetectionsRapides != null)
{
// Vide les détections rapides datant de plus d'un tour
for (int i = 0; i < DetectionsRapides.Count - nbDetectionsRapides; i++)
DetectionsRapides.RemoveAt(0);
}
nbDetectionsRapides = 0;
// Réception d'une mesure sur un tour de rotation
// Vérification checksum
// Calcul de la vitesse de rotation
int nbTicks = trame[2] * 256 + trame[3];
VitesseToursSecActuelle = 1 / (nbTicks * 0.0000064);
int nouvelleVitesse = 0;
if (ReglageVitesse)
nouvelleVitesse = AsservissementVitesse();
// Réception des données angulaires
int nbMesures1 = trame[4];
int nbMesures2 = trame[5];
// Si on a un nombre impair de fronts on laisse tomber cette mesure, elle n'est pas bonne
if (nbMesures1 % 2 != 0 || nbMesures2 % 2 != 0)
{
Console.WriteLine("Erreur de détection (fronts impairs)");
return;
}
nbMesures1 = nbMesures1 / 2;
nbMesures2 = nbMesures2 / 2;
// Vérification de la taille de la trame
if (trame.Length != 6 + nbMesures1 * 4 + nbMesures2 * 4)
{
Console.WriteLine("Erreur de taille de trame");
return;
}
// Réception des mesures du capteur 1
DetectionsCapteur1.Clear();
List<int> tabAngle = new List<int>();
long verif = 0;
for (int i = 0; i < nbMesures1 * 4; i += 2)
{
int valeur = trame[6 + i] * 256 + trame[6 + i + 1];
tabAngle.Add(valeur);
verif += valeur * (i / 2 + 1);
}
tabAngle.Sort();
for (int i = 0; i < nbMesures1 * 2; i++)
{
verif -= tabAngle[i] * (i + 1);
}
if (verif != 0)
{
Console.WriteLine("Inversion détectée capteur 1");
}
for (int i = 0; i < nbMesures1; i++)
{
double debut = 360 - (tabAngle[i * 2] / 100.0) + Config.CurrentConfig.GetOffsetBalise(1);
double fin = 360 - (tabAngle[i * 2 + 1] / 100.0) + Config.CurrentConfig.GetOffsetBalise(1);
debut = debut + Position.Angle;
fin = fin + Position.Angle;
DetectionBalise detect = new DetectionBalise(this, debut, fin);
bool recalcul = false;
if (detect.Distance > 600)
{
recalcul = true;
detect.Distance = 600;
}
else if (detect.Distance < 300)
{
recalcul = true;
detect.Distance = 300;
}
if (recalcul)
{
// Un peu de trigo pas bien compliquée
double xPoint = Position.Coordonnees.X + Math.Cos(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance;
double yPoint = Position.Coordonnees.Y + Math.Sin(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance;
detect.Position = new PointReel(xPoint, yPoint);
}
DetectionsCapteur1.Add(detect);
}
// Réception des mesures du capteur 2
int offSet = nbMesures1 * 4 + 6;
DetectionsCapteur2.Clear();
// tableau pour trier les angles ( correction logiciel )
tabAngle.Clear();
verif = 0;
for (int i = 0; i < nbMesures2 * 4; i += 2)
{
int valeur = trame[offSet + i] * 256 + trame[offSet + i + 1];
tabAngle.Add(valeur);
verif += valeur * (i / 2 + 1);
}
tabAngle.Sort();
for (int i = 0; i < nbMesures2 * 2; i++)
{
verif -= tabAngle[i] * (i + 1);
}
if (verif != 0)
{
Console.WriteLine("Inversion détectée capteur 2");
}
for (int i = 0; i < nbMesures2; i++)
{
double debut = 360 - (tabAngle[i * 2] / 100.0) + Config.CurrentConfig.GetOffsetBalise(2);
double fin = 360 - (tabAngle[i * 2 + 1] / 100.0) + Config.CurrentConfig.GetOffsetBalise(2);
debut = debut + Position.Angle;
fin = fin + Position.Angle;
DetectionBalise detect = new DetectionBalise(this, debut, fin);
bool recalcul = false;
if (detect.Distance > 1250)
{
recalcul = true;
detect.Distance = 1250;
}
else if (detect.Distance < 480)
{
recalcul = true;
detect.Distance = 480;
}
if (recalcul)
{
// Un peu de trigo pas bien compliquée
double xPoint = Position.Coordonnees.X + Math.Cos(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance;
double yPoint = Position.Coordonnees.Y + Math.Sin(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance;
detect.Position = new PointReel(xPoint, yPoint);
}
DetectionsCapteur2.Add(detect);
}
// Réglage de l'offset d'angle des capteurs
if (ReglageOffset)
{
// Si on a une mesure incorrecte (une mesure correcte demande 2 détections sur chaque balise : un reflecteur au "centre" et un autre en bas de la piste)
// Le réglage est annulé
/*if (DetectionsCapteur1.Count == 2 && DetectionsCapteur2.Count == 2)
{
// 3 cas où on passe d'abord par la balise en bas de la piste avant celle du milieu (sens de rotation anti-horaire)
if (Carte == GoBot.Carte.RecBun && Plateau.NotreCouleur == Plateau.CouleurGaucheJaune ||
Carte == GoBot.Carte.RecBeu && Plateau.NotreCouleur == Plateau.CouleurGaucheJaune ||
Carte == GoBot.Carte.RecBoi && Plateau.NotreCouleur == Plateau.CouleurDroiteVert)
{
DetectionsCapteur1.RemoveAt(0);
DetectionsCapteur2.RemoveAt(0);
}
if (ReglageOffset)
{
compteurReglageOffset--;
// On ajoute les angles mesurés à l'historique
anglesMesuresPourOffsetCapteur2.Add(DetectionsCapteur2[0].AngleCentral);
anglesMesuresPourOffsetCapteur1.Add(DetectionsCapteur1[0].AngleCentral);
}
}*/
if (DetectionsCapteur1.Count == 1 && DetectionsCapteur2.Count == 1)
{
compteurReglageOffset--;
// On ajoute les angles mesurés à l'historique
anglesMesuresPourOffsetCapteur2.Add(DetectionsCapteur2[0].AngleCentral);
anglesMesuresPourOffsetCapteur1.Add(DetectionsCapteur1[0].AngleCentral);
}
if (ReglageOffset && compteurReglageOffset == 0)
{
// Compteur arrivé à la fin, on calcule l'offset
double moyenne = 0;
foreach (double dv in anglesMesuresPourOffsetCapteur1)
moyenne += dv;
moyenne /= anglesMesuresPourOffsetCapteur1.Count;
// Calcul l'offset en fonction de ce qu'il est censé mesurer au centre
moyenne = 0 - moyenne;
// On le sauve dans la config (haut)
Config.CurrentConfig.SetOffsetBalise(1, moyenne + Config.CurrentConfig.GetOffsetBalise(1));
moyenne = 0;
foreach (double dv in anglesMesuresPourOffsetCapteur2)
moyenne += dv;
moyenne /= anglesMesuresPourOffsetCapteur2.Count;
moyenne = 0 - moyenne;
// On le sauve dans la config (bas)
Config.CurrentConfig.SetOffsetBalise(2, moyenne + Config.CurrentConfig.GetOffsetBalise(2));
Config.Save();
// Réglage terminé
ReglageOffset = false;
if (CalibrationAngulaireTerminee != null) CalibrationAngulaireTerminee();
}
}
Detections = new List<DetectionBalise>();
Detections.AddRange(DetectionsCapteur1);
Detections.AddRange(DetectionsCapteur2);
// Retire les détections correspondant à la position de robots alliés
if (!ReglageOffset && Plateau.ReflecteursNosRobots)
{
foreach (Robot robot in Robots.DicRobots.Values)
{
for (int i = 0; i < Detections.Count; i++)
{
DetectionBalise detection = Detections[i];
// Calcul du 3ème point du triangle rectangle Balise / Gros robot
Droite droiteBalise0Degres = new Droite(Position.Coordonnees, new PointReel(Position.Coordonnees.X + 500, Position.Coordonnees.Y));
Droite perpendiculaire = droiteBalise0Degres.GetPerpendiculaire(robot.Position.Coordonnees);
PointReel troisiemePoint = perpendiculaire.getCroisement(droiteBalise0Degres);
double distanceBaliseTroisiemePoint = troisiemePoint.Distance(Position.Coordonnees);
double distanceBaliseRobot = robot.Position.Coordonnees.Distance(Position.Coordonnees);
double a = Math.Acos(distanceBaliseTroisiemePoint / distanceBaliseRobot);
Angle angleGrosRobot = new Angle(a, AnglyeType.Radian);
Angle angleDetection = new Angle(detection.AngleCentral);
double marge = 4;
if (Plateau.NotreCouleur == Plateau.CouleurDroiteVert)
{
Angle diff = new Angle(180) - (angleDetection - angleGrosRobot);
if (Math.Abs((diff).AngleDegres) < marge)
{
Detections.RemoveAt(i);
i--;
}
}
else if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet)
{
Angle diff = angleGrosRobot - angleDetection;
if (Math.Abs((diff).AngleDegres) < marge)
{
Detections.RemoveAt(i);
i--;
}
}
}
}
}
// Génération de l'event de notification de détection
PositionsChange();
Actualisation();
}
}
}
catch (Exception)
{ }
Connexion.ConnexionCheck.MajConnexion();
}
private static void DessinerForme(Graphics graphics, Color color, int epaisseur, Droite droite)
{
// Un peu douteux mais bon
PointReel p1 = droite.getCroisement(new Droite(new PointReel(-10000, -10000), new PointReel(-10001, 10000)));
PointReel p2 = droite.getCroisement(new Droite(new PointReel(10000, -10000), new PointReel(10001, 10000)));
if (p1 == null || p2 == null)
{
p1 = droite.getCroisement(new Droite(new PointReel(-10000, -10000), new PointReel(10000, -10001)));
p2 = droite.getCroisement(new Droite(new PointReel(10000, 10000), new PointReel(-10000, 10001)));
}
if (p1 != null && p2 != null)
DessinerForme(graphics, color, epaisseur, new Segment(p1, p2));
}
