/// <summary>
    /// Draws a gizmo cylinder with the given TRS matrix and color
    /// </summary>
    /// <param name="trs"></param>
    /// <param name="color"></param>
    public static void DrawCylinder(Matrix4x4 trs, Color color)
    {
        if (cylVerts == null || cylTris == null)
        {
            GameObject cyl = GameObject.CreatePrimitive(
                PrimitiveType.Cylinder);
            MeshFilter filter = cyl.GetComponent<MeshFilter>();
            cylVerts = filter.sharedMesh.vertices;
            cylTris = filter.sharedMesh.triangles;
            GameObject.DestroyImmediate(cyl);
        }

        Vector3[] verts = new Vector3[cylVerts.Length];
        for (int i = 0; i < cylVerts.Length; i++)
            verts[i] = trs.MultiplyPoint(cylVerts[i]);

        Gizmos.color = color;
        for (int i = 0; i < cylTris.Length / 3; i++)
        {
            int j = i * 3;
            Gizmos.DrawLine(verts[cylTris[j]],
                verts[cylTris[j + 1]]);
            Gizmos.DrawLine(verts[cylTris[j + 1]],
                verts[cylTris[j + 2]]);
            Gizmos.DrawLine(verts[cylTris[j + 2]],
                verts[cylTris[j]]);
        }
    }

 void Start()
 {
     // This creates the diagonalised inertia tensor matrix from the Vector3 by the physics engine.
     // Note the physics engine is using the colliders of the object and it's children, and
     // the mass of the parent object to calculate an approximate inertia tensor.
     __inertiaTensor = Matrix4x4.Scale (GetComponent<Rigidbody>().inertiaTensor);
 }

    /// <summary>
    /// Converts vector data stored within a texture using the specified basis.  Assume all calculations are performed in tangent space.
    /// </summary>
    /// <param name='basis'>
    /// Basis to multiply the vector values against.
    /// </param>
    /// <param name='vectorData'>
    /// Texture2D containing vector data.  Textures are passed by reference, so make sure to copy beforehand if you don't want to overwrite your data!
    /// </param>
    public static void ConvertTangentBasis(Matrix4x4 basis, ref Texture2D vectorData, bool recomputeZ = false)
    {
        Color[] colorData = vectorData.GetPixels();
        Texture2D tmpTexture = new Texture2D(vectorData.width, vectorData.height, TextureFormat.ARGB32, false);

        for (int i = 0; i < colorData.Length; i++)
        {
            Color vecData = new Color(colorData[i].r, colorData[i].g, colorData[i].b, 1);
            vecData.r = Vector3.Dot(new Vector3(basis.m00, basis.m01, basis.m02), UnpackUnitVector(new Vector3(colorData[i].r, colorData[i].g, colorData[i].b))) * 0.5f + 0.5f;
            vecData.g = Vector3.Dot(new Vector3(basis.m10, basis.m11, basis.m12), UnpackUnitVector(new Vector3(colorData[i].r, colorData[i].g, colorData[i].b))) * 0.5f + 0.5f;
            if (recomputeZ)
            {
                vecData.r = vecData.r * 2 - 1;
                vecData.g = vecData.g * 2 - 1;
                vecData.b = Mathf.Sqrt(1 - vecData.r * vecData.r - vecData.g * vecData.g) * 0.5f + 0.5f;
                vecData.r = vecData.r * 0.5f + 0.5f;
                vecData.g = vecData.g * 0.5f + 0.5f;
            } else {
                vecData.b = Vector3.Dot(new Vector3(basis.m20, basis.m21, basis.m22), UnpackUnitVector(new Vector3(colorData[i].r, colorData[i].g, colorData[i].b))) * 0.5f + 0.5f;
            }
            colorData[i] = vecData;
        }
        tmpTexture.SetPixels(colorData);
        tmpTexture.Apply();
        vectorData = tmpTexture;
    }

	// Use this for initialization
	void Start () {
		kinect = devOrEmu.getKinect();
		players = new Kinect.NuiSkeletonTrackingState[Kinect.Constants.NuiSkeletonCount];
		trackedPlayers = new int[Kinect.Constants.NuiSkeletonMaxTracked];
		trackedPlayers[0] = -1;
		trackedPlayers[1] = -1;
		bonePos = new Vector3[2,(int)Kinect.NuiSkeletonPositionIndex.Count];
		rawBonePos = new Vector3[2,(int)Kinect.NuiSkeletonPositionIndex.Count];
		boneVel = new Vector3[2,(int)Kinect.NuiSkeletonPositionIndex.Count];
		
		boneState = new Kinect.NuiSkeletonPositionTrackingState[2,(int)Kinect.NuiSkeletonPositionIndex.Count];
		boneLocalOrientation = new Quaternion[2, (int)Kinect.NuiSkeletonPositionIndex.Count];
		boneAbsoluteOrientation = new Quaternion[2, (int)Kinect.NuiSkeletonPositionIndex.Count];
		
		//create the transform matrix that converts from kinect-space to world-space
		Matrix4x4 trans = new Matrix4x4();
		trans.SetTRS( new Vector3(-kinect.getKinectCenter().x,
		                          kinect.getSensorHeight()-kinect.getKinectCenter().y,
		                          -kinect.getKinectCenter().z),
		             Quaternion.identity, Vector3.one );
		Matrix4x4 rot = new Matrix4x4();
		Quaternion quat = new Quaternion();
		double theta = Mathf.Atan((kinect.getLookAt().y+kinect.getKinectCenter().y-kinect.getSensorHeight()) / (kinect.getLookAt().z + kinect.getKinectCenter().z));
		float kinectAngle = (float)(theta * (180 / Mathf.PI));
		quat.eulerAngles = new Vector3(-kinectAngle, 0, 0);
		rot.SetTRS( Vector3.zero, quat, Vector3.one);

		//final transform matrix offsets the rotation of the kinect, then translates to a new center
		kinectToWorld = flipMatrix*trans*rot;
	}

 // From http://docs.unity3d.com/ScriptReference/Camera-projectionMatrix.html
 static Matrix4x4 PerspectiveOffCenter(float left, float right, float bottom, float top, float near, float far)
 {
     float x = 2.0F * near / (right - left);
     float y = 2.0F * near / (top - bottom);
     float a = (right + left) / (right - left);
     float b = (top + bottom) / (top - bottom);
     float c = -(far + near) / (far - near);
     float d = -(2.0F * far * near) / (far - near);
     float e = -1.0F;
     Matrix4x4 m = new Matrix4x4();
     m[0, 0] = x;
     m[0, 1] = 0;
     m[0, 2] = a;
     m[0, 3] = 0;
     m[1, 0] = 0;
     m[1, 1] = y;
     m[1, 2] = b;
     m[1, 3] = 0;
     m[2, 0] = 0;
     m[2, 1] = 0;
     m[2, 2] = c;
     m[2, 3] = d;
     m[3, 0] = 0;
     m[3, 1] = 0;
     m[3, 2] = e;
     m[3, 3] = 0;
     return m;
 }

        public Matrix4x4 GetMatrix(){
            Matrix4x4 keystone = new Matrix4x4();
            // a b c d
            // e f g h
            // i j 1 0
            // m n 0 1
            keystone[0, 0] = a;
            keystone[0, 1] = b;
            //keystone[0, 2] = c;
            keystone[0, 3] = d;
            keystone[1, 0] = e;
            keystone[1, 1] = f;
            //keystone[1, 2] = g;
            keystone[1, 3] = h;
            keystone[2, 0] = 0;
            keystone[2, 1] = 0;
            keystone[2, 2] = 1 - Mathf.Abs(m) - Mathf.Abs(n);
            keystone[2, 3] = 0;
            keystone[3, 0] = m;
            keystone[3, 1] = n;
            keystone[3, 2] = 0;
            keystone[3, 3] = 1;

            return keystone;
        }

		public void DrawMesh(Mesh mesh, Matrix4x4 matrix, Material material)
		{
			MaterialPropertyBlock properties = null;
			int shaderPass = -1;
			int submeshIndex = 0;
			CommandBuffer.INTERNAL_CALL_DrawMesh(this, mesh, ref matrix, material, submeshIndex, shaderPass, properties);
		}

        public void Matrix4x4DeterminantTest1()
        {
            Matrix4x4 a = new Matrix4x4();
            a.M11 = 5.0f;
            a.M12 = 2.0f;
            a.M13 = 8.25f;
            a.M14 = 1.0f;
            a.M21 = 12.0f;
            a.M22 = 6.8f;
            a.M23 = 2.14f;
            a.M24 = 9.6f;
            a.M31 = 6.5f;
            a.M32 = 1.0f;
            a.M33 = 3.14f;
            a.M34 = 2.22f;
            a.M41 = 0f;
            a.M42 = 0.86f;
            a.M43 = 4.0f;
            a.M44 = 1.0f;
            Matrix4x4 i;
            Assert.True(Matrix4x4.Invert(a, out i));

            float detA = a.GetDeterminant();
            float detI = i.GetDeterminant();
            float t = 1.0f / detI;

            // only accurate to 3 precision
            Assert.True(System.Math.Abs(detA - t) < 1e-3, "Matrix4x4.Determinant was not set correctly.");
        }

    public void BeginUIResizing()
    {
        Vector2 nativeSize = NativeResolution;
        _didResizeUI = true;

        stack.Add (GUI.matrix);
        Matrix4x4 m = new Matrix4x4 ();
        float w = Screen.width;
        float h = Screen.height;
        float aspect = w / h;

        if (aspect < (nativeSize.x / nativeSize.y)) {
            //screen is taller
            _guiScaleFactor = (Screen.width / nativeSize.x);
            _offset.y = (Screen.height - (nativeSize.y * _guiScaleFactor)) * 0.5f;
        }
        else {
            //screen is wider
            _guiScaleFactor = (Screen.height/nativeSize.y);
            _offset.x = (Screen.width-(nativeSize.x*_guiScaleFactor))*0.5f;
        }

        m.SetTRS (_offset,Quaternion.identity,Vector3.one*_guiScaleFactor);
        GUI.matrix *= m;
    }

        public void Matrix4x4IdentityTest()
        {
            Matrix4x4 val = new Matrix4x4();
            val.M11 = val.M22 = val.M33 = val.M44 = 1.0f;

            Assert.True(MathHelper.Equal(val, Matrix4x4.Identity), "Matrix4x4.Indentity was not set correctly.");
        }

    void ConvertToMatrix(Quaternion q, Vector3 pos, ref Matrix4x4 mat)
    {
        Matrix4x4 m=new Matrix4x4();
        qtomatrix(ref m, q);

        //added by yamen: include head position in the matrix
        m[3,0] = pos[0];
        m[3,1] = pos[1];
        m[3,2] = pos[2];

        mat[0] = m[0,0];
        mat[1] = m[1,0];
        mat[2] = m[2,0];
        mat[3] = 0;

        mat[4] = m[0,1];
        mat[5] = m[1,1];
        mat[6] = m[2,1];
        mat[7] = 0;

        mat[8] = m[0,2];
        mat[9] = m[1,2];
        mat[10] = m[2,2];
        mat[11] = 0;

        mat[12] = m[3,0];
        mat[13] = m[3,1];
        mat[14] = m[3,2];
        mat[15] = 1;
    }

    void CalculateTargetValues(ref float[] param)
    {
        float len=vecLength3(targetMatrix[12],targetMatrix[13],targetMatrix[14]-0.17f);
        param[2]=len-L1;

        if(len!=0){
            float tmp=targetMatrix[12]/len;
            tmp=tmp>+1?+1:tmp;
            tmp=tmp<-1?-1:tmp;
            param[1]=Mathf.Asin(tmp);

            tmp=-targetMatrix[13]/(len*Mathf.Cos(param[1]));
            tmp=tmp>+1?+1:tmp;
            tmp=tmp<-1?-1:tmp;
            param[0]=Mathf.Asin(tmp);
        }else{
            param[0]=0;
            param[1]=0;
        }

        Matrix4x4 mat,tmpM,tmpM2;
        mat=new Matrix4x4();
        MakeMatrix(myAxis.axisX,0,0,0,-0.17f,ref mat);
        tmpM2=mat*targetMatrix;
        MakeMatrix(myAxis.axisX,-param[0],0,0,0,ref mat);
        tmpM=mat*tmpM2;
        MakeMatrix(myAxis.axisY,-param[1],0,0,-len,ref mat);
        tmpM2=mat*tmpM;
        MatrixtoXYZ(tmpM2,ref param[3],ref param[4],ref param[5]);

        param[3]=-param[3];
        param[4]=-param[4];
        param[5]=-param[5];
    }

        /// <inheritdoc/>
        protected override void OnApply(ImageBase source, ImageBase target, Rectangle targetRectangle, Rectangle sourceRectangle)
        {
            float saturationFactor = this.saturation / 100f;

            // Stop at -1 to prevent inversion.
            saturationFactor++;

            // The matrix is set up to "shear" the colour space using the following set of values.
            // Note that each colour component has an effective luminance which contributes to the
            // overall brightness of the pixel.
            // See http://graficaobscura.com/matrix/index.html
            float saturationComplement = 1.0f - saturationFactor;
            float saturationComplementR = 0.3086f * saturationComplement;
            float saturationComplementG = 0.6094f * saturationComplement;
            float saturationComplementB = 0.0820f * saturationComplement;

            Matrix4x4 matrix4X4 = new Matrix4x4()
            {
                M11 = saturationComplementR + saturationFactor,
                M12 = saturationComplementR,
                M13 = saturationComplementR,
                M21 = saturationComplementG,
                M22 = saturationComplementG + saturationFactor,
                M23 = saturationComplementG,
                M31 = saturationComplementB,
                M32 = saturationComplementB,
                M33 = saturationComplementB + saturationFactor,
            };

            this.matrix = matrix4X4;
        }

    // Update is called once per frame
    void Update()
    {
        if(Input.GetButtonDown("Fire1")) {
            Shot();
        }
        if(Input.GetButtonDown("Fire2")) {
            Blow();
        }
        {
            Vector3 move = Vector3.zero;
            move.x = Input.GetAxis ("Horizontal");
            move.z = Input.GetAxis ("Vertical");
            trans.position += move * 0.1f;
        }
        {
            Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
            Plane plane = new Plane(Vector3.up, Vector3.zero);
            float distance = 0;
            if (plane.Raycast(ray, out distance)){
                trans.rotation = Quaternion.LookRotation(ray.GetPoint(distance) - trans.position);
            }
        }

        {
            Matrix4x4 ts = Matrix4x4.identity;
            ts.SetColumn (3, new Vector4(0.0f,0.0f,0.5f,1.0f));
            ts = Matrix4x4.Scale(new Vector3(5.0f, 5.0f, 10.0f)) * ts;
            blowMatrix = trans.localToWorldMatrix * ts;
        }
    }

    public static void DrawWireCube(Matrix4x4 mat, Color col)
    {
        const float s = 0.5f;
        Vector4[] vertices = new Vector4[8] {
            new Vector4( s, s, s, 1.0f),
            new Vector4(-s, s, s, 1.0f),
            new Vector4( s,-s, s, 1.0f),
            new Vector4( s, s,-s, 1.0f),
            new Vector4(-s,-s, s, 1.0f),
            new Vector4( s,-s,-s, 1.0f),
            new Vector4(-s, s,-s, 1.0f),
            new Vector4(-s,-s,-s, 1.0f),
        };
        for (int i = 0; i < vertices.Length; ++i )
        {
            vertices[i] = mat * vertices[i];
        }
        int[] indices = new int[24] {
            0,1, 0,2, 0,3,
            1,4, 1,6,
            2,4, 2,5,
            3,5, 3,6,
            4,7,
            5,7,
            6,7
        };

        GL.Begin(GL.LINES);
        GL.Color(col);
        for (int i = 0; i < indices.Length; ++i )
        {
            GL.Vertex(vertices[indices[i]]);
        }
        GL.End();
    }

  //Scale point along stretchAxis by stretchFactor
  public static Vector3 StretchAlongAxis( Vector3 point, Vector3 stretchAxis, float stretchFactor )
  {
    var upVector = Vector3.up;

    //Check if vectors are colliniar
    if( Vector3.Dot(upVector, stretchAxis) >= 1.0f - epsilon )
      upVector = Vector3.left;

    var right = Vector3.Cross(upVector, stretchAxis);
    var up = Vector3.Cross(stretchAxis, right);
    var forward = stretchAxis;

    right.Normalize();
    up.Normalize();
    forward.Normalize();

    Matrix4x4 rotate = new Matrix4x4();
    rotate.SetColumn(0, right);
    rotate.SetColumn(1, up);
    rotate.SetColumn(2, forward);
    rotate[3, 3] = 1.0F;

    Matrix4x4 scale = new Matrix4x4();
    scale[0, 0] = 1.0f;
    scale[1, 1] = 1.0f;
    scale[2, 2] = stretchFactor;
    scale[3, 3] = 1.0f;

    Matrix4x4 trans = rotate * scale * rotate.transpose;

    return trans.MultiplyPoint3x4( point );
  }

 public void SetPose(Matrix4x4 pose, CoordinateSpace space = CoordinateSpace.Local)
 {
     if (space == CoordinateSpace.Global)
         _rigidBody.WrappedRigidBody.PoseCenterOfMass = pose.ToDigitalRune();
     else
         _rigidBody.WrappedRigidBody.MassFrame.Pose = pose.ToDigitalRune();
 }

    public void createCylinder(float radius, float height, int slices, GameObject go, Matrix4x4 matrix)
    {
        Mesh cylinderMesh = new Mesh();
        vertices = new Vector3[(slices+1) * 4];
        Vector3[] cylPoints1 = createCylinderPoints(radius, height, slices, true);
        Vector3[] cylPoints2 = createCylinderPoints(radius, height, slices, false);
        for (int i = 0; i <cylPoints1.Length; i++) {
            vertices[i] = cylPoints1[i];
        }
        for (int k = 0; k < cylPoints2.Length; k++) {
            vertices[k + cylPoints1.Length] = cylPoints2[k];
        }

        createCylinderNormals(vertices, radius);
        for (int j = 0; j < vertices.Length; j++) {
            vertices[j] = matrix.MultiplyPoint(vertices[j]);
            normals[j] = matrix.MultiplyPoint(normals[j]);
        }

        trianglesIndex = 0;
        createCylinderTriangles(slices+1);

        cylinderMesh.vertices = vertices;
        cylinderMesh.triangles = triangles;
        cylinderMesh.normals = normals;
        cylinderMesh.uv = uvs;

        MeshFilter filter = (MeshFilter)go.GetComponent("MeshFilter");
        filter.mesh = cylinderMesh;
    }

    private static Matrix4x4 PerspectiveOffCenter( NearPlane plane )
    {
        float x = ( 2.0f*plane.near )/( plane.right - plane.left );
        float y = ( 2.0f*plane.near )/( plane.top - plane.bottom );
        float a = ( plane.right + plane.left )/( plane.right - plane.left );
        float b = ( plane.top + plane.bottom )/( plane.top - plane.bottom );
        float c = -( plane.far + plane.near )/( plane.far - plane.near );
        float d = -( 2.0f*plane.far*plane.near )/( plane.far - plane.near );
        float e = -1.0f;
        Matrix4x4 m = new Matrix4x4 ();

        m[0, 0] = x;
        m[0, 1] = 0.0f;
        m[0, 2] = a;
        m[0, 3] = 0.0f;
        m[1, 0] = 0.0f;
        m[1, 1] = y;
        m[1, 2] = b;
        m[1, 3] = 0.0f;
        m[2, 0] = 0.0f;
        m[2, 1] = 0.0f;
        m[2, 2] = c;
        m[2, 3] = d;
        m[3, 0] = 0.0f;
        m[3, 1] = 0.0f;
        m[3, 2] = e;
        m[3, 3] = 0.0f;

        return m;
    }

        /// <summary>
        /// Calculate bone transforms from world transforms
        /// </summary>
        /// <param name="hierarchy">Hierarchy of the skeleton, value for a given index indicates the parent index of the bone at the given index</param>
        /// <param name="worldTransforms">The world transforms to turn into bone transforms</param>
        /// <param name="calculatedBoneTransforms">An array to write the bone transforms into</param>
        public static void CalculateBoneTransformsFromWorldTransforms(IList<int> hierarchy, IReadOnlyList<Matrix4x4> worldTransforms, Matrix4x4[] calculatedBoneTransforms)
        {
            if (calculatedBoneTransforms == null)
                throw new ArgumentNullException("calculatedBoneTransforms");
            if (worldTransforms == null)
                throw new ArgumentNullException("worldTransforms");

            unsafe
            {
                //Allocate a place to temporarily store the inverted transforms
                Matrix4x4* inverseWorldTransforms = stackalloc Matrix4x4[worldTransforms.Count];

                //Calculate inverse world transforms for each bone
                for (var i = 0; i < calculatedBoneTransforms.Length; i++)
                    Matrix4x4.Invert(worldTransforms[i], out inverseWorldTransforms[i]);

                //Calculate bone transforms for each bone
                for (var bone = 0; bone < worldTransforms.Count; bone++)
                {
                    var parentBone = hierarchy[bone];
                    if (parentBone == -1)
                        calculatedBoneTransforms[bone] = worldTransforms[bone];
                    else
                        calculatedBoneTransforms[bone] = Matrix4x4.Multiply(worldTransforms[bone], inverseWorldTransforms[parentBone]);
                }
            }
        }