// returns the local joint position of the specified user, relative to the parent joint, in meters public Vector3 GetJointLocalPosition(int joint) { int parent = KinectWrapper.GetSkeletonJointParent(joint); return(joint >= 0 && joint < player1JointsPos.Length ? (player1JointsPos[joint] - player1JointsPos[parent]) : Vector3.zero); }
// returns the joint rotation of the specified user, relative to the parent joint public Quaternion GetJointLocalOrientation(uint UserId, int joint, bool flip) { int parent = KinectWrapper.GetSkeletonJointParent(joint); if (UserId == Player1ID) { if (joint >= 0 && joint < player1JointsOri.Length && player1JointsTracked[joint]) { Matrix4x4 localMat = (player1JointsOri[parent].inverse * player1JointsOri[joint]); return(Quaternion.LookRotation(localMat.GetColumn(2), localMat.GetColumn(1))); } } return(Quaternion.identity); }
// returns the local joint position of the specified user, relative to the parent joint, in meters public Vector3 GetJointLocalPosition(uint UserId, int joint) { int parent = KinectWrapper.GetSkeletonJointParent(joint); if (UserId == _player1Id) { return(joint >= 0 && joint < _player1JointsPos.Length ? (_player1JointsPos[joint] - _player1JointsPos[parent]) : Vector3.zero); } else if (UserId == _player2Id) { return(joint >= 0 && joint < _player2JointsPos.Length ? (_player2JointsPos[joint] - _player2JointsPos[parent]) : Vector3.zero); } return(Vector3.zero); }
// ApplyBoneOrientationConstraints and constrain rotations. public void Constrain(ref Matrix4x4[] jointOrientations, ref bool[] jointTracked) { // Constraints are defined as a vector with respect to the parent bone vector, and a constraint angle, // which is the maximum angle with respect to the constraint axis that the bone can move through. // Calculate constraint values. 0.0-1.0 means the bone is within the constraint cone. Greater than 1.0 means // it lies outside the constraint cone. for (int i = 0; i < this.jointConstraints.Count; i++) { BoneOrientationConstraint jc = this.jointConstraints[i]; if (!jointTracked[i] || jc.joint == (int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter) { // End joint is not tracked or Hip Center has no parent to perform this calculation with. continue; } // If the joint has a parent, constrain the bone direction to be within the constraint cone int parentIdx = KinectWrapper.GetSkeletonJointParent(jc.joint); // Local bone orientation relative to parent Matrix4x4 localOrientationMatrix = jointOrientations[parentIdx].inverse * jointOrientations[jc.joint]; Vector3 localOrientationZ = (Vector3)localOrientationMatrix.GetColumn(2); Vector3 localOrientationY = (Vector3)localOrientationMatrix.GetColumn(1); if (localOrientationZ == Vector3.zero || localOrientationY == Vector3.zero) { continue; } Quaternion localRotation = Quaternion.LookRotation(localOrientationZ, localOrientationY); Vector3 eulerAngles = localRotation.eulerAngles; bool isConstrained = false; //Matrix4x4 globalOrientationMatrix = jointOrientations[jc.joint]; //Quaternion globalRotation = Quaternion.LookRotation(globalOrientationMatrix.GetColumn(2), globalOrientationMatrix.GetColumn(1)); for (int a = 0; a < jc.axisConstrainrs.Count; a++) { AxisOrientationConstraint ac = jc.axisConstrainrs[a]; Quaternion axisRotation = Quaternion.AngleAxis(localRotation.eulerAngles[ac.axis], ac.rotateAround); //Quaternion axisRotation = Quaternion.AngleAxis(globalRotation.eulerAngles[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)) { // Keep the current rotations around other axes and only // correct the axis that has fallen out of range. //Vector3 euler = globalRotation.eulerAngles; if (angleFromMin > angleFromMax) { eulerAngles[ac.axis] = ac.angleMax; } else { eulerAngles[ac.axis] = ac.angleMin; } isConstrained = true; } } if (isConstrained) { Quaternion constrainedRotation = Quaternion.Euler(eulerAngles); // Put it back into the bone orientations localOrientationMatrix.SetTRS(Vector3.zero, constrainedRotation, Vector3.one); jointOrientations[jc.joint] = jointOrientations[parentIdx] * localOrientationMatrix; //globalOrientationMatrix.SetTRS(Vector3.zero, constrainedRotation, Vector3.one); //jointOrientations[jc.joint] = globalOrientationMatrix; switch (jc.joint) { case (int)KinectWrapper.NuiSkeletonPositionIndex.ShoulderCenter: jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.Head] = jointOrientations[jc.joint]; break; case (int)KinectWrapper.NuiSkeletonPositionIndex.WristLeft: jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.HandLeft] = jointOrientations[jc.joint]; break; case (int)KinectWrapper.NuiSkeletonPositionIndex.WristRight: jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.HandRight] = jointOrientations[jc.joint]; break; case (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleLeft: jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.FootLeft] = jointOrientations[jc.joint]; break; case (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleRight: jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.FootRight] = jointOrientations[jc.joint]; break; } } // globalRotation = Quaternion.LookRotation(globalOrientationMatrix.GetColumn(2), globalOrientationMatrix.GetColumn(1)); // string stringToDebug = string.Format("{0}, {2}", (KinectWrapper.NuiSkeletonPositionIndex)jc.joint, // globalRotation.eulerAngles, localRotation.eulerAngles); // Debug.Log(stringToDebug); // // if(debugText != null) // debugText.guiText.text = stringToDebug; } }