bool NavigationMesh::makeSpline(PointPath& path) {
if( path.size() < 2 )
return false;
PointPath::iterator p0, p1, p2, p3;
p0 = p1 = path.begin();
++p1;
Ogre::Vector3 pextra = *p0 - ((*p1 - *p0).normalise() * 0.5);
path.push_front(pextra);
p0 = p1 = path.end();
--p1;
--p0;
--p0;
pextra = *p1 + ((*p1 - *p0).normalise() * 0.5);
path.push_back(pextra);
int dotsPerUnit = 2;
p0 = path.begin();
p1 = p0;
++p1;
p2 = p1;
++p2;
p3 = p2;
++p3;
while( p3 != path.end() ){
int n = (*p1 - *p2).length() * dotsPerUnit;
Ogre::Real step = 1.0/n;
Ogre::Real s = step;
for(int i = 1; i < n; ++i) {
path.insert(p2, CatmullRollSpline(*p0, *p1, *p2, *p3, s));
s += step;
}
p0 = p1;
p1 = p2;
++p2;
++p3;
}
path.pop_front();
path.pop_back();
return true;
}
bool NavigationMesh::buildPath(PointPath& path,
const Ogre::Vector3& startPos,
const Ogre::Vector3& endPos,
Cell* startCell,
Cell* endCell) {
if (!startCell)
startCell = findCell(startPos);
if (!endCell)
endCell = findCell(endPos);
if (!startCell || !endCell) {
cout << "Celdas no pertenecen a la rejilla" << endl;
return false;
}
int startId = startCell->getId();
int endId = endCell->getId();
if (_graph[startId * _cellNumber + endId] == Ogre::Math::POS_INFINITY) {
cout << "No se ha encontrado camino" << endl;
return false;
}
CellPath cellPath;
recoverPath(startId, endId, cellPath);
path.clear();
for(CellPath::iterator i = cellPath.begin(); i != cellPath.end(); ++i)
path.push_back((*i)->getCenter());
path.push_front(startPos);
path.push_back(endPos);
makeSpline(path);
return true;
}
