#include <iostream>
#include <fstream>
#include <sstream>
#include <math.h>
#include <vector>
using namespace std;
struct point
{
float x,y;
};
float norm (point p) // get the norm of a vector
{
return pow(pow(p.x,2)+pow(p.y,2),.5);
}
point trilateration(point point1, point point2, point point3, double r1, double r2, double r3) {
point resultPose;
//unit vector in a direction from point1 to point 2
double p2p1Distance = pow(pow(point2.x-point1.x,2) + pow(point2.y- point1.y,2),0.5);
//cout<<"p2p1Distance ::: "<<p2p1Distance <<endl;
point ex = {(point2.x-point1.x)/p2p1Distance, (point2.y-point1.y)/p2p1Distance};
//cout<<"ex ::: "<<ex <<endl;
point aux = {point3.x-point1.x,point3.y-point1.y};
//cout<<"aux ::: "<<aux <<endl;
//cout << "He has " << aux << " cats." << endl;
//signed magnitude of the x component
double i = ex.x * aux.x + ex.y * aux.y;
//cout<<"i ::: "<<i <<endl;
//the unit vector in the y direction.
point aux2 = { point3.x-point1.x-i*ex.x, point3.y-point1.y-i*ex.y};
point ey = { aux2.x / norm (aux2), aux2.y / norm (aux2) };
//the signed magnitude of the y component
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;
cout<<"y ::: "<<y <<endl;
//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;
return resultPose;
}
int main(int argc, char* argv[]){
point finalPose;
point p1 = {4.0,4.0};
point p2 = {9.0,7.0};
point p3 = {9.0,1.0};
double r1,r2,r3;
r1 = 4;
r2 = 3;
r3 = 3.25;
finalPose = trilateration(p1,p2,p3,r1,r2,r3);
cout<<"X::: "<<finalPose.x<<endl;
cout<<"Y::: "<<finalPose.y<<endl;
}
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