[三邊定位] C# 演示程序
阿新 • • 發佈：2017-09-08
convert 橢圓 src const load new eve fill sep
計劃用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; namespaceWindowsFormsApplication1 { /* 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 doubleY; 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); } } }
[三邊定位] C# 演示程序