/**
* map joint properties from same joint on skeleton
*
* Joint j
* Skeleton s
*/
void mapJointFromSkeleton(Joint &j, nite::Skeleton s) {
j.xPos = (int) s.getJoint( (nite::JointType) j.type).getPosition().x;
j.yPos = (int) s.getJoint( (nite::JointType) j.type).getPosition().y;
j.zPos = (int) s.getJoint( (nite::JointType) j.type).getPosition().z;
const nite::Quaternion &o = s.getJoint( (nite::JointType) j.type).getOrientation();
j.xRotation = atan2(2*o.y*o.w-2*o.x*o.z , 1 - 2*pow(o.y,2) - 2*pow(o.z, 2)) * (180/PI);
j.yRotation = asin(2*o.x*o.y + 2*o.z*o.w) * (180/PI);
j.zRotation = atan2(2*o.x*o.w-2*o.y*o.z , 1 - 2*pow(o.x, 2) - 2*pow(o.z, 2)) * (180/PI);
if (s.getJoint( (nite::JointType) j.type).getPositionConfidence() > .5) {
j.isActive = true;
} else {
j.isActive = false;
}
j.positionConfidence = s.getJoint( (nite::JointType) j.type).getPositionConfidence();
// calculate how far arm is extended for hands
if ((nite::JointType) j.type == nite::JOINT_LEFT_HAND ) {
float dist_lh2le = sqrt( pow(skeleton.leftHand.xPos - skeleton.leftElbow.xPos, 2) + pow(skeleton.leftHand.yPos- skeleton.leftElbow.yPos, 2) + pow(skeleton.leftHand.zPos - skeleton.leftElbow.zPos, 2) );
float dist_lh2ls = sqrt( pow(skeleton.leftHand.xPos - skeleton.leftShoulder.xPos, 2) + pow(skeleton.leftHand.yPos - skeleton.leftShoulder.yPos, 2) + pow(skeleton.leftHand.zPos - skeleton.leftShoulder.zPos, 2) );
float dist_le2ls = sqrt( pow(skeleton.leftElbow.xPos - skeleton.leftShoulder.xPos, 2) + pow(skeleton.leftElbow.yPos - skeleton.leftShoulder.yPos, 2) + pow(skeleton.leftElbow.zPos - skeleton.leftShoulder.zPos, 2) );
float left_percentExtended = (int)( dist_lh2ls/(dist_le2ls + dist_lh2le) *100);
j.percentExtended = left_percentExtended;
}
if ((nite::JointType) j.type == nite::JOINT_RIGHT_HAND ) {
float dist_rh2re = sqrt( pow(skeleton.rightHand.xPos - skeleton.rightElbow.xPos, 2) + pow(skeleton.rightHand.yPos- skeleton.rightElbow.yPos, 2) + pow(skeleton.rightHand.zPos - skeleton.rightElbow.zPos, 2) );
float dist_rh2rs = sqrt( pow(skeleton.rightHand.xPos - skeleton.rightShoulder.xPos, 2) + pow(skeleton.rightHand.yPos - skeleton.rightShoulder.yPos, 2) + pow(skeleton.rightHand.zPos - skeleton.rightShoulder.zPos, 2) );
float dist_re2rs = sqrt( pow(skeleton.rightElbow.xPos - skeleton.rightShoulder.xPos, 2) + pow(skeleton.rightElbow.yPos - skeleton.rightShoulder.yPos, 2) + pow(skeleton.rightElbow.zPos - skeleton.rightShoulder.zPos, 2) );
float right_percentExtended = (int) dist_rh2rs/(dist_re2rs + dist_rh2re) *100;
j.percentExtended = right_percentExtended;
}
}
bool ExpressionGesture::checkExpressionGesture(const HandsTracker& handsTracker, const nite::Skeleton& skeleton)
{
if (! tryToCalibrateGesture(handsTracker, skeleton))
return false;
//Detect if current movement is compatible with this gesture
if (gestureHand == nite::JOINT_LEFT_HAND)
{
if (fabs(mDot(handsTracker.leftHandDirection, GESTURE_VECTOR)) < GESTURE_SIMILARITY_THRESHOLD
|| handsTracker.leftHandSpeedMMPerMS > GESTURE_MAX_SPEED_MM_PER_MS)
return false;
}
else if (gestureHand == nite::JOINT_RIGHT_HAND)
{
if (fabs(mDot(handsTracker.rightHandDirection, GESTURE_VECTOR)) < GESTURE_SIMILARITY_THRESHOLD
|| handsTracker.rightHandSpeedMMPerMS > GESTURE_MAX_SPEED_MM_PER_MS)
return false;
}
const nite::SkeletonJoint& hand = skeleton.getJoint(gestureHand);
float handY = hand.getPosition().y;
float handX = hand.getPosition().x;
if (gestureZone.isCoordinateInZone(handX, handY))
{
expressionDetected = (handY - gestureZone.bottom) / gestureZone.length();
return true;
}
return false;
}
bool ExpressionGesture::tryToCalibrateGesture(const HandsTracker& handsTracker, const nite::Skeleton& skeleton)
{
const nite::SkeletonJoint& leftHip = skeleton.getJoint(nite::JOINT_LEFT_HIP);
const nite::SkeletonJoint& hand = skeleton.getJoint(gestureHand);
const nite::SkeletonJoint& elbow = skeleton.getJoint(gestureElbow);
const nite::SkeletonJoint& shoulder = skeleton.getJoint(gestureShoulder);
if (leftHip.getPositionConfidence() < 0.5f
|| elbow.getPositionConfidence() < 0.5f
|| shoulder.getPositionConfidence() < 0.5f)
return false;
const float hipPosition = leftHip.getPosition().y;
const float headPosition = handsTracker.head.y;
const float distanceHipToHead = headPosition - hipPosition;
//Zone starts vertically 10% above the hip and ends 10% above the head
gestureZone.bottom = hipPosition + distanceHipToHead / 10.f;
gestureZone.top = headPosition + distanceHipToHead /10.f;
const Point3F phand(hand.getPosition().x, hand.getPosition().y, hand.getPosition().z);
const Point3F pelbow(elbow.getPosition().x, elbow.getPosition().y, elbow.getPosition().z);
const Point3F pshoulder(shoulder.getPosition().x, shoulder.getPosition().y, shoulder.getPosition().z);
const VectorF handToElbow = phand - pelbow;
const VectorF elbowToShoulder = pelbow - pshoulder;
const float distanceHandToShoulder = handToElbow.len() + elbowToShoulder.len();
//Zone horizontal limit change function of handedness
if (gestureHand == nite::JOINT_RIGHT_HAND)
{
gestureZone.right = pshoulder.x + distanceHandToShoulder * GESTURE_WIDTH_PERCENTAGE;
gestureZone.left = pshoulder.x;
}
else
{
gestureZone.right = pshoulder.x;
gestureZone.left = pshoulder.x - distanceHandToShoulder * GESTURE_WIDTH_PERCENTAGE;
}
return true;
}
// FUNCTION:PosCalc()
void PosCalc( const nite::Skeleton& rSkeleton, nite::JointType eJoint, D3DXVECTOR3* point )
{
const nite::SkeletonJoint& rJoint = rSkeleton.getJoint( eJoint );
if( rJoint.getPositionConfidence() < 0.5f )
{
const nite::Point3f& rPos = rJoint.getPosition();
point->x = ( rPos.x - g_BP_Zero.x );
point->y = ( rPos.y - g_BP_Zero.y );
point->z = ( rPos.z - g_BP_Zero.z );
}
else
{
point->y = -999.0f;
}
}
void OpenNIDevice::copySkeleton(const nite::Skeleton& srcSkeleton, dai::Skeleton& dstSkeleton)
{
for (int j=0; j<15; ++j)
{
// Load nite::SkeletonJoint
const nite::SkeletonJoint& niteJoint = srcSkeleton.getJoint((nite::JointType) j);
const nite::Point3f& nitePos = niteJoint.getPosition();
const nite::Quaternion& niteOrientation = niteJoint.getOrientation();
// Copy nite joint pos to my own Joint converting from nite milimeters to meters
SkeletonJoint joint(Point3f(nitePos.x, nitePos.y, nitePos.z), _staticMap[j]);
joint.setOrientation(Quaternion(niteOrientation.w, niteOrientation.x,
niteOrientation.y,
niteOrientation.z));
joint.setPositionConfidence(niteJoint.getPositionConfidence());
joint.setOrientationConfidence(niteJoint.getOrientationConfidence());
dstSkeleton.setJoint(_staticMap[j], joint);
}
dstSkeleton.setDistanceUnits(dai::DISTANCE_MILIMETERS);
}