vector<Vector3> splitPaths(const vector<int> &discreteEmbedding, const PtGraph &graph,
const Skeleton &skeleton)
{
FP fp(graph, skeleton, vector<Sphere>());
vector<Vector3> out;
out.push_back(graph.verts[discreteEmbedding[0]]);
for(int i = 1; i < (int)discreteEmbedding.size(); ++i) {
int prev = skeleton.cPrev()[i];
vector<Vector3> pathPts = splitPath(&fp, i, discreteEmbedding[i], discreteEmbedding[prev]);
out.insert(out.end(), pathPts.begin() + 1, pathPts.end());
}
return out;
}
vector<int> discreteEmbed(const PtGraph &graph, const vector<Sphere> &spheres,
const Skeleton &skeleton, const vector<vector<int> > &possibilities)
{
int i, j;
FP fp(graph, skeleton, spheres);
fp.footBase = 1.;
for(i = 0; i < (int)graph.verts.size(); ++i)
fp.footBase = min(fp.footBase, graph.verts[i][1]);
vector<PenaltyFunction *> penaltyFunctions = getPenaltyFunctions(&fp);
int toMatch = skeleton.cGraph().verts.size();
std::cout << "Matching!" << endl;
priority_queue<PartialMatch> todo;
PartialMatch output(graph.verts.size());
todo.push(output);
int maxSz = 0;
while(!todo.empty()) {
PartialMatch cur = todo.top();
todo.pop();
int idx = cur.match.size();
int curSz = (int)log((double)todo.size());
if(curSz > maxSz) {
maxSz = curSz;
if(maxSz > 3)
std::cout << "Reached " << todo.size() << endl;
}
if(idx == toMatch) {
output = cur;
std::cout << "Found: residual = " << cur.penalty << endl;
break;
}
for(i = 0; i < (int)possibilities[idx].size(); ++i) {
int candidate = possibilities[idx][i];
int k;
double extraPenalty = computePenalty(penaltyFunctions, cur, candidate);
if(extraPenalty < 0)
std::cout << "ERR = " << extraPenalty << endl;
if(cur.penalty + extraPenalty < 1.) {
PartialMatch next = cur;
next.match.push_back(candidate);
next.penalty += extraPenalty;
next.heuristic = next.penalty;
//compute taken vertices and edges
if(idx > 0) {
vector<int> path = fp.paths.path(candidate, next.match[skeleton.cPrev()[idx]]);
for(j = 0; j < (int)path.size(); ++j)
next.vTaken[path[j]] = true;
}
//compute heuristic
for(j = idx + 1; j < toMatch; ++j) {
if(skeleton.cPrev()[j] > idx)
continue;
double minP = 1e37;
for(k = 0; k < (int)possibilities[j].size(); ++k) {
minP = min(minP, computePenalty(penaltyFunctions, next, possibilities[j][k], j));
}
next.heuristic += minP;
if(next.heuristic > 1.)
break;
}
if(next.heuristic > 1.)
continue;
todo.push(next);
}
}
}
if(output.match.size() == 0)
{
std::cout << "No Match" << endl;
}
for(i = 0; i < (int)penaltyFunctions.size(); ++i)
delete penaltyFunctions[i];
return output.match;
}
