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

#include "utils.h"

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()
{
}

double LFRVector::angle(const LFRVector& other) const
{
    return acos(dot(other)/(this->norm()*other.norm())) * (180.0/M_PI);
}

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()
{
}

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

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

LFRLine::~LFRLine()
{
}



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

        int refAngle = 0;

        if(deltaX > 10E-12){
        refAngle = (int)((atan(deltaY/deltaX) * 180.0/M_PI) + 0.5 - (refAngle<0));  //Here 0.5 (or -0.5) is added to round a float number to int right

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

        if (refAngle > 90)
            refAngle = refAngle - 180; 
        }else{
            refAngle = -90;
        }
        return refAngle;
   
}