/* int Dstar::computeShortestPath()
* --------------------------
* As per [S. Koenig, 2002] except for 2 main modifications:
* 1. We stop planning after a number of steps, 'maxsteps' we do this
* because this algorithm can plan forever if the start is
* surrounded by obstacles.
* 2. We lazily remove states from the open list so we never have to
* iterate through it.
*/
int Dstar::computeShortestPath() {
list<state> s;
list<state>::iterator i;
if (openList.empty()) return 1;
int k=0;
while ((!openList.empty()) &&
(openList.top() < (s_start = calculateKey(s_start))) ||
(getRHS(s_start) != getG(s_start))) {
if (k++ > maxSteps) {
fprintf(stderr, "At maxsteps\n");
return -1;
}
state u;
bool test = (getRHS(s_start) != getG(s_start));
// lazy remove
while(1) {
if (openList.empty()) return 1;
u = openList.top();
openList.pop();
if (!isValid(u)) continue;
if (!(u < s_start) && (!test)) return 2;
break;
}
ds_oh::iterator cur = openHash.find(u);
openHash.erase(cur);
state k_old = u;
if (k_old < calculateKey(u)) { // u is out of date
insert(u);
} else if (getG(u) > getRHS(u)) { // needs update (got better)
setG(u,getRHS(u));
getPred(u,s);
for (i=s.begin();i != s.end(); i++) {
updateVertex(*i);
}
} else { // g <= rhs, state has got worse
setG(u,INFINITY);
getPred(u,s);
for (i=s.begin();i != s.end(); i++) {
updateVertex(*i);
}
updateVertex(u);
}
}
return 0;
}
/* int Dstar::computeShortestPath()
* --------------------------
* As per [S. Koenig, 2002] except for 2 main modifications:
* 1. We stop planning after a number of steps, 'maxsteps' we do this
* because this algorithm can plan forever if the start is
* surrounded by obstacles.
* 2. We lazily remove states from the open list so we never have to
* iterate through it.
*/
int Dstar::computeShortestPath() {
list<state> s;
list<state>::iterator i;
if (openList.empty()) return 1;
int k=0;
while ((!openList.empty()) &&
(openList.top() < (calculateKey(s_start))) ||
(!isConsistent(s_start))) {
if (k++ > maxSteps) {
fprintf(stderr, "At maxsteps\n");
return -1;
}
state u;
// check consistency (one of the loop conditions)
bool test = isConsistent(s_start);
//(getRHS(s_start) != getG(s_start));
// lazy remove
while(1) {
if (openList.empty()) return 1; // checks outer loop condition #1
u = openList.top();
if (!queuePop()) continue;
if (!(u < s_start) && test) return 2; // checks outer loop conditions #2,3 still hold
break;
}
state k_old = u;
if (k_old < calculateKey(u)) { // u is out of date
insert(u); // u has been removed already, reinsert into pq with new key
} else if (getG(u) > getRHS(u)) { // needs update (got better)
setG(u,getRHS(u));
getPred(u,s);
for (i=s.begin();i != s.end(); i++) {
updateVertex(*i);
}
} else { // g <= rhs, state has got worse
setG(u,INFINITY);
getPred(u,s);
for (i=s.begin();i != s.end(); i++) {
updateVertex(*i);
}
updateVertex(u);
}
}
return 0;
}
static bool
exportFaceGroupToShape(
shape_t& shape,
std::map<vertex_index, unsigned int> vertexCache,
const std::vector<float> &in_positions,
const std::vector<float> &in_normals,
const std::vector<float> &in_texcoords,
const std::vector<std::vector<vertex_index> >& faceGroup,
const int material_id,
const std::string &name,
bool clearCache)
{
if (faceGroup.empty()) {
return false;
}
size_t offset;
offset = shape.mesh.indices.size();
// Flatten vertices and indices
for (size_t i = 0; i < faceGroup.size(); i++) {
const std::vector<vertex_index>& face = faceGroup[i];
vertex_index i0 = face[0];
vertex_index i1(-1);
vertex_index i2 = face[1];
size_t npolys = face.size();
// Polygon -> triangle fan conversion
for (size_t k = 2; k < npolys; k++) {
i1 = i2;
i2 = face[k];
unsigned int v0 = updateVertex(vertexCache, shape.mesh.positions, shape.mesh.normals, shape.mesh.texcoords, in_positions, in_normals, in_texcoords, i0);
unsigned int v1 = updateVertex(vertexCache, shape.mesh.positions, shape.mesh.normals, shape.mesh.texcoords, in_positions, in_normals, in_texcoords, i1);
unsigned int v2 = updateVertex(vertexCache, shape.mesh.positions, shape.mesh.normals, shape.mesh.texcoords, in_positions, in_normals, in_texcoords, i2);
shape.mesh.indices.push_back(v0);
shape.mesh.indices.push_back(v1);
shape.mesh.indices.push_back(v2);
shape.mesh.material_ids.push_back(material_id);
}
}
shape.name = name;
if (clearCache)
vertexCache.clear();
return true;
}
static bool exportFaceGroupToShape( std::map<vertex_index, ssize_t>& vertexCache, ObjLoader::shapes_t& shapes, const std::vector<float> &in_positions,
const std::vector<float> &in_normals, const std::vector<float> &in_texcoords, const std::vector<std::vector<vertex_index> >& faceGroup,
const ObjLoader::material_t &material, const std::string &name)
{
if (faceGroup.empty())
{
return false;
}
// Flattened version of vertex data
std::vector<float>& positions = shapes.positions;
std::vector<float>& normals = shapes.normals;
std::vector<float>& texcoords = shapes.texcoords;
std::vector<unsigned short> indices;
// Flatten vertices and indices
for (size_t i = 0; i < faceGroup.size(); i++)
{
const std::vector<vertex_index>& face = faceGroup[i];
vertex_index i0 = face[0];
vertex_index i1(-1);
vertex_index i2 = face[1];
size_t npolys = face.size();
// Polygon -> triangle fan conversion
for (size_t k = 2; k < npolys; k++)
{
i1 = i2;
i2 = face[k];
unsigned short v0 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i0);
unsigned short v1 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i1);
unsigned short v2 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i2);
indices.push_back(v0);
indices.push_back(v1);
indices.push_back(v2);
}
}
ObjLoader::shape_t shape;
shape.name = name;
shape.material = material;
shape.mesh.indices.swap(indices);
shapes.shapes.push_back(shape);
return true;
}
void Field::updateAllVertex(bool endgame, bool force) {
bool was_debugging = is_debugging;
is_debugging = false;
unsigned int no_updates = 0;
for (uint i = 0; i < field.size(); ++i) {
if (field[i].update_vertex || force) {
updateVertex(i,endgame);
field[i].update_vertex = false;
no_updates++;
}
}
is_debugging = was_debugging;
if (is_debugging && no_updates > 0) {
if (no_updates == field.size())
cout << "All";
else
cout << no_updates;
cout << " vertex(es) updated (";
if (force)
cout << "forced";
else
cout << "not forced";
cout << ")\n";
}
}
//--------------------------------------------------------------
void ofxFatLine::updateMesh(){
meshVertices.clear();
meshColors.clear();
meshIndices.clear();
for (int i =0; i<getVertices().size(); i++) {
updateVertex(i);
/*
ofVec3f a (getVertices()[i-1] - getVertices()[i]);
ofVec3f b (getVertices()[i+1] - getVertices()[i]);
float angle = a.angle(b);
ofVec3f p = getMidVector(a, b);
bool flip = !sameSideOfLine(p, flippepMidVectors.back(), getVertices()[i-1], getVertices()[i]);
float cs = cos(DEG_TO_RAD * (90 - angle*0.5f));
pushNewVertex(getVertices()[i], p, i, cs, flip);
//*/
}
mesh.clear();
mesh.addVertices(meshVertices);
mesh.addColors(meshColors);
mesh.addIndices(meshIndices);
// updateMeshIndices();
}
/* void Dstar::updateCell(int x, int y, double val)
* --------------------------
* As per [S. Koenig, 2002]
*/
void Dstar::updateCell(int x, int y, double val) {
state u;
u.x = x;
u.y = y;
if ((u == s_start) || (u == s_goal)) return;
makeNewCell(u);
cellHash[u].cost = val;
updateVertex(u);
}
/* void Dstar::updateCell(int x, int y, double val)
* --------------------------
* As per [S. Koenig, 2002]
*/
void Dstar::updateCell(int x, int y, double val) {
state u;
u.x = x;
u.y = y;
if ((u == s_start) || (u == s_goal)) return;
// if the value is still the same, don't need to do anything
ds_ch::iterator cur = cellHash.find(u);
if ((cur != cellHash.end()) && (near(cur->second.cost,val))) return;
makeNewCell(u);
cellHash[u].cost = val;
updateVertex(u);
}
void Sprite::scale(float factorX, float factorY)
{
Transformable::scale(factorX, factorY);
updateVertex();
}
void TriangleMesh::updateAllVertices() {
VertexIterator vitr = vertexBegin();
VertexIterator end = vertexEnd();
for(; vitr != end; ++vitr)
updateVertex(**vitr);
}
static bool
exportFaceGroupToShape(
shape_t& shape,
const std::vector<float> &in_positions,
const std::vector<float> &in_normals,
const std::vector<float> &in_texcoords,
const std::vector<std::vector<vertex_index> >& faceGroup,
const material_t &material,
const std::string &name,
const bool is_material_seted)
{
if (faceGroup.empty()) {
return false;
}
// Flattened version of vertex data
std::vector<float> positions;
std::vector<float> normals;
std::vector<float> texcoords;
std::map<vertex_index, unsigned int> vertexCache;
std::vector<unsigned int> indices;
// Flatten vertices and indices
for (size_t i = 0; i < faceGroup.size(); i++) {
const std::vector<vertex_index>& face = faceGroup[i];
vertex_index i0 = face[0];
vertex_index i1(-1);
vertex_index i2 = face[1];
size_t npolys = face.size();
// Polygon -> triangle fan conversion
for (size_t k = 2; k < npolys; k++) {
i1 = i2;
i2 = face[k];
unsigned int v0 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i0);
unsigned int v1 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i1);
unsigned int v2 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i2);
indices.push_back(v0);
indices.push_back(v1);
indices.push_back(v2);
}
}
//
// Construct shape.
//
shape.name = name;
shape.mesh.positions.swap(positions);
shape.mesh.normals.swap(normals);
shape.mesh.texcoords.swap(texcoords);
shape.mesh.indices.swap(indices);
if(is_material_seted) {
shape.material = material;
} else {
InitMaterial(shape.material);
shape.material.diffuse[0] = 1.f;
shape.material.diffuse[1] = 1.f;
shape.material.diffuse[2] = 1.f;
}
return true;
}
void Sprite::setPosition(float x, float y)
{
Transformable::setPosition(x, y);
updateVertex();
}
void Field::updateVertex(unsigned int x,unsigned int y, bool endgame) {
updateVertex(y*size.y+x, endgame);
}
void Sprite::setPosition(const sf::Vector2f &position)
{
Transformable::setPosition(position);
updateVertex();
}
/*
This method does the same thing as the pseudo-code's updateVertex(),
except for grids instead of graphs.
Pathfinding algorimths tend to be demonstraited with a graph rather than a grid,
in order to update the cost between two tiles we must update both the tile and its neighbour.
*/
void GridWorld::updateCost(unsigned int x, unsigned int y, double newCost){
static int count = 1;
count++;
Tile* tile = getTileAt(x, y);
printf("Updating <%d, %d> from %2.0lf to %2.0lf - Update: %d\n", x, y, tile->cost, newCost, count);
km += calculateH(previous);
previous = goal;
//I am aware that the following code below could be refactored by 50%
//since it's repeating itself with only a few changes
double oldCost = tile->cost;
double oldCostToTile, newCostToTile;
//Update CURRENT by finding its new minimum RHS-value from NEIGHBOURS
std::vector<Tile*> neighbours(getNeighbours(tile));
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
if (oldCostToTile > newCostToTile){
if (tile != start && tile->rhs > neighbours[i]->g + newCostToTile){
tile->successor = neighbours[i];
tile->rhs = neighbours[i]->g + newCostToTile;
}
}
else if (tile != start && tile->successor == neighbours[i]){
TilePair minSucc(getMinSuccessor(tile));
tile->rhs = minSucc.second;
tile->successor = (tile->rhs == PF_INFINITY ? 0 : minSucc.first);
}
}
updateVertex(tile);
//Update all NEIGHBOURING cells by finding their new min RHS-values from CURRENT
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
if (oldCostToTile > newCostToTile){
if (neighbours[i] != start && neighbours[i]->rhs > tile->g + newCostToTile){
neighbours[i]->successor = tile;
neighbours[i]->rhs = tile->g + newCostToTile;
updateVertex(neighbours[i]);
}
}
else if (neighbours[i] != start && neighbours[i]->successor == tile){
TilePair minSucc(getMinSuccessor(neighbours[i]));
neighbours[i]->rhs = minSucc.second;
neighbours[i]->successor = (neighbours[i]->rhs == PF_INFINITY ? 0 : minSucc.first);
updateVertex(neighbours[i]);
}
}
computeShortestPath();
}
vector<Grid> Planner::dStarLite(Map &map)
{
// this is the implementation of the D star lite algorithm
vector<Grid> wayPoint;
// Initialization
vector<vector<int> > g(map._rows,vector<int>(map._cols,map._rows*map._cols+1));
vector<vector<int> > rhs(map._rows,vector<int>(map._cols,map._rows*map._cols+1));
km =0;
rhs[map.goal.y][map.goal.x] =0;
Key n = calculateKey(map.goal,g,rhs,map.start,0);
U.push_back({map.goal,n});
// Computer current shortest path
// Update the states of the map if the first element in the priority list can be updated or the goal is not reached
while(U.front().key<calculateKey(map.goal,g,rhs,map.start,0)||(rhs[map.start.y][map.start.x]!=g[map.start.y][map.start.x]))
{
// take the key value and postion of the first element in the priority queue
Key kold = U.front().key;
Grid u = U.front().point;
U.pop_front();
Key knew = calculateKey(u,g,rhs,map.start,km); // calculate the new key value
// update map if old value is different from the new value
if (kold<knew)
{
// if the new key is larger, the cost of the edge of the grid might be change
// the current grid should be updated and re-expanded
// insert it in the priority queue
auto pt =find_if(U.begin(),U.end(),[knew](Uelem &u){return u.key<knew;});
U.insert(pt,{u,knew});
}
else if (g[u.y][u.x]>rhs[u.y][u.x])
{
// if the grid is overconstraint, there are new shorter paths detected
g[u.y][u.x] = rhs[u.y][u.x];
// update all its neighbour value
vector<Grid> neightbour = findNeighbour(map, u, 8);
for(auto &n:neightbour)
{
updateVertex(U,n,g,rhs,map,km);
}
}
else
{
// if the grid is underconstraint, the grid it self and its neightbour should all be updated
g[u.y][u.x] = map._cols*map._rows;
vector<Grid> neightbour = findNeighbour(map, u, 8);
for(auto &n:neightbour)
{
updateVertex(U,n,g,rhs,map,km);
}
updateVertex(U,u,g,rhs,map,km);
}
}
return wayPoint;
}
/*
This method does the same thing as the pseudo-code's updateVertex(),
except for grids instead of graphs.
Pathfinding algorimths tend to be demonstraited with a graph rather than a grid,
in order to update the cost between two tiles we must update both the tile and its neighbour.
*/
void GridWorld::updateCost(unsigned int x, unsigned int y, double newCost){
static int count = 1;
count++;
Tile* tile = getTileAt(x, y);
printf("Updating <%d, %d> from %2.0lf to %2.0lf - Update: %d\n", x, y, tile->cost, newCost, count);
km += calculateH(previous);
previous = start;
//I am aware that the following code below could be refactored by 50%
//since it's repeating itself with only a few changes
double oldCost = tile->cost;
double oldCostToTile, newCostToTile;
double currentRHS, otherG;
//Update CURRENT by finding its new minimum RHS-value from NEIGHBOURS
std::vector<Tile*> neighbours(getNeighbours(tile));
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
currentRHS = tile->rhs;
otherG = neighbours[i]->g;
if (oldCostToTile > newCostToTile){
if (tile != goal){
tile->rhs = std::min(currentRHS, (newCostToTile + otherG));
}
}
else if (currentRHS == (oldCostToTile + otherG)){
if (tile != goal){
tile->rhs = getMinSuccessor(tile).second;
}
}
}
updateVertex(tile);
//Update all NEIGHBOURING cells by finding their new min RHS-values from CURRENT
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
currentRHS = neighbours[i]->rhs;
otherG = tile->g;
if (oldCostToTile > newCostToTile){
if (neighbours[i] != goal){
neighbours[i]->rhs = std::min(currentRHS, (newCostToTile + otherG));
}
}
else if (currentRHS == (oldCostToTile + otherG)){
if (neighbours[i] != goal){
neighbours[i]->rhs = getMinSuccessor(neighbours[i]).second;
}
}
updateVertex(neighbours[i]);
}
computeShortestPath();
}
bool GridWorld::computeShortestPath(){
if (open.empty()){
std::cout << "ERROR: No tiles to expand on... can't do anything" << std::endl;
return false;
}
while (!open.empty() && (compareKeys(open.front()->key, goal->key = calculateKey(goal)) || goal->rhs != goal->g)){
Tile* current = open.front();
//Notice that CURRENT wasn't pop/removed yet
//std::cout << "Expanding:";
//current->info();
//std::cout << std::endl;
KeyPair k_new = calculateKey(current);
if (compareKeys(current->key, k_new)){
//Tiles under this branch will have to be updated as incremental search has happened
current->key = k_new;
make_heap(open.begin(), open.end(), GridWorld::compareTiles);
}
else if (current->g > current->rhs){
//Majority of the tiles will fall under this conditional branch as
//it undergoes normal A* pathfinding
current->g = current->rhs;
open.erase(open.begin());
make_heap(open.begin(), open.end(), GridWorld::compareTiles);
current->isOpen = false;
std::vector<Tile*> neighbours(getNeighbours(current));
for (int i = 0; i < neighbours.size(); i++){
if (neighbours[i] != start && neighbours[i]->rhs > current->g + calculateC(current, neighbours[i])){
neighbours[i]->successor = current;
neighbours[i]->rhs = current->g + calculateC(current, neighbours[i]);
updateVertex(neighbours[i]);
}
}
}
else {
//Tiles under this branch will need to be updated during incremental search
current->g = PF_INFINITY;
//Update CURRENT
if (current != start && current->successor == current){
TilePair minSucc(getMinSuccessor(current));
current->rhs = minSucc.second;
current->successor = current->rhs == PF_INFINITY ? 0 : minSucc.first;
}
updateVertex(current);
//Update NEIGHBOURS
std::vector<Tile*> neighbours(getNeighbours(current));
for (int i = 0; i < neighbours.size(); i++){
if (neighbours[i] != start && neighbours[i]->successor == current){
TilePair minSucc(getMinSuccessor(neighbours[i]));
neighbours[i]->rhs = minSucc.second;
neighbours[i]->successor = neighbours[i]->rhs == PF_INFINITY ? 0 : minSucc.first;
}
updateVertex(neighbours[i]);
}
}
//Uncomment this to see CURRENT'S new values
//std::cout << "Expanded:";
//current->info();
//std::cout << std::endl;
}
return true;
}
void SceneE::update(){
updateVertex();
}
void Sprite::rotate(float angle)
{
Transformable::rotate(angle);
updateVertex();
}
void Sprite::move(const sf::Vector2f &offset)
{
Transformable::move(offset);
updateVertex();
}
void Sprite::move(float offsetX, float offsetY)
{
Transformable::move(offsetX, offsetY);
updateVertex();
}
void Sprite::setRotation(float angle)
{
Transformable::setRotation(angle);
updateVertex();
}
void Sprite::scale(const sf::Vector2f &factor)
{
Transformable::scale(factor);
updateVertex();
}
bool GridWorld::computeShortestPath(){
if (open.empty()){
std::cout << "ERROR: No tiles to expand on... can't do anything" << std::endl;
return false;
}
double currentRHS, otherG, previousG;
while (!open.empty() && (compareKeys(open.front()->key, start->key = calculateKey(start)) || start->rhs != start->g)){
Tile* current = open.front();
//Notice that CURRENT wasn't pop/removed yet..
KeyPair k_old = current->key;
KeyPair k_new = calculateKey(current);
currentRHS = current->rhs;
otherG = current->g;
/*std::cout << "Expanding:";
current->info();
std::cout << std::endl;*/
if (compareKeys(k_old, k_new)){
//This branch updates tile that were already in the OPEN list originally
//This branch tends to execute AFTER the else branch
current->key = k_new;
make_heap(open.begin(), open.end(), GridWorld::compareTiles);
}
else if (otherG > currentRHS){
//Majority of the execution will fall under this conditional branch as
//it is undergoing normal A* pathfinding
current->g = current->rhs;
otherG = currentRHS;
open.erase(open.begin());
make_heap(open.begin(), open.end(), GridWorld::compareTiles);
current->isOpen = false;
std::vector<Tile*> neighbours(getNeighbours(current));
for (int i = 0; i < neighbours.size(); i++){
if (neighbours[i] != 0){
if (neighbours[i] != goal){
neighbours[i]->rhs = std::min(neighbours[i]->rhs, calculateC(current, neighbours[i]) + otherG);
}
updateVertex(neighbours[i]);
}
}
}
else {
//Execution of this branch updates the tile during incremental search
previousG = otherG;
current->g = PF_INFINITY;
//Update CURRENT'S RHS
if (current != goal){
current->rhs = getMinSuccessor(current).second;
}
updateVertex(current);
//Update NEIGHBOUR'S RHS to their minimum successor
std::vector<Tile*> neighbours(getNeighbours(current));
for (int i = 0; i < neighbours.size(); i++){
if (neighbours[i] != 0){
if (neighbours[i]->rhs == (calculateC(current, neighbours[i]) + previousG) && neighbours[i] != goal){
neighbours[i]->rhs = getMinSuccessor(neighbours[i]).second;
}
updateVertex(neighbours[i]);
}
}
}
//Uncomment this to see CURRENT'S new values
//std::cout << "Expanded:";
//current->info();
//std::cout << std::endl;
}
return true;
}
