// Get body data from the body manager and track the joint for the active body
void Update()
{
if (_bodyManager == null)
{
return;
}
Body[] data = _bodyManager.GetData();
if (data == null)
{
return;
}
// Use for actual multi-player environments!
if ((data.Length >= ActiveBodyNumber) && (data[ActiveBodyNumber] != null) && (data[ActiveBodyNumber].IsTracked))
{
GetComponent <Rigidbody>().isKinematic = true;
m_jointFilter.UpdateFilter(data[ActiveBodyNumber]);
var Joints = m_jointFilter.GetFilteredJoints();
m_leftHandState = data[ActiveBodyNumber].HandLeftState;
m_rightHandState = data[ActiveBodyNumber].HandRightState;
// Grab the mid spine position, we'll use this to make all other joint movement relative to the spine (this way we can limit the Y position of the character)
var midSpinePosition = Joints[(int)JointType.SpineMid];
var jointPos = Joints[(int)JointToUse];
//jointPos.X -= midSpinePosition.X;
//jointPos.Y -= midSpinePosition.Y;
jointPos.Z -= midSpinePosition.Z;
float zValue = (useZValue == true) ? jointPos.Z : 0f;
Vector3 targetPosition = new Vector3((midSpinePosition.X + jointPos.X) * scale, ((yOffset + jointPos.Y) * scale), zValue);
this.transform.position = targetPosition;
}
else
{
// Hide the object by moving it far away from the camera.
this.transform.position = new Vector3(-100f, -100f, 0f);
// Attempt to find the active body number by iterating through the current bodies, finding a relevant body, and then assigning the active body. Once we have one
// the user will be reacting to it from that point forward.
int bodyIndex = 0;
foreach (Body body in data)
{
if ((body != null) && (body.IsTracked))
{
ActiveBodyNumber = bodyIndex;
break;
}
bodyIndex++;
}
}
}
private void Processing()
{
CameraSpacePoint[] p = filter.GetFilteredJoints();
joints = Norm.ToModel(p);
if (GestureIsRecognized)
{
RecogGesture(joints);
}
RecordJoints(joints);
DrawSkeleton(p);
}
void BodyFrameReaderOnFrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
using (var frame = e.FrameReference.AcquireFrame())
{
if (bodies == null)
{
bodies = new Body[frame.BodyCount];
}
frame.GetAndRefreshBodyData(bodies);
var trackedBody = bodies.FirstOrDefault(body => body.IsTracked);
if (trackedBody == null)
{
return;
}
jointFilter.UpdateFilter(trackedBody);
var filteredJoints = jointFilter.GetFilteredJoints();
var joints = new Dictionary <BodyJointType, BodyJoint>();
for (var index = 0; index < filteredJoints.Length; index++)
{
var joint = filteredJoints[index];
joints[(BodyJointType)index] = new BodyJoint
{
Position = new Vec3 {
X = joint.X, Y = joint.Y, Z = joint.Z
}
};
}
foreach (var jointOrientation in trackedBody.JointOrientations.Values)
{
joints[(BodyJointType)jointOrientation.JointType].Orientation = new Vec4
{
X = jointOrientation.Orientation.X,
Y = jointOrientation.Orientation.Y,
Z = jointOrientation.Orientation.Z,
W = jointOrientation.Orientation.W
}
}
;
listener.OnBodyFrameReceived(joints);
}
}
}
private void Kinect_OnTrackedBody(object sender, BodyEventArgs e)
{
Body body = e.BodyData;
if (Mode == ControlMode.Disabled)
{
return;
}
for (int i = 1; i >= 0; i--) // Starts looking from right hand.
{
bool isLeft = (i == 0);
if (body.IsHandLiftForward(isLeft))
{
if (usedHandIndex == -1)
{
usedHandIndex = i;
}
else if (usedHandIndex != i)
{
// In two-hand control mode, non-used hand would be used for pressing/releasing mouse button.
if (Mode == ControlMode.MoveGripPressing)
{
DoMouseControlByHandState(i, body.GetHandState(isLeft));
}
continue;
}
if (body.IsHandLiftUpward(isLeft) && body.IsHandLiftUpward(!isLeft))
{
HandDist.Add(body.TwoHandsDistance());
//System.Diagnostics.Trace.WriteLine(HandDist.Count);
if (HandDist.Count == 2)
{
if (HandDist[1] - HandDist[0] >= 0.25)
{
MouseControl.Wheel(120);
//System.Diagnostics.Trace.WriteLine(1);
HandDist[0] = HandDist[1];
HandDist.RemoveAt(1);
}
else if (HandDist[0] - HandDist[1] >= 0.25)
{
MouseControl.Wheel(-120);
//System.Diagnostics.Trace.WriteLine(-1);
HandDist[0] = HandDist[1];
HandDist.RemoveAt(1);
}
else
{
HandDist.RemoveAt(1);
}
}
//System.Diagnostics.Trace.WriteLine(body.TwoHandsDistance());
}
else
{
HandDist.Clear();
kinectJointFilter.UpdateFilter(body);
//MVector2 handPos = body.GetHandRelativePosition(isLeft);
MVector2 handPos = KinectBodyHelper.GetHandSmoothedRelativePosition(kinectJointFilter.GetFilteredJoints(), isLeft);
MVector2 targetPos = cursorMapper.GetSmoothedOutputPosition(handPos);
//System.Diagnostics.Trace.WriteLine(handPos.ToString());
MouseControl.MoveTo(targetPos.X, targetPos.Y);
if (Mode == ControlMode.GripToPress)
{
DoMouseControlByHandState(i, body.GetHandState(isLeft));
}
else if (Mode == ControlMode.HoverToClick)
{
if ((targetPos - lastCursorPos).Length() > HoverRange)
{
ToggleHoverTimer(false);
hoverClicked = false;
}
lastCursorPos = targetPos;
}
}
}
else
{
if (usedHandIndex == i)
{
// Reset to none.
usedHandIndex = NONE_USED;
ReleaseGrip(i);
}
else if (Mode == ControlMode.MoveLiftClicking)
{
DoMouseClickByHandLifting(i, body.GetHandRelativePosition(isLeft));
//System.Diagnostics.Trace.WriteLine(body.GetHandRelativePosition(isLeft).Y);
}
else // Release mouse button when it's not regularly released, such as hand tracking lost.
{
ReleaseGrip(i);
}
}
}
ToggleHoverTimer(Mode == ControlMode.HoverToClick && usedHandIndex != -1);
}
//update body from filtered data
private void RefreshBodyObject(Kinect.Body body, KinectJointFilter bodyFilter, GameObject bodyObject, ulong id)
{
bodyFilter.UpdateFilter(body);
filteredJoints = bodyFilter.GetFilteredJoints();
orientJoints.Clear();
orientJoints = CalculateJointRotations(body.JointOrientations);
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
filteredJointPos = GetVector3FromCameraSpacePoint(filteredJoints[(int)jt]);
sourceJoint = body.Joints[jt];
targetJoint = null;
filteredTargetJointPos = null;
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
filteredTargetJointPos = GetVector3FromCameraSpacePoint(filteredJoints[(int)_BoneMap[jt]]);
}
//jointObj = bodyObject.transform.Find(jt.ToString());
jointObj = jointTransforms[id][jt];
//calculate orientations of end joints that are not captured by the kinect
if (zeroQuaternion.Equals(orientJoints[jt]) && filteredTargetJointPos.HasValue)
{
Vector3 direction = filteredJointPos - filteredTargetJointPos.Value;
if (jt == Kinect.JointType.AnkleLeft || jt == Kinect.JointType.AnkleRight) //the ankle roations have to be pointed at the foot to match the mesh
{
if (jt == Kinect.JointType.AnkleLeft)
{
direction = GetVector3FromCameraSpacePoint(filteredJoints[(int)Kinect.JointType.FootLeft]) - filteredJointPos;
}
else
{
direction = GetVector3FromCameraSpacePoint(filteredJoints[(int)Kinect.JointType.FootRight]) - filteredJointPos;
}
Vector3 perpendicular = Vector3.Cross(direction, Vector3.up);
Vector3 normal = Vector3.Cross(direction, perpendicular);
if (normal.sqrMagnitude != 0 && direction.sqrMagnitude != 0)
{
orientJoints[jt] = Quaternion.LookRotation(normal, direction); //normal, direction
}
else
{
orientJoints[jt] = Quaternion.identity;
}
}
else if (jt == Kinect.JointType.ThumbLeft || jt == Kinect.JointType.ThumbRight) //the thumbs are along their parents forward vector so they are calculated
{
Vector3 perpendicular = Vector3.Cross(direction, Vector3.up);
Vector3 normal = Vector3.Cross(perpendicular, direction);
if (normal.sqrMagnitude != 0 && direction.sqrMagnitude != 0)
{
orientJoints[jt] = Quaternion.LookRotation(normal, direction);
}
else
{
orientJoints[jt] = Quaternion.identity;
}
}
else if (jt == Kinect.JointType.Neck) //rotate the neck from side to side while keeping the Z axis pointing forwards
{
Vector3 forward = orientJoints[_BoneMap[jt]] * Vector3.forward;
Vector3 childFilteredJointPos = GetVector3FromCameraSpacePoint(filteredJoints[(int)_JointChildMap[jt]]);
Vector3 y = childFilteredJointPos - filteredJointPos;
orientJoints[jt] = Quaternion.LookRotation(forward, y);
}
else //by default set the up axis to point away from the joint and forward axis towards the parent's forward axis
{
Vector3 forward = orientJoints[_BoneMap[jt]] * Vector3.forward;
// calculate a rotation, Y forward for Kinect
if (direction.sqrMagnitude != 0)
{
orientJoints[jt] = Quaternion.LookRotation(forward, direction);
}
else
{
orientJoints[jt] = Quaternion.identity;
}
}
}
else //if joints are not computed in above, then point their Y value at the next joint while keeping their current Z value
{
if (_JointChildMap.ContainsKey(jt) && rotateToNextJoint)
{
Vector3 childFilteredJointPos = GetVector3FromCameraSpacePoint(filteredJoints[(int)_JointChildMap[jt]]);
Vector3 y = childFilteredJointPos - filteredJointPos;
orientJoints[jt] = Quaternion.LookRotation(orientJoints[jt] * Vector3.forward, y);
}
}
//check tracking state of joint to make sure it is tracked, turn off rendered objects if not.
if (debugJoints)
{
switch (sourceJoint.TrackingState)
{
case Kinect.TrackingState.NotTracked:
if (jointObj.GetChild(0).gameObject.activeSelf)
{
jointObj.GetChild(0).gameObject.SetActive(useUntrackedJoints);
}
JointTracked[body.TrackingId][jt.ToString()] = useUntrackedJoints;
break;
case Kinect.TrackingState.Inferred:
if (jointObj.GetChild(0).gameObject.activeSelf)
{
jointObj.GetChild(0).gameObject.SetActive(useInferredJoints);
}
jointObj.localPosition = filteredJointPos;
jointObj.localRotation = orientJoints[jt];
JointTracked[body.TrackingId][jt.ToString()] = useInferredJoints;
break;
case Kinect.TrackingState.Tracked:
if (!jointObj.GetChild(0).gameObject.activeSelf)
{
jointObj.GetChild(0).gameObject.SetActive(true);
}
jointObj.localPosition = filteredJointPos;
jointObj.localRotation = orientJoints[jt];
JointTracked[body.TrackingId][jt.ToString()] = true;
break;
default:
break;
}
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
if (targetJoint.HasValue)
{
lr.useWorldSpace = false;
lr.SetPosition(0, Vector3.zero);
lr.SetPosition(1, Quaternion.Inverse(jointObj.localRotation) * (filteredTargetJointPos.Value - filteredJointPos));
lr.startColor = GetColorForState(sourceJoint.TrackingState);
lr.endColor = GetColorForState(targetJoint.Value.TrackingState);
}
else
{
lr.enabled = false;
}
}
else
{
switch (sourceJoint.TrackingState)
{
case Kinect.TrackingState.NotTracked:
JointTracked[body.TrackingId][jt.ToString()] = useUntrackedJoints;
break;
case Kinect.TrackingState.Inferred:
JointTracked[body.TrackingId][jt.ToString()] = useInferredJoints;
break;
case Kinect.TrackingState.Tracked:
JointTracked[body.TrackingId][jt.ToString()] = true;
break;
default:
break;
}
jointObj.localPosition = filteredJointPos;
jointObj.localRotation = orientJoints[jt];
}
/*if(jointObj.localRotation == zeroQuaternion)
* {
* Vector3 perpendicular = Vector3.Cross(jointObj.localPosition, Vector3.up);
* Vector3 normal = Vector3.Cross(perpendicular, jointObj.localPosition);
*
* // calculate a rotation
* jointObj.rotation.SetLookRotation(normal, jointObj.localPosition);
* }*/
}
}
