Problema con potenziale

Buona sera, io devo fare un programma in c++ usando delle classi. L'obiettivo e far muovere un “robot” da una posizione iniziale ad una finale calcolando il potenziale attrattivo. Il problema è che il mio funziona solo se i dati sono in diagonale altrimenti no. Allego file Robot.cpp, oltre a questo ho creato un'altra classe Cordinate.

#include "Robot.h"
#include "Cordinate.h"

#include <iostream>
#include <cmath>
#include <vector>
#include <iterator>
using std::vector;
#include <array>
using std::array;
using std::min;
using std::max;
using std::cout;
using std::endl;
#include <algorithm>
using std::min_element;
using std::distance;
#define k 10

Robot::Robot()
    : p_robot_{0, 0}, goal_{0, 0}, passo_{0}, ccvicine_{}, pcelle_{0, 0, 0, 0, 0, 0, 0, 0}, track_{}
{}

Robot::Robot(Cordinate p_robot, Cordinate goal, int passo)
    : p_robot_(p_robot), goal_(goal), passo_(passo)

{}
void Robot::sccvicine(){
    ccvicine_[0] = Cordinate(p_robot().x() + passo(), p_robot().y() + passo());         //(x+1, y+1)
    ccvicine_[1] = Cordinate(p_robot().x(), p_robot().y() + passo());                   //(x. y+1)
    ccvicine_[2] = Cordinate(p_robot().x() - passo(), p_robot().y() + passo());         //(x-1, y+1)
    ccvicine_[3] = Cordinate(p_robot().x() - passo(), p_robot().y());                   //(x-1, y)
    ccvicine_[4] = Cordinate(p_robot().x() - passo(), p_robot().y() - passo());         //(x-1, y-1)
    ccvicine_[5] = Cordinate(p_robot().x(), p_robot().y() - passo());                   //(x, y-1)
    ccvicine_[6] = Cordinate(p_robot().x() + passo(), p_robot().y() - passo());         //(x+1, y-1)
    ccvicine_[7] = Cordinate(p_robot().x() + passo(), p_robot().y());                   //(x+1, y)
   
}  
         

double Robot::distanza(Cordinate& punto_uno, Cordinate& punto_due){
    double xmin = min (punto_uno.x(), punto_due.x());
    double ymin = min (punto_uno.y(), punto_due.y());
    double xmax = max (punto_uno.x(), punto_due.x());
    double ymax = max (punto_uno.y(), punto_due.y());

    double d = sqrt ((xmax - xmin)*(xmax - xmin) + (ymax - ymin)*(ymax - ymin));
    return d;
}

double Robot::F_att(Cordinate& punto_uno, Cordinate& punto_due){
    double Fatt =(k * distanza(punto_uno, punto_due));
    return Fatt; 
}

void Robot::potenzialecc(){
    for (size_t i=0; i < ccvicine_.size(); ++i)
        pcelle_[i] = F_att(ccvicine_.at(i), goal_);
    
}

void Robot::spostamento(){

     while ((p_robot_.x() != goal_.x()) && (p_robot_.y() != goal_.y())){
        distanza(p_robot_, goal_);
        F_att(p_robot_, goal_);
        sccvicine();
        potenzialecc();
        double *mpotenziale = min_element (std::begin(pcelle_), std::end(pcelle_));      // va solo in diagonale                               
        p_robot_ = ccvicine_[pcelle_[*mpotenziale]];
        track_.push_back(p_robot_);
        cout << track_.back() << endl;
     }
     }