fork download
copy 
  1. #include <iostream>
  2. #include <fstream>
  3. #include <sstream>
  4. #include <math.h> 
  5. #include <vector>
  6. using namespace std;
  7.  
  8. struct point 
  9. {
  10.     float x,y;
  11. };
  12.  
  13. float norm (point p) // get the norm of a vector
  14. {
  15.     return pow(pow(p.x,2)+pow(p.y,2),.5);
  16. }
  17.  
  18. point trilateration(point point1, point point2, point point3, double r1, double r2, double r3) {
  19.     point resultPose;
  20.     //unit vector in a direction from point1 to point 2
  21.     double p2p1Distance = pow(pow(point2.x-point1.x,2) + pow(point2.y-   point1.y,2),0.5);
  22. //cout<<"p2p1Distance :::  "<<p2p1Distance <<endl;
  23.     point ex = {(point2.x-point1.x)/p2p1Distance, (point2.y-point1.y)/p2p1Distance};
  24. //cout<<"ex  :::  "<<ex  <<endl;
  25.     point aux = {point3.x-point1.x,point3.y-point1.y};
  26. //cout<<"aux :::  "<<aux <<endl;
  27. //cout << "He has " << aux << " cats." << endl;     
  28.     //signed magnitude of the x component
  29.     double i = ex.x * aux.x + ex.y * aux.y;
  30.     //cout<<"i :::  "<<i <<endl;
  31.     //the unit vector in the y direction. 
  32.     point aux2 = { point3.x-point1.x-i*ex.x, point3.y-point1.y-i*ex.y};
  33.  
  34.     point ey = { aux2.x / norm (aux2), aux2.y / norm (aux2) };
  35.     //the signed magnitude of the y component
  36.     double j = ey.x * aux.x + ey.y * aux.y;
  37.  
  38.     //coordinates
  39.     double x = (pow(r1,2) - pow(r2,2) + pow(p2p1Distance,2))/ (2 * p2p1Distance);
  40.  
  41.     double y = (pow(r1,2) - pow(r3,2) + pow(i,2) + pow(j,2))/(2*j) - i*x/j;
  42.  
  43.     //result coordinates
  44.     double finalX = point1.x+ x*ex.x + y*ey.x;
  45.  
  46.     double finalY = point1.y+ x*ex.y + y*ey.y;
  47.       cout<<"finalY :::  "<<finalY <<endl;
  48.     resultPose.x = finalX;
  49.     resultPose.y = finalY;
  50.     return resultPose;
  51. }
  52.  
  53. int main(int argc, char* argv[]){
  54.     point finalPose;
  55.     point p1 = {4.0,4.0};
  56.     point p2 = {9.0,7.0};
  57.     point p3 = {9.0,1.0};
  58.     double r1,r2,r3;
  59.     r1 = 4;
  60.     r2 = 3;
  61.     r3 = 3.25;
  62.     finalPose = trilateration(p1,p2,p3,r1,r2,r3);
  63.     cout<<"X:::  "<<finalPose.x<<endl;
  64.     cout<<"Y:::  "<<finalPose.y<<endl; 
  65. }
Success #stdin #stdout 0s 15240KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
finalY :::  4.13021
X:::  8.02188
Y:::  4.13021
https://ideone.com/8kezN5
language:
C++ (gcc 8.3)
created:
6 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!