fork download

copy

#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;
    //signed magnitude of the x component
    double i = ex.x * aux.x + ex.y * aux.y;
    //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;
    //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; 
}

prog.cpp: In function ‘point trilateration(point, point, point, double, double, double)’:
prog.cpp:23:36: warning: narrowing conversion of ‘(((double)(point2.point::x - point1.point::x)) / p2p1Distance)’ from ‘double’ to ‘float’ inside { } [-Wnarrowing]
     point ex = {(point2.x-point1.x)/p2p1Distance, (point2.y-point1.y)/p2p1Distance};
                 ~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~
prog.cpp:23:70: warning: narrowing conversion of ‘(((double)(point2.point::y - point1.point::y)) / p2p1Distance)’ from ‘double’ to ‘float’ inside { } [-Wnarrowing]
     point ex = {(point2.x-point1.x)/p2p1Distance, (point2.y-point1.y)/p2p1Distance};
                                                   ~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~
prog.cpp:26:18: error: no match for ‘operator<<’ (operand types are ‘std::basic_ostream<char>’ and ‘point’)
 cout<<"aux :::  "<<aux <<endl;
 ~~~~~~~~~~~~~~~~~^~~~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:108:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(std::basic_ostream<_CharT, _Traits>::__ostream_type& (*)(std::basic_ostream<_CharT, _Traits>::__ostream_type&)) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(__ostream_type& (*__pf)(__ostream_type&))
       ^~~~~~~~
/usr/include/c++/6/ostream:108:7: note:   no known conversion for argument 1 from ‘point’ to ‘std::basic_ostream<char>::__ostream_type& (*)(std::basic_ostream<char>::__ostream_type&) {aka std::basic_ostream<char>& (*)(std::basic_ostream<char>&)}’
/usr/include/c++/6/ostream:117:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(std::basic_ostream<_CharT, _Traits>::__ios_type& (*)(std::basic_ostream<_CharT, _Traits>::__ios_type&)) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>; std::basic_ostream<_CharT, _Traits>::__ios_type = std::basic_ios<char>]
       operator<<(__ios_type& (*__pf)(__ios_type&))
       ^~~~~~~~
/usr/include/c++/6/ostream:117:7: note:   no known conversion for argument 1 from ‘point’ to ‘std::basic_ostream<char>::__ios_type& (*)(std::basic_ostream<char>::__ios_type&) {aka std::basic_ios<char>& (*)(std::basic_ios<char>&)}’
/usr/include/c++/6/ostream:127:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(std::ios_base& (*)(std::ios_base&)) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(ios_base& (*__pf) (ios_base&))
       ^~~~~~~~
/usr/include/c++/6/ostream:127:7: note:   no known conversion for argument 1 from ‘point’ to ‘std::ios_base& (*)(std::ios_base&)’
/usr/include/c++/6/ostream:166:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(long int) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(long __n)
       ^~~~~~~~
/usr/include/c++/6/ostream:166:7: note:   no known conversion for argument 1 from ‘point’ to ‘long int’
/usr/include/c++/6/ostream:170:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(long unsigned int) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(unsigned long __n)
       ^~~~~~~~
/usr/include/c++/6/ostream:170:7: note:   no known conversion for argument 1 from ‘point’ to ‘long unsigned int’
/usr/include/c++/6/ostream:174:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(bool) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(bool __n)
       ^~~~~~~~
/usr/include/c++/6/ostream:174:7: note:   no known conversion for argument 1 from ‘point’ to ‘bool’
In file included from /usr/include/c++/6/ostream:638:0,
                 from /usr/include/c++/6/iostream:39,
                 from prog.cpp:1:
/usr/include/c++/6/bits/ostream.tcc:91:5: note: candidate: std::basic_ostream<_CharT, _Traits>& std::basic_ostream<_CharT, _Traits>::operator<<(short int) [with _CharT = char; _Traits = std::char_traits<char>]
     basic_ostream<_CharT, _Traits>::
     ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/include/c++/6/bits/ostream.tcc:91:5: note:   no known conversion for argument 1 from ‘point’ to ‘short int’
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:181:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(short unsigned int) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(unsigned short __n)
       ^~~~~~~~
/usr/include/c++/6/ostream:181:7: note:   no known conversion for argument 1 from ‘point’ to ‘short unsigned int’
In file included from /usr/include/c++/6/ostream:638:0,
                 from /usr/include/c++/6/iostream:39,
                 from prog.cpp:1:
/usr/include/c++/6/bits/ostream.tcc:105:5: note: candidate: std::basic_ostream<_CharT, _Traits>& std::basic_ostream<_CharT, _Traits>::operator<<(int) [with _CharT = char; _Traits = std::char_traits<char>]
     basic_ostream<_CharT, _Traits>::
     ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/include/c++/6/bits/ostream.tcc:105:5: note:   no known conversion for argument 1 from ‘point’ to ‘int’
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:192:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(unsigned int) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(unsigned int __n)
       ^~~~~~~~
/usr/include/c++/6/ostream:192:7: note:   no known conversion for argument 1 from ‘point’ to ‘unsigned int’
/usr/include/c++/6/ostream:201:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(long long int) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(long long __n)
       ^~~~~~~~
/usr/include/c++/6/ostream:201:7: note:   no known conversion for argument 1 from ‘point’ to ‘long long int’
/usr/include/c++/6/ostream:205:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(long long unsigned int) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(unsigned long long __n)
       ^~~~~~~~
/usr/include/c++/6/ostream:205:7: note:   no known conversion for argument 1 from ‘point’ to ‘long long unsigned int’
/usr/include/c++/6/ostream:220:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(double) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(double __f)
       ^~~~~~~~
/usr/include/c++/6/ostream:220:7: note:   no known conversion for argument 1 from ‘point’ to ‘double’
/usr/include/c++/6/ostream:224:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(float) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(float __f)
       ^~~~~~~~
/usr/include/c++/6/ostream:224:7: note:   no known conversion for argument 1 from ‘point’ to ‘float’
/usr/include/c++/6/ostream:232:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(long double) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(long double __f)
       ^~~~~~~~
/usr/include/c++/6/ostream:232:7: note:   no known conversion for argument 1 from ‘point’ to ‘long double’
/usr/include/c++/6/ostream:245:7: note: candidate: std::basic_ostream<_CharT, _Traits>::__ostream_type& std::basic_ostream<_CharT, _Traits>::operator<<(const void*) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__ostream_type = std::basic_ostream<char>]
       operator<<(const void* __p)
       ^~~~~~~~
/usr/include/c++/6/ostream:245:7: note:   no known conversion for argument 1 from ‘point’ to ‘const void*’
In file included from /usr/include/c++/6/ostream:638:0,
                 from /usr/include/c++/6/iostream:39,
                 from prog.cpp:1:
/usr/include/c++/6/bits/ostream.tcc:119:5: note: candidate: std::basic_ostream<_CharT, _Traits>& std::basic_ostream<_CharT, _Traits>::operator<<(std::basic_ostream<_CharT, _Traits>::__streambuf_type*) [with _CharT = char; _Traits = std::char_traits<char>; std::basic_ostream<_CharT, _Traits>::__streambuf_type = std::basic_streambuf<char>]
     basic_ostream<_CharT, _Traits>::
     ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/include/c++/6/bits/ostream.tcc:119:5: note:   no known conversion for argument 1 from ‘point’ to ‘std::basic_ostream<char>::__streambuf_type* {aka std::basic_streambuf<char>*}’
In file included from /usr/include/c++/6/string:52:0,
                 from /usr/include/c++/6/bits/locale_classes.h:40,
                 from /usr/include/c++/6/bits/ios_base.h:41,
                 from /usr/include/c++/6/ios:42,
                 from /usr/include/c++/6/ostream:38,
                 from /usr/include/c++/6/iostream:39,
                 from prog.cpp:1:
/usr/include/c++/6/bits/basic_string.h:5340:5: note: candidate: template<class _CharT, class _Traits, class _Alloc> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const std::__cxx11::basic_string<_CharT, _Traits, _Alloc>&)
     operator<<(basic_ostream<_CharT, _Traits>& __os,
     ^~~~~~~~
/usr/include/c++/6/bits/basic_string.h:5340:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   ‘point’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Alloc>’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/bits/ios_base.h:46:0,
                 from /usr/include/c++/6/ios:42,
                 from /usr/include/c++/6/ostream:38,
                 from /usr/include/c++/6/iostream:39,
                 from prog.cpp:1:
/usr/include/c++/6/system_error:209:5: note: candidate: template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const std::error_code&)
     operator<<(basic_ostream<_CharT, _Traits>& __os, const error_code& __e)
     ^~~~~~~~
/usr/include/c++/6/system_error:209:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘const std::error_code&’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:497:5: note: candidate: template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, _CharT)
     operator<<(basic_ostream<_CharT, _Traits>& __out, _CharT __c)
     ^~~~~~~~
/usr/include/c++/6/ostream:497:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   deduced conflicting types for parameter ‘_CharT’ (‘char’ and ‘point’)
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:502:5: note: candidate: template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, char)
     operator<<(basic_ostream<_CharT, _Traits>& __out, char __c)
     ^~~~~~~~
/usr/include/c++/6/ostream:502:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘char’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:508:5: note: candidate: template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, char)
     operator<<(basic_ostream<char, _Traits>& __out, char __c)
     ^~~~~~~~
/usr/include/c++/6/ostream:508:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘char’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:514:5: note: candidate: template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, signed char)
     operator<<(basic_ostream<char, _Traits>& __out, signed char __c)
     ^~~~~~~~
/usr/include/c++/6/ostream:514:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘signed char’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:519:5: note: candidate: template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, unsigned char)
     operator<<(basic_ostream<char, _Traits>& __out, unsigned char __c)
     ^~~~~~~~
/usr/include/c++/6/ostream:519:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘unsigned char’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:539:5: note: candidate: template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const _CharT*)
     operator<<(basic_ostream<_CharT, _Traits>& __out, const _CharT* __s)
     ^~~~~~~~
/usr/include/c++/6/ostream:539:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   mismatched types ‘const _CharT*’ and ‘point’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/ostream:638:0,
                 from /usr/include/c++/6/iostream:39,
                 from prog.cpp:1:
/usr/include/c++/6/bits/ostream.tcc:321:5: note: candidate: template<class _CharT, class _Traits> std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&, const char*)
     operator<<(basic_ostream<_CharT, _Traits>& __out, const char* __s)
     ^~~~~~~~
/usr/include/c++/6/bits/ostream.tcc:321:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘const char*’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:556:5: note: candidate: template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, const char*)
     operator<<(basic_ostream<char, _Traits>& __out, const char* __s)
     ^~~~~~~~
/usr/include/c++/6/ostream:556:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘const char*’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:569:5: note: candidate: template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, const signed char*)
     operator<<(basic_ostream<char, _Traits>& __out, const signed char* __s)
     ^~~~~~~~
/usr/include/c++/6/ostream:569:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘const signed char*’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:574:5: note: candidate: template<class _Traits> std::basic_ostream<char, _Traits>& std::operator<<(std::basic_ostream<char, _Traits>&, const unsigned char*)
     operator<<(basic_ostream<char, _Traits>& __out, const unsigned char* __s)
     ^~~~~~~~
/usr/include/c++/6/ostream:574:5: note:   template argument deduction/substitution failed:
prog.cpp:26:20: note:   cannot convert ‘aux’ (type ‘point’) to type ‘const unsigned char*’
 cout<<"aux :::  "<<aux <<endl;
                    ^~~
In file included from /usr/include/c++/6/iostream:39:0,
                 from prog.cpp:1:
/usr/include/c++/6/ostream:628:5: note: candidate: std::basic_ostream<_CharT, _Traits>& std::operator<<(std::basic_ostream<_CharT, _Traits>&&, const _Tp&) [with _CharT = char; _Traits = std::char_traits<char>; _Tp = point] <near match>
     operator<<(basic_ostream<_CharT, _Traits>&& __os, const _Tp& __x)
     ^~~~~~~~
/usr/include/c++/6/ostream:628:5: note:   conversion of argument 1 would be ill-formed:
prog.cpp:26:5: error: cannot bind ‘std::basic_ostream<char>’ lvalue to ‘std::basic_ostream<char>&&’
 cout<<"aux :::  "<<aux <<endl;
 ~~~~^~~~~~~~~~~~~
prog.cpp:30:37: warning: narrowing conversion of ‘(((double)(point3.point::x - point1.point::x)) - (i * ((double)ex.point::x)))’ from ‘double’ to ‘float’ inside { } [-Wnarrowing]
     point aux2 = { point3.x-point1.x-i*ex.x, point3.y-point1.y-i*ex.y};
                    ~~~~~~~~~~~~~~~~~^~~~~~~
prog.cpp:30:63: warning: narrowing conversion of ‘(((double)(point3.point::y - point1.point::y)) - (i * ((double)ex.point::y)))’ from ‘double’ to ‘float’ inside { } [-Wnarrowing]
     point aux2 = { point3.x-point1.x-i*ex.x, point3.y-point1.y-i*ex.y};
                                              ~~~~~~~~~~~~~~~~~^~~~~~~

Standard output is empty