//// FULL BODY - MID
/*
* private GameObject CreateBodyObject(ulong id) {
* GameObject body = new GameObject("Body_" + id);
* body.transform.SetParent(gameObject.transform.parent, false);
*
* GameObject head = new GameObject();
* GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
* cube.transform.localScale = new Vector3(0.15f, 0.25f, 0.2f);
* cube.transform.parent = head.transform;
*
* LineRenderer lr = head.AddComponent<LineRenderer>();
* lr.positionCount = 2;
* lr.material = BoneMaterial;
* lr.startWidth = 0.05f;
* lr.endWidth = 0.05f;
*
* head.name = Kinect.JointType.Head.ToString();
* head.transform.parent = body.transform;
*
* return body;
* }
*/
//// FULL BODY - END
//// FULL BODY - BEGIN
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.Joint?targetJoint = null;
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
}
//TODO: find more optimised way for this!
Transform jointObj = bodyObject.transform.Find(jt.ToString());
jointObj.localPosition = GetVector3FromJoint(sourceJoint);
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
if (targetJoint.HasValue)
{
lr.SetPosition(0, jointObj.position);
//TODO: find solution for local/world coordinates problem!
//=> THIS IS HORRIBLE!
// lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
Transform targetObj = bodyObject.transform.Find(_BoneMap[jt].ToString());
targetObj.localPosition = GetVector3FromJoint(targetJoint.Value);
lr.SetPosition(1, targetObj.position);
lr.startColor = GetColorForState(sourceJoint.TrackingState);
lr.endColor = GetColorForState(targetJoint.Value.TrackingState);
}
else
{
lr.enabled = false;
}
}
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
joints = new List <Vector3>(new Vector3[0]);
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.Joint?targetJoint = null;
joints.Add(GetVector3FromJoint(sourceJoint));
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
}
Transform jointObj = bodyObject.transform.Find(jt.ToString());
jointObj.localPosition = GetVector3FromJoint(sourceJoint);
if (jt == 0)
{
//GameObject.FindGameObjectWithTag("comparison").transform.localPosition = new Vector3(jointObj.localPosition.x, jointObj.localPosition.y, jointObj.localPosition.z);
//GameObject.FindGameObjectWithTag("comparision").transform.position = new Vector3(GameObject.FindGameObjectWithTag("comparision").transform.position.x, jointObj.localPosition.y, GameObject.FindGameObjectWithTag("comparision").transform.position.z);
Realtime_Player.SaveCurrentPosition(new Vector3(jointObj.localPosition.x, jointObj.localPosition.y, jointObj.localPosition.z));
}
if (jt == Kinect.JointType.FootLeft)
{
variable_holder.minPos = jointObj.position.y;
print(variable_holder.minPos);
//GameObject.Find("FloorCube").transform.position = new Vector3 (0, variable_holder.minPos, 0);
}
if (jt == Kinect.JointType.FootLeft)
{
variable_holder.yvalue = jointObj.transform.position.y;
}
//if (jt == 0)
//{
// GameObject.FindGameObjectWithTag("KinectPlayer").transform.position = new Vector3(jointObj.localPosition.x, GameObject.FindGameObjectWithTag("KinectPlayer").transform.position.y, 3.45f - jointObj.localPosition.z);
// Realtime_Player_MCS.SaveCurrentPosition(new Vector3(jointObj.localPosition.x, GameObject.FindGameObjectWithTag("MCSRealtime").transform.position.y, jointObj.localPosition.z));
//}
//LineRenderer lr = jointObj.GetComponent<LineRenderer>();
//if (targetJoint.HasValue)
//{
// lr.SetPosition(0, jointObj.localPosition);
// lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
// lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
//}
//else
//{
// lr.enabled = false;
//}
}
}
public SkeletonFrame(int index, Body[] bodies, Floor floor, TimeSpan timeSpan)
{
this.index = index;
this.floor = floor;
this.relativeTime = timeSpan;
this.skeletons = bodies.Select((body) => new Skeleton(body)).ToArray();
}
private GameObject CreateBodyObject(Kinect.Body body, ulong id)
{
GameObject bodyObj = new GameObject("body:" + id);
bodyObj.transform.SetParent(parentTransform);
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
if (FilterJoint(jt))
{
continue;
}
// joints
GameObject jointObj = ConstructJoint();
jointObj.transform.localScale = new Vector3(1.0f, 1.0f, 1.0f);
jointObj.name = string.Format(JOINT_ID_FORMAT, jt.ToString());
jointObj.transform.SetParent(bodyObj.transform);
// bones
if (_BoneMap.ContainsKey(jt))
{
Kinect.Joint destJoint = body.Joints[_BoneMap[jt]];
GameObject boneObj = ConstructBone(body.Joints[jt], destJoint);
string boneId = string.Format(BONE_ID_FORMAT, jt, _BoneMap[jt]);
boneObj.name = boneId;
boneObj.transform.SetParent(bodyObj.transform);
}
}
return(bodyObj);
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
if (FilterJoint(jt))
{
continue;
}
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.Joint?nonNullabledestJoint = null;
Kinect.Joint destJoint = nonNullabledestJoint ?? new Kinect.Joint();
if (_BoneMap.ContainsKey(jt))
{
destJoint = body.Joints[_BoneMap[jt]];
}
Transform jointObj = bodyObject.transform.Find(string.Format(JOINT_ID_FORMAT, jt.ToString()));
jointObj.localPosition = GetVector3FromJoint(sourceJoint);
if (_BoneMap.ContainsKey(jt))
{
string boneId = string.Format(BONE_ID_FORMAT, jt, _BoneMap[jt]);
Transform boneObj = bodyObject.transform.Find(boneId);
UpdateBone(boneObj.gameObject, sourceJoint, destJoint);
}
}
}
void BodyFrameReader_FrameArrived(object sender, Kinect2.BodyFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
using (var frame = reference)
{
if (frame != null)
{
// Bodyを入れる配列を作る
_Bodies = new Kinect2.Body[frame.BodyFrameSource.BodyCount];
// ボディデータを取得
frame.GetAndRefreshBodyData(_Bodies);
foreach (Kinect2.Body body in _Bodies)
{
if (body.IsTracked)
{
_Body = body;
}
}
frame.Dispose();
}
}
}
SimpleBody GenerateSimpleBody2(Kinect.Body body)
{
SimpleBody simpleBody;
var joints = new List <SimpleJoint>();
for (Kinect.JointType jt = Kinect.JointType.KneeLeft; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint joint = body.Joints[jt];
SimpleJoint sendJoint;
sendJoint.Position = GetVector3FromJoint(joint);
sendJoint.TrackingState = (int)joint.TrackingState;
// sendJoint.X = body.JointOrientations[jt].Orientation.X;
// sendJoint.Y = body.JointOrientations[jt].Orientation.Y;
// sendJoint.Z = body.JointOrientations[jt].Orientation.Z;
// sendJoint.W = body.JointOrientations[jt].Orientation.W;
sendJoint.X = (int)(body.JointOrientations[jt].Orientation.X * 10000);
sendJoint.Y = (int)(body.JointOrientations[jt].Orientation.Y * 10000);
sendJoint.Z = (int)(body.JointOrientations[jt].Orientation.Z * 10000);
sendJoint.W = (int)(body.JointOrientations[jt].Orientation.W * 10000);
joints.Add(sendJoint);
}
simpleBody.Joints = joints;
return(simpleBody);
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
Debug.Log(body.Joints [Kinect.JointType.Head].Position.Y - body.Joints [Kinect.JointType.FootLeft].Position.Y);
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.Joint?targetJoint = null;
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
}
Transform jointObj = bodyObject.transform.Find(jt.ToString());
jointObj.localPosition = Vector3.MoveTowards(jointObj.localPosition, GetVector3FromJoint(sourceJoint), 100 * Time.deltaTime);
if (jt.Equals(Kinect.JointType.HandRight))
{
Windows.Kinect.Vector4 orientation = body.JointOrientations [jt].Orientation;
jointObj.localRotation = Quaternion.RotateTowards(jointObj.localRotation, new Quaternion(orientation.X, orientation.Y, orientation.Z, orientation.W), 800 * Time.deltaTime);
//jointObj.Rotate(0, 90, 0);
}
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
if (targetJoint.HasValue)
{
lr.SetPosition(0, jointObj.localPosition);
lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
}
else
{
lr.enabled = false;
}
}
}
public GameObject CreateBodyObject(Body kinectBody, GameObject newBody)
{
ulong id = kinectBody.TrackingId;
_trackedBody = kinectBody;
this.Id = id;
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
GameObject jointObj = new GameObject();
//jointObj.transform.localScale = new Vector3(0.1f, 0.1f, 0.1f);
jointObj.transform.localScale = new Vector3(5f, 5f, 5f);
jointObj.name = jt.ToString();
jointObj.transform.parent = newBody.transform;
this.BodyTransforms.Add(jt, jointObj.transform);
this.BodyPositions.Add(jt, GetVector3FromJoint(kinectBody.Joints[jt]));
this.BodyFilters.Add(jt, new KalmanFilter());
this.PlayerObjects.Add(jt, jointObj);
}
RightHand.init(this, Kinect);
LeftHand.init(this, Kinect);
if (IsMainPlayer)
{
StartCoroutine(AlignTracking());
}
else
{
Head.GetComponentInChildren <Camera>().tag = "Untagged";
}
return(newBody);
}
//--------------------------------------------------------------------------------------
// Implementation of a Holt Double Exponential Smoothing filter. The double exponential
// smooths the curve and predicts. There is also noise jitter removal. And maximum
// prediction bounds. The paramaters are commented in the init function.
//--------------------------------------------------------------------------------------
public void Update(Kinect.Body body)
{
if (body == null)
{
return;
}
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
smoothParams.fJitterRadius = Mathf.Max(0.0001f, smoothParams.fJitterRadius);
TRANSFORM_SMOOTH_PARAMETERS smoothingParams;
foreach (Kinect.JointType jt in Kinect.JointType.GetValues(typeof(Kinect.JointType)))
{
smoothingParams.fSmoothing = smoothParams.fSmoothing;
smoothingParams.fCorrection = smoothParams.fCorrection;
smoothingParams.fPrediction = smoothParams.fPrediction;
smoothingParams.fJitterRadius = smoothParams.fJitterRadius;
smoothingParams.fMaxDeviationRadius = smoothParams.fMaxDeviationRadius;
// If inferred, we smooth a bit more by using a bigger jitter radius
Windows.Kinect.Joint joint = body.Joints[jt];
if (joint.TrackingState == Kinect.TrackingState.Inferred)
{
smoothingParams.fJitterRadius *= 2.0f;
smoothingParams.fMaxDeviationRadius *= 2.0f;
}
// set initial joint value from Kinect
Joint fj = new Joint(body.Joints[jt].Position, body.JointOrientations[jt].Orientation);
UpdateJoint(jt, fj, smoothingParams);
}
}
public GameObject CreateBodyObject(Kinect.Body kinectBody)
{
GameObject body = new GameObject("Body:" + kinectBody.TrackingId);
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
GameObject jointObj = new GameObject();
if (ShowSkeleton)
{
jointObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
LineRenderer lr = jointObj.AddComponent <LineRenderer>();
lr.SetVertexCount(2);
lr.material = BoneMaterial;
lr.SetWidth(0.05f, 0.05f);
}
jointObj.transform.localScale = new Vector3(0.03f, 0.03f, 0.03f);
jointObj.name = jt.ToString();
jointObj.transform.parent = body.transform;
}
Kinect.Joint Neck = kinectBody.Joints[Kinect.JointType.Neck];
Kinect.Joint SpineMid = kinectBody.Joints[Kinect.JointType.SpineMid];
float TorsoLength = GetBoneSize(Neck, SpineMid);
// TorsoLength is .30 (about a foot) for me (6'1").
transform.parent.localScale = new Vector3(TorsoLength * BODY_SCALE, TorsoLength * BODY_SCALE, TorsoLength * BODY_SCALE);
return(body);
}
// Update is called once per frame
void Update()
{
_BodyManager = BodySourceManager.GetComponent <BodySourceManager>();
if (_BodyManager == null)
{
return;
}
Kinect.Body[] data = _BodyManager.GetData();
if (data == null)
{
return;
}
Kinect.Body body = data [0];
if (!body.IsTracked)
{
return;
}
transform.localPosition = GetVector3FromJoint(body.Joints[Kinect.JointType.Head]); // 形状位置を更新
Vector3 centerpoint;
Vector3 wr = GetVector3FromJoint(body.Joints [Kinect.JointType.WristRight]);
Vector3 er = GetVector3FromJoint(body.Joints [Kinect.JointType.ElbowRight]);
centerpoint = (wr + er) / 2;
Vector3 front = wr - er;
rightarm.transform.rotation = Quaternion.LookRotation(front) * r;
rightarm.transform.position = centerpoint;
}
//To be refined with dictonary data
private void RefreshRobot(Kinect.Body body)
{
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
if (jt.ToString() == "Neck")
{
botHead.transform.rotation = GetQuaternionFromJointOrientation(body.JointOrientations[jt]) * Quaternion.Euler(0.0f, 160.0f, 0.0f);
}
if (jt.ToString() == "SpineShoulder")
{
botTorso.transform.rotation = GetQuaternionFromJointOrientation(body.JointOrientations[jt]) * Quaternion.Euler(0.0f, 160.0f, 0.0f);
}
if (jt.ToString() == "ElbowLeft")
{
botShoulderRight.transform.rotation = GetQuaternionFromJointOrientation(body.JointOrientations[jt]) * Quaternion.Euler(180.0f, 160.0f, 0.0f);
}
if (jt.ToString() == "WristLeft")
{
botElbowRight.transform.rotation = GetQuaternionFromJointOrientation(body.JointOrientations[jt]) * Quaternion.Euler(0.0f, 0.0f, 180.0f);
}
if (jt.ToString() == "ElbowRight")
{
botShoulderLeft.transform.rotation = GetQuaternionFromJointOrientation(body.JointOrientations[jt]) * Quaternion.Euler(180.0f, 160.0f, 0.0f);
}
if (jt.ToString() == "WristRight")
{
botElbowLeft.transform.rotation = GetQuaternionFromJointOrientation(body.JointOrientations[jt]) * Quaternion.Euler(0.0f, 0.0f, 180.0f);
}
}
}
private void RefreshHandObject(Kinect.Body body, GameObject bodyObject)
{
Transform Lefthand =
bodyObject.transform.Find(Kinect.JointType.HandLeft.ToString());
Transform Righthand =
bodyObject.transform.Find(Kinect.JointType.HandRight.ToString());
if (Lefthand == null || Righthand == null)
{
return;
}
Lefthand.localPosition =
GetVector3FromJoint(body.Joints[Kinect.JointType.HandLeft]);
Righthand.localPosition =
GetVector3FromJoint(body.Joints[Kinect.JointType.HandRight]);
_righthand_pos = Righthand.localPosition;
_lefthand_pos = Lefthand.localPosition;
lefthandstate = body.HandLeftState.ToString();
righthandstate = body.HandRightState.ToString();
}
internal void UpdateJoints(Kinect.Body body)
{
if (body == null)
{
return;
}
if (this.Joints.Count == 0 || this.jointSmoother == null)
{
Init();
}
// get the floorClipPlace from the body information
Vector4 floorClipPlane = Helpers.FloorClipPlane;
//floorClipPlane = Vector4.one;
// get rotation of camera
Quaternion cameraRotation = Helpers.CalculateFloorRotationCorrection(floorClipPlane);
// generate a vertical offset from floor plane
//Vector3 cameraPosition = Vector3.up * floorClipPlane.w;
Vector3 cameraPosition = Vector3.zero;
//Debug.Log("CameraPosition:" + cameraPosition);
//cameraPosition = Vector3.zero;
// visualize where the camera is
// uncomment if you want to see the camera rotation visualized
//Helpers.DrawDebugQuaternion(cameraPosition, cameraRotation, Helpers.ColorRange.CMYK, .25f);
// update joints for a body
UpdateJoint(GetRootJoint(), body, cameraPosition, cameraRotation);
}
// Refresh the body game object
private void RefreshBodyObject(Kinect.Body body, GameObject bodyobject)
{
//loop over each joint type
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints [jt]; // Current joint in loop is source joint
Kinect.Joint?targetJoint = null; // Target joint
// if bonemap contains joint --> it is the targetjoint
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints [_BoneMap [jt]]; // Target joint is next joint
}
Transform jointObj = bodyobject.transform.FindChild(jt.ToString()); // Transform of child joint of bodyobject through string
jointObj.localPosition = GetVector3FromJoint(sourceJoint); // Position of transform joint is position of sourceJoint
LineRenderer lr = jointObj.GetComponent <LineRenderer> ();
// Draw line from source to target if targetJoint is present
if (targetJoint.HasValue)
{
lr.SetPosition(0, jointObj.localPosition); // current join position
lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value)); // position of target join
lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState)); // color line regarding state
}
else
{
lr.enabled = false;
}
}
}
/**
* 뼈의 위치를 가져와 설정하는 함수
* @param 키넥트 바디, 뼈 부모 오브젝트
*/
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
// 테스트용 : 필요한 부분인 SpineBase 뼈 데이터만 가져온다.
Kinect.Joint sourceJoint = body.Joints[Kinect.JointType.SpineBase];
// 사용되는 SpineBase를 가져온다
Transform jointObj = bodyObject.transform.FindChild("SpineBase");
Vector3 v = GetVector3FromJoint(sourceJoint);
if (_OverLab [bodyObject])
{
// 뼈의 위치를 확인. 인식 범위빡으로 나갔을 경우 사용여부 설정
float over = float.Parse(PlayerPrefs.GetString("MaxRange")) - float.Parse(PlayerPrefs.GetString("MinRange"));
if (v.z > over - (over / 10))
{
return;
}
_OverLab [bodyObject] = false;
}
if (Mathf.Abs(v.z - jointObj.position.z) > 0.5 || Mathf.Abs(v.x - jointObj.position.x) > 0.5)
{
jointObj.localPosition = GetVector3FromJoint(sourceJoint);
}
}
/* New Code (7/28 ) : Getting Hand Coordinate */
private Vector3 getHandCoordinates(Kinect.Body body)
{
Vector3 HandCoordinates = new Vector3(0, 0, 0);
// Change Cursor Type
if (Input.GetMouseButton(0)) // 0
//When Right Clicked
{
Debug.Log("Right : Mouse Down Called");
HandCoordinates = GetVector3FromJoint(body.Joints [Kinect.JointType.HandTipRight]);
Debug.Log("Pressed right Click At: " + HandCoordinates);
CreateMesh(HandCoordinates);
}
if (Input.GetMouseButton(1)) // 1
//When Left Clicked
{
HandCoordinates = GetVector3FromJoint(body.Joints [Kinect.JointType.HandTipLeft]);
CreateMesh(HandCoordinates);
}
if (Input.GetMouseButton(2))
{
if (curMesh != null)
{
//Transform root_transform =
GameObject root = curMesh.transform.root.gameObject;
Rigidbody rootRigidBody = root.AddComponent <Rigidbody>();
rootRigidBody.mass = 5;
//curMesh = null;
Destroy(curMesh);
}
}
return(HandCoordinates);
}
SimpleBody GenerateSimpleBody1(Kinect.Body body)
{
SimpleBody simpleBody;
var joints = new List <SimpleJoint>();
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.HipLeft; jt++)
{
Kinect.Joint joint = body.Joints [jt];
SimpleJoint sendJoint;
if (jt != Kinect.JointType.Neck && jt != Kinect.JointType.Head)
{
sendJoint.Position = GetVector3FromJoint(joint);
sendJoint.TrackingState = (int)joint.TrackingState;
sendJoint.X = (int)(body.JointOrientations[jt].Orientation.X * 10000);
sendJoint.Y = (int)(body.JointOrientations[jt].Orientation.Y * 10000);
sendJoint.Z = (int)(body.JointOrientations[jt].Orientation.Z * 10000);
sendJoint.W = (int)(body.JointOrientations[jt].Orientation.W * 10000);
}
else
{
sendJoint.Position = Vector3.zero;
sendJoint.TrackingState = 0;
sendJoint.X = 0;
sendJoint.Y = 0;
sendJoint.Z = 0;
sendJoint.W = 0;
}
joints.Add(sendJoint);
}
simpleBody.Joints = joints;
return(simpleBody);
}
private void MoveIKGoal(Kinect.Body body)
{
Vector3 offset = GetVector3FromJoint(body.Joints[Kinect.JointType.Head]);
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint joint = body.Joints[jt];
GameObject temp = null;
if (_Joints.TryGetValue(jt, out temp))
{
GameObject jointPos = _Joints[jt];
jointPos.transform.localPosition = GetVector3WithOffset(joint, offset); //USE OFFSET?
}
// HAND ANGLE AND ROTATION
if (jt == Kinect.JointType.WristLeft)
{
GameObject wrist = _Joints[jt];
wrist.transform.LookAt(handTipLeft.transform.position);
// wrist.transform.LookAt(ThumbLeft.transform.position);
}
if (jt == Kinect.JointType.WristRight)
{
GameObject wrist = _Joints[jt];
wrist.transform.LookAt(handTipRight.transform.position);
// wrist.transform.LookAt(ThumbRight.transform.position);
}
}
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
if ((body.HandRightConfidence == Kinect.TrackingConfidence.High && body.HandRightState == Kinect.HandState.Closed) || (body.HandLeftConfidence == Kinect.TrackingConfidence.High && body.HandLeftState == Kinect.HandState.Closed))
{
zoom = 1;
}
else if ((body.HandRightConfidence == Kinect.TrackingConfidence.High && body.HandRightState == Kinect.HandState.Open) || body.HandLeftConfidence == Kinect.TrackingConfidence.High && body.HandLeftState == Kinect.HandState.Open)
{
zoom = 2;
}
foreach (Kinect.JointType _joint in _joints)
{
Joint sourceJoint = body.Joints[_joint];
Vector3 targetPosition = GetVector3FromJoint(sourceJoint);
targetPosition.z = 0;
Transform jointObject = bodyObject.transform.Find(_joint.ToString());
jointObject.position = targetPosition;
if (_joint.ToString().Equals("HandRight"))
{
HandRightX = jointObject.position.x;
HandRightY = jointObject.position.y;
}
if (_joint.ToString().Equals("HandLeft"))
{
HandLeftX = jointObject.position.x;
HandLeftY = jointObject.position.y;
}
if (_joint.ToString().Equals("SpineMid"))
{
spineX = jointObject.position.x;
spineY = jointObject.position.y;
}
}
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
// Get position and rotation of every body joint.
// Position will be computed as 2D vector for displaying purposes
Vector3[] bodyJoints = BodyAnalysis.convertToUnityPosition(body.Joints);
Quaternion[] bodyOrientation = BodyAnalysis.convertToUnityOrientation(body.JointOrientations);
int i = 0;
foreach (Transform child in bodyObject.transform)
{
// If the current body joint is not being tracked (infinity), ignore
if ((bodyJoints[i].x == Mathf.Infinity || bodyJoints[i].x == Mathf.NegativeInfinity) ||
(bodyJoints[i].y == Mathf.Infinity || bodyJoints[i].y == Mathf.NegativeInfinity))
{
i++;
if (i >= bodyJoints.Length)
{
break;
}
continue;
}
// Place the current body joint on the desired location
child.transform.position = bodyJoints[i] * scale;
child.transform.localRotation = bodyOrientation[i];
i++;
if (i >= bodyJoints.Length)
{
break;
}
}
}
//method for detecting a body in kinect frame
void _bodyFrameReader_FrameArrived(object sender, Windows.Kinect.BodyFrameArrivedEventArgs args)
{
var frame = args.FrameReference;
using (var multiSourceFrame = frame.AcquireFrame())
{
multiSourceFrame.GetAndRefreshBodyData(_bodies); //обновляем данные о найденных людях
_currentBody = null;
foreach (var body in _bodies)
{
if (body != null && body.IsTracked)
{
_currentBody = body; // для простоты берем первого найденного человека
break;
}
}
if (_currentBody != null)
{
_gestureFrameSource.TrackingId = _currentBody.TrackingId;
_gestureFrameReader.IsPaused = false;
}
else
{
_gestureFrameSource.TrackingId = 0;
_gestureFrameReader.IsPaused = true;
}
}
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
Transform avatar = bodyObject.transform.FindChild("Avatar");
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.JointOrientation sourceJointOrientation = body.JointOrientations[jt];
if (_BoneMap.ContainsKey(jt))
{
if (_RigMap.ContainsKey(jt.ToString()))
{
Transform avatarItem = avatar.FindChild(_RigMap[jt.ToString()]);
Transform bodyItem = bodyObject.transform.FindChild(jt.ToString());
if (jt.ToString() == "SpineBase")
{
avatarItem.position = bodyItem.position;
}
avatarItem.rotation = bodyItem.rotation * _RigMapOffsets[jt.ToString()];
}
}
Transform jointObj = bodyObject.transform.FindChild(jt.ToString());
jointObj.localPosition = GetVector3FromJoint(sourceJoint);
jointObj.localRotation = GetQuaternionFromJointOrientation(sourceJointOrientation);
}
}
public Kinect.Body GetClosestBody()
{
Kinect.Body result = null;
double closestBodyDistance = double.MaxValue;
if (this.Bodies != null)
{
foreach (var body in this.Bodies)
{
if (body.IsTracked)
{
var currentLocation = body.Joints[Kinect.JointType.SpineBase].Position;
var currentDistance = VectorLength(currentLocation);
if (result == null || currentDistance < closestBodyDistance)
{
result = body;
closestBodyDistance = currentDistance;
}
}
}
}
return(result);
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.Joint?targetJoint = null;
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
}
Transform jointObj = bodyObject.transform.Find(jt.ToString());
jointObj.localPosition = GetVector3FromJoint(sourceJoint);
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
if (targetJoint.HasValue)
{
lr.SetPosition(0, jointObj.localPosition);
lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
}
else
{
lr.enabled = false;
}
}
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.Joint?targetJoint = null;
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
}
Transform jointObj = bodyObject.transform.FindChild(jt.ToString());
jointObj.localPosition = GetVector3FromJoint(sourceJoint, groundPosition);
jointObj.RotateAround(CameraPivot, transform.right, _BodyManager.CameraAngle * -1);
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
if (targetJoint.HasValue)
{
lr.SetPosition(0, jointObj.localPosition);
Vector3 endpoint = RotateAroundPoint(GetVector3FromJoint(targetJoint.Value, groundPosition), CameraPivot, Quaternion.Euler(_BodyManager.CameraAngle * -1, 0, 0));
lr.SetPosition(1, endpoint);
lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
}
else
{
lr.enabled = false;
}
}
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
Vector3 center = GetVector3FromJoint(body.Joints[Kinect.JointType.SpineBase]);
for (Kinect.JointType jt = Kinect.JointType.SpineMid; jt <= Kinect.JointType.SpineMid; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
// Kinect.Joint? targetJoint = null;
// if (_BoneMap.ContainsKey(jt))
// {
// targetJoint = body.Joints[_BoneMap[jt]];
// }
Transform jointObj = bodyObject.transform.Find(jt.ToString());
jointObj.localPosition = GetVector3FromJoint(sourceJoint);
/*
* LineRenderer lr = jointObj.GetComponent<LineRenderer>();
* if (targetJoint.HasValue)
* {
* lr.SetPosition(0, jointObj.localPosition);
* lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
* lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
* }
* else
* {
* lr.enabled = false;
* }
*/
}
}
private void TrackingSpecific(Kinect.Body body)
{
Kinect.Joint sourceJoint = body.Joints[Kinect.JointType.Head];
pickBone.localPosition = GetVector3FromJoint(sourceJoint);
//Debug.Log(pickBone.position);
}
// Update is called once per frame
void Update()
{
_body = _bodySourceManager.GetData().FirstOrDefault(b => b.IsTracked);
if (_body.IsTracked)
{
SetBoneTransform(Head, Kinect.JointType.Head);
SetBoneTransform(Neck, Kinect.JointType.Neck);
SetBoneTransform(Spine_Shoulder, Kinect.JointType.SpineShoulder);
SetBoneTransform(Spine_Mid, Kinect.JointType.SpineMid);
SetBoneTransform(Spine_Base, Kinect.JointType.SpineBase);
SetBoneTransform(Shoulder_Left, Kinect.JointType.ShoulderLeft);
SetBoneTransform(Shoulder_Right, Kinect.JointType.ShoulderRight);
SetBoneTransform(Elbow_Left, Kinect.JointType.ElbowLeft);
SetBoneTransform(Elbow_Right, Kinect.JointType.ElbowRight);
SetBoneTransform(Wrist_Left, Kinect.JointType.WristLeft);
SetBoneTransform(Wrist_Right, Kinect.JointType.WristRight);
SetBoneTransform(Hip_Left, Kinect.JointType.HipLeft);
SetBoneTransform(Hip_Right, Kinect.JointType.HipRight);
SetBoneTransform(Knee_Left, Kinect.JointType.KneeLeft);
SetBoneTransform(Knee_Right, Kinect.JointType.KneeRight);
SetBoneTransform(Ankle_Left, Kinect.JointType.AnkleLeft);
SetBoneTransform(Ankle_Right, Kinect.JointType.AnkleRight);
SetBoneTransform(Foot_Left, Kinect.JointType.FootLeft);
SetBoneTransform(Foot_Right, Kinect.JointType.FootRight);
}
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
outputList.Add(Time.frameCount.ToString());
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.Joint?targetJoint = null;
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
}
Transform jointObj = bodyObject.transform.Find(jt.ToString());
//Transform jointObjDancer = DancerBody.transform.Find(jt.ToString());
jointObj.localPosition = GetVector3FromJoint(sourceJoint);
//jointObjDancer.localPosition = GetVector3FromJoint(sourceJoint);
// write the coordinates to a file
// get coordinates
float x = jointObj.localPosition.x;
float y = jointObj.localPosition.y;
float z = jointObj.localPosition.z;
// text to be printed
string outputline = sourceJoint.JointType.ToString() + ": " + x.ToString() + "," + y.ToString() + "," + z.ToString();
outputList.Add(outputline);
// lines between the joints
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
//LineRenderer lr2 = jointObjDancer.GetComponent<LineRenderer>();
if (targetJoint.HasValue)
{
lr.SetPosition(0, jointObj.localPosition);
lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
}
else
{
lr.enabled = false;
}
}
// check if current body position matches anything
moveMatch(body, bodyObject);
}
public Skeleton(Body body)
{
this.ID = body.TrackingId;
this.Lean = body.Lean;
this.LeanTrackingState = (TrackingState)body.LeanTrackingState;
this.State |= body.IsRestricted ? 0x01 : 0x00;
this.State |= body.HandLeftState == HandState.Closed ? 0x02 : body.HandLeftState == HandState.Open ? 0x04 : body.HandLeftState == HandState.Lasso ? 0x08 : 0x00;
this.State |= body.HandRightState == HandState.Closed ? 0x20 : body.HandRightState == HandState.Open ? 0x40 : body.HandRightState == HandState.Lasso ? 0x80 : 0x00;
this.joints = new SkeletonJoint[Body.JointCount];
for (int i = 0; i < Body.JointCount; ++i)
{
joints[i] = new SkeletonJoint(body.Joints[(JointType)i], body.JointOrientations[(JointType)i]);
}
}
public void TrackHead(Body[] bodies)
{
bool foundTrackedBody = false;
float distanceToNearest = float.MaxValue;
foreach (Body body in bodies) {
if (body.IsTracked) {
var cameraSpacePoint = body.Joints[JointType.Head].Position;
if (cameraSpacePoint.Z < distanceToNearest) {
distanceToNearest = cameraSpacePoint.Z;
headPosition = ConvertCameraSpacePoint.ToVector3(cameraSpacePoint);
foundTrackedBody = true;
}
}
}
if (!foundTrackedBody) {
headPosition = defaultHeadPosition;
}
}
public Bone GetBoneOrientations(Body body)
{
Dictionary<JointType, Quaternion> orientations = new Dictionary<JointType,Quaternion>();
Queue<Bone> boneQueue = new Queue<Bone>();
boneQueue.Enqueue(_rootBone);
while (boneQueue.Count > 0)
{
var parentBone = boneQueue.Dequeue();
foreach (var childBone in parentBone.ChildBones)
{
UpdateBone(childBone, body);
boneQueue.Enqueue(childBone);
}
}
return _rootBone;
}
void FixedUpdate()
{
BodySourceManager bodyManager = BodySourceManager.GetComponent<BodySourceManager>();
WindowsKinect.Body[] allBodiesInTheScreen = bodyManager == null
? new WindowsKinect.Body[0]
: GetAllSkeletonsInTheScreen(bodyManager);
if (allBodiesInTheScreen.Length < 1)
{
PlayerSkeleton = null;
return;
}
bool noTrackingId = trackingId == null;
WindowsKinect.Body firstBodyEnteringTheScreen = allBodiesInTheScreen[0];
PlayerSkeleton = noTrackingId ? firstBodyEnteringTheScreen : GetCurrentSkeleton(trackingId.Value, allBodiesInTheScreen);
trackingId = noTrackingId
? (ulong?)firstBodyEnteringTheScreen.TrackingId
: (PlayerSkeleton == null ? (ulong?)null : (ulong?)PlayerSkeleton.TrackingId);
}
public void GetAndRefreshBodyData(RootSystem.Collections.Generic.IList<Windows.Kinect.Body> bodies)
{
if (_pNative == RootSystem.IntPtr.Zero) {
throw new RootSystem.ObjectDisposedException ("BodyFrame");
}
int _bodies_idx = 0;
var _bodies = new RootSystem.IntPtr[bodies.Count];
for (int i = 0; i < bodies.Count; i++) {
if (bodies [i] == null) {
bodies [i] = new Body ();
}
_bodies [_bodies_idx] = bodies [i].GetIntPtr ();
_bodies_idx++;
}
Windows_Kinect_BodyFrame_GetAndRefreshBodyData (_pNative, _bodies, bodies.Count);
Helper.ExceptionHelper.CheckLastError ();
for (int i = 0; i < bodies.Count; i++) {
bodies [i].SetIntPtr (_bodies [i]);
}
}
void Reader_MultiSourceFrameArrived(object sender, Kinect2.MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
using (var frame = reference.BodyFrameReference.AcquireFrame()) {
if (frame != null) {
_Bodies = new Kinect2.Body[frame.BodyFrameSource.BodyCount];
frame.GetAndRefreshBodyData (_Bodies);
foreach(Kinect2.Body body in _Bodies)
{
if(body.IsTracked)
{
if (sAutoMotionCaptureDevicesSelecter != null)
{
sAutoMotionCaptureDevicesSelecter.bMoveTransform = true;
}
fFBXRecStart -= Time.deltaTime;
if (fFBXRecStart <= 0.0) {
if (sFBXExporterForUnity != null)
{
sFBXExporterForUnity.bOutAnimation = true;
}
}
_Body = body;
for( int ii = 0; ii < (int)Kinect2.JointType.ThumbRight - 4; ii++)
{
if( ((uint)Mask & (uint)(1 << ii) ) > 0 && (_nullMask & (uint)(1 << ii)) <= 0 )
{
RotateJoint(body, ii);
}
}
}
}
frame.Dispose();
}
}
}
// Update is called once per frame
void Update () {
if (bodySourceManager == null)
{
return;
}
bodyManager = bodySourceManager.GetComponent<BodySourceManager>();
if(bodyManager == null)
{
return;
}
// get data from kinect
Kinect.Body[] data = bodyManager.GetData();
if(data == null)
{
return;
}
// get list of currently tracked bodies
Kinect.Body potentialNewTrackedBody = null;
bool stillTrackingPreviousBody = false; // check if the previously tracked body is still there
foreach(Kinect.Body body in data)
{
if(body == null)
{
continue;
}
if (body.IsTracked)
{
// no body being track, get the first body found
if (currentTrackedObject == null)
{
currentTrackedObject = body;
}
// the previous body may already disappearred, save the first body found just in case
if (potentialNewTrackedBody == null)
{
potentialNewTrackedBody = body;
}
// still tracking previous body
if (body.TrackingId == currentTrackedObject.TrackingId)
{
stillTrackingPreviousBody = true;
break;
}
}
}
//
// the previously tracked body have disappear, get the new trackid found
if(!stillTrackingPreviousBody)
{
currentTrackedObject = potentialNewTrackedBody;
}
if(currentTrackedObject == null)
{
return;
}
//////////////////////////////////////////
// Everything after this is getting data of the body and manipulate the world
ScoreManager.ready = true;
// if holding something and left hand is closed, move the object according to hand movement
if (currentTrackedObject.HandLeftState == Kinect.HandState.Closed && grabbed)
{
Debug.Log("Holding Object");
DragObject();
}
// if not holding anything and left hand is closed, attempt to grab the object in the desired direction
else if (!grabbed && currentTrackedObject.HandLeftState == Kinect.HandState.Closed)
{
Debug.Log("Trying to Grab Object");
GrabObject();
}
// release hand while holding something will initiate throw
else if (currentTrackedObject.HandLeftState == Kinect.HandState.Open && grabbed)
{
Debug.Log("Throwing Object");
ThrowObject();
}
// shoot wave bullet if two hand open
else if (currentTrackedObject.HandLeftState == Kinect.HandState.Open && currentTrackedObject.HandRightState == Kinect.HandState.Open) {
Debug.Log("Wave Attack");
UpdateAimTarget();
GetComponent<Shooting>().Shoot(waveBullet, aimTarget.transform.position, aimTarget.transform.rotation);
}
// shoot wave bullet if left hand open and not holding anything
else if (currentTrackedObject.HandLeftState == Kinect.HandState.Open) {
Debug.Log("Bullet Attack");
UpdateAimTarget();
GetComponent<Shooting>().Shoot(lightBullet, aimTarget.transform.position, aimTarget.transform.rotation);
}
// other action should reset holding state
else
{
//currentlyGrabbedObj = null;
Debug.Log("Unknow Action");
grabbed = false;
}
//Debug.Log(currentTrackedObject.HandRightState);
//Debug.Log(previousHandState);
previousHandState = currentTrackedObject.HandRightState;
SaberControl();
}
// Use this for initialization
void Start () {
currentlyGrabbedObj = null;
currentTrackedObject = null;
}
// Update is called once per frame
void Update () {
if (BodySourceManager == null)
{
return;
}
_BodyManager = BodySourceManager.GetComponent<BodySourceManager>();
if (_BodyManager == null)
{
return;
}
Kinect.Body[] data = _BodyManager.GetData();
if (data == null)
{
return;
}
List<ulong> trackedIds = new List<ulong>();
foreach(var body in data)
{
if (body == null)
{
continue;
}
if(body.IsTracked)
{
trackedIds.Add (body.TrackingId);
trackedBody = body;
break;
}
}
// just get the first tracked id
if (trackedIds.Count == 0) {
Debug.Log ("没有追踪到目标用户");
} else {
Debug.Log("追踪到了目标用户");
// analysis the action of user
analysisUserAction();
}
}
public void UpdateBodyData(Kinect.Body body)
{
LastBodyData = body;
var jointUpdates = body.Joints; //the kinect sdk makes a new dictionary EVERY CALL to .Joints and .JointOrientations
var jointOrientations = body.JointOrientations;
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = jointUpdates[jt];
Kinect.Vector4 sourceOrientation = jointOrientations[jt].Orientation;
Kinect.Joint? targetJoint = null;
if (JointConnections.ContainsKey(jt))
{
targetJoint = jointUpdates[JointConnections[jt]];
}
Transform jointObj = JointMapping[jt];
Vector3 mewPosition = Unmirror(sourceJoint.Position.GetVector3()) * BodyScale;
Quaternion mewRotation = sourceOrientation.GetQuaternion();
if (FirstDataSet)
{
jointObj.localPosition = mewPosition;
jointObj.localRotation = mewRotation;
}
else
{
jointObj.localPosition = Vector3.Lerp(jointObj.localPosition, mewPosition, Time.deltaTime * PositionSmoothing);
jointObj.localRotation = Quaternion.Lerp(jointObj.localRotation, mewRotation, Time.deltaTime * RotationSmoothing);
}
if (ShowJointConnections == true)
{
LineRenderer lr = JointLineMapping[jt];
if (targetJoint.HasValue)
{
Transform targetJointObj = JointMapping[targetJoint.Value.JointType];
lr.SetPosition(0, jointObj.position);
lr.SetPosition(1, targetJointObj.position);
lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
}
else
{
lr.enabled = false;
}
}
}
if (FirstDataSet == true)
{
FirstDataSet = false;
Initialize();
}
if (CurrentlyActiveAvatar == true)
{
OVRManager.instance.transform.position = JointMapping[Kinect.JointType.Head].position;
}
}
private void TrackHandMovement(Body body)
{
double MoveThreshold = 0.0008;
Windows.Kinect.Joint leftHand = body.Joints[JointType.HandLeft];
Windows.Kinect.Joint rightHand = body.Joints[JointType.HandRight];
Windows.Kinect.Joint leftShoulder = body.Joints[JointType.ShoulderLeft];
Windows.Kinect.Joint rightShoulder = body.Joints[JointType.ShoulderRight];
Windows.Kinect.Joint rightHip = body.Joints[JointType.HipRight];
// the right hand joint is being tracked
if (rightHand.TrackingState == TrackingState.Tracked)
{
// the hand is sufficiently in front of the shoulder
if (rightShoulder.Position.Z - rightHand.Position.Z > 0.2)
{
double xScaled = (rightHand.Position.X - leftShoulder.Position.X) / ((rightShoulder.Position.X - leftShoulder.Position.X) * 2) * Screen.width;
double yScaled = (rightHand.Position.Y - rightShoulder.Position.Y) / (rightHip.Position.Y - rightShoulder.Position.Y) * Screen.height;
// the hand has moved enough to update screen position (jitter control / smoothing)
if (Math.Abs(rightHand.Position.X - xPrevious) > MoveThreshold || Math.Abs(rightHand.Position.Y - yPrevious) > MoveThreshold)
{
RightHandX = xScaled;
RightHandY = yScaled;
xPrevious = rightHand.Position.X;
yPrevious = rightHand.Position.Y;
}
}
}
}
public void updateCurrentPlayer(List<ulong> trackedIds)
{
if (StoredBodies.Count == 1) {
CurrentPlayer = StoredBodies[trackedIds[0]];
}
else if (StoredBodies.Count > 1)
{
CurrentPlayer = StoredBodies[trackedIds[0]]; //Do something better here!
}
else
{
//Debug.Log("Stored Bodies not initilized");
}
}
void Update()
{
Animator av = goBody.GetComponent<Animator>();
if (BodySourceManager == null) {
return;
}
_BodyManager = BodySourceManager.GetComponent<BodySourceManager>();
if (_BodyManager == null) {
return;
}
Kinect.Body[] data = _BodyManager.GetData();
if (data == null) {
return;
}
List<ulong> trackedIds = new List<ulong>();
foreach (var body in data) {
if (body == null) {
continue;
}
if (body.IsTracked) {
trackedIds.Add(body.TrackingId);
}
}
List<ulong> knownIds = new List<ulong>(_Bodies.Keys);
// First delete untracked bodies
foreach (ulong trackingId in knownIds) {
if (!trackedIds.Contains(trackingId)) {
//Destroy(_Bodies[trackingId]);
_Bodies.Remove(trackingId);
}
}
foreach (var body in data) {
if (body == null) {
continue;
_body = null;
}
_body = body;
if (body.IsTracked) {
if (!_Bodies.ContainsKey(body.TrackingId)) {
_Bodies[body.TrackingId] = CreateBodyObject(body.TrackingId);
}
RefreshBodyObject(body, _Bodies[body.TrackingId]);
}
}
}
void UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
CameraSpacePoint vPrevRawPosition;
CameraSpacePoint vPrevFilteredPosition;
CameraSpacePoint vPrevTrend;
CameraSpacePoint vRawPosition;
CameraSpacePoint vFilteredPosition;
CameraSpacePoint vPredictedPosition;
CameraSpacePoint vDiff;
CameraSpacePoint vTrend;
float fDiff;
bool bJointIsValid;
Windows.Kinect.Joint joint = body.Joints[jt];
vRawPosition = joint.Position;
vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition;
vPrevTrend = m_pHistory[(int)jt].m_vTrend;
vPrevRawPosition = m_pHistory[(int)jt].m_vRawPosition;
bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
m_pHistory[(int)jt].m_dwFrameCount = 0;
}
// Initial start values
if (m_pHistory[(int)jt].m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = CSVectorZero();
m_pHistory[(int)jt].m_dwFrameCount++;
}
else if (m_pHistory[(int)jt].m_dwFrameCount == 1)
{
vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
m_pHistory[(int)jt].m_dwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),
CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),
CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
m_pHistory[(int)jt].m_vRawPosition = vRawPosition;
m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition;
m_pHistory[(int)jt].m_vTrend = vTrend;
// Output the data
m_pFilteredJoints[(int)jt] = vPredictedPosition;
}
//--------------------------------------------------------------------------------------
// Implementation of a Holt Double Exponential Smoothing filter. The double exponential
// smooths the curve and predicts. There is also noise jitter removal. And maximum
// prediction bounds. The paramaters are commented in the init function.
//--------------------------------------------------------------------------------------
public void UpdateFilter(Body pBody)
{
if (pBody == null)
{
return;
}
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
m_fJitterRadius = Mathf.Max( 0.0001f, m_fJitterRadius );
TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
SmoothingParams.fSmoothing = m_fSmoothing;
SmoothingParams.fCorrection = m_fCorrection;
SmoothingParams.fPrediction = m_fPrediction;
SmoothingParams.fJitterRadius = m_fJitterRadius;
SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;
// If inferred, we smooth a bit more by using a bigger jitter radius
Windows.Kinect.Joint joint = pBody.Joints[jt];
if ( joint.TrackingState == TrackingState.Inferred )
{
SmoothingParams.fJitterRadius *= 2.0f;
SmoothingParams.fMaxDeviationRadius *= 2.0f;
}
UpdateJoint(pBody, jt, SmoothingParams);
}
}
private void UpdateBone(Bone bone, Body body)
{
var joints = body.Joints;
Joint parentJoint = joints[bone.ParentJoint];
Joint childJoint = joints[bone.ChildJoint];
bone.TrackingState = GetBoneTrackingState(parentJoint, childJoint);
var orientation = body.JointOrientations[bone.ChildJoint].Orientation;
Quaternion q = new Quaternion(orientation.X, orientation.Y, orientation.Z, orientation.W);
//Vector3 parentVector = GetBoneVector(nearJoint, parentJoint);
//Vector3 childVector = GetBoneVector(parentJoint, childJoint);
//Quaternion quaternion = GetQuaternionForVectors(parentVector, childVector);
bone.Rotation = q;
}
