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.     cout<<"norm :::  "<<norm <<endl;
  17. }
  18.  
  19. point trilateration(point point1, point point2, point point3, double r1, double r2, double r3) {
  20.     point resultPose;
  21.     //unit vector in a direction from point1 to point 2
  22.     double p2p1Distance = pow(pow(point2.x-point1.x,2) + pow(point2.y-   point1.y,2),0.5);
  23. //cout<<"p2p1Distance :::  "<<p2p1Distance <<endl;
  24.     point ex = {(point2.x-point1.x)/p2p1Distance, (point2.y-point1.y)/p2p1Distance};
  25. //cout<<"ex  :::  "<<ex  <<endl;
  26.     point aux = {point3.x-point1.x,point3.y-point1.y};
  27. //cout<<"aux :::  "<<aux <<endl;
  28. //cout << "He has " << aux << " cats." << endl;     
  29.     //signed magnitude of the x component
  30.     double i = ex.x * aux.x + ex.y * aux.y;
  31.     //cout<<"i :::  "<<i <<endl;
  32.     //the unit vector in the y direction. 
  33.     point aux2 = { point3.x-point1.x-i*ex.x, point3.y-point1.y-i*ex.y};
  34.  
  35.     point ey = { aux2.x / norm (aux2), aux2.y / norm (aux2) };
  36.     //the signed magnitude of the y component
  37.     double j = ey.x * aux.x + ey.y * aux.y;
  38.  
  39.     //coordinates
  40.     double x = (pow(r1,2) - pow(r2,2) + pow(p2p1Distance,2))/ (2 * p2p1Distance);
  41.  
  42.     double y = (pow(r1,2) - pow(r3,2) + pow(i,2) + pow(j,2))/(2*j) - i*x/j;
  43.  
  44.     //result coordinates
  45.     double finalX = point1.x+ x*ex.x + y*ey.x;
  46.  
  47.     double finalY = point1.y+ x*ex.y + y*ey.y;
  48.  
  49.     resultPose.x = finalX;
  50.     resultPose.y = finalY;
  51.     return resultPose;
  52. }
  53.  
  54. int main(int argc, char* argv[]){
  55.     point finalPose;
  56.     point p1 = {4.0,4.0};
  57.     point p2 = {9.0,7.0};
  58.     point p3 = {9.0,1.0};
  59.     double r1,r2,r3;
  60.     r1 = 4;
  61.     r2 = 3;
  62.     r3 = 3.25;
  63.     finalPose = trilateration(p1,p2,p3,r1,r2,r3);
  64.     cout<<"X:::  "<<finalPose.x<<endl;
  65.     cout<<"Y:::  "<<finalPose.y<<endl; 
  66. }
Success #stdin #stdout 0s 16064KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
X:::  8.02188
Y:::  4.13021
https://ideone.com/QdzfHg
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!