Introduce angle calculation

AutonomousMode/Utils/utils.cpp

@@ -31,6 +31,22 @@ LFRVector::~LFRVector()
 {
 }
 
+double LFRVector::angle(const LFRVector& other) const
+{
+    return acos(dot(other)/(this->norm()*other.norm()));
+}
+
+double LFRVector::dot(const LFRVector& other) const
+{
+    return x*other.x + y*other.y;
+}
+
+double LFRVector::norm() const
+{
+    return sqrt(x*x+y*y);
+}
+
+
 LFRLine::LFRLine(/* args */) : start(), dir()
 {
 }

AutonomousMode/Utils/utils.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <opencv2/opencv.hpp>
+#include <cmath>
 
 using namespace cv;
 using namespace std;
@@ -30,6 +31,10 @@ public:
     LFRVector(const LFRPoint& pt);
     ~LFRVector();
 
+    double angle(const LFRVector& other) const;
+    double dot(const LFRVector& other) const;
+    double norm() const;
+
 };
 
 class LFRLine
@@ -60,7 +65,7 @@ public:
     std::vector<cv::Rect> boundingBoxes;
     std::vector<cv::Point> leftEdges;
     std::vector<cv::Point> middlePoints;
-    int angle; //Angle of the contour the robot has to follow to
+    double angle; //Angle of the contour the robot has to follow to
     int index; //Index of the contour the robot has to follow to
 
     FrameData(): contours(), boundingBoxes(), leftEdges(), middlePoints() {}