public Body()
{
this.IsTracked = true;
this.Joints = new Dictionary <JointType, Joint>();
this.JointOrientations = new Dictionary <JointType, JointOrientation>();
foreach (JointType jointType in Enum.GetValues(typeof(JointType)))
{
Joints[jointType] = new Joint();
JointOrientations[jointType] = new JointOrientation();
}
}
void UpdateJoints(Transform parent, Vector3 totalTrans)
{
foreach (Transform child in parent)
{
//Debug.Log(child.name);
if (kinectJointIDX.ContainsKey(child.name))
{
//Transform JointTransform = BodySourceView.JointTransforms[kinectJointIDX[child.name]];
Windows.Kinect.JointOrientation JointOrientation =
BodySourceView.JointOrientations[kinectJointIDX[child.name]];
//Vector3 kinectPos = (JointTransform != null) ? JointTransform.localPosition : child.position;
Quaternion kinectRot = (JointOrientation != null) ?
new Quaternion(JointOrientation.Orientation.X,
JointOrientation.Orientation.Y,
JointOrientation.Orientation.Z,
JointOrientation.Orientation.W) : child.rotation;
//kinectRot = Quaternion.Inverse(parent.rotation) * kinectRot;
Vector3 eulerRot = kinectRot.eulerAngles;
eulerRot = new Vector3(eulerRot.z, eulerRot.y, eulerRot.x);
kinectRot = Quaternion.Euler(eulerRot);
// Vector3 rotateVec = kinectPos-parent.position;
// kinectRot = Quaternion.LookRotation(rotateVec, Vector3.up);
//kinectPos.z = -kinectPos.z;
//Debug.Log(child.name + kinectPos);
//child.position = kinectPos;// - totalTrans;
child.localRotation = kinectRot;
//Debug.Log(rotateVec);
}
else
{
child.rotation = parent.rotation;
}
UpdateJoints(child, child.position + totalTrans);
}
}
public bool Equals(JointOrientation obj)
{
return JointType.Equals(obj.JointType) && Orientation.Equals(obj.Orientation);
}
// Token: 0x06002D1E RID: 11550 RVA: 0x000DF3F8 File Offset: 0x000DD7F8
public bool Equals(JointOrientation obj)
{
return(this.JointType.Equals(obj.JointType) && this.Orientation.Equals(obj.Orientation));
}
private static Quaternion GetQuaternion3FromJoint(Kinect.JointOrientation joint)
{
return(new Quaternion(joint.Orientation.X, joint.Orientation.Y, joint.Orientation.Z, joint.Orientation.W));
}
public void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
// Set the position of the avatar based on the kinect body location
Kinect.Joint SpineMid = body.Joints[Kinect.JointType.SpineMid];
transform.parent.position = GetVector3FromJoint(SpineMid);
// Set the rotation of the avatar based on the kinect body rotation
Kinect.JointOrientation SpineShoulderOrientation = body.JointOrientations[Kinect.JointType.SpineShoulder];
RotateAvatar(transform, SpineShoulderOrientation);
// Get the position and rotation of both hands from Kinect
Kinect.Joint HandRight = body.Joints[Kinect.JointType.HandRight];
Kinect.Joint HandLeft = body.Joints[Kinect.JointType.HandLeft];
Kinect.JointOrientation HandRightOrientation = body.JointOrientations[Kinect.JointType.HandRight];
Kinect.JointOrientation HandLeftOrientation = body.JointOrientations[Kinect.JointType.HandLeft];
// Get the position and rotation of both feet from Kinect
Kinect.Joint FootRight = body.Joints[Kinect.JointType.FootRight];
Kinect.Joint FootLeft = body.Joints[Kinect.JointType.FootLeft];
Kinect.JointOrientation FootRightOrientation = body.JointOrientations[Kinect.JointType.AnkleRight];
Kinect.JointOrientation FootLeftOrientation = body.JointOrientations[Kinect.JointType.AnkleLeft];
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);
// Map the left and right IK targets to the left and right hand and foot joints in the Kinect model
if (jt.ToString().Equals("HandLeft"))
{
_HandRightTarget.transform.position = jointObj.localPosition;
_HandRightTarget.transform.rotation = GetQuaternionFromJointOrientation(HandLeftOrientation);
_HandRightTarget.transform.Rotate(HandLeftRotationAdjustment);
}
if (jt.ToString().Equals("HandRight"))
{
_HandLeftTarget.transform.position = jointObj.localPosition;
_HandLeftTarget.transform.rotation = GetQuaternionFromJointOrientation(HandRightOrientation);
_HandLeftTarget.transform.Rotate(HandRightRotationAdjustment);
}
if (jt.ToString().Equals("FootLeft"))
{
_FootRightTarget.transform.position = jointObj.localPosition;
_FootRightTarget.transform.rotation = GetQuaternionFromJointOrientation(FootLeftOrientation);
_FootRightTarget.transform.Rotate(FootLeftRotationAdjustment);
}
if (jt.ToString().Equals("FootRight"))
{
_FootLeftTarget.transform.position = jointObj.localPosition;
_FootLeftTarget.transform.rotation = GetQuaternionFromJointOrientation(FootRightOrientation);
_FootLeftTarget.transform.Rotate(FootRightRotationAdjustment);
}
if (ShowSkeleton)
{
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 RotateAvatar(Transform t, Kinect.JointOrientation jo)
{
transform.parent.rotation = GetQuaternionFromJointOrientation(jo);
transform.parent.Rotate(new Vector3(0f, 180f, 0f));
}
private Vector3 GetEulerAnglesFromJointOrientation(Kinect.JointOrientation jo)
{
Quaternion joQuaternion = new Quaternion(jo.Orientation.X, jo.Orientation.Y, jo.Orientation.Z, jo.Orientation.W);
return(joQuaternion.eulerAngles);
}
private Quaternion GetQuaternionFromJointOrientation(Kinect.JointOrientation jo)
{
Quaternion joQuaternion = new Quaternion(jo.Orientation.X, jo.Orientation.Y, jo.Orientation.Z, jo.Orientation.W);
return(joQuaternion);
}
public static Quaternion KinectData2Quaternion(Kinect.JointOrientation joint)
{
return(new Quaternion(joint.Orientation.X, joint.Orientation.Y, joint.Orientation.Z, joint.Orientation.Z));
}
public static Quaternion GetQuaternion(Kinect.JointOrientation jointOrientation)
{
return(GetQuaternion(jointOrientation.Orientation));
}
private static Quaternion GetQuaternionFromJointOrientation(Kinect.JointOrientation jointOrientation)
{
return(new Quaternion(-jointOrientation.Orientation.X, jointOrientation.Orientation.Y, -jointOrientation.Orientation.Z, jointOrientation.Orientation.W));
}
public SkeletonJoint(Windows.Kinect.Joint joint, JointOrientation orientation)
{
this.Type = joint.JointType;
this.Confidence = (int)joint.TrackingState;
this.State = 0;
this.Position = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
this.Rotation = new Quaternion(orientation.Orientation.X, orientation.Orientation.Y, orientation.Orientation.Z, orientation.Orientation.W);
}
public void JointUpdate(Dictionary <Kinect.JointType, Kinect.Joint> joints, Dictionary <Kinect.JointType, Kinect.JointOrientation> orients, int JointID, Kinect.JointType type, Kinect.TrackingState state, TRANSFORM_SMOOTH_PARAMETERS smoothingParams, int body_id = 0)
{
//printf("body: %d\n", body_id);
Vector3 vPrevRawPosition;
Vector3 vPrevFilteredPosition;
Vector3 vPrevTrend;
Vector3 vRawPosition;
Vector3 vFilteredPosition;
Vector3 vPredictedPosition;
Vector3 vDiff;
Vector3 vTrend;
float vLength;
float fDiff;
bool bJointIsValid;
Quaternion filteredOrientation;
Quaternion trend;
Quaternion rawOrientation = new Quaternion(orients[(Kinect.JointType)JointID].Orientation.X, orients[(Kinect.JointType)JointID].Orientation.Y, orients[(Kinect.JointType)JointID].Orientation.Z, orients[(Kinect.JointType)JointID].Orientation.W);
Quaternion prevFilteredOrientation = m_pHistory[body_id, JointID].m_vFilteredOrientation;
Quaternion prevTrend = m_pHistory[body_id, JointID].m_vTrendOrientation;
Kinect.Joint joint = joints[(Kinect.JointType)JointID];
Kinect.JointOrientation orient = orients[(Kinect.JointType)JointID];
vRawPosition = new Vector4(joint.Position.X, joint.Position.Y, joint.Position.Z, 0.0f);
vPrevFilteredPosition = m_pHistory[body_id, JointID].m_vFilteredPosition;
vPrevTrend = m_pHistory[body_id, JointID].m_vTrend;
vPrevRawPosition = m_pHistory[body_id, JointID].m_vRawPosition;
bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
rawOrientation = m_pHistory[body_id, JointID].m_vFilteredOrientation;
m_pHistory[body_id, JointID].m_dwFrameCount = 0;
}
// Initial start values
if (m_pHistory[body_id, JointID].m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = Vector3.zero;
m_pHistory[body_id, JointID].m_dwFrameCount++;
filteredOrientation = rawOrientation;
trend = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
}
else if (m_pHistory[body_id, JointID].m_dwFrameCount == 1)
{
vFilteredPosition = (vRawPosition + vPrevRawPosition) * 0.5f;
vDiff = vFilteredPosition - vPrevFilteredPosition;
vTrend = (vDiff * smoothingParams.fCorrection) + vPrevTrend * (1.0f - smoothingParams.fCorrection);
m_pHistory[body_id, JointID].m_dwFrameCount++;
Quaternion prevRawOrientation = m_pHistory[body_id, JointID].m_vRawOrientation;
filteredOrientation = EnhansedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f);
Quaternion diffStarted = RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = EnhansedQuaternionSlerp(prevTrend, diffStarted, smoothingParams.fCorrection);
}
else
{
// First apply jitter filter
vDiff = vRawPosition - vPrevFilteredPosition;
vLength = Mathf.Sqrt(vDiff.sqrMagnitude);
fDiff = Mathf.Abs(vLength);
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = vRawPosition * (fDiff / smoothingParams.fJitterRadius) + vPrevFilteredPosition * (1.0f - fDiff / smoothingParams.fJitterRadius);
}
else
{
vFilteredPosition = vRawPosition;
}
Quaternion diffJitter = RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation);
float diffValJitter = Mathf.Abs(QuaternionAngle(diffJitter));
if (diffValJitter <= smoothingParams.fJitterRadius)
{
filteredOrientation = EnhansedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParams.fJitterRadius);
}
else
{
filteredOrientation = rawOrientation;
}
// Now the double exponential smoothing filter
vFilteredPosition = vFilteredPosition * (1.0f - smoothingParams.fSmoothing) + (vPrevFilteredPosition + vPrevTrend) * smoothingParams.fSmoothing;
vDiff = vFilteredPosition - vPrevFilteredPosition;
vTrend = ((vDiff * smoothingParams.fCorrection) + (vPrevTrend * (1.0f - smoothingParams.fCorrection)));
// Now the double exponential smoothing filter
filteredOrientation = EnhansedQuaternionSlerp(filteredOrientation, (prevFilteredOrientation * prevTrend), smoothingParams.fSmoothing);
diffJitter = RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = EnhansedQuaternionSlerp(prevTrend, diffJitter, smoothingParams.fCorrection);
}
// Predict into the future to reduce latency
vPredictedPosition = (vFilteredPosition + (vTrend * smoothingParams.fPrediction));
Quaternion predictedOrientation = (filteredOrientation * (EnhansedQuaternionSlerp(new Quaternion(0.0f, 0.0f, 0.0f, 1.0f), trend, smoothingParams.fPrediction)));
// Check that we are not too far away from raw data
vDiff = (vRawPosition - vPrevFilteredPosition);
vLength = Mathf.Sqrt(vDiff.sqrMagnitude);
fDiff = Mathf.Abs(vLength);
Quaternion diff = RotationBetweenQuaternions(predictedOrientation, filteredOrientation);
float diffVal = Mathf.Abs(QuaternionAngle(diff));
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = ((vPredictedPosition * (smoothingParams.fMaxDeviationRadius / fDiff)) + (vRawPosition * (1.0f - smoothingParams.fMaxDeviationRadius / fDiff)));
}
if (diffVal > smoothingParams.fMaxDeviationRadius)
{
predictedOrientation = EnhansedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParams.fMaxDeviationRadius / diffVal);
}
predictedOrientation = QuaternionNormalise(predictedOrientation);
filteredOrientation = QuaternionNormalise(filteredOrientation);
trend = QuaternionNormalise(trend);
// Save the data from this frame
m_pHistory[body_id, JointID].m_vRawPosition = vRawPosition;
m_pHistory[body_id, JointID].m_vFilteredPosition = vFilteredPosition;
m_pHistory[body_id, JointID].m_vTrend = vTrend;
if (!(rawOrientation == null) && !(filteredOrientation == null) && !(trend == null))
{
m_pHistory[body_id, JointID].m_vRawOrientation = rawOrientation;
m_pHistory[body_id, JointID].m_vFilteredOrientation = filteredOrientation;
m_pHistory[body_id, JointID].m_vTrendOrientation = trend;
}
// Output the data
//vPredictedPosition = new Vector4(vPredictedPosition , 1.0f);
Kinect.Joint j = new Windows.Kinect.Joint();
Kinect.CameraSpacePoint CameraSpacePoint = new Windows.Kinect.CameraSpacePoint();
CameraSpacePoint.X = vPredictedPosition.x;
CameraSpacePoint.Y = vPredictedPosition.y;
CameraSpacePoint.Z = vPredictedPosition.z;
j.Position = CameraSpacePoint;
j.JointType = type;
j.TrackingState = state;
m_pFilteredJoints[(Kinect.JointType)JointID] = j;
// HipCenter has no parent and is the root of our skeleton - leave the HipCenter absolute set as it is
if (type != Kinect.JointType.SpineBase)
{
Kinect.JointOrientation jo = new Windows.Kinect.JointOrientation();
Kinect.Vector4 v4 = new Kinect.Vector4();
v4.X = predictedOrientation.x;
v4.Y = predictedOrientation.y;
v4.Z = predictedOrientation.z;
v4.W = predictedOrientation.w;
jo.Orientation = v4;
jo.JointType = type;
m_pFilteredOrientations[(Kinect.JointType)JointID] = jo;
}
else
{
m_pFilteredOrientations[(Kinect.JointType)JointID] = orients[(Kinect.JointType)JointID];
}
//m_pFilteredJoints[JointID]
}
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.JointOrientation source_ori = body.JointOrientations [jt];
Kinect.Joint? targetJoint = null;
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
}
Material zzz_obj = bodyObject.transform.Find(jt.ToString()).gameObject.GetComponent <Renderer> ().material;
zzz_obj.color = MyColorPicker.ShowColor;
Transform jointObj = bodyObject.transform.Find(jt.ToString());
jointObj.localPosition = GetVector3FromJoint(sourceJoint, body_scale);
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
if (targetJoint.HasValue)
{
lr.SetPosition(0, jointObj.localPosition);
lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value, body_scale));
lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
}
else
{
lr.enabled = false;
}
/*if (jt == Kinect.JointType.ElbowLeft) {
* L_Elbow.position = GetVector3FromJoint(sourceJoint, body_scale);
* Quaternion temp;
* temp.w = source_ori.Orientation.W;
* temp.x = source_ori.Orientation.X;
* temp.y = source_ori.Orientation.Y;
* temp.z = source_ori.Orientation.Z;
* L_Elbow.rotation = temp;
* }
* if (jt == Kinect.JointType.ShoulderLeft) {
* L_UpperArm.position = GetVector3FromJoint(sourceJoint, body_scale);
* Quaternion temp;
* temp.w = source_ori.Orientation.W;
* temp.x = source_ori.Orientation.X;
* temp.y = source_ori.Orientation.Y;
* temp.z = source_ori.Orientation.Z;
* L_UpperArm.rotation = temp;
* }
* if (jt == Kinect.JointType.WristLeft) {
* L_Hand.position = GetVector3FromJoint(sourceJoint, body_scale);
* Quaternion temp;
* temp.w = source_ori.Orientation.W;
* temp.x = source_ori.Orientation.X;
* temp.y = source_ori.Orientation.Y;
* temp.z = source_ori.Orientation.Z;
* L_Hand.rotation = temp;
* }
* if (jt == Kinect.JointType.Head) {
* Head.position = GetVector3FromJoint(sourceJoint, body_scale);
* Quaternion temp;
* temp.w = source_ori.Orientation.W;
* temp.x = source_ori.Orientation.X;
* temp.y = source_ori.Orientation.Y;
* temp.z = source_ori.Orientation.Z;
* Head.rotation = temp;
* }
* if (jt == Kinect.JointType.Neck) {
* Neck.position = GetVector3FromJoint(sourceJoint, body_scale);
* Quaternion temp;
* temp.w = source_ori.Orientation.W;
* temp.x = source_ori.Orientation.X;
* temp.y = source_ori.Orientation.Y;
* temp.z = source_ori.Orientation.Z;
* Neck.rotation = temp;
* }
* if (jt == Kinect.JointType.SpineShoulder) {
* Spineshoulder.position = GetVector3FromJoint(sourceJoint, body_scale);
* Quaternion temp;
* temp.w = source_ori.Orientation.W;
* temp.x = source_ori.Orientation.X;
* temp.y = source_ori.Orientation.Y;
* temp.z = source_ori.Orientation.Z;
* Spineshoulder.rotation = temp;
* }
* if (jt == Kinect.JointType.SpineMid) {
* SpineMid.position = GetVector3FromJoint(sourceJoint, body_scale);
* SpineMid2.position = GetVector3FromJoint(sourceJoint, body_scale);
* Quaternion temp;
* temp.w = source_ori.Orientation.W;
* temp.x = source_ori.Orientation.X;
* temp.y = source_ori.Orientation.Y;
* temp.z = source_ori.Orientation.Z;
* SpineMid.rotation = temp;
* SpineMid2.rotation = temp;
* }
* if (jt == Kinect.JointType.SpineBase) {
* SpineBase.position = GetVector3FromJoint(sourceJoint, body_scale);
* SpineBase2.position = GetVector3FromJoint(sourceJoint, body_scale);
* Quaternion temp;
* temp.w = source_ori.Orientation.W;
* temp.x = source_ori.Orientation.X;
* temp.y = source_ori.Orientation.Y;
* temp.z = source_ori.Orientation.Z;
* SpineBase.rotation = temp;
* SpineBase2.rotation = temp;
* }*/
}
}
