This piece of code was written for a robot that needs to solve a maze autonomously. Using its laser range finder data, the detected wall points are given a repelling force as function of the distance. Then, the desired target is given an attracting force. The code finally returns a net force vector, in whose direction the robot should start driv…

potentialfield.cpp

#include "potentialfield.h"

// CONSTRUCTOR OF POTENTIALFIELD
PotentialField::PotentialField()
{
    rMult = 15.0;
    meanNumber = 8;
    maxPointDiff = 0.05;
}

// DESTRUCTOR OF POTENTIALFIELD
PotentialField::~PotentialField()
{

}
// ///////////////////////////////////////////////////////////////////////////////////
// POTENTIAL FIELD ENVIRONMENT. SET UP AS FOLLOWS;                                  //
// - FILL POTENTIALFIELD WITH WALL POINTS                                           //
// - CALCULATE WALL FORCES, DETERMINE NET FORCE, LOCATE GAPS AND STORE THEM         //
// - FUNCTION DECLARATION                                                           //
// ///////////////////////////////////////////////////////////////////////////////////



// FILL THE POTENTIALFIELD WITH WALL POINTS IN CASE THE CURRENT POTENTIALFIELD IS EMPTY
void PotentialField::fillWithWalls(std::vector<Point*>* walls)
{
    wallPoints.clear();

    std::vector<Point*>::iterator it = walls->begin();
    Point lastPoint = **it;
    it++;
    int i = 0;
    Point p;

// make everything empty in loop over points
    while(it!=walls->end())
    {
        if(lastPoint.getDistanceTo(**it)<maxPointDiff)
        {
            p.addPoint((*it)->getX()/meanNumber,(*it)->getY()/meanNumber);
            lastPoint =**it;
            it++;
            i++;
            if(i == meanNumber)
            {
                i = 0;
                wallPoints.push_back(p);
                p = Point();
            }
        }
        else
        {
            i = 0;
            p = Point();
            lastPoint = **it;
            it++;

        }

    }
}

// UPDATES THE CURRENT POTENTIALFIELD
void PotentialField::update()
{
    std::vector<Point>  gapsVector;
    std::vector<Point>  gapsNormalVector;
    wallForce = Point();            // Points have two entries, just as a vector. Therefore the Point class can be used here.
    wallVectors.clear();
    gapsVector.clear();
    gapsNormalVector.clear();

    if (!wallPoints.empty())
    {
        std::vector<Point>::iterator it = wallPoints.begin();

        Point lastWallPoint = *it;
        Point lastWallVector;
        it++;

        while(it!=wallPoints.end())
        {
            float r = it->getRadius();
            float F;
            if(r!= 0.15)
                F = 1/pow(rMult*(r-0.15),5);            // Function to calculate repelling force
            else
                F = 1.0e8;                              // To prevent division by zero. Nevertheless, we need a high repelling force here.
            wallForce.addPoint(-F*cos(it->getAngle()),-F*sin(it->getAngle()));


            Point wallVector = it->getDifferenceTo(lastWallPoint);
            wallVectors.push_back(wallVector);


            if(isGapGreat(lastWallPoint,wallVector))   // function to determine whether a gap in the wall vector is actually a corridor
            {
                Point t = lastWallPoint;
                t.addPoint(wallVector.getX()/2.0,wallVector.getY()/2.0);  // locate center of gap and store it in gapsVectors
                gapsVector.push_back(t);
                Point n(Polar(1.0, wallVector.getAngle() + MATHSUPPORT::degToRad(90)));
                gapsNormalVector.push_back(n);

            }

            lastWallVector = wallVector;
            lastWallPoint = *it;
            it++;
        }
        loopGapsNormalVector.push_back(gapsNormalVector);
        loopGapsVector.push_back(gapsVector);


        // Update minGaps and maxGaps, containing the number of gaps detected (maxGaps), and the number of robustly detected gaps (minGaps) over multiple loops.
        if(minGaps > gapsVector.size())
            minGaps = gapsVector.size();

        if(maxGaps < gapsVector.size())
            maxGaps = gapsVector.size();

    }
}

// /////////////////////////////////////////
// IMPORTANT FUNCTIONS                    //
// /////////////////////////////////////////

// SETS TARGET AS POINT IN THE POTENTIALFIELD
void PotentialField::setTarget(Point target)
{
    this->target = target;
    targetForce  = Point(Polar(pow(target.getRadius(),2),target.getAngle()));
}


// RETURNS PICO FORCE (COMBINATION OF WALLFORCE AND TARGETFORCE)
Point PotentialField::getForceVector()
{
    Point p = wallForce;
    p.addPoint(targetForce);
    return p;
}


// RETURNS TARGETFORCE
Point PotentialField::getTargetForceVector()
{
    return targetForce;
}


// RETURNS WALLFORCE
Point PotentialField::getWallForceVector()
{
    return wallForce;
}


// RETURNS INTERSECTION POINT BETWEEN TWO GAPS
Point PotentialField::getGapIntersection(int gap1, int gap2)
{
    std::vector<Point> gVector = loopGapsVector.back();
    std::vector<Point> nVector = loopGapsNormalVector.back();

    if(gap1 < gVector.size() && gap2 < gVector.size())
    {
        Point g1 = gVector[gap1];
        Point g2 = gVector[gap2];
        Point n1 = nVector[gap1];
        Point n2 = nVector[gap2];

        float b,g,f;
        f = (g2.getX() - g1.getX())/n1.getX();
        g = n2.getX()/n1.getX();
        b = (g2.getY() - g1.getY() - f*n1.getY())/(g*n1.getY() - n2.getY());

        float xc,yc;
        xc = g2.getX() + b*n2.getX();
        yc = g2.getY() + b*n2.getY();

        return Point(xc,yc);

    }
    else
    {
        return Point(1.0,0.0);
    }



}

// FUNCTION TO DETERMINE WHETHER A GAP IS BIG ENOUGH TO BE A CORRIDOR
bool PotentialField::isGapGreat(Point wallPoint, Point wallVector)
{

    float gap = wallVector.getRadius();
    Point midPoint = wallPoint;
    midPoint.addPoint(wallVector.getX()/2.0,wallVector.getY()/2.0);

    float distance = midPoint.getRadius();

    float minGap = MATHSUPPORT::nonNeg(distance-0.7)/4.0 + 0.3;

    if(gap>minGap)
        return true;
    else
        return false;

}