Beispiel #1
0
        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}");
        }
Beispiel #2
0
    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);
        }
    }
Beispiel #3
0
    //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)
            });
        }
    }
Beispiel #4
0
    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)
            });
        }
    }
Beispiel #5
0
        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);
        }
Beispiel #6
0
        /// <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();
        }
Beispiel #7
0
        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));
        }