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;
}