//*****************************************
// SDL Functions
//*****************************************
void init() {
//Initialise SDL.
if(SDL_Init(SDL_INIT_VIDEO) < 0) exit(1);
//Use double buffering (24-bit).
SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24);
//Set vertical sync.
//SDL_GL_SetSwapInterval(1);
//Create window.
if (SDL_SetVideoMode(800, 800, 32, SDL_OPENGL) == 0) exit(1);
SDL_WM_SetCaption("Primitives", "Primitives");
//Set up orthographic camera.
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-2, 2, -2, 2, 2, -2);
glMatrixMode(GL_MODELVIEW);
//Set the clear color.
glClearColor(0, 0, 0, 1);
//Use the z-buffer.
glEnable(GL_DEPTH_TEST);
//Use vertex and colour arrays.
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY);
//Calculate the vertex and colour data.
calculateV();
calculateC();
}
VectorDamper::VectorDamper()
{
pos = VC3(0, 0, 0);
speed = VC3(0, 0, 0);
target = VC3(0, 0, 0);
mass = 1.0f;
k = 0.1f;
overdampFactor = 1.0f;
calculateC();
}
GridWorld::TilePair GridWorld::getMinSuccessor(GridWorld::Tile*& tile){
Tile* minTile = 0;
double minCost = PF_INFINITY;
std::vector<Tile*> neighbours(getNeighbours(tile));
for (int i = 0; i < neighbours.size(); i++){
double cost = calculateC(tile, neighbours[i]);
double g = neighbours[i]->g;
if (cost == PF_INFINITY || g == PF_INFINITY){
continue;
}
if (cost + g < minCost){ //potential overflow?
minTile = neighbours[i];
minCost = cost + g;
}
}
return TilePair(minTile, minCost);
}
/*
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();
}
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 VectorDamper::setOverdampFactor(float overdampFactor)
{
this->overdampFactor = overdampFactor;
calculateC();
}
void VectorDamper::setK(float k)
{
this->k = k;
calculateC();
}
int OSM_Model400::calculate( )
{
// Return value, assume successful
int retVal = 0;
// Vectors for internal component streams
OSM_Vector x( nSize() ); // net crusher contents
OSM_Vector y( nSize() ); // material classified for breakage
OSM_Vector z( nSize() ); // breakage products of y
// Calculate classification function: C[]
calculateC( );
// Create contents matrix if required
if( contents.rows()!=nComp() || contents.columns()!=nSize() )
{
contents.dimension( nComp(), nSize() );
}
//-- Main Loop - loop over components in the feed -----------------------
for( int comp=0; comp<nComp(); comp++ )
{
// Aliases to component vectors
OSM_Vector& feedC = feed()[comp];
OSM_Vector& prodC = product()[comp];
OSM_Vector& x = contents[comp];
if( feedC.sum() > 0 ) // Component present ?
{
calculateA( comp ); // Make appearance function
double misconvergence = 1e20; // Force misconvergence
long iteration = 0; // No iterations yet
//-- Iteration Loop - until converged or maximum iterations -----
while( misconvergence>tolerance && iteration<maxIteration )
{
// classify material for breakage y = C(x)
y.loadProduct( C, x );
// obtain breakage products z = A(y)
makeDaughters( y, z );
// obtain next iteration of recycle: x = feed + z
misconvergence = x.loadSumWithCompare( feedC, z );
// a successful iteration (hopefully)
iteration++;
}
// product component by balance
prodC.loadSubtraction( x, y );
}
else
{
// product component is 0
prodC = 0;
}
}
// indicate success
return retVal;
}
/*
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;
}
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;
}
