Simple AI movement algorithm not working correctly

Question

I'm working on what should be a very simple movement algorithm that takes in a vector of D3DXVECTOR3s and moves the AI to each point. The problem is that if I pass it more than one point, the AI seems to get stuck at a point equal to the average of the points.

The points are (x, z):
10, 10
10, 20
30, 30
60, 20

maxSpeed is 10 just for testing.

void Obj::MoveToLocation(D3DXVECTOR3 newLocation, float deltaTime)
{
    D3DXVECTOR3 directionToTarget = newLocation - location;
    D3DXVec3Normalize(&directionToTarget, &directionToTarget);

    location += maxSpeed * directionToTarget * deltaTime;   
}

void Obj::Patrol(std::vector<D3DXVECTOR3> locations, float deltaTime)
{
    hasArrived = false;

    for (int i = 0; i < locations.size(); ++i)
    {
        if (!hasArrived)
            MoveToLocation(locations[i], deltaTime);

        if ((location.x <= locations[i].x + radius.x) && (location.x >= locations[i].x - radius.x) &&
            (location.z <= locations[i].z + radius.z) && (location.z >= locations[i].z - radius.z))
        {
            hasArrived = true;
        }
    }
}

I'm just looking for some tips on how I can get this working. I'm at a loss right now even though this seems like an extremely simple problem.

Answer 1

void Obj::MoveToLocation(D3DXVECTOR3 newLocation, float deltaTime)
{
    D3DXVECTOR3 directionToTarget = newLocation - location;

    if (D3DXVec3Length(&directionToTarget) <= maxSpeed * deltaTime)
    {
        // If this step would take us past the destination, just stop there
        // instead.
        location = newLocation;
    }
    else
    {
        D3DXVec3Normalize(&directionToTarget, &directionToTarget);

        location += maxSpeed * directionToTarget * deltaTime;   
    }
}

// Call this once to set the patrol route
void Obj::Patrol(std::vector<D3DXVECTOR3> locations)
{
    patrolRoute = locations;
    patrolIndex = 0; // Maybe pick the closest point instead?
}

// Call this each time the object should move (once per frame/turn)
void Obj::UpdatePatrol(float deltaTime)
{
    if (patrolRoute == NULL || patrolRoute.empty())
    {
        return;
    }

    if (patrolIndex >= patrolRoute.size())
    {
        // Start again from the beginning
        patrolIndex -= patrolRoute.size();
    }

    // Move towards the next location
    D3DXVECTOR3 nextLocation = patrolRoute[patrolIndex];
    MoveToLocation(nextLocation, deltaTime);

    float dx = location.x - nextLocation.x;
    float dz = location.z - nextLocation.z;

    if ((dx <= radius.x) && (dx >= -radius.x) &&
        (dz <= radius.z) && (dz >= -radius.z))
    {
        // We have reached it. Select the next destionation.
        patrolIndex++;
    }
}