C++ (Cpp) BranchPtr::getVelocities示例

TEST(Skeleton, Referential)
{
  std::vector<SkeletonPtr> skeletons = getSkeletons();

#ifndef NDEBUG // Debug mode
  size_t numIterations = 1;
#else // Release mode
  size_t numIterations = 20;
#endif

  for(size_t i=0; i<skeletons.size(); ++i)
  {
    SkeletonPtr skeleton = skeletons[i];
    for(size_t j=0; j<skeleton->getNumTrees(); ++j)
    {
      BranchPtr tree = Branch::create(skeleton->getRootBodyNode(j));

      const std::vector<BodyNode*>& skelBns = skeleton->getTreeBodyNodes(j);
      EXPECT_TRUE(tree->getNumBodyNodes() == skelBns.size());
      for(BodyNode* bn : skelBns)
      {
        EXPECT_FALSE(tree->getIndexOf(bn) == INVALID_INDEX);
        EXPECT_TRUE(tree->getBodyNode(tree->getIndexOf(bn)) == bn);
      }

      const std::vector<DegreeOfFreedom*>& skelDofs = skeleton->getTreeDofs(j);
      EXPECT_TRUE(tree->getNumDofs() == skelDofs.size());
      for(DegreeOfFreedom* dof : skelDofs)
      {
        EXPECT_FALSE(tree->getIndexOf(dof) == INVALID_INDEX);
        EXPECT_TRUE(tree->getDof(tree->getIndexOf(dof)) == dof);
      }

      Eigen::VectorXd q = tree->getPositions();
      Eigen::VectorXd dq = tree->getVelocities();
      Eigen::VectorXd ddq = tree->getAccelerations();

      for(size_t k=0; k<numIterations; ++k)
      {
        for(int r=0; r<q.size(); ++r)
        {
          q[r] = math::random(-10, 10);
          dq[r] = math::random(-10, 10);
          ddq[r] = math::random(-10, 10);
        }

        tree->setPositions(q);
        tree->setVelocities(dq);
        tree->setAccelerations(ddq);

        EXPECT_TRUE( equals(q, tree->getPositions(), 0.0) );
        EXPECT_TRUE( equals(dq, tree->getVelocities(), 0.0) );
        EXPECT_TRUE( equals(ddq, tree->getAccelerations(), 0.0) );

        const Eigen::MatrixXd& skelMassMatrix = skeleton->getMassMatrix();
        const Eigen::MatrixXd& treeMassMatrix = tree->getMassMatrix();

        const Eigen::MatrixXd& skelAugM = skeleton->getAugMassMatrix();
        const Eigen::MatrixXd& treeAugM = tree->getAugMassMatrix();

        const Eigen::MatrixXd& skelInvM = skeleton->getInvMassMatrix();
        const Eigen::MatrixXd& treeInvM = tree->getInvMassMatrix();

        const Eigen::MatrixXd& skelInvAugM = skeleton->getInvAugMassMatrix();
        const Eigen::MatrixXd& treeInvAugM = tree->getInvAugMassMatrix();

        const Eigen::VectorXd& skelCvec = skeleton->getCoriolisForces();
        const Eigen::VectorXd& treeCvec = tree->getCoriolisForces();

        const Eigen::VectorXd& skelFg = skeleton->getGravityForces();
        const Eigen::VectorXd& treeFg = tree->getGravityForces();

        const Eigen::VectorXd& skelCg = skeleton->getCoriolisAndGravityForces();
        const Eigen::VectorXd& treeCg = tree->getCoriolisAndGravityForces();

        const Eigen::VectorXd& skelFext = skeleton->getExternalForces();
        const Eigen::VectorXd& treeFext = tree->getExternalForces();

        const Eigen::VectorXd& skelFc = skeleton->getConstraintForces();
        const Eigen::VectorXd& treeFc = tree->getConstraintForces();

        const size_t nDofs = tree->getNumDofs();
        for(size_t r1=0; r1<nDofs; ++r1)
        {
          const size_t sr1 = tree->getDof(r1)->getIndexInSkeleton();
          for(size_t r2=0; r2<nDofs; ++r2)
          {
            const size_t sr2 = tree->getDof(r2)->getIndexInSkeleton();

            EXPECT_TRUE( skelMassMatrix(sr1,sr2) == treeMassMatrix(r1,r2) );
            EXPECT_TRUE( skelAugM(sr1,sr2) == treeAugM(r1,r2) );
            EXPECT_TRUE( skelInvM(sr1,sr2) == treeInvM(r1,r2) );
            EXPECT_TRUE( skelInvAugM(sr1,sr2) == treeInvAugM(r1,r2) );
          }

          EXPECT_TRUE( skelCvec[sr1] == treeCvec[r1] );
          EXPECT_TRUE( skelFg[sr1]   == treeFg[r1] );
          EXPECT_TRUE( skelCg[sr1]   == treeCg[r1] );
          EXPECT_TRUE( skelFext[sr1] == treeFext[r1] );
          EXPECT_TRUE( skelFext[sr1] == treeFext[r1] );
          EXPECT_TRUE( skelFc[sr1]   == treeFc[r1] );
        }

        const size_t numBodyNodes = tree->getNumBodyNodes();
        for(size_t m=0; m<numBodyNodes; ++m)
        {
          const BodyNode* bn = tree->getBodyNode(m);
          const Eigen::MatrixXd Jtree = tree->getJacobian(bn);
          const Eigen::MatrixXd Jskel = skeleton->getJacobian(bn);

          for(size_t r2=0; r2<nDofs; ++r2)
          {
            const size_t sr2 = tree->getDof(r2)->getIndexInSkeleton();
            for(size_t r1=0; r1<6; ++r1)
            {
              EXPECT_TRUE( Jtree(r1,r2) == Jskel(r1, sr2) );
            }
          }
        }
      }
    }
  }
}