三边定位_位置解析(C++)

一、三边定位位置解析
通过对已知的三个基站的坐标和三个基站分别到未知点的距离来确定，未知点的坐标。
注：这个解析算法代码是通过对Decawave公司的开源代码剪切更改而来的。
代码：

// -------------------------------------------------------------------------------------------------------------------
//
//  File: trilateration.cpp
//
//  Copyright 2016 (c) Decawave Ltd, Dublin, Ireland.
//
//  All rights reserved.
//
//  Author:
//
// -------------------------------------------------------------------------------------------------------------------


#include <stdio.h>
#include <string.h>
#include <math.h>
//#include <stdlib.h>
#include <math.h>




struct point_t
{
	double x, y;
};

struct circle_t
{
	struct point_t center;
	double r;
};


int double_equals(double a, double b)
{
	static const double ZERO = 1e-9;
	return fabs(a - b) < ZERO;
}

double distance_sqr(struct point_t* a, struct point_t* b)
{
	return (a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y);
}

double distance(struct point_t* a, struct point_t* b)
{
	return sqrt(distance_sqr(a, b));
}




int insect(struct circle_t circles[], struct point_t points[])
{
	double d, a, b, c, p, q, r;
	double cos_value[2], sin_value[2];
	if (double_equals(circles[0].center.x, circles[1].cent