bool operator==(const IndexContainer & other) const {
return other.GetIndex()[0] == m_Index[0] && other.GetIndex()[1] == m_Index[1] && other.GetIndex()[2] == m_Index[2];
}
template<int dim> double ttt::AdherensJunctionSegmentationDijkstraCommand<dim>::ComputePath(
const SkeletonVertexType & a, const SkeletonVertexType & b) {
double weight = 0;
typedef std::deque<IndexContainer<Index> > HeapType;
typename DistanceImageType::Pointer distances = DistanceImageType::New();
distances->SetRegions(m_Plateness->GetLargestPossibleRegion());
distances->SetSpacing(m_Plateness->GetSpacing());
distances->SetOrigin(m_Plateness->GetOrigin());
distances->Allocate();
distances->FillBuffer(INFINITY);
typename ColorImageType::Pointer colors = ColorImageType::New();
colors->SetRegions(m_Plateness->GetLargestPossibleRegion());
colors->SetSpacing(m_Plateness->GetSpacing());
colors->SetOrigin(m_Plateness->GetOrigin());
colors->Allocate();
colors->FillBuffer(COLOR_WHITE);
Index sourceIndex = m_IndexToVertex.right.find(a)->second->GetPosition();
Index targetIndex = m_IndexToVertex.right.find(b)->second->GetPosition();
IndexContainer<Index> sourceIndexContainer;
sourceIndexContainer.SetValue(0);
sourceIndexContainer.SetIndex(sourceIndex);
HeapType trialHeap;
trialHeap.push_back(sourceIndexContainer);
std::make_heap(trialHeap.begin(), trialHeap.end());
distances->SetPixel(sourceIndex, 0);
bool found = false;
int exploredNodes = 0;
while (!trialHeap.empty() && !found) {
IndexContainer<Index> current = trialHeap.front();
Index currentIndex = current.GetIndex();
double currentDistance = current.GetValue();
std::pop_heap(trialHeap.begin(), trialHeap.end());
trialHeap.pop_back();
colors->SetPixel(current.GetIndex(), COLOR_BLACK);
exploredNodes++;
if (exploredNodes % 10000 == 0) {
//std::cout << exploredNodes << "/" << " Pendientes: " << trialHeap.size() << " " << currentDistance << std::endl;
}
if (currentIndex == targetIndex) {
found = true;
} else {
std::vector<Index> neighs;
this->GetNeighbors(currentIndex, neighs);
for (typename std::vector<Index>::iterator it = neighs.begin();
it != neighs.end(); it++) {
Index neigh = *it;
char neighcolor = colors->GetPixel(neigh);
if (neighcolor == COLOR_BLACK)
continue;
if (m_Labels->GetPixel(neigh) == a
|| m_Labels->GetPixel(neigh) == b) {
double speedValue = m_Speed->GetPixel(neigh);
double nextDistance = 1.0 / speedValue;
double candDist = currentDistance + nextDistance;
double neighDist = distances->GetPixel(neigh);
if (candDist < neighDist) {
IndexContainer<Index> current;
current.SetValue(candDist);
current.SetIndex(neigh);
if (neighcolor == COLOR_GRAY) {
typename HeapType::iterator findres = std::find(
trialHeap.begin(), trialHeap.end(),
current);
findres->SetValue(candDist);
std::update_heap_pos(trialHeap.begin(),
trialHeap.end(), findres);
} else {
trialHeap.push_back(current);
std::push_heap(trialHeap.begin(), trialHeap.end());
colors->SetPixel(neigh, COLOR_GRAY);
}
distances->SetPixel(neigh, candDist);
}
}
}
}
}
if (found) {
bool backtrackingFinished = false;
std::list<Index> path;
path.clear();
Index currentIndex = targetIndex;
int length = 0;
while (!backtrackingFinished) {
//PATH = PATH + current
if (currentIndex == sourceIndex) {
backtrackingFinished = true;
} else {
double currentValue = distances->GetPixel(currentIndex);
weight += m_Speed->GetPixel(currentIndex);
length++;
path.push_back(currentIndex);
Index minNeigh;
double minValue = currentValue;
std::vector<Index> neighbors;
this->GetNeighbors(currentIndex, neighbors);
for (typename std::vector<Index>::iterator itNeigh = neighbors.begin();
itNeigh != neighbors.end(); ++itNeigh) {
Index neigh = *itNeigh;
double neighValue = distances->GetPixel(neigh);
if (neighValue < minValue) {
minValue = neighValue;
minNeigh = neigh;
}
}
assert(currentValue > minValue);
currentIndex = minNeigh;
}
}
return weight / length;
} else {
return -1;
}
}
IndexContainer(const IndexContainer<IndexType> & other) {
m_Index = other.GetIndex();
m_Time = other.GetValue();
}