C++ Motion::GetPosture方法代码示例

double MotionGraph::distance(int ma, int mb, int i, int j, double *theta, double *x0, double *z0)
{
    int k = numBlendingFrames;

    Skeleton *skeleton = library->getSkeleton();
    Motion *motionA = library->getMotion(ma);
    Motion *motionB = library->getMotion(mb);

    if (skeleton == NULL || motionA == NULL || motionB == NULL)
    {
        printf("Error: in distance().\n");
        exit(-1);
    }

    PointCloud **pcListA = new PointCloud *[k];
    PointCloud **pcListB = new PointCloud *[k];
    for (int f = i, ii = 0; f < i + k; f++, ii++)
    {
        skeleton->setPosture(*(motionA->GetPosture(f)));
        pcListA[ii] = new PointCloud(skeleton);
    }
    for (int f = j - k + 1, ii = 0; f < j + 1; f++, ii++)
    {
        skeleton->setPosture(*(motionB->GetPosture(f)));
        pcListB[ii] = new PointCloud(skeleton);
    }

    PointCloud pcA(&pcListA[0], k);
    PointCloud pcB(&pcListB[0], k);
    double dist = distance(&pcA, &pcB, theta, x0, z0);

    for (int i = 0; i < k; i++)
        delete pcListA[i];
    for (int i = 0; i < k; i++)
        delete pcListB[i];
    delete [] pcListA;
    delete [] pcListB;

    return dist;
}

//.........这里部分代码省略.........
                        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");
}

// k := window size
void MotionGraph::genCandidates(std::ofstream &os, int motionIdxA, int motionIdxB, int k)
{
    Skeleton *skeleton = library->getSkeleton();
    Motion *motionA = library->getMotion(motionIdxA);
    Motion *motionB = library->getMotion(motionIdxB);
    if (skeleton == NULL || motionA == NULL || motionB == NULL)
    {
        printf("Error: in genCandidates().\n");
        exit(-1);
    }

    int numFramesA = motionA->GetNumFrames();
    int numFramesB = motionB->GetNumFrames();

    printf("motionIdxA = %d, motionIdxB = %d\n", motionIdxA, motionIdxB);
    printf("numFramesA = %d, numFramesB = %d\n", numFramesA, numFramesB);
    os << "#motionIndex" << std::endl << motionIdxA << ' ' << motionIdxB << std::endl;
    os << "#motionName" << std::endl << library->getName(motionIdxA) << ' ' << library->getName(motionIdxB) << std::endl;
    os << "#numFrames" << std::endl << numFramesA << ' ' << numFramesB << std::endl;

    PointCloud **pcListA = new PointCloud *[numFramesA];
    PointCloud **pcListB = new PointCloud *[numFramesB];

    for (int i = 0; i < numFramesA; i++)
    {
        skeleton->setPosture(*(motionA->GetPosture(i)));
        pcListA[i] = new PointCloud(skeleton);
    }
    for (int i = 0; i < numFramesB; i++)
    {
        skeleton->setPosture(*(motionB->GetPosture(i)));
        pcListB[i] = new PointCloud(skeleton);
    }

    printf("Form distance matrix...\n");

    double *dist = new double[numFramesA * numFramesB]();   // distance of any pair of windows

    int iBegin = 0;
    int iEnd = numFramesA - k;
    int jBegin = k - 1;
    int jEnd = numFramesB - 1;

    printf("Calculating distances...\n");

    for (int i = iBegin; i <= iEnd; i++)             // window of motionA: [i..i+k-1]
    {
        if (i % 10 == 0)
            printf("Calculating (%d, %d)...\n", i, 0);

        for (int j = jBegin; j <= jEnd; j++)         // window of motionB: [j-k+1..j]
        {
            // form point cloud over k frames
            PointCloud pcA(&pcListA[i], k);
            PointCloud pcB(&pcListB[j - k + 1], k);

            int index = i * numFramesB + j;
            dist[index] = distance(&pcA, &pcB);     // distance
        }
    }

    printf("Calculating distances... done.\n");

    //std::vector< std::pair<int, int> > minList;     // local minimum list
    // local minimum list
    std::vector<WindowPair> minList;
    
    // search local minimums
    //bool *mark = new bool[numFramesA * numFramesB]();   // traversed
    for (int i = iBegin; i <= iEnd; i++)
    {
        for (int j = jBegin; j <= jEnd; j++)
        {
            int index = i * numFramesB + j;
            bool isMin = true;
            for (int ii = i - 2; ii <= i + 2; ii++)
            {
                for (int jj = j - 2; jj <= j + 2; jj++)
                {
                    if (ii >= iBegin && ii <= iEnd &&
                        jj >= jBegin && jj <= jEnd &&
                        dist[index] > dist[ii * numFramesB + jj])
                        isMin = false;
                }
            }
            if (isMin)
            {
                WindowPair pair;
                pair.i = i;
                pair.j = j;
                pair.dist = dist[index];
                minList.push_back(pair);
            }
        }
    }

    std::sort(minList.begin(), minList.end());

    printf("Local minimum list:\n");
//.........这里部分代码省略.........