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