void MotionGraph::genGraph()
{
int k = numBlendingFrames;
int restPoseIdx = 0; // rest pose motion idx
restLabel = 0;
int numMotions = library->getNumMotions();
std::vector< std::vector<int> > nodeId; // nodeId[motionIdx][frameIdx]
for (int m = 0; m < numMotions; m++)
{
nodeId.push_back(std::vector<int>());
for (int f = 0; f < library->getMotion(m)->GetNumFrames(); f++)
nodeId[m].push_back(-1); // -1 := not a node
nodeId[m][0] = 1; // first frame
//nodeId[m][nodeId[m].size() - 1] = 1; // last frame
// transition point to the rest pose
int f = nodeId[m].size() - k;
if (f >= 0)
nodeId[m][f] = 1;
}
// rest pose
if ((int)nodeId[restPoseIdx].size() >= k)
nodeId[restPoseIdx][k - 1] = 1;
std::vector< std::vector< std::vector<WindowPair> > > candidates; // [motionIdxA][motionIdxB][pairIdx]
for (int ma = 0; ma < numMotions; ma++)
{
candidates.push_back(std::vector< std::vector<WindowPair> >());
for (int mb = 0; mb < numMotions; mb++)
{
candidates[ma].push_back(std::vector<WindowPair>());
}
}
// load candidates
printf("Loading transition candidates... ");
std::ifstream ifs;
ifs.open("./mocap_data/candidates.txt", std::ios::in);
char buf[256];
ifs >> buf; // #numMotions
int nmotions;
ifs >> nmotions;
double threshold = 100.0;
int minCandidateCount = 50;
for (int ma = 0; ma < nmotions; ma++) for (int mb = 0; mb < nmotions; mb++)
{
ifs >> buf; // #motionIndex
int motionIdxA, motionIdxB;
ifs >> motionIdxA >> motionIdxB;
//printf("motionIndex: %d, %d\n", motionIdxA, motionIdxB);
ifs >> buf; // #motionName
ifs >> buf; // motionNameA
ifs >> buf; // motionNameB
ifs >> buf; // #numFrames
int numFramesA, numFramesB;
ifs >> numFramesA >> numFramesB;
//printf("numFrames: %d, %d\n", numFramesA, numFramesB);
ifs >> buf; // #minListSize
int minListSize;
ifs >> minListSize;
ifs >> buf; // #i
ifs.getline(buf, sizeof(buf)); // j distance theta x0 z0
int count = 0;
for (int ii = 0; ii < minListSize; ii++)
{
int i, j;
double dist;
double theta, x0, z0;
ifs >> i >> j >> dist >> theta >> x0 >> z0;
if (j > (int)nodeId[mb].size() - k) // later than last transition point
continue;
// check threshold & conditions
if (dist > threshold &&
(count > minCandidateCount ||
j > numFramesB / 2))
//count > minListSize / 20)
continue;
count++;
WindowPair pair;
pair.i = i;
pair.j = j;
pair.dist = dist;
pair.theta = theta;
pair.x0 = x0;
pair.z0 = z0;
candidates[ma][mb].push_back(pair);
nodeId[ma][i] = 1;
nodeId[mb][j] = 1;
}
}
ifs.close();
printf("done.\n");
/*for (int aa = 0; aa < numMotions; aa++)
for (int bb = 0; bb < numMotions; bb++)
{
printf("motion(%d, %d): %d\n", aa, bb, candidates[aa][bb].size());
for (size_t ii = 0; ii < candidates[aa][bb].size(); ii++)
{
WindowPair &p = candidates[aa][bb][ii];
printf("%d %d %lf %lf %lf %lf\n", p.i, p.j, p.dist, p.theta, p.x0, p.z0);
}
}
*/
printf("Construcing graph...\n");
int numNodes = 0;
int numEdges = 0;
// construct nodes and edges within motions
for (int m = 0; m < numMotions; m++)
{
int lastNode = -1;
int count = 0;
for (int f = 0; f < (int)nodeId[m].size(); f++)
if (nodeId[m][f] >= 0)
{
// form a new node
nodeId[m][f] = numNodes++;
//printf("node(%d): %d, %d\n", nodeId[m][f], m, f);
Node node;
node.id = nodeId[m][f];
node.motionIdx = m;
node.frameIdx = f;
node.label = m; // use motion idx as label
nodes.push_back(node);
count++;
if (lastNode != -1) // not the first node of the motion
{
// construct an edge fram last node to current node
int id = numEdges++;
int src = nodeId[m][lastNode];
int dst = nodeId[m][f];
addEdge(id, src, dst, 0.0, 0.0, 0.0, f - lastNode);
nodes[src].link.push_back(id);
}
lastNode = f;
}
printf("Motion(%d): total nodes: %d\n", m, count);
}
// construct edges crossing two motions
for (int ma = 0; ma < numMotions; ma++)
{
for (int mb = 0; mb < numMotions; mb++)
{
for (size_t ii = 0; ii < candidates[ma][mb].size(); ii++)
{
WindowPair &pair = candidates[ma][mb][ii];
// construct an edge from A(i) to B(j)
int id = numEdges++;
int src = nodeId[ma][pair.i];
int dst = nodeId[mb][pair.j];
addEdge(id, src, dst, pair.theta, pair.x0, pair.z0, k - 1);
nodes[src].link.push_back(id);
}
}
}
// construct edges from the end of motions to the rest pose
if ((int)nodeId[restPoseIdx].size() >= k)
{
for (int m = 0; m < numMotions; m++)
{
double theta, x0, z0;
distance(m, restPoseIdx, nodeId[m].size() - k, k - 1, &theta, &x0, &z0);
int id = numEdges++;
int src = nodeId[m][nodeId[m].size() - k];
int dst = nodeId[restPoseIdx][k - 1];
addEdge(id, src, dst, theta, x0, z0, k - 1, 1);
nodes[src].link.push_back(id);
}
}
// test: dump
for (size_t eid = 0; eid < edges.size(); eid++)
{
Edge &e = edges[eid];
int srcMotion = nodes[e.src].motionIdx;
int dstMotion = nodes[e.dst].motionIdx;
int srcFrame = nodes[e.src].frameIdx;
int dstFrame = nodes[e.dst].frameIdx;
//printf("edge(%d): %d(%d) --> %d(%d)\n", eid, srcMotion, srcFrame, dstMotion, dstFrame);
}
printf("Construcing graph... done.\n");
////////////////////////////////////////////////////////////////////////////
printf("Constructing shortest path table... ");
int numLabels = numMotions; // can be different
for (int dstLabel = 0; dstLabel < numLabels; dstLabel++)
{
// find shortest path to a certain label
std::vector<int> dist;
std::vector<int> next;
for (size_t i = 0; i < nodes.size(); i++)
{
int initDist = 999999999;
if (nodes[i].label == dstLabel)
initDist = 0;
dist.push_back(initDist);
next.push_back(-1);
}
// Bellman-Ford
for (int i = 0; i < (int)nodes.size() - 1; i++)
{
for (int j = 0; j < (int)edges.size(); j++)
{
int src = edges[j].src;
int dst = edges[j].dst;
if (dist[src] > dist[dst] + edges[j].length)
{
dist[src] = dist[dst] + edges[j].length;
next[src] = j;
}
}
}
for (int i = 0; i < (int)nodes.size(); i++)
{
int nextEdge = next[i];
if (nodes[i].label == dstLabel)
{
if (nodes[i].link.size() == 0)
{
printf("Warning: nodes[%d](%d, %d).link.size() == 0\n", i, nodes[i].motionIdx, nodes[i].frameIdx);
exit(-1);
}
else
{
int ndst0 = edges[nodes[i].link[0]].dst;
if (nodes[ndst0].motionIdx == nodes[i].motionIdx && nodes[ndst0].frameIdx > nodes[i].frameIdx)
{
// set the next edge to the edge towards the next frame
nextEdge = nodes[i].link[0];
}
else
{
//printf("%d(%d)\n", nodes[i].motionIdx, nodes[i].frameIdx);
nextEdge = nodes[i].link[0];
// force to transit to rest pose
if (nodes[i].label != 1 && // walk
nodes[i].label != 2) // run
//if (false)
{
for (int j = 0; j < (int)nodes[i].link.size(); j++)
{
if (edges[nodes[i].link[j]].rest != 0)
{
nextEdge = nodes[i].link[j];
break;
}
}
}
//printf("%d(%d) --> %d(%d)\n", nodes[edges[nextEdge].src].motionIdx, nodes[edges[nextEdge].src].frameIdx,
// nodes[edges[nextEdge].dst].motionIdx, nodes[edges[nextEdge].dst].frameIdx);
}
/*for (int j = 0; j < (int)nodes[i].link.size(); j++)
{
int dst = edges[nodes[i].link[j]].dst;
if (nodes[dst].motionIdx == nodes[i].motionIdx &&
nodes[dst].frameIdx == nodes[i].frameIdx + 1)
{
nextEdge = nodes[i].link[j];
break;
}
}*/
}
}
nodes[i].next.push_back(nextEdge);
if (nextEdge == -1)
{
printf("Warning: node[%d](%d, %d) cannot reach label %d.\n", i, nodes[i].motionIdx, nodes[i].frameIdx, dstLabel);
//exit(-1);
}
}
}
printf("done.\n");
printf("Total edges: %d\n", edges.size());
////////////////////////////////////////////////////////////////////////////
printf("Constructing clips... ");
// construct edge clips
for (size_t eid = 0; eid < edges.size(); eid++)
{
//printf("edge(%d) ", eid);
Motion *clip = NULL;
Edge &e = edges[eid];
Node &nsrc = nodes[e.src];
Node &ndst = nodes[e.dst];
if (nsrc.motionIdx != ndst.motionIdx || e.length != ndst.frameIdx - nsrc.frameIdx) // transition between two motions
{
//printf("src(%d, %d) dst(%d, %d) %d\n", nsrc.motionIdx, nsrc.frameIdx, ndst.motionIdx, ndst.frameIdx, e.length);
try
{
Transition *tran = new Transition(library->getMotion(nsrc.motionIdx), nsrc.frameIdx, library->getMotion(ndst.motionIdx), ndst.frameIdx, e.theta, e.x0, e.z0);
clip = tran->getBlendedMotion();
delete tran;
}
catch (std::exception &e)
{
printf("%s\n", e.what());
exit(-1);
}
}
else // forward edge
{
//printf("forward src(%d, %d) dst(%d, %d) %d\n", nsrc.motionIdx, nsrc.frameIdx, ndst.motionIdx, ndst.frameIdx, e.length);
Motion *motion = library->getMotion(nsrc.motionIdx);
clip = new Motion(e.length + 1, library->getSkeleton());
if (e.length != ndst.frameIdx - nsrc.frameIdx)
{
printf("Error: edges[%d].length != nodes[%d].frameIdx - nodes[%d].frameIdx\n", eid, e.dst, e.src);
}
for (int i = 0, j = nsrc.frameIdx; i < e.length + 1; i++, j++)
{
clip->SetPosture(i, *motion->GetPosture(j));
}
}
clips.push_back(clip);
}
printf("done.\n");
reset();
printf("Generating motion graph: done.\n");
}
