[三边定位] 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("输入有误\n按任意键退出...");
                     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("两圆相同\n");
                         break;
                     case 0:
                         Console.Write("不相交\n");
                         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-;//误差，小于这个就当作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[], sin_value = new double[];
            if (double_equals(circles[].Center.X, circles[].Center.X)
                && double_equals(circles[].Center.Y, circles[].Center.Y)
                && double_equals(circles[].Radius, circles[].Radius))
            {
                return -;
            }

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

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

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

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

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

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

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

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

            int R = ran.Next();
            int G = ran.Next();
            int B = ran.Next();
            B = (R + G > ) ? R + G -  : B;//0 : 380 - R - G;
            B = (B > ) ?  : B;

            Pen pen = new Pen(Color.Red, );//画笔颜色
            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(* circles.Radius), Convert.ToInt32(* 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[];
             Point[] points = new Point[];

             circles[].Center.X = ; ;
             circles[].Center.Y = ;
             circles[].Radius =;

             circles[].Center.X = ;
             circles[].Center.Y = ;
             circles[].Radius =;

             circles[].Center.X = ; ;
             circles[].Center.Y = ;
             circles[].Radius = ;

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

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

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

             Point Pbc;

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

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

             Point Pbd;

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

             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), , );

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

        }
    }
}