private void handleNewPoseEvent(BodyPositionState pose)
{
// calculateFloorLevel(pose);
handleNodeMovement(lastPose.root, root, ref prevRootCoord, "Root", ref currentPelvisPos);
// handleNodeMovement(middleSpinePose, middleSpine, ref prevMiddleSpineCoord, "MiddleSpine");
handleLeftShoulderNodeMovement(lastPose.leftShoulder, leftShoulder, ref prevLeftShoulderCoord, "leftShoulder");
handleRightShoulderNodeMovement(lastPose.rightShoulder, rightShoulder, ref prevRightShoulderCoord, "rightShoulder");
handleNodeMovement(lastPose.nose, nose, ref prevNoseCoord, "nose", ref currentHeadPos);
// handleNodeMovement(lastPose.leftEye, leftEye, ref prevLeftEyeCoord, "leftEye");
// handleNodeMovement(lastPose.rightEye, rightEye, ref prevRightEyeCoord, "rightEye");
//handleNodeMovement(lastPose.leftEar, leftEar, ref prevLeftEarCoord, "leftEar");
// handleNodeMovement(lastPose.rightEar, rightEar, ref prevRightEarCoord, "rightEar");
// handleNodeMovement(lastPose.leftElbow, leftElbow, ref prevLeftElbowCoord, "leftElbow");
// handleNodeMovement(lastPose.rightElbow, rightElbow, ref prevRightElbowCoord, "rightElbow");
handleLeftHandNodeMovement(lastPose.leftWrist, leftHand, ref prevLeftWristCoord, "leftWrist");
handleRightHandNodeMovement(lastPose.rightWrist, rightHand, ref prevRightWristCoord, "rightWrist");
// handleNodeMovement(lastPose.leftHip, leftHip, ref prevLeftHipCoord, "leftHip");
// handleNodeMovement(lastPose.rightHip, rightHip, ref prevRightHipCoord, "rightHip");
// handleNodeMovement(lastPose.leftKnee, leftKnee, ref prevLeftKneeCoord, "leftKnee");
// handleNodeMovement(lastPose.rightKnee, rightKnee, ref prevRightKneeCoord, "rightKnee");
handleNodeMovement(lastPose.leftFoot, leftFoot, ref prevLeftFootCoord, "leftFoot", ref currentLeftFootPos);
handleNodeMovement(lastPose.rightFoot, rightFoot, ref prevRightFootCoord, "rightFoot", ref currentRightFootPos);
}
public void onPoseCalibrationDone(BodyPositionState pose, float xAdjustmentToZero)
{
basePose = new BodyPositionState(pose);
adjustmentToZero.Set(pose.root.x, xAdjustmentToZero + 1, zLevel);
Debug.Log("adjustmentToZero: " + adjustmentToZero.ToString());
Debug.Log("basePose: " + basePose.root.ToString());
}
private void onStandingInTPose(START_END state, BodyPositionState pose)
{
if (state == START_END.STARTING)
{
currentState = POSE_ACTION_STATE.STANDING_IN_T_POSE;
}
else
{
currentState = POSE_ACTION_STATE.NONE;
}
}
private void onCrouching(START_END state, BodyPositionState pose)
{
if (state == START_END.STARTING)
{
currentState = POSE_ACTION_STATE.CROUCHING;
}
else
{
currentState = POSE_ACTION_STATE.NONE;
}
}
private void onJumping(START_END state, BodyPositionState pose)
{
if (state == START_END.STARTING)
{
currentState = POSE_ACTION_STATE.JUMPING;
}
else
{
currentState = POSE_ACTION_STATE.NONE;
}
}
public static BodyPositionState operator -(BodyPositionState a, BodyPositionState b)
{
BodyPositionState p = new BodyPositionState();
p.nose = a.nose - b.nose;
p.leftFoot = a.leftFoot - b.leftFoot;
p.rightFoot = a.rightFoot - b.rightFoot;
p.leftWrist = a.leftWrist - b.leftWrist;
p.rightWrist = a.rightWrist - b.rightWrist;
p.root = a.root - b.root;
p.spine3 = a.spine3 - b.spine3;
return(p);
}
/**
* This event handler will be called every time a new pose is received
*/
public void onPoseEventReceived(BodyPositionState pose)
{
// Debug.Log("PoseEvent handled by PoseEventHandler");
lastPose = pose;
updateMaxValues(lastPose);
updateMinValues(lastPose);
if (storeReceivedPoses)
{
receivedPoseEvents.Add(new BodyPositionState(pose));
if (averagePose != null)
{
diffPoses.Add(pose - averagePose);
}
}
}
/**
* Using the adjustment info to prepare the system variables needed for operation
*/
private void handleAdjustmentInfo()
{
// Calculate factors
Vector3 rootVector = getAveragePosition(rootStr);
Vector3 leftFootvector = getAveragePosition(leftFootStr);
Vector3 rightFootvector = getAveragePosition(rightFootStr);
Vector3 leftShoulderVector = getAveragePosition(leftShoulderStr);
Vector3 rightShoulderVector = getAveragePosition(rightShoulderStr);
Vector3 leftWristVector = getAveragePosition(leftHandStr);
Vector3 rightWristVector = getAveragePosition(rightHandStr);
Vector3 headVector = getAveragePosition(headStr);
referencePose = new BodyPositionState();
referencePose.root = new PosePosition(rootVector);
referencePose.leftFoot = new PosePosition(leftFootvector);
referencePose.rightFoot = new PosePosition(rightFootvector);
referencePose.leftShoulder = new PosePosition(leftShoulderVector);
referencePose.rightShoulder = new PosePosition(rightShoulderVector);
referencePose.leftWrist = new PosePosition(leftWristVector);
referencePose.rightWrist = new PosePosition(rightWristVector);
referencePose.nose = new PosePosition(headVector);
if (leftFootvector.y > 0)
{
if (rightFootvector.y > 0)
{
xAdjustmentToZero = Math.Min(leftFootvector.y, rightFootvector.y);
}
else
{
xAdjustmentToZero = leftFootvector.y;
}
}
else if (rightFootvector.y > 0)
{
xAdjustmentToZero = rightFootvector.y;
}
BodyPositionState state = new BodyPositionState();
state.root = new PosePosition(rootVector);
state.leftFoot = new PosePosition(leftFootvector);
state.rightFoot = new PosePosition(rightFootvector);
if (onPoseCalibrationDone != null)
{
onPoseCalibrationDone(state, xAdjustmentToZero);
}
}
private void updateMinValues(BodyPositionState pose)
{
if (minPose.root.x > pose.root.x)
{
// Update min to new value
minPose.root.x = pose.root.x;
}
if (minPose.root.y > pose.root.y)
{
// Update min to new value
minPose.root.y = pose.root.y;
}
if (minPose.root.z > pose.root.z)
{
// Update min to new value
minPose.root.z = pose.root.z;
}
}
private void updateMaxValues(BodyPositionState pose)
{
if (maxPose.root.x < pose.root.x)
{
// Update max to new value
maxPose.root.x = pose.root.x;
}
if (maxPose.root.y < pose.root.y)
{
// Update max to new value
maxPose.root.y = pose.root.y;
}
if (maxPose.root.z < pose.root.z)
{
// Update max to new value
maxPose.root.z = pose.root.z;
}
}
public BodyPositionState(BodyPositionState state)
{
this.nose = new PosePosition(state.nose);
this.leftFoot = new PosePosition(state.leftFoot);
this.rightFoot = new PosePosition(state.rightFoot);
this.leftEar = new PosePosition(state.leftEar);
this.rightEar = new PosePosition(state.rightEar);
this.leftElbow = new PosePosition(state.leftElbow);
this.rightElbow = new PosePosition(state.rightElbow);
this.leftEye = new PosePosition(state.leftEye);
this.rightEye = new PosePosition(state.rightEye);
this.leftHip = new PosePosition(state.leftHip);
this.rightHip = new PosePosition(state.rightHip);
this.leftKnee = new PosePosition(state.leftKnee);
this.rightKnee = new PosePosition(state.rightKnee);
this.leftShoulder = new PosePosition(state.leftShoulder);
this.rightShoulder = new PosePosition(state.rightShoulder);
this.leftWrist = new PosePosition(state.leftWrist);
this.rightWrist = new PosePosition(state.rightWrist);
this.root = new PosePosition(state.root);
this.spine3 = new PosePosition(state.spine3);
}
public void onPoseCalibrationDone(BodyPositionState pose, float xAdjustmentToZero)
{
averagePose = pose;
this.xAdjustmentToZero = xAdjustmentToZero;
}
/**
* Check for T-pose
*
* Definition of T-pose:
* - Hands & elbows should be in +/-5% of shoulder in Y value
* - Hands should be a bit from elbows that should be a bit from shoulder in X value
* - Feet should be a bit from knees that should be a bit from root in Y value
* - Left foot should be left of root that should be left of right foot in X value
* - Spine3 should be a bit above root
*/
private bool isStandingInTPose(BodyPositionState pose)
{
// Ensure that person is standing up OK
if ((pose.spine3.y > pose.root.y) &&
(pose.leftShoulder.y > pose.spine3.y) &&
(pose.rightShoulder.y > pose.spine3.y)
)
{
// Upper body ok -> check under body if it is in picture
if (pose.leftKnee.y > 0.1f && pose.rightKnee.y > 0.1)
{
// Knees are in picture -> check them
if ((pose.root.y < pose.leftKnee.y) ||
(pose.root.y < pose.rightKnee.y))
{
// knees are over root
Debug.Log("No T-pose: knees are over root");
return(false);
}
// Knees are out of picture -> ignore them
}
if (pose.leftFoot.y > 0.1f && pose.rightFoot.y > 0.1)
{
if ((pose.leftKnee.y < pose.leftFoot.y) ||
(pose.rightKnee.y < pose.rightFoot.y)
)
{
// feet are over knees
Debug.Log("No T-pose: feet are over knees");
return(false);
}
// Feet are out of picture -> ignore them
}
// All is Ok - standing up
}
else
{
// Person is not standing ok -> NOK
Debug.Log("No T-pose: Person is not standing ok -> NOK");
return(false);
}
float deltaY = Mathf.Abs(pose.leftShoulder.y - pose.spine3.y) / 2;
// Ensure arms are straight out
if ((pose.leftShoulder.x > pose.leftElbow.x) &&
(pose.leftElbow.x > pose.leftWrist.x) &&
(pose.rightShoulder.x < pose.rightElbow.x) &&
(pose.rightElbow.x < pose.rightWrist.x) &&
// Left hand & elbow should be between spine 3 & nose
(pose.leftWrist.y < pose.nose.y) && (pose.leftWrist.y > pose.spine3.y) &&
(pose.leftElbow.y < pose.nose.y) && (pose.leftElbow.y > pose.spine3.y) &&
// Righthand & elbow should be between spine 3 & nose
(pose.rightWrist.y < pose.nose.y) && (pose.rightWrist.y > pose.spine3.y) &&
(pose.rightElbow.y < pose.nose.y) && (pose.rightElbow.y > pose.spine3.y) &&
// Y-lvl of hand/elbow & shoulder should not differ by to much (straight arm)
(pose.leftShoulder.y - pose.leftWrist.y) < deltaY &&
(pose.leftShoulder.y - pose.leftElbow.y) < deltaY &&
(pose.rightShoulder.y - pose.rightWrist.y) < deltaY &&
(pose.rightShoulder.y - pose.rightElbow.y) < deltaY
)
{
// All is Ok - arms straight out
}
else
{
// Person is not with arms straight out -> NOK
Debug.Log("No T-pose: Person is not with arms straight out -> NOK");
return(false);
}
// Ensure legs are not crossed
if ((pose.leftFoot.x < pose.rightFoot.x) &&
(pose.leftShoulder.x < pose.rightShoulder.x)
)
{
// All is Ok
}
else
{
// Person is not standing ok with legs -> NOK
Debug.Log("No T-pose: Person is not standing ok with legs -> NOK");
return(false);
}
// Standing in T-pose OK
Debug.Log("T-pose OK");
return(true);
}
/**
* Processing if we are moving in to a new state.
* If so we also notify about this to any event listeners.
*/
private void processCalcState(BodyPositionState pose)
{
if (calibrating)
{
// Check for T-pose
bool standingInTPose = isStandingInTPose(pose);
if (standingInTPose)
{
// if this has been the same for more than x ms -> go to T-pose
if (currentState != POSE_ACTION_STATE.STANDING_IN_T_POSE)
{
calibrationEndTime = System.DateTime.Now;
calibrationStartTime = System.DateTime.Now;
calibrationStartTime = calibrationStartTime.AddSeconds(calibrationDelay);
calibrationEndTime = calibrationEndTime.AddSeconds(secsForCalibration + calibrationDelay);
currentState = POSE_ACTION_STATE.STANDING_IN_T_POSE;
if (onStandingInTPose != null)
{
onStandingInTPose(START_END.STARTING, pose);
}
if (onPoseCalibrationStarted != null)
{
onPoseCalibrationStarted(pose);
}
}
}
return; // We are calibrating -> no more handling
}
if (referencePose == null)
{
return; // ref pose not set -> we cannot process the states
}
if (currentState == POSE_ACTION_STATE.NONE)
{
if ((pose.root.y - referencePose.root.y) < crouchingAdjustment)
{
// We are crouching
currentState = POSE_ACTION_STATE.CROUCHING;
if (onCrouching != null)
{
onCrouching(START_END.STARTING, pose);
}
}
else if ((pose.root.y - referencePose.root.y) > jumpingAdjustment)
{
// We are jumping
currentState = POSE_ACTION_STATE.JUMPING;
if (onJumping != null)
{
onJumping(START_END.STARTING, pose);
}
}
else if (isStandingInTPose(pose))
{
currentState = POSE_ACTION_STATE.STANDING_IN_T_POSE;
if (onStandingInTPose != null)
{
onStandingInTPose(START_END.STARTING, pose);
}
}
}
else if (currentState == POSE_ACTION_STATE.CROUCHING)
{
if ((pose.root.y - referencePose.root.y) > crouchingAdjustment / 2)
{
// We are NOT crouching anymore
currentState = POSE_ACTION_STATE.NONE;
if (onCrouching != null)
{
onCrouching(START_END.ENDING, pose);
}
}
}
else if (currentState == POSE_ACTION_STATE.JUMPING)
{
if ((pose.root.y - referencePose.root.y) < jumpingAdjustment / 3)
{
// We are NOT jumping anymore
currentState = POSE_ACTION_STATE.NONE;
if (onJumping != null)
{
onJumping(START_END.ENDING, pose);
}
}
}
else if (currentState == POSE_ACTION_STATE.STANDING_IN_T_POSE)
{
if (!isStandingInTPose(pose))
{
// We are NOT standing in T-pose anymore
currentState = POSE_ACTION_STATE.NONE;
if (onStandingInTPose != null)
{
onStandingInTPose(START_END.ENDING, pose);
}
}
}
}
/**
* A new posenet event was received
*/
public void HandlePoseEvent(PoseEvent pose)
{
// Debug.Log("PoseEvent handled by PoseCoreEventManager: " + pose);
baseHandlingOfPoseEvent(pose);
BodyPositionState currentPose = new BodyPositionState(pose);
if (onPoseEventReceived != null)
{
// Inform all handlers of this event
onPoseEventReceived(lastPose);
}
if (onNosePoseEventReceived != null)
{
onNosePoseEventReceived(pose.nose);
}
if (onLeftEyePoseEventReceived != null)
{
onLeftEyePoseEventReceived(pose.leftEye);
}
if (onRightEyePoseEventReceived != null)
{
onRightEyePoseEventReceived(pose.rightEye);
}
if (onLeftEarPoseEventReceived != null)
{
onLeftEarPoseEventReceived(pose.leftEar);
}
if (onRightEarPoseEventReceived != null)
{
onRightEarPoseEventReceived(pose.rightEar);
}
if (onLeftShoulderPoseEventReceived != null)
{
onLeftShoulderPoseEventReceived(pose.leftShoulder);
}
if (onRightShoulderPoseEventReceived != null)
{
onRightShoulderPoseEventReceived(pose.rightShoulder);
}
if (onLeftElbowPoseEventReceived != null)
{
onLeftElbowPoseEventReceived(pose.leftElbow);
}
if (onRightElbowPoseEventReceived != null)
{
onRightElbowPoseEventReceived(pose.rightElbow);
}
if (onLeftWristPoseEventReceived != null)
{
onLeftWristPoseEventReceived(pose.leftWrist);
}
if (onRightWristPoseEventReceived != null)
{
onRightWristPoseEventReceived(pose.rightWrist);
}
if (onLeftHipPoseEventReceived != null)
{
onLeftHipPoseEventReceived(pose.leftHip);
}
if (onRightHipPoseEventReceived != null)
{
onRightHipPoseEventReceived(pose.rightHip);
}
if (onLeftKneePoseEventReceived != null)
{
onLeftKneePoseEventReceived(pose.leftKnee);
}
if (onRightKneePoseEventReceived != null)
{
onRightKneePoseEventReceived(pose.rightKnee);
}
if (onLeftFootPoseEventReceived != null)
{
onLeftFootPoseEventReceived(pose.leftFoot);
}
if (onRightFootPoseEventReceived != null)
{
onRightFootPoseEventReceived(pose.rightFoot);
}
if (onRootPoseEventReceived != null)
{
onRootPoseEventReceived(pose.root);
}
if (onMiddleSpinePoseEventReceived != null)
{
onMiddleSpinePoseEventReceived(pose.spine3);
}
processCalcState(currentPose);
lastPose = currentPose;
}
/**
* This event handler will be called every time a new pose is received
*/
public void onPoseEventReceived(BodyPositionState pose)
{
// Debug.Log("PoseEvent handled by PoseEventHandler");
lastPose = pose;
}
