// Update the filter for all body joints
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointsCount = manager.GetJointCount();
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter feet highly as they are so noisy
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked ||
jointIndex == (int)KinectInterop.JointType.FootLeft || jointIndex == (int)KinectInterop.JointType.FootRight ||
jointIndex == (int)KinectInterop.JointType.HandTipLeft || jointIndex == (int)KinectInterop.JointType.HandTipRight ||
jointIndex == (int)KinectInterop.JointType.ThumbLeft || jointIndex == (int)KinectInterop.JointType.ThumbRight ||
jointIndex == (int)KinectInterop.JointType.Head)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].posVel = FilterJoint(bodyData.joint[jointIndex].posVel, bodyIndex, jointIndex, ref tempSmoothingParams);
}
}
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointsCount = manager.GetJointCount();
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked ||
jointIndex == (int)KinectInterop.JointType.FootLeft || jointIndex == (int)KinectInterop.JointType.FootRight ||
jointIndex == (int)KinectInterop.JointType.HandTipLeft || jointIndex == (int)KinectInterop.JointType.HandTipRight ||
jointIndex == (int)KinectInterop.JointType.ThumbLeft || jointIndex == (int)KinectInterop.JointType.ThumbRight ||
jointIndex == (int)KinectInterop.JointType.Head)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].position = FilterJoint(bodyData.joint[jointIndex].position, bodyIndex, jointIndex, ref tempSmoothingParams);
}
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
if (jointIndex == 0)
{
bodyData.position = bodyData.joint[jointIndex].position;
bodyData.orientation = bodyData.joint[jointIndex].orientation;
bodyData.joint[jointIndex].direction = Vector3.zero;
}
else
{
int jParent = (int)manager.GetParentJoint((KinectInterop.JointType)jointIndex);
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[jParent].trackingState != KinectInterop.TrackingState.NotTracked)
{
bodyData.joint[jointIndex].direction =
bodyData.joint[jointIndex].position - bodyData.joint[jParent].position;
}
}
}
}
// Update the filter for all body joints
/// <summary>
/// 更新所有身体关节的过滤器
/// </summary>
/// <param name="bodyData">身体数据</param>
/// <param name="bodyIndex">身体索引</param>
/// <param name="tempSmoothingParams">平滑参数</param>
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointsCount = manager.GetSensorJointCount();
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter feet highly as they are so noisy
//如果没有跟踪，我们通过使用更大的抖动半径来平滑一点总是尽可能高效地过滤脚，因为它们很嘈杂
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].position = FilterJoint(bodyData.joint[jointIndex].position, bodyIndex, jointIndex, ref tempSmoothingParams);
}
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
if (jointIndex == 0)
{
bodyData.position = bodyData.joint[jointIndex].position;
bodyData.orientation = bodyData.joint[jointIndex].orientation;
bodyData.joint[jointIndex].direction = Vector3.zero;
}
else
{
int jParent = (int)manager.GetParentJoint(bodyData.joint[jointIndex].jointType);
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[jParent].trackingState != KinectInterop.TrackingState.NotTracked)
{
bodyData.joint[jointIndex].direction =
bodyData.joint[jointIndex].position - bodyData.joint[jParent].position;
}
}
}
}
// gets the position and rotation of the given user
private bool GetUserPose(KinectInterop.SensorData sensorData, int uIndex, ref Vector3 userPos, ref Quaternion userRot)
{
if (sensorData != null && sensorData.trackedBodiesCount > uIndex)
{
KinectInterop.BodyData bodyData = sensorData.alTrackedBodies[uIndex];
Vector3 spaceScale = sensorData.sensorSpaceScale;
KinectInterop.TrackingState trackingState = bodyData.joint != null && bodyData.joint.Length > 0 ? bodyData.joint[0].trackingState : KinectInterop.TrackingState.NotTracked;
if (trackingState == KinectInterop.TrackingState.Tracked || trackingState == KinectInterop.TrackingState.HighConf)
{
userPos = new Vector3(bodyData.kinectPos.x * spaceScale.x, bodyData.kinectPos.y * spaceScale.y, bodyData.kinectPos.z);
userRot = initialUserRot * bodyData.mirroredRotation;
}
return(true);
}
return(false);
}
public void FixJointOrientations(KinectInterop.SensorData sensorData, ref KinectInterop.BodyData bodyData)
{
// fix the hips-to-spine tilt (it is about 40 degrees to the back)
int hipsIndex = (int)KinectInterop.JointType.SpineBase;
Quaternion quat = bodyData.joint[hipsIndex].normalRotation;
quat *= Quaternion.Euler(40f, 0f, 0f);
bodyData.joint[hipsIndex].normalRotation = quat;
Vector3 mirroredAngles = quat.eulerAngles;
mirroredAngles.y = -mirroredAngles.y;
mirroredAngles.z = -mirroredAngles.z;
bodyData.joint[hipsIndex].mirroredRotation = Quaternion.Euler(mirroredAngles);
bodyData.normalRotation = bodyData.joint[hipsIndex].normalRotation;
bodyData.mirroredRotation = bodyData.joint[hipsIndex].mirroredRotation;
}
// Update the filter for all body joints
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
int jointsCount = KinectInterop.Constants.JointCount;
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter feet highly as they are so noisy
if ((int)bodyData.joint[jointIndex].trackingState != 2)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].position = FilterJoint(bodyData.joint[jointIndex].position, bodyIndex, jointIndex, ref tempSmoothingParams);
}
}
// gets the position and rotation of the given user
private bool GetUserPose(int sIndex, KinectInterop.SensorData sensorData, int uIndex, ulong lastBodyTime, ref UserBodyPose userPose)
{
if (sensorData != null && sensorData.trackedBodiesCount > uIndex && (lastBodyTime == 0 || sensorData.lastBodyFrameTime != lastBodyTime))
{
KinectInterop.BodyData bodyData = sensorData.alTrackedBodies[uIndex];
Vector3 spaceScale = sensorData.sensorSpaceScale;
userPose.bodyTime = sensorData.lastBodyFrameTime;
if (lastBodyTime != 0)
{
//Debug.Log(string.Format("S{0}, Time: {1}, Last: {2}", sIndex, sensorData.lastBodyFrameTime, lastBodyTime));
}
if (bodyData.joint != null && bodyData.joint.Length > 0)
{
for (int j = 0; j < numJoints; j++)
{
int joint = trackedBodyJoint[j];
KinectInterop.TrackingState trackingState = bodyData.joint[joint].trackingState;
userPose.jointTracked[j] = (trackingState == KinectInterop.TrackingState.Tracked) || (trackingState == KinectInterop.TrackingState.HighConf);
if (userPose.jointTracked[j])
{
Vector3 kinectPos = bodyData.joint[joint].kinectPos;
userPose.jointPos[j] = new Vector3(kinectPos.x * spaceScale.x, kinectPos.y * spaceScale.y, kinectPos.z);
userPose.jointRot[j] = initialUserRot * bodyData.joint[joint].mirroredRotation;
//Debug.Log(string.Format("S{0}: {1} - p:{2}, r:{3}", sIndex, (KinectInterop.JointType)joint, userPose.jointPos[j], userPose.jointRot[j].eulerAngles));
}
}
}
return(userPose.jointTracked[0]);
}
return(false);
}
// Apply the orientation constraints
public void Constrain(ref KinectInterop.BodyData bodyData)
{
KinectManager manager = KinectManager.Instance;
frameNum++;
for (int i = 0; i < jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (jc.thisJoint == (int)KinectInterop.JointType.SpineBase || bodyData.joint[jc.thisJoint].normalRotation == Quaternion.identity)
{
continue;
}
if (bodyData.joint[jc.thisJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
int prevJoint = (int)manager.GetParentJoint((KinectInterop.JointType)jc.thisJoint);
if (bodyData.joint[prevJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
Quaternion rotParentN = bodyData.joint[prevJoint].normalRotation;
Quaternion rotDefaultN = Quaternion.FromToRotation(KinectInterop.JointBaseDir[prevJoint], KinectInterop.JointBaseDir[jc.thisJoint]);
rotParentN = rotParentN * rotDefaultN;
Quaternion rotJointN = bodyData.joint[jc.thisJoint].normalRotation;
Quaternion rotLocalN = Quaternion.Inverse(rotParentN) * rotJointN;
Vector3 eulerAnglesN = rotLocalN.eulerAngles;
// if(jc.thisJoint == (int)KinectInterop.JointType.KneeLeft)
// {
// float angle1X = eulerAnglesN.x <= 180f ? eulerAnglesN.x : eulerAnglesN.x - 360f;
// float angle1Y = eulerAnglesN.y <= 180f ? eulerAnglesN.y : eulerAnglesN.y - 360f;
// float angle1Z = eulerAnglesN.z <= 180f ? eulerAnglesN.z : eulerAnglesN.z - 360f;
//
// string sDebugText = string.Format("{0}. {1} - ({2:000}, {3:000}, {4:000})",
// frameNum, ((KinectInterop.JointType)jc.thisJoint).ToString(), angle1X, angle1Y, angle1Z);
//
// if(debugText != null && (Time.time - currentTime) >= 0.5f)
// {
// currentTime = Time.time;
// //debugText.GetComponent<GUIText>().text = sDebugText;
// }
//
// //Debug.Log(sDebugText);
// }
bool isConstrained = false;
string sDebug = string.Empty;
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quaternion rotLimited = rotLocalN;
switch (ac.consType)
{
case 0:
break;
case CT.LimA:
eulerAnglesN = LimitAngles(eulerAnglesN, ac.axis, ac.angleMin, ac.angleMax);
rotLimited = Quaternion.Euler(eulerAnglesN);
break;
case CT.LimST:
rotLimited = LimitSwing(rotLocalN, ac.axis, ac.angleMin);
rotLimited = LimitTwist(rotLimited, ac.axis, ac.angleMax);
break;
case CT.LimH:
float lastAngle = bodyData.joint[jc.thisJoint].lastAngle;
rotLimited = LimitHinge(rotLocalN, ac.axis, ac.angleMin, ac.angleMax, ref lastAngle);
bodyData.joint[jc.thisJoint].lastAngle = lastAngle;
break;
default:
throw new Exception("Undefined constraint type found: " + (int)ac.consType);
}
if (rotLimited != rotLocalN)
{
rotLocalN = rotLimited;
isConstrained = true;
}
}
if (sDebug.Length > 0)
{
// if(debugText != null && jc.thisJoint == (int)KinectInterop.JointType.ElbowLeft)
// {
// debugText.GetComponent<GUIText>().text = sDebug;
// }
Debug.Log(sDebug);
}
if (isConstrained)
{
rotJointN = rotParentN * rotLocalN;
Vector3 eulerJoint = rotJointN.eulerAngles;
Vector3 eulerJointM = new Vector3(eulerJoint.x, -eulerJoint.y, -eulerJoint.z);
Quaternion rotJointM = Quaternion.Euler(eulerJointM);
// put it back into the bone orientations
bodyData.joint[jc.thisJoint].normalRotation = rotJointN;
bodyData.joint[jc.thisJoint].mirroredRotation = rotJointM;
}
}
}
public void FixJointOrientations(KinectInterop.SensorData sensorData, ref KinectInterop.BodyData bodyData)
{
// no fixes are needed
}
public bool IsBodyTurned(ref KinectInterop.BodyData bodyData)
{
return(false);
}
public void FixJointOrientations(KinectInterop.SensorData sensorData, ref KinectInterop.BodyData bodyData)
{
}
// ApplyBoneOrientationConstraints and constrain rotations.
public void Constrain(ref KinectInterop.BodyData bodyData)
{
KinectManager manager = KinectManager.Instance;
for (int i = 0; i < this.jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (jc.thisJoint == (int)KinectInterop.JointType.SpineBase || bodyData.joint[jc.thisJoint].normalRotation == Quaternion.identity)
{
continue;
}
if (bodyData.joint[jc.thisJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
int prevJoint = (int)manager.GetParentJoint((KinectInterop.JointType)jc.thisJoint);
if (bodyData.joint[prevJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
Quaternion rotJointN = bodyData.joint[jc.thisJoint].normalRotation;
Quaternion rotParentN = bodyData.joint[prevJoint].normalRotation;
Quaternion rotLocalN = Quaternion.Inverse(rotParentN) * rotJointN;
Vector3 eulerAnglesN = rotLocalN.eulerAngles;
Quaternion rotJointM = bodyData.joint[jc.thisJoint].mirroredRotation;
Quaternion rotParentM = bodyData.joint[prevJoint].mirroredRotation;
Quaternion rotLocalM = Quaternion.Inverse(rotParentM) * rotJointM;
Vector3 eulerAnglesM = rotLocalM.eulerAngles;
bool isConstrained = false;
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quaternion axisRotation = Quaternion.AngleAxis(eulerAnglesN[ac.axis], ac.rotateAround);
float angleFromMin = Quaternion.Angle(axisRotation, ac.minQuaternion);
float angleFromMax = Quaternion.Angle(axisRotation, ac.maxQuaternion);
if (!(angleFromMin <= ac.angleRange && angleFromMax <= ac.angleRange))
{
// correct the axis that has fallen out of range.
if (angleFromMin > angleFromMax)
{
eulerAnglesN[ac.axis] = ac.angleMax;
}
else
{
eulerAnglesN[ac.axis] = ac.angleMin;
}
// fix mirrored rotation as well
if (ac.axis == 0)
{
eulerAnglesM[ac.axis] = eulerAnglesN[ac.axis];
}
else
{
eulerAnglesM[ac.axis] = -eulerAnglesN[ac.axis];
}
isConstrained = true;
}
}
if (isConstrained)
{
rotLocalN = Quaternion.Euler(eulerAnglesN);
rotJointN = rotParentN * rotLocalN;
rotLocalM = Quaternion.Euler(eulerAnglesM);
rotJointM = rotParentM * rotLocalM;
// Put it back into the bone directions
bodyData.joint[jc.thisJoint].normalRotation = rotJointN;
bodyData.joint[jc.thisJoint].mirroredRotation = rotJointM;
// dirJoint = constrainedRotation * dirParent;
// bodyData.joint[jc.thisJoint].direction = dirJoint;
}
}
}
// Use this for initialization
void Start()
{
// Holds our _bones for later.
_bones = new Transform[25];
// Initial rotations of said _bones.
initialRotations = new Quaternion[_bones.Length];
// Map _bones to the points the Kinect tracks.
//Map_bones();
bodyData = new KinectInterop.BodyData();
bodyData.joint = new KinectInterop.JointData[_bones.Length];
for (int i = 0; i < _bones.Length; i++)
{
bodyData.joint[i] = new KinectInterop.JointData();
bodyData.joint[i].position = new Vector3();
bodyData.joint[i].orientation = new Quaternion();
bodyData.joint[i].kinectPos = new Vector3();
bodyData.joint[i].jointType = boneIndex2JointMap[i];
}
Debug.Log("init [OK]");
Map_Bones_auto();
// Get initial rotations to return to later.
GetInitialRotations();
print("init Draw skeleton lines");
//draw_skeleton();
print("init Draw skeleton lines OK");
}
