C++ Skeleton类代码示例

//-----------------------------------------------------------------------
void SkeletonHelper::ToGlobalPose(Skeleton const& skeleton,
    SkeletonPose const& skeletonPose, std::vector<Matrix3x2> & globalPose)
{
    POMDOG_ASSERT(skeleton.JointCount() > 1);
    POMDOG_ASSERT(skeleton.Root().Index);

    SkeletonHelper::Traverse(skeleton, skeleton.Root().Index, [&](Joint const& bone)
    {
        POMDOG_ASSERT(*bone.Index < skeletonPose.JointPoses.size());
        auto & pose = skeletonPose.JointPoses[*bone.Index];

        Matrix3x2 matrix = Matrix3x2::CreateScale(pose.Scale);
        matrix *= Matrix3x2::CreateRotation(pose.Rotation);
        matrix *= Matrix3x2::CreateTranslation(pose.Translate);

        if (bone.Parent)
        {
            POMDOG_ASSERT(*bone.Parent < globalPose.size());
            matrix *= globalPose[*bone.Parent];
        }

        POMDOG_ASSERT(*bone.Index < globalPose.size());
        globalPose[*bone.Index] = std::move(matrix);
    });
}

Skeleton* Mapper::newSkeleton(string id) {
	Skeleton skeleton;
	skeleton.init(id);
	skeleton.setSmoothing(defaultSmoothing);
	skeletons->push_back(skeleton);
	return &skeletons->at(skeletons->size()-1);
}

//-----------------------------------------------------------------------
void SkeletonHelper::Traverse(Skeleton const& skeleton,
    std::function<void(Joint const&)> const& traverser)
{
    POMDOG_ASSERT(skeleton.JointCount() > 0);
    POMDOG_ASSERT(skeleton.Root().Index);
    Traverse(skeleton, skeleton.Root().Index, traverser);
}

void SpinningCube::Draw() {
	//glMatrixMode(GL_MODELVIEW);
	//glLoadMatrixf(WorldMtx);
	//glutWireCube(Size);
	/*
	Joint test = Joint("Test");
	test.setPose(Vector3(45, 45, 0));
	test.setTranslation(Vector3(0, 0, 0));
	test.setScale(Vector3(1, 1, 1), Vector3(0, 0, 0));
	test.calculate();
	Joint test1 = Joint("Test2");
	test1.setDOFX(0);
	test1.setDOFY(0);
	test1.setDOFZ(0);
	test1.setTranslation(Vector3(0, 1.5, 0));
	test1.setScale(Vector3(1, 1, 1), Vector3(0, 0, 0));
	test1.calculate();
	test.addChild(test1);
	Joint *test2 = test1.getParent();
	test.draw(WorldMtx.IDENTITY);*/

	Skeleton test = Skeleton();
	test.Load("test.skel");
	test.calculate(WorldMtx.IDENTITY);
	test.draw();
}

void AnimationSystem::MatrixPaletteGeneration()
{
    // todo: can be easly run in parallel.
    for( u32 i = 0; i < m_controllers.Count(); ++i )
    {
        AnimController& controller = m_controllers[i];
        AnimHierarchy* hierarchy = controller.GetHierarchy();
        Skeleton* skeleton = controller.GetSkeleton();
        Matrix4x4* palette = controller.GetSkinningPalette();
        
        // we need inverse matrix of root node's world transformation to calculate model-space palette.
        Matrix4x4 parentInvMatrix = hierarchy->GetNode(0).GetWorldTransformation();
        parentInvMatrix.InverseIt();
        
        for( u16 j = 0; j < hierarchy->GetNodeCount(); ++j )
        {
            // creating skinning palette.
            // K = (Bj_M)^-1 * Cj_M
            // palette matrix is inverse bind pose in model-space multiplied by current pose in model-space.
            
            // world-space matrix
            palette[j] = hierarchy->GetNode(j).GetWorldTransformation();
            
            // world-space -> model-space
            palette[j] = parentInvMatrix * palette[j];
            
            // skinning palette matrix ( model-space * inverse bind pose )
            palette[j] *= skeleton->GetInvBindPose(j);
        }
    }
}

void Entity::SetCurrentSkeleton(sqlite3_int64 id)
{
	Reset();
	
	Skeleton* newSkel = new Skeleton(m_db, id);
	if(newSkel->Valid())
	{
		m_skeleton = newSkel;
		m_clips = new ClipDB(m_db, id);

		sqlite3_int64 mg_id;
		if(FindFirstMotionGraph(id, &mg_id))
			SetCurrentMotionGraph(mg_id);

		sqlite3_int64 meshid;
		if(FindFirstMesh(id, &meshid)) {
			m_mesh = new Mesh(m_db, id, meshid);
		}
	} else {
		delete newSkel; 
	}
		
	Events::EntitySkeletonChangedEvent ev;
	m_evsys->Send(&ev);		
}

void WorldConstructor::Debug()
{
	return;
	// Create a humanoid controller
	//msHumanoid = new bioloidgp::robot::HumanoidController(robot, msWorld->getConstraintSolver());
	DartLoader urdfLoader;
	Skeleton* robot
		= urdfLoader.parseSkeleton(
		DATA_DIR"/urdf/BioloidGP/BioloidGP.URDF");
	robot->enableSelfCollision();
	msWorld->withdrawSkeleton(msHumanoid->robot());
	msWorld->addSkeleton(robot);

	msHumanoid->reset();

	Skeleton* tmp = msHumanoid->robot();
	msHumanoid->robot() = robot;
	msHumanoid->setJointDamping(0.0);
	msHumanoid->reset();

	Eigen::VectorXd qOrig = tmp->getPositions();
	Eigen::VectorXd q = msHumanoid->robot()->getPositions();
	Eigen::VectorXd err = qOrig - q;

	Eigen::VectorXd qDotOrig = tmp->getVelocities();
	Eigen::VectorXd qDot = msHumanoid->robot()->getVelocities();
	Eigen::VectorXd errDot = qDotOrig - qDot;


}

Action::ResultE
CPUSkinningAlgorithm::intersectEnter(Action *action)
{
    Action::ResultE  res     = Action::Continue;
    SkinnedGeometry *skinGeo = getSkin    ();
    Skeleton        *skel    = getSkeleton();
    IntersectAction *iact    =
        boost::polymorphic_downcast<IntersectAction *>(action);

    CPUSkinningDataAttachmentUnrecPtr data = getCPUSkinningData(skinGeo);

    if(data == NULL)
    {
        data = CPUSkinningDataAttachment::create();
        skinGeo->addAttachment(data);
    }

    skel->intersectEnter(action, skinGeo);

    if(data->getDataValid() == false)
    {
        transformGeometry(skinGeo, skel, data);

        data->setDataValid(true);
    }

    intersectGeometry(iact, skinGeo, data);

    return res;
}

void MyWindow::draw()
{
    Skeleton* model = mMainMotion.getSkel();

    // validate the frame index
    int nFrames = mMainMotion.getNumFrames();
    int fr = mFrame;
    if (fr >= nFrames) fr = nFrames-1;
    
    // update and draw the motion
    model->setPose(mMainMotion.getPoseAtFrame(fr));
    model->draw(mRI);
    
    Skeleton* model2 = mCompareMotion.getSkel();
    if (fr >= mCompareMotion.getNumFrames()) 
        fr = mCompareMotion.getNumFrames() - 1;
    model2->setPose(mCompareMotion.getPoseAtFrame(fr));
    model2->draw(mRI);       

    if(mShowMarker) model->drawMarkers(mRI);
    if(mShowProgress) yui::drawProgressBar(fr,mMaxFrame);

    // display the frame count in 2D text
    char buff[64];
    sprintf(buff,"%d/%d",mFrame,mMaxFrame);
    string frame(buff);
    glDisable(GL_LIGHTING);
    glColor3f(0.0,0.0,0.0);
    yui::drawStringOnScreen(0.02f,0.02f,frame);
    glEnable(GL_LIGHTING);
}

	void KinectMeshAnimator::LinkJoints(int kinectJoint, std::string skeletonJointName)
	{
		Skeleton* skeleton = animator->GetSkeleton();
		Joint* joint = skeleton->GetJointByName(skeletonJointName);
		assert(joint != 0 && "Could not find requested joint");		
		bindings[kinectJoint] = joint;
	}

	void Skeleton::Interpolate(const Skeleton& skeletonA, const Skeleton& skeletonB, float interpolation)
	{
		#if NAZARA_UTILITY_SAFE
		if (!m_impl)
		{
			NazaraError("Skeleton not created");
			return;
		}

		if (!skeletonA.IsValid())
		{
			NazaraError("Skeleton A is invalid");
			return;
		}

		if (!skeletonB.IsValid())
		{
			NazaraError("Skeleton B is invalid");
			return;
		}

		if (skeletonA.GetJointCount() != skeletonB.GetJointCount() || m_impl->joints.size() != skeletonA.GetJointCount())
		{
			NazaraError("Skeletons must have the same joint count");
			return;
		}
		#endif

		Joint* jointsA = &skeletonA.m_impl->joints[0];
		Joint* jointsB = &skeletonB.m_impl->joints[0];
		for (std::size_t i = 0; i < m_impl->joints.size(); ++i)
			m_impl->joints[i].Interpolate(jointsA[i], jointsB[i], interpolation, CoordSys_Local);

		InvalidateJoints();
	}

void RegionAttachment::draw (Slot *slot) {
	Skeleton* skeleton = (Skeleton*)slot->skeleton;

	GLubyte r = skeleton->r * slot->r * 255;
	GLubyte g = skeleton->g * slot->g * 255;
	GLubyte b = skeleton->b * slot->b * 255;
	GLubyte a = skeleton->a * slot->a * 255;
	quad.bl.colors.r = r;
	quad.bl.colors.g = g;
	quad.bl.colors.b = b;
	quad.bl.colors.a = a;
	quad.tl.colors.r = r;
	quad.tl.colors.g = g;
	quad.tl.colors.b = b;
	quad.tl.colors.a = a;
	quad.tr.colors.r = r;
	quad.tr.colors.g = g;
	quad.tr.colors.b = b;
	quad.tr.colors.a = a;
	quad.br.colors.r = r;
	quad.br.colors.g = g;
	quad.br.colors.b = b;
	quad.br.colors.a = a;

	updateWorldVertices(slot->bone);

	// cocos2dx doesn't handle batching for us, so we'll just force a single texture per skeleton.
	skeleton->addQuad(atlas, quad);
}

Skeleton * BVHParser::createSkeleton()
{
	Skeleton * s = new Skeleton();
	
	// set default pose...
	Joint * b = createJoint(_root);
	if( !s->setJoints(b) )
	{
		delete s;
		return 0;
	}
	
	Pose * pose = new Pose(s->getNumJoints());
	
	for(int i = 0; i < _linearNodes.size(); i++ )
	{
		BVHNode * n = _linearNodes[i];
		pose->transforms[i].rotation.identity();
		pose->transforms[i].position(0,0,0);
	}
	
	s->pose = pose;
    s->init();
    
	return s;
}

// TODO: Use link lengths in expectations explicitly
TEST(FORWARD_KINEMATICS, TWO_ROLLS) {

    // Create the world
    const double link1 = 1.5, link2 = 1.0;
    Skeleton* robot = createTwoLinkRobot(Vector3d(0.3, 0.3, link1), DOF_ROLL, Vector3d(0.3, 0.3, link2),
        DOF_ROLL);

    // Set the test cases with the joint values and the expected end-effector positions
    const size_t numTests = 2;
    Vector2d joints [numTests] = {Vector2d(0.0, DART_PI/2.0), Vector2d(3*DART_PI/4.0, -DART_PI/4.0)};
    Vector3d expectedPos [numTests] = {Vector3d(0.0, -1.0, 1.5), Vector3d(0.0, -2.06, -1.06)};

    // Check each case by setting the joint values and obtaining the end-effector position
    for (size_t i = 0; i < numTests; i++) {
        robot->setConfig(twoLinkIndices, joints[i]);
        Vector3d actual = robot->getBodyNode("ee")->getWorldTransform().translation();
        bool equality = equals(actual, expectedPos[i], 1e-3);
        EXPECT_TRUE(equality);
        if(!equality) {
            std::cout << "Joint values: " << joints[i].transpose() << std::endl;
            std::cout << "Actual pos: " << actual.transpose() << std::endl;
            std::cout << "Expected pos: " <<  expectedPos[i].transpose() << std::endl;
        }
    }
}

void AnimationControl::loadCharacters(list<Object*>& render_list)
{
	data_manager.addFileSearchPath(AMC_MOTION_FILE_PATH);
	char* ASF_filename = NULL;
	char* AMC_filename = NULL;

	try
	{
		ASF_filename = data_manager.findFile(character_ASF.c_str());
		if (ASF_filename == NULL)
		{
			logout << "AnimationControl::loadCharacters: Unable to find character ASF file <" << character_ASF << ">. Aborting load." << endl;
			throw BasicException("ABORT");
		}
	
		AMC_filename = data_manager.findFile(character_AMC.c_str());
		if (AMC_filename == NULL)
		{
			logout << "AnimationControl::loadCharacters: Unable to find character AMC file <" << character_AMC << ">. Aborting load." << endl;
			throw BasicException("ABORT");
		}
		
		pair<Skeleton*, MotionSequence*> read_result;
		try {
			read_result = data_manager.readASFAMC(ASF_filename, AMC_filename);
		}
		catch (const DataManagementException& dme)
		{
			logout << "AnimationControl::loadCharacters: Unable to load character data files. Aborting load." << endl;
			logout << "   Failure due to " << dme.msg << endl;
			throw BasicException("ABORT");
		}
		
		Skeleton* skel = read_result.first;
		MotionSequence* ms = read_result.second;
		MotionSequenceController* controller = new MotionSequenceController(ms);
		
		// create rendering model for the character and put the character's 
		// bone objects in the rendering list
		Color color(1.0f,0.4f,0.3f);
		skel->constructRenderObject(render_list, color);

		// attach motion controller to animated skeleton
		skel->attachMotionController(controller);
		
		// create a character to link all the pieces together.
		string d1 = string("skeleton: ") + character_ASF;
		string d2 = string("motion: ") + character_AMC;
		skel->setDescription1(d1.c_str());
		skel->setDescription2(d2.c_str());
		character = skel;
	} 
	catch (BasicException&) { }

	strDelete(ASF_filename); ASF_filename = NULL;
	strDelete(AMC_filename); AMC_filename = NULL;

	ready = true;
}