示例#1
0
void NiutHumanReader::humanJointCallBack(const niut_msgs::niut_HUMAN_LIST::ConstPtr& msg) {
    Joint curJoint(10000, 100);
    std::vector<int> trackedJoints;
    trackedJoints.push_back(0);
    trackedJoints.push_back(3);
    trackedJoints.push_back(9);
    trackedJoints.push_back(15);
    trackedJoints.push_back(22);

    //msg->filtered_users[i];
    for (int i = 0; i < NB_MAX_NIUT; i++) {
        if (msg->filtered_users[i].trackedId > -1 && msg->filtered_users[i].date.t_sec != 0) {

            unsigned int toasterId = humanIdOffset_ + msg->filtered_users[i].trackedId;
            //If this human was not assigned yet, we create it.
            if (lastConfig_[toasterId] == NULL) {
                Human* tmpHuman = new Human(toasterId);
                lastConfig_[toasterId] = tmpHuman;
            }


            if (fullHuman_) {
                for (unsigned int j = 0; j < trackedJoints.size(); j++) {
                    updateJoint(i, j, curJoint, toasterId, trackedJoints, msg);

                    lastConfig_[toasterId]->skeleton_[curJoint.getName()]->position_.set<0>(curJoint.position_.get<0>());
                    lastConfig_[toasterId]->skeleton_[curJoint.getName()]->position_.set<1>(curJoint.position_.get<1>());
                    lastConfig_[toasterId]->skeleton_[curJoint.getName()]->position_.set<2>(curJoint.position_.get<2>());
                    lastConfig_[toasterId]->skeleton_[curJoint.getName()]->setTime(msg->filtered_users[i].date.t_sec * pow(10, 9) +
                            msg->filtered_users[i].date.t_usec);


                    // We update the human position with the torso (joint 3)
                    if (j == 3) {
                        lastConfig_[toasterId]->position_.set<0>(curJoint.position_.get<0>());
                        lastConfig_[toasterId]->position_.set<1>(curJoint.position_.get<1>());
                        lastConfig_[toasterId]->position_.set<2>(curJoint.position_.get<2>());
                        lastConfig_[toasterId]->setTime(msg->filtered_users[i].date.t_sec * pow(10, 9) +
                                msg->filtered_users[i].date.t_usec);

                        // TODO; Compute the human orientation
                    }

                }
            } else {// not fullHuman_
                // We update the human position with the torso (joint 3)
                updateJoint(i, 3, curJoint, toasterId, trackedJoints, msg);
                lastConfig_[toasterId]->position_.set<0>(curJoint.position_.get<0>());
                lastConfig_[toasterId]->position_.set<1>(curJoint.position_.get<1>());
                lastConfig_[toasterId]->position_.set<2>(curJoint.position_.get<2>());
                lastConfig_[toasterId]->setTime(msg->filtered_users[i].date.t_sec * pow(10, 9) +
                        msg->filtered_users[i].date.t_usec);

                // TODO: compute the human orientation


            }

        }
    }
}
MStatus CIKSolverNode::doSimpleSolver()
{
	MStatus stat;

	// Get the handle and create a function set for it
	//	
	MIkHandleGroup* handle_group = handleGroup();
	if (NULL == handle_group) {
		return MS::kFailure;
	}
	MObject handle = handle_group->handle(0);
	MDagPath handlePath = MDagPath::getAPathTo(handle);
	MFnIkHandle fnHandle(handlePath, &stat);

	// End-Effector
	MDagPath endEffectorPath;
	fnHandle.getEffector(endEffectorPath);
	MFnIkEffector  fnEffector(endEffectorPath);
	MPoint effectorPos = fnEffector.rotatePivot(MSpace::kWorld);

	unsigned int numJoints = endEffectorPath.length();
	std::vector<MDagPath> jointsDagPaths; jointsDagPaths.reserve(numJoints);
	while (endEffectorPath.length() > 1)
	{
		endEffectorPath.pop();
		jointsDagPaths.push_back( endEffectorPath );
	}
	std::reverse(jointsDagPaths.begin(), jointsDagPaths.end());

	static bool builtLocalSkeleton = false;
	if (builtLocalSkeleton == false)
	{
		for (int jointIdx = 0; jointIdx < jointsDagPaths.size(); ++jointIdx)
		{
			MFnIkJoint curJoint(jointsDagPaths[jointIdx]);
			m_localJointsPos.push_back( curJoint.getTranslation(MSpace::kWorld) );
		}
		m_localJointsPos.push_back(effectorPos );
		builtLocalSkeleton = true;
	}

	MPoint startJointPos = MFnIkJoint(jointsDagPaths.front()).getTranslation(MSpace::kWorld);

	MVector startToEndEff = m_localJointsPos.back() - m_localJointsPos.front();
	double curveLength = (getPosition(1.0) - getPosition(0.0)).length();
	double chainLength = startToEndEff.length(); // in local space.
	double stretchFactor = curveLength / chainLength;
	
	double uVal = 0.0f;
	MVector jointPosL = m_localJointsPos[0];
	for (int jointIdx = 0; jointIdx < jointsDagPaths.size(); ++jointIdx)
	{
		MFnIkJoint curJoint(jointsDagPaths[jointIdx]);

		MVector curJointPosL = m_localJointsPos[jointIdx];

		double dist = stretchFactor * (curJointPosL - jointPosL).length();
		uVal = uVal + dist / curveLength;

		MVector curCurveJointPos = getPosition(uVal);
		curJoint.setTranslation(curCurveJointPos, MSpace::kWorld);
		jointPosL = curJointPosL;
	}
	MVector effectorCurvePos = getPosition(1.0);
	MVector curCurveJointPos = getPosition(uVal);
	MVector effectorVec = (effectorCurvePos - curCurveJointPos).normal();
	double endJointAngle[3]; 
	MVector effectorVecXY = MVector(effectorVec(0), effectorVec(1), 0.0);
	endJointAngle[2] = effectorVecXY.angle(MVector(1, 0, 0));
	if ((MVector(1, 0, 0) ^ effectorVecXY) * MVector(0, 0, 1) < 0.0) { endJointAngle[2] = -endJointAngle[2]; }
	MVector effectorVecXZ = MVector(effectorVec(0), 0.0, effectorVec(2));
	endJointAngle[1] = effectorVecXZ.angle(MVector(1, 0, 0));
	if ((MVector(1, 0, 0) ^ effectorVecXZ) * MVector(0, 1, 0) < 0.0) { endJointAngle[1] = -endJointAngle[1]; }
	endJointAngle[0] = 0.0;
	MFnIkJoint curJoint(jointsDagPaths.back()); curJoint.setRotation(endJointAngle, curJoint.rotationOrder());
	return MS::kSuccess;
}