cSpace::cSpace(btVector3 center, float range, float margin, obstacles *obs, simGLView* glView)
:
simGLObject(glView),
m_blocks(obs),
m_centerPoint(center),
m_detectRange(range),
m_detectRangeSq(range*range),
m_margin(margin)
{
m_vertices[0] = btVector3(1,1,1);
m_vertices[1] = btVector3(-1,1,1);
m_vertices[2] = btVector3(-1,-1,1);
m_vertices[3] = btVector3(1,-1,1);
m_vertices[4] = btVector3(1,1,-1);
m_vertices[5] = btVector3(1,-1,-1);
m_vertices[6] = btVector3(-1,-1,-1);
m_vertices[7] = btVector3(-1,1,-1);
arena = physicsWorld::instance();
if(glView) drawCspace(false);
generateCSpace();
groupOverlapCSpace();
}
//! function to find a solution path
bool HFPlanner::trySolve()
{
//verify init and goal samples
if(goalSamp()->isFree()==false || initSamp()->isFree()==false)
{
cout<<"init or goal configuration are in COLLISION!"<<endl;
drawCspace();
return false;
}
//set init and goal vertices
gridVertex vg = _samples->indexOf(goalSamp());
gridVertex vi = _samples->indexOf(initSamp());
Sample* curr;
gridVertex vc;
gridVertex vmin;
curr = _init;
vc = vi;
_path.clear();
clearSimulationPath();
graph_traits<gridGraph>::adjacency_iterator avi, avi_end;
//relax HF
computeHF(vg);
int count = 0;
int countmax = _mainiter;
std::vector<int> cellpath;
cellpath.push_back(vi);
_path.push_back(locations[vi]);
while(vc != vg && count < countmax)
{
vmin = vc;
for(tie(avi,avi_end)=adjacent_vertices(vc, *g); avi!=avi_end; ++avi)
{
KthReal pneigh = getPotential(*avi);
KthReal pcurr = getPotential(vmin);
if(pneigh < pcurr) {
vmin = *avi;
cellpath.push_back(vmin);
_path.push_back(locations[vmin]);
}
}
if(vc == vmin) {
//relax HF again and resume
computeHF(vg);
_path.clear();
cellpath.clear();
vc = vi;
count++;
}
else vc = vmin;
}
if(count < countmax) _solved = true;
else
{
_path.clear();
cellpath.clear();
_solved = false;
}
if(_solved)
{
cout<<"PATH:";
for(int i=0;i<cellpath.size();i++)
{
cout<<" "<<cellpath[i]<<"("<<getPotential(cellpath[i])<<"), ";
}
cout<<endl;
}
drawCspace();
return _solved;
}