C++ - Pathfinding corner problems - c++

Question

So i'm trying to make characters walk around a tile map, using the following code. tilePath returns a vector of tiles in the path to their target, then their code makes them advance to the next tile in the vector. The problem is that when they get to a corner, they try to move around it too early and end up getting stuck on the wall.
The white squares are walkable tiles, the gray is a wall, and the red is a character. If the character gets to the corner walkable tile, it tries to move around the wall too soon, thus getting blocked by it. They usually end up making it around, but its very noticeable that they've gotten stuck on the wall for a bit.
std::vector<Tile*> TileMap::tilePath(Tile *p_start, Tile *p_end)
{
std::vector<Tile*> path;
std::vector<Tile*> open;
std::map<uint64_t, Tile*> closed;
p_start->previousTile = p_start;
p_start->g = 0;
p_start->h = 0;
p_start->f = 0;
float g, h, f, cost;
Tile* current = p_start;
Tile* previous = nullptr;
while(current != p_end) {
current->m_sprite.setColor(sf::Color(255,255,255,255));
for(auto neighbor : getNeighbors(current))
{
if(neighbor == current) continue;
if(!(neighbor->m_walkable)) continue;
previous = current;
if(tileTrace(current->previousTile, neighbor)) {
cost = manhattan(current->previousTile, neighbor);
g = current->previousTile->g + cost;
h = manhattan(neighbor, p_end);
f = g + h;
previous = current->previousTile;
} else {
cost = ((current->m_coordinates.x != neighbor->m_coordinates.x) && (current->m_coordinates.y != neighbor->m_coordinates.y)) ? 1.4 : 1;
g = current->g + cost;
h = manhattan(neighbor, p_end);
f = g + h;
}
if(std::find(open.begin(), open.end(), neighbor) != open.end() ||
closed.find(neighbor->key()) != closed.end()) {
if(neighbor->f > f) {
neighbor->f = f;
neighbor->g = g;
neighbor->h = h;
neighbor->previousTile = previous;
}
} else {
neighbor->f = f;
neighbor->g = g;
neighbor->h = h;
neighbor->previousTile = previous;
for(auto i = open.begin(); i != open.end(); i++)
{
if(neighbor->f < (*i)->f) {
open.insert(i, neighbor);
break;
}
}
open.push_back(neighbor);
}
}
closed[current->key()] = current;
if(open.size() == 0) {
return std::vector<Tile*>();
}
current = open.front();
open.erase(open.begin());
}
current = p_end;
path.insert(path.begin(), current);
while(current->previousTile != p_start)
{
current->m_sprite.setColor(sf::Color(255,0,0,255));
path.insert(path.begin(), current->previousTile);
current = current->previousTile;
}
return path;
}
bool TileMap::tileTrace(Tile* p_start, Tile* p_end)
{
int sx = p_start->m_coordinates.x;
int sy = p_start->m_coordinates.y;
int ex = p_end->m_coordinates.x;
int ey = p_end->m_coordinates.y;
int dx = fabsf(ex - sx);
int dy = fabsf(ey - sy);
int _x = sx, _y = sy;
int x_inc = (ex > sx) ? 1 : -1;
int y_inc = (ey > sy) ? 1 : -1;
int error = dx - dy;
for(int n = dx + dy; n > 0; n--) {
Tile& t = m_tiles[_y * m_width + _x];
if(!(t.m_walkable)) return false;
if(error < 0) {
_y += y_inc;
error += dx;
} else {
_x += x_inc;
error -= dy;
}
}
return true;
}
What can I do to make this a little smoother? I'd rather the characters not bounce off of the walls either.
Update:
I've added this to see if the character is moving towards or away from the tile it's "on". Granted it's more effective in getting the character around corners, but it sometimes gives a little strange movement.
sf::Vector2f d = start->m_position - p_object->m_position;
float dot = p_object->m_velocity.x * d.x + p_object->m_velocity.y * d.y;
if(dot > 0) {
path.insert(path.begin(), start);
}