utility.cpp
Go to the documentation of this file.
00001 #include "utility.h"
00002 
00003 Utility::Utility() {}
00004 
00005 
00007 const float Utility::euclideanDistance(const std::vector<float> a, const std::vector<float> b) const {
00008 
00009   float d_x = b.at(0) - a.at(0);
00010   float d_y = b.at(1) - a.at(1);
00011   return sqrt( pow(d_x,2) + pow(d_y,2) );
00012 } //End euclideanDistance
00013 
00015 const float Utility::findAngleFromAToB(const std::vector<float> a, const std::vector<float> b) const {
00016   float result;
00017 
00018   // Find the distances in x,y directions and Euclidean distance
00019   float d_x = b.at(0) - a.at(0);
00020   float d_y = b.at(1) - a.at(1);
00021   float euc_dist = sqrt( pow(d_x,2) + pow(d_y,2) );
00022   
00023   // If the positions are the same,
00024   // Set the result to the starting orientation if one is provided
00025   // Or to 0 if no starting orientation is provided
00026   // TODO: Make the comparison proportionate to size of space
00027   if(euc_dist <= 0.01) {
00028     if(a.size() > 2) {
00029       result = a.at(2);
00030     }
00031     else {
00032       result = 0;
00033     }
00034   }
00035 
00036   // If b is in the 1st or 2nd quadrants
00037   else if(d_y > 0) {
00038     result = acos(d_x / euc_dist);
00039   }
00040 
00041   // If b is in the 3rd quadrant, d_y<0 & d_x<0
00042   else if(d_x < 0) {
00043     result = -PI - asin(d_y / euc_dist);
00044   }
00045 
00046   // If b is in the 4th quadrant, d_y<=0 & d_x>=0
00047   else {
00048     result = asin(d_y / euc_dist); 
00049   }
00050 
00051   return result;
00052 } //End findAngleFromAToB
00053 
00054 
00056 const float Utility::findDistanceBetweenAngles(const float a1, const float a2) const {
00057   float result;
00058   float difference = a1 - a2;
00059   
00060   
00061   // If difference > pi, the result should be in [-PI,0] range
00062   if(difference > PI) {
00063     difference = fmodf(difference, PI);
00064     result = difference - PI;
00065   }
00066 
00067   // If difference < -pi, the result should be in [0,PI] range
00068   else if(difference < -PI) {
00069     result = difference + (2*PI);
00070   }
00071 
00072   // Else, the difference is fine
00073   else {
00074     result = difference;
00075   }
00076 
00077   return result;
00078 } //End findDistanceBetweenAngles
00079 
00080 
00081 
00082 const float Utility::displaceAngle(const float a1, float a2) const {
00083 
00084   a2 = fmodf(a2, 2*PI);
00085 
00086   if(a2 > PI) {
00087     a2 = fmodf(a2,PI) - PI;
00088   }
00089 
00090   return findDistanceBetweenAngles(a1, -a2);
00091 } //End displaceAngle
00092 
00093 
00094 
00096 const float Utility::getEuclideanDist(const std::vector<float> a, const std::vector<float> b) const {
00097   float result=0;
00098 
00099   for(unsigned int i=0;i<a.size();i++) {
00100     result += pow(a.at(i) - b.at(i), 2);
00101   }
00102   
00103   result = sqrt(result);
00104   return result;
00105 }
00106 
00107 


corobot_state_tf
Author(s):
autogenerated on Sun Oct 5 2014 23:17:54