Line-Following-Robot/AutonomousMode/Utils/utils.cpp

#include "utils.h"

#define PI 3.14159265359

LFRPoint::LFRPoint(/* args */) : x(0.0), y(0.0)
{
}

LFRPoint::LFRPoint(double x, double y): x(x), y(y)
{
}

LFRPoint::~LFRPoint()
{
}

LFRVector::LFRVector(/* args */) : LFRPoint()
{
}

LFRVector::LFRVector(double x, double y) : LFRPoint(x, y)
{
}

LFRVector::LFRVector(const LFRPoint& pt) : LFRPoint(pt)
{
    
}

LFRVector::~LFRVector()
{
}

LFRLine::LFRLine(/* args */) : start(), dir()
{
}

LFRLine::LFRLine(LFRPoint start, LFRVector dir) : start(start), dir(dir)
{
}

LFRLine::LFRLine(LFRPoint start, LFRPoint end) : start(start)
{
    dir = end - start;
}

LFRLine::~LFRLine()
{
}

VectorOfLines::VectorOfLines()
{
}

VectorOfLines::~VectorOfLines()
{
}

double VectorOfLines::calcGradient(Point p0, Point p1)
{
    double gradient = (p1.y - p0.y)/(p1.x - p0.x + 1e-10);
    return p1.x > p0.x ? gradient : - gradient;
}
    
float VectorOfLines::calcZeroPoint(cv::Point x, float m)
{
    return 0.0;
}

double VectorOfLines::calcDistance(Point p0, Point p1)
{
    return sqrt(pow(p1.y - p0.y, 2) + pow(p1.x - p0.x, 2));
}

vector<Vec4i> VectorOfLines::findMiddleLine(vector<Vec4i> &lines){
    Point point11;
    Point point12;
    Point point21;
    Point point22;
    vector<Vec4i> middleLines;
    for( size_t i = 0; i < (lines.size() - 1); i++ )
    {
        point11 = Point(lines[i][0], lines[i][1]);
        point12 = Point( lines[i][2], lines[i][3]);
        double gradient1 = VectorOfLines::calcGradient(point11, point12);

        //Compare every Line with the other
        for( size_t j = 0; j < (lines.size()); j++ )
        {
            if(j != i)
            {
                point21 = Point(lines[j][0], lines[j][1]);
                point22 = Point(lines[j][2], lines[j][3]);
                double gradient2 = VectorOfLines::calcGradient(point21, point22);
                if(norm(gradient1 - gradient2) < 0.15)
                {
                    middleLines.push_back(Vec4i((point11.x+point21.x)/2, (point11.y+point21.y)/2, (point12.x+point22.x)/2, (point12.y+point22.y)/2));   
                }
            }
        }
    
    }
    return middleLines;
}

int VectorOfLines::calcAngle(int deltaX, int deltaY){

        int refAngle = 0;

        if(deltaX > 10E-12){
        refAngle = atan(deltaY/deltaX) * 180/PI;

        // convert from img coordinates to regbot coordinates
        refAngle = -(refAngle);

        if (refAngle > 90)
            refAngle = refAngle - 180;

        cout << refAngle << "\n";
        }else{
            refAngle = -90;
        }
        return refAngle;
   
}