[三边定位] C# 演示程序

  　计划用CC2530做定位，网上找了一些求圆交点的程序， 修改成3个圆求交点的质心，感觉算法还行。 粗略写了一下程序，结果还行。

　　现在只能手动输入3个圆的信息。 后面需要再优化。

　　

全部未优化的程序:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;


namespace WindowsFormsApplication1
{
     
        /* static void Main(string[] args)
         {
             Circle[] circles = new Circle[2];
             Point[] points = new Point[2];
             Console.Write("请输入两圆x，y，半径(以逗号分开)：");
             char[] sep = new char[] { ' ', ',', '，' };
             string[] tempstr;
             while (true)
             {
                 tempstr = Console.ReadLine().Split(sep, StringSplitOptions.RemoveEmptyEntries);
                 if (tempstr.Length != 6)
                 {
                     Console.Write("输入有误
按任意键退出...");
                     Console.ReadKey();
                     Environment.Exit(0);
                 }
                 circles[0].X = double.Parse(tempstr[0]);
                 circles[0].Y = double.Parse(tempstr[1]);
                 circles[0].Radius = double.Parse(tempstr[2]);
                 circles[1].X = double.Parse(tempstr[3]);
                 circles[1].Y = double.Parse(tempstr[4]);
                 circles[1].Radius = double.Parse(tempstr[5]);
                 switch (insect(circles, points))
                 {
                     case -1:
                         Console.Write("两圆相同
");
                         break;
                     case 0:
                         Console.Write("不相交
");
                         break;
                     case 1:
                         Console.WriteLine(points[0]);
                         break;
                     case 2:
                         Console.WriteLine(string.Join(" ", points));
                         break;
                 }
             }
         }*/

    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        private void Form1_Load(object sender, EventArgs e)
        {
        
        }
        struct Point
        {
            public double X;
            public double Y;
            public override string ToString()
            {
                return string.Format("({0},{1})", X, Y);
            }
        }

        struct Circle
        {
            public Point Center;
            public double X { get { return Center.X; } set { Center.X = value; } }
            public double Y { get { return Center.Y; } set { Center.X = value; } }
            public double Radius;
        };
           const double ZERO = 1e-9;//误差，小于这个就当作0
        static bool double_equals(double a, double b)
        {
            return Math.Abs(a - b) < ZERO;
        }

        static double distance_sqr(Point a, Point b)
        {
            return (a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y);
        }

        static double distance(Point a, Point b)
        {
            return Math.Sqrt(distance_sqr(a, b));
        }

        int insect(Circle[] circles, Point[] points)
        {
            double d, a, b, c, p, q, r;
            double[] cos_value = new double[2], sin_value = new double[2];
            if (double_equals(circles[0].Center.X, circles[1].Center.X)
                && double_equals(circles[0].Center.Y, circles[1].Center.Y)
                && double_equals(circles[0].Radius, circles[1].Radius))
            {
                return -1;
            }

            d = distance(circles[0].Center, circles[1].Center);
            if (d > circles[0].Radius + circles[1].Radius
                || d < Math.Abs(circles[0].Radius - circles[1].Radius))
            {
                return 0;
            }

            a = 2.0 * circles[0].Radius * (circles[0].Center.X - circles[1].Center.X);
            b = 2.0 * circles[0].Radius * (circles[0].Center.Y - circles[1].Center.Y);
            c = circles[1].Radius * circles[1].Radius - circles[0].Radius * circles[0].Radius
                - distance_sqr(circles[0].Center, circles[1].Center);
            p = a * a + b * b;
            q = -2.0 * a * c;
            if (double_equals(d, circles[0].Radius + circles[1].Radius)
                || double_equals(d, Math.Abs(circles[0].Radius - circles[1].Radius)))
            {
                cos_value[0] = -q / p / 2.0;
                sin_value[0] = Math.Sqrt(1 - cos_value[0] * cos_value[0]);

                points[0].X = circles[0].Radius * cos_value[0] + circles[0].Center.X;
                points[0].Y = circles[0].Radius * sin_value[0] + circles[0].Center.Y;

                if (!double_equals(distance_sqr(points[0], circles[1].Center),
                                   circles[1].Radius * circles[1].Radius))
                {
                    points[0].Y = circles[0].Center.Y - circles[0].Radius * sin_value[0];
                }
                return 1;
            }

            r = c * c - b * b;
            cos_value[0] = (Math.Sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;
            cos_value[1] = (-Math.Sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;
            sin_value[0] = Math.Sqrt(1 - cos_value[0] * cos_value[0]);
            sin_value[1] = Math.Sqrt(1 - cos_value[1] * cos_value[1]);

            points[0].X = circles[0].Radius * cos_value[0] + circles[0].Center.X;
            points[1].X = circles[0].Radius * cos_value[1] + circles[0].Center.X;
            points[0].Y = circles[0].Radius * sin_value[0] + circles[0].Center.Y;
            points[1].Y = circles[0].Radius * sin_value[1] + circles[0].Center.Y;

            if (!double_equals(distance_sqr(points[0], circles[1].Center),
                               circles[1].Radius * circles[1].Radius))
            {
                points[0].Y = circles[0].Center.Y - circles[0].Radius * sin_value[0];
            }
            if (!double_equals(distance_sqr(points[1], circles[1].Center),
                               circles[1].Radius * circles[1].Radius))
            {
                points[1].Y = circles[0].Center.Y - circles[0].Radius * sin_value[1];
            }
            if (double_equals(points[0].Y, points[1].Y)
                && double_equals(points[0].X, points[1].X))
            {
                if (points[0].Y > 0)
                {
                    points[1].Y = -points[1].Y;
                }
                else
                {
                    points[0].Y = -points[0].Y;
                }
            }
            return 2;
        }

        void Draw_circle(Graphics grap, Circle circles)
        {
           // int iSeed = 10;
            Random ro = new Random(10);
            long tick = DateTime.Now.Ticks;
            Random ran = new Random((int)(tick & 0xffffffffL) | (int)(tick >> 32));

            int R = ran.Next(255);
            int G = ran.Next(255);
            int B = ran.Next(255);
            B = (R + G > 400) ? R + G - 400 : B;//0 : 380 - R - G;
            B = (B > 255) ? 255 : B;
                       
            Pen pen = new Pen(Color.Red, 5);//画笔颜色    
            pen.Color = Color.FromArgb(R, G, B);
            grap.DrawEllipse(pen, Convert.ToInt32(circles.Center.X - circles.Radius), Convert.ToInt32(circles.Center.Y - circles.Radius), Convert.ToInt32(2* circles.Radius), Convert.ToInt32(2* circles.Radius));//画椭圆的方法，x坐标、y坐标、宽、高，如果是100，则半径为50
        }

        private void button1_Click(object sender, EventArgs e)
        {
            Graphics gra = this.pictureBox1.CreateGraphics();
            gra.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;
             Circle[] circles = new Circle[3];
             Point[] points = new Point[2];

             circles[0].Center.X = 130; ;
             circles[0].Center.Y = 130;
             circles[0].Radius =120;

             circles[1].Center.X = 400;
             circles[1].Center.Y = 100;
             circles[1].Radius =180;

             circles[2].Center.X = 320; ;
             circles[2].Center.Y = 350;
             circles[2].Radius = 180;

             Circle[] copy = (Circle[])circles.Clone(); //copy
             //  Draw_circle(gra, circles[0]);
             Draw_circle(gra, circles[0]);
             Draw_circle(gra, circles[1]);
             insect(circles, points);// 0 1
             Point Pac;

             double points_AC_0 = distance(points[0], circles[2].Center);
             double points_AC_1 = distance(points[1], circles[2].Center);
             if (points_AC_0 < points_AC_1)
             {
                 Pac.X = points[0].X;
                 Pac.Y = points[0].Y;
             }
             else
             {
                 Pac.X = points[1].X;
                 Pac.Y = points[1].Y;
             }
             gra.FillRectangle(new SolidBrush(Color.Red), Convert.ToInt32(points[0].X), Convert.ToInt32(points[0].Y), 8, 8);
             gra.FillRectangle(new SolidBrush(Color.Red), Convert.ToInt32(points[1].X), Convert.ToInt32(points[1].Y), 8, 8);



             circles[1].Center.X = circles[2].Center.X; ;
             circles[1].Center.Y = circles[2].Center.Y;
             circles[1].Radius = circles[2].Radius;
             //  Draw_circle(gra, circles[0]);
             Draw_circle(gra, circles[0]);
             Draw_circle(gra, circles[1]);
             Draw_circle(gra, circles[2]);
             insect(circles, points);// 0 2

             Point Pbc;

             points_AC_0 = distance(points[0], copy[1].Center);
             points_AC_1 = distance(points[1], copy[1].Center);
             if (points_AC_0 < points_AC_1)
             {
                 Pbc.X = points[0].X;
                 Pbc.Y = points[0].Y;
             }
             else
             {
                 Pbc.X = points[1].X;
                 Pbc.Y = points[1].Y;
             }
             gra.FillRectangle(new SolidBrush(Color.Red), Convert.ToInt32(points[0].X), Convert.ToInt32(points[0].Y), 8, 8);
             gra.FillRectangle(new SolidBrush(Color.Red), Convert.ToInt32(points[1].X), Convert.ToInt32(points[1].Y), 8, 8);
             circles = (Circle[])copy.Clone(); //copy

             circles[0].Center.X = circles[2].Center.X; ;
             circles[0].Center.Y = circles[2].Center.Y;
             circles[0].Radius = circles[2].Radius;
             insect(circles, points); // 1 3

             Point Pbd;

             points_AC_0 = distance(points[0], copy[2].Center);
             points_AC_1 = distance(points[1], copy[2].Center);
             if (points_AC_0 < points_AC_1)
             {
                 Pbd.X = points[0].X;
                 Pbd.Y = points[0].Y;
             }
             else
             {
                 Pbd.X = points[1].X;
                 Pbd.Y = points[1].Y;
             }
             gra.FillRectangle(new SolidBrush(Color.Red), Convert.ToInt32(points[0].X), Convert.ToInt32(points[0].Y), 8, 8);
             gra.FillRectangle(new SolidBrush(Color.Red), Convert.ToInt32(points[1].X), Convert.ToInt32(points[1].Y), 8, 8);

             double P_1, P_2;
             P_1 = (Pac.X + Pbc.X + Pbd.X) / 3.0;
             P_2 = (Pac.Y + Pbc.Y + Pbd.Y) / 3.0;
             gra.FillRectangle(new SolidBrush(Color.Blue), Convert.ToInt32(P_1), Convert.ToInt32(P_2), 20, 20);



            gra.FillRectangle(new SolidBrush(Color.Red), Convert.ToInt32(points[0].X), Convert.ToInt32(points[0].Y), 4, 4);
            gra.FillRectangle(new SolidBrush(Color.Red), Convert.ToInt32(points[1].X), Convert.ToInt32(points[1].Y), 4, 4);


        }
    }
}