/* * KinectModelController.cs - Handles rotating the bones of a model to match * rotations derived from the bone positions given by the kinect * * Developed by Peter Kinney -- 6/30/2011 * */ using UnityEngine; using System; using System.Collections; public class KinectModelControllerV2 : MonoBehaviour { //Assignments for a bitmask to control which bones to look at and which to ignore public enum BoneMask { None = 0x0, //EMPTY = 0x1, Spine = 0x2, Shoulder_Center = 0x4, Head = 0x8, Shoulder_Left = 0x10, Elbow_Left = 0x20, Wrist_Left = 0x40, Hand_Left = 0x80, Shoulder_Right = 0x100, Elbow_Right = 0x200, Wrist_Right = 0x400, Hand_Right = 0x800, Hips = 0x1000, Knee_Left = 0x2000, Ankle_Left = 0x4000, Foot_Left = 0x8000, //EMPTY = 0x10000, Knee_Right = 0x20000, Ankle_Right = 0x40000, Foot_Right = 0x80000, All = 0xEFFFE, Torso = 0x1000000 | Spine | Shoulder_Center | Head, //the leading bit is used to force the ordering in the editor Left_Arm = 0x1000000 | Shoulder_Left | Elbow_Left | Wrist_Left | Hand_Left, Right_Arm = 0x1000000 | Shoulder_Right | Elbow_Right | Wrist_Right | Hand_Right, Left_Leg = 0x1000000 | Hips | Knee_Left | Ankle_Left | Foot_Left, Right_Leg = 0x1000000 | Hips | Knee_Right | Ankle_Right | Foot_Right, R_Arm_Chest = Right_Arm | Spine, No_Feet = All & ~(Foot_Left | Foot_Right), Upper_Body = Torso | Left_Arm | Right_Arm } public GameObject Hip_Center; public GameObject Spine; public GameObject Shoulder_Center; public GameObject Head; public GameObject Collar_Left; public GameObject Shoulder_Left; public GameObject Elbow_Left; public GameObject Wrist_Left; public GameObject Hand_Left; public GameObject Fingers_Left; //unused public GameObject Collar_Right; public GameObject Shoulder_Right; public GameObject Elbow_Right; public GameObject Wrist_Right; public GameObject Hand_Right; public GameObject Fingers_Right; //unused public GameObject Hip_Override; public GameObject Hip_Left; public GameObject Knee_Left; public GameObject Ankle_Left; public GameObject Foot_Left; public GameObject Hip_Right; public GameObject Knee_Right; public GameObject Ankle_Right; public GameObject Foot_Right; public int player; public BoneMask Mask = BoneMask.All; public bool animated; public float blendWeight = 1; private GameObject[] _bones; //internal handle for the bones of the model private uint _nullMask = 0x0; private Quaternion[] _baseRotation; //starting orientation of the joints private Vector3[] _boneDir; //in the bone's local space, the direction of the bones private Vector3[] _boneUp; //in the bone's local space, the up vector of the bone private Vector3 _hipRight; //right vector of the hips private Vector3 _chestRight; //right vectory of the chest [HideInInspector] public Vector3[,] bonePos; private float[] stion=new float[80] {0.2f,-0.6f,1.7f,1.0f,-0.1f,-0.2f,1.9f,1.0f,-0.2f,0.2f,2.0f,1.0f,-0.3f, 0.4f, 2.0f, 1.0f,-0.3f, 0.1f, 2.1f, 1.0f, -0.3f, -0.2f, 2.0f, 1.0f,-0.2f, -0.3f, 1.8f, 1.0f,-0.3f, -0.3f, 1.7f, 1.0f,-0.1f, 0.1f, 1.9f, 1.0f, -0.1f, 0.0f, 1.6f, 1.0f,-0.3f, 0.0f, 1.5f, 1.0f,-0.4f, 0.0f, 1.5f, 1.0f,-0.2f, -0.3f, 1.8f, 1.0f,-0.3f, -0.6f, 1.9f, 1.0f, -0.4f, -0.8f, 2.1f, 1.0f,-0.4f, -0.8f, 2.1f, 1.0f,-0.1f, -0.3f, 1.9f, 1.0f,-0.2f, -0.4f, 1.5f, 1.0f,0.1f, -0.6f, 1.7f, 1.0f ,0.0f, -0.6f, 1.7f, 1.0f};//手动载入的20个骨骼点的数据,以后可以需改传入数据 private Vector4 dfds; private Matrix4x4 kinectToWorld ; // Use this for initialization void Start () { //store bones in a list for easier access, everything except Hip_Center will be one //higher than the corresponding Kinect.NuiSkeletonPositionIndex (because of the hip_override) _bones = new GameObject[25] { null, Hip_Center, Spine, Shoulder_Center, Collar_Left, Shoulder_Left, Elbow_Left, Wrist_Left, Collar_Right, Shoulder_Right, Elbow_Right, Wrist_Right, Hip_Override, Hip_Left, Knee_Left, Ankle_Left, null, Hip_Right, Knee_Right, Ankle_Right, //extra joints to determine the direction of some bones Head, Hand_Left, Hand_Right, Foot_Left, Foot_Right}; //determine which bones are not available for(int ii = 0; ii < _bones.Length; ii++) { if(_bones[ii] == null) { _nullMask |= (uint)(1 << ii); } } //store the base rotations and bone directions (in bone-local space) _baseRotation = new Quaternion[20]; _boneDir = new Vector3[20]; //first save the special rotations for the hip and spine _hipRight = Hip_Right.transform.position - Hip_Left.transform.position; _hipRight = Hip_Override.transform.InverseTransformDirection(_hipRight); _chestRight = Shoulder_Right.transform.position - Shoulder_Left.transform.position; _chestRight = Spine.transform.InverseTransformDirection(_chestRight); //get direction of all other bones for( int ii = 0; ii < 20; ii++) { if((_nullMask & (uint)(1 << ii)) <= 0) { //save initial rotation _baseRotation[ii] = _bones[ii].transform.localRotation; //if the bone is the end of a limb, get direction from this bone to one of the extras (hand or foot). if(ii % 4 == 3 && ((_nullMask & (uint)(1 << (ii/4) + 20)) <= 0)) { _boneDir[ii] = _bones[(ii/4) + 20].transform.position - _bones[ii].transform.position; } //if the bone is the hip_override (at boneindex Hip_Left, get direction from average of left and right hips else if(ii == 12) { _boneDir[ii] = ((Hip_Right.transform.position + Hip_Left.transform.position) / 2F) - Hip_Override.transform.position; } //otherwise, get the vector from this bone to the next. else if((_nullMask & (uint)(1 << ii+1)) <= 0) { _boneDir[ii] = _bones[ii+1].transform.position - _bones[ii].transform.position; } else { continue; } //Since the spine of the kinect data is ~40 degrees back from the hip, //check what angle the spine is at and rotate the saved direction back to match the data if(ii == 1) { float angle = Vector3.Angle(transform.up,_boneDir[ii]); _boneDir[ii] = Quaternion.AngleAxis(-40 + angle,transform.right) * _boneDir[ii]; } //transform the direction into local space. _boneDir[ii] = _bones[ii].transform.InverseTransformDirection(_boneDir[ii]); } } //make _chestRight orthogonal to the direction of the spine. _chestRight -= Vector3.Project(_chestRight, _boneDir[1]); //make _hipRight orthogonal to the direction of the hip override Vector3.OrthoNormalize(ref _boneDir[12],ref _hipRight); bonePos = new Vector3[2,20]; kinectToWorld = Matrix4x4.zero; kinectToWorld[0,0] = 1; kinectToWorld[1,1] = 1; kinectToWorld[1,3] = 1; kinectToWorld [2, 2] = -1; kinectToWorld [2, 3] = 2; kinectToWorld [3, 3] = 1; } void Update () { for (int bone = 0; bone < 20; bone++) { dfds = new Vector4 (stion[bone*4],stion[bone*4+1],stion[bone*4+2],stion[bone*4+3]); bonePos[0,bone] = kinectToWorld.MultiplyPoint3x4(dfds); } //update the data from the kinect if necessary //if(sw.pollSkeleton()){ for( int ii = 0; ii < 20; ii++) { if( ((uint)Mask & (uint)(1 << ii) ) > 0 && (_nullMask & (uint)(1 << ii)) <= 0 ) { RotateJoint(ii); } // } } } void RotateJoint(int bone) { //if blendWeight is 0 there is no need to compute the rotations if( blendWeight <= 0 ){ return; } //if the model is not animated, reset rotations to fix twisted joints if(!animated){_bones[bone].transform.localRotation = _baseRotation[bone];} Vector3 dir = _boneDir[bone]; Vector3 target; //if bone % 4 == 0 then it is either an outside shoulder or the hip override if(bone % 4 == 0) { if(bone == 12) { target = ((bonePos[player,12] + bonePos[player,16]) / 2F) - bonePos[player,0]; } else { //target = vector from shoulder_center to bone target = bonePos[player,bone] - bonePos[player,2]; } } else { //target = vector from previous bone to bone target = bonePos[player,bone] - bonePos[player,bone-1]; } //transform it into bone-local space (independant of the transform of the controller) target = transform.TransformDirection(target); target = _bones[bone].transform.InverseTransformDirection(target); //create a rotation that rotates dir into target Quaternion quat = Quaternion.FromToRotation(dir,target); //if bone is the spine, add in the rotation along the spine if(bone == 12) { //rotate the chest so that it faces forward (determined by the shoulders) dir = _chestRight; target = bonePos[player,4] - bonePos[player,8]; target = transform.TransformDirection(target); target = _bones[bone].transform.InverseTransformDirection(target); target -= Vector3.Project(target,_boneDir[bone]);//Projects a vector onto another vector. quat *= Quaternion.FromToRotation(dir,target); //Creates a rotation which rotates from fromDirection to toDirection. } //if bone is the hip override, add in the rotation along the hips else if(bone == 12) { //rotate the hips so they face forward (determined by the hips) dir = _hipRight; target = bonePos[player,16] - bonePos[player,12]; target = transform.TransformDirection(target); target = _bones[bone].transform.InverseTransformDirection(target); target -= Vector3.Project(target,_boneDir[bone]); quat *= Quaternion.FromToRotation(dir,target); } //reduce the effect of the rotation using the blend parameter quat = Quaternion.Lerp(Quaternion.identity, quat, blendWeight); //apply the rotation to the local rotation of the bone _bones[bone].transform.localRotation = _bones[bone].transform.localRotation * quat; return; } } |
|