path_finder.hpp 2.04 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
#include "Algo/Geometry/centroid.h"
#include <limits>
#include <iostream>

namespace CGoGN
{

namespace PathFinder
{


template <typename PFP>
float pathCostSqr(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos)
{
	return VEC3(Algo::Geometry::faceCentroid<PFP>(map,stopPos,position)-Algo::Geometry::faceCentroid<PFP>(map,startPos,position)).norm2();
}

template <typename PFP>
std::vector<Dart> pathFindAStar(typename PFP::MAP& map,const typename PFP::TVEC3& position, Dart startPos, Dart stopPos, FunctorType& bad)
{
	std::vector<Dart> path;

	AutoAttributeHandler< Dart > tablePred(map,FACE_ORBIT);
	std::vector<std::pair<float,Dart> > vToDev;

	Dart toDev = stopPos;
27 28
	vToDev.push_back(std::make_pair(0, toDev));
	tablePred[toDev] = toDev;
Pierre Kraemer's avatar
Pierre Kraemer committed
29 30 31 32 33 34 35 36 37
	do {
		//dev cell
		//get all vertex-adjacent cells 
		Dart dd = toDev;
		do {
			Dart ddd = map.alpha1(dd);
			do {
				ddd = map.alpha1(ddd);
				if(ddd!=dd) {
38
					if(!map.foreach_dart_of_face(ddd,bad) && tablePred[ddd]==EMBNULL) {
Pierre Kraemer's avatar
Pierre Kraemer committed
39 40 41 42
						//evaluate their cost and push them in the vector to dev
						if(tablePred[ddd]==EMBNULL)
							tablePred[ddd]=toDev;
						std::vector<std::pair<float,Dart> >::iterator it=vToDev.begin();
43
						float costDDD=pathCostSqr<PFP>(map,position,startPos,ddd)+(vToDev.begin())->first;
Pierre Kraemer's avatar
Pierre Kraemer committed
44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
						while(it!=vToDev.end() && (*it).first<costDDD)
							++it;
						vToDev.insert(it, std::make_pair(costDDD, ddd));
					}
				}
			} while(ddd!=dd);
			dd = map.phi1(dd);
		} while(dd!=toDev);

		//remove developped cell and get new cell to dev
		if(vToDev.size()>0) {
			toDev = (vToDev.begin())->second;
			vToDev.erase(vToDev.begin());
		}
	} while(vToDev.size()>0 && !map.sameOrientedFace(startPos,toDev));

	//if path found : from start to stop -> push all predecessors
	if(map.sameOrientedFace(startPos,toDev))
	{
63
		std::cout << "found" << std::endl;
Pierre Kraemer's avatar
Pierre Kraemer committed
64 65 66 67 68 69 70 71 72 73 74 75 76 77
		Dart toPush=startPos;
		std::cout << tablePred[startPos] << std::endl;
		do {
			path.push_back(toPush);
			toPush = tablePred[toPush];
		} while(!map.sameOrientedFace(toPush,stopPos));
	}

	return path;
}

//namespace
}

78
}