void vtkPolyDataSingleSourceShortestPath::ShortestPath(int startv, int endv)
{
int i, u, v;
InitSingleSource(startv);
HeapInsert(startv);
this->f->SetValue(startv, 1);
int stop = 0;
while ((u = HeapExtractMin()) >= 0 && !stop)
{
// u is now in s since the shortest path to u is determined
this->s->SetValue(u, 1);
// remove u from the front set
this->f->SetValue(u, 0);
if (u == endv && StopWhenEndReached)
stop = 1;
// Update all vertices v adjacent to u
for (i = 0; i < Adj[u]->GetNumberOfIds(); i++)
{
v = Adj[u]->GetId(i);
// s is the set of vertices with determined shortest path...do not use them again
if (!this->s->GetValue(v))
{
// Only relax edges where the end is not in s and edge is in the front set
double w = EdgeCost(this->GetInput(), u, v);
if (this->f->GetValue(v))
{
Relax(u, v, w);
}
// add edge v to front set
else
{
this->f->SetValue(v, 1);
this->d->SetValue(v, this->d->GetValue(u) + w);
// Set Predecessor of v to be u
this->pre->SetValue(v, u);
HeapInsert(v);
}
}
}
}
}
/**
* @function CalculateDistanceFromPathSet
*/
void LJM2::CalculateDistanceFromPathSet( std::vector<int> _path ) {
printf("--> Calculating Distance From Path Set \n");
std::vector<int> queue = _path;
//-- 1. Initialize all to infinity
for( int i = 0; i < mNumNodes; ++i ) {
if( GetState( i ) == OBSTACLE_STATE || GetState( i ) == INFLATED_STATE ) {
mNodes[i].s.brushDist = 0;
}
else {
mNodes[i].s.brushDist = LJM2_INF;
}
}
// Path nodes with zero initial value
for( int i = 0; i < _path.size(); ++i ) {
mNodes[ _path[i] ].s.brushDist = 0;
}
//-- 2. Loop until no more new elements in Queue
std::vector<int> newQueue(0);
while( queue.size() > 0 ) {
for( int i = 0; i < queue.size(); ++i ) {
std::vector<int> n = mGeometricNeighbors[ queue[i] ];
double dist = mNodes[ queue[i] ].s.brushDist;
for( int j = 0; j < n.size(); ++j ) {
double new_dist = dist + EdgeCost( queue[i], n[j], sNominalValue );
if( new_dist < mNodes[ n[j] ].s.brushDist ) {
mNodes[ n[j] ].s.brushDist = new_dist;
newQueue.push_back( n[j] );
}
}
}
queue.clear();
queue = newQueue;
newQueue.clear();
}
printf("--) Finished calculating DT Paths \n");
}
/**
* @function FindPath
* @brief This is in the whole free space
*/
std::vector<Eigen::Vector3i> LJM2::FindPath( int _n1, int _n2 ) {
std::vector<Eigen::Vector3i> path;
printf( "--> Start FindPath in whole free space \n" );
int nodeStart = _n1;
int nodeTarget = _n2;
//-- Fill start space
Node3D *node;
node = &mNodes[nodeStart];
node->s.costG = 0;
node->s.costH = CostHeuristic( nodeStart, nodeTarget )*( 0 + mAlpha*1 );
node->s.costF = node->s.costG + node->s.costH;
node->s.parent = -1;
node->s.status = IN_NO_SET;
//-- Push it into the open set
PushOpenSet( nodeStart );
//-- Loop
int x;
int count = 0;
while( count < sMaxIter ) {
count++;
//-- Remove top node in OpenSet
try {
x = PopOpenSet();
} catch( int i ) {
printf( "-- (%d) No more nodes to pop out \n", i );
break;
}
//-- Check if it is goal
if( x == nodeTarget ) {
printf( "--> Found a path! : iters: %d Cost: %.3f \n", count, mNodes[x].s.costF ); TracePath( x, path ); break;
}
//-- Add node to closed set
mNodes[x].s.status = IN_CLOSED_SET;
std::vector<int> neighbors = mGeometricNeighbors[x];
//--
for( int i = 0; i < neighbors.size(); i++ ) {
if( mNodes[ neighbors[i] ].s.status == IN_CLOSED_SET ) {
continue;
}
int y = mNodes[ neighbors[i] ].index; // Same as neighbors[i] actually
float tentative_G_score = mNodes[x].s.costG + EdgeCost( x, y, sNominalValue )*( mNodes[y].s.value + mAlpha*1 );
if( mNodes[y].s.status != IN_OPEN_SET ) {
node = &mNodes[y];
node->s.parent = x;
node->s.costG = tentative_G_score;
node->s.costH = CostHeuristic( y, nodeTarget )*( 0 + mAlpha*1 );
node->s.costF = node->s.costG + node->s.costH;
PushOpenSet(y);
}
else {
if( tentative_G_score < mNodes[y].s.costG ) {
node = &mNodes[y];
node->s.parent = x;
node->s.costG = tentative_G_score;
node->s.costH = CostHeuristic( y, nodeTarget )*( 0 + mAlpha*1 );
node->s.costF = node->s.costG + node->s.costH;
//-- Reorder your OpenSet
UpdateLowerOpenSet( y );
}
}
} //-- End for every neighbor
} //-- End of while
node = NULL;
printf("Finished FindPath whole free space \n");
return path;
}
