//
// GetFreeIndex
//
// Returns a free instance index
//
static U32 GetFreeIndex()
{
ASSERT(initialized);
// Fatal if limit exceeded
if (instanceCount >= MAX_INSTANCES)
{
ERR_FATAL(("Maximum footprint instances exceeded!! (%d)", MAX_INSTANCES));
}
// Find next available slot
for (U32 count = 0, index = nextIndex; count < MAX_INSTANCES; count++)
{
ASSERT(ValidInstanceIndex(index));
// Is this space available
if (!instances[index])
{
nextIndex = index;
IncrementIndex(nextIndex);
return (index);
}
// Wrap index around
IncrementIndex(index);
}
// This should never happen, but paranoia has set in...
ERR_FATAL(("Instance scan found no available index! (%d)", instanceCount));
}
static IndexType Index()
{
static_assert(!std::is_pointer<ComponentType>::value, "T is not pointer.");
static_assert(std::is_object<ComponentType>::value, "T is not object type.");
static const auto value = IncrementIndex();
return value;
}
virtual void GetRoadmap(RoadmapPlanner& roadmap) const {
//simply output all the visited grid cells
if(planner.distances.numValues()==0) return;
vector<int> index(qStart.n,0);
do {
if(!IsInf(planner.distances[index])) {
Vector config = planner.FromGrid(index);
roadmap.AddMilestone(config);
}
} while(IncrementIndex(index,planner.distances.dims)==0);
if(planner.solution.edges.empty()) {
//add debugging path
int prev = -1;
for(size_t i=0;i<planner.failedCheck.edges.size();i++) {
if(prev < 0)
prev = roadmap.AddMilestone(planner.failedCheck.GetMilestone(0));
int n = roadmap.AddMilestone(planner.failedCheck.GetMilestone(i+1));
roadmap.ConnectEdge(prev,n,planner.failedCheck.edges[i]);
prev = n;
}
}
}
