void trilateration_1(struct point point1, struct point point2, struct point point3, double r1, double r2, double r3) {
struct point resultPose;
//unit vector in a direction from point1 to point 2 从点1到点2方向上的单位向量
double p2p1Distance = pow(pow(point2.x - point1.x, 2) + pow(point2.y - point1.y, 2), 0.5);
struct point ex = { (point2.x - point1.x) / p2p1Distance, (point2.y - point1.y) / p2p1Distance };
struct point aux = { point3.x - point1.x,point3.y - point1.y };
//signed magnitude of the x component x分量的符号大小
double i = ex.x * aux.x + ex.y * aux.y;
//the unit vector in the y direction. y方向的单位向量。T
struct point aux2 = { point3.x - point1.x - i * ex.x, point3.y - point1.y - i * ex.y };
struct point ey = { aux2.x / norm(aux2), aux2.y / norm(aux2) };
//the signed magnitude of the y component y分量的符号大小
double j = ey.x * aux.x + ey.y * aux.y;
//coordinates 协调
double x = (pow(r1, 2) - pow(r2, 2) + pow(p2p1Distance, 2)) / (2 * p2p1Distance);
double y = (pow(r1, 2) - pow(r3, 2) + pow(i, 2) + pow(j, 2)) / (2 * j) - i * x / j;
//result coordinates 结果坐标
double finalX = point1.x + x * ex.x + y * ey.x;
double finalY = point1.y + x * ex.y + y * ey.y;
resultPose.x = finalX;
resultPose.y = finalY;
printf("TAG LOC My1:x = %3.2f,y = %3.2f\r\n", resultPose.x, resultPose.y);//打印未知点坐标
}