Création d'un compte pour un collaborateur extérieur au laboratoire depuis l'intranet ICube : https://intranet.icube.unistra.fr/fr/labs/member/profile

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
}