Using Quaternions and C# script to animate a mesh

I am using a script to animate a mesh character. My values are being fed in realtime through a TCP connection and I am sorting them into their 4x4 quaternion matrices. When I apply these rotations to each bone, I should get the same animation being sent from the live feed, but I'm getting very odd rotations that don't seem to match in any way. I've already tried some changes of coordinate systems and bone structure.

Here is what my code for the rotation looks like:

using UnityEngine;
using System.Collections;
using System;
using System.Text;
using System.IO;
using System.Net;
using System.Net.Sockets;

public class BoneControlTest : MonoBehaviour {

    GameObject hipRight;
    GameObject kneeRight;
    GameObject ankleRight;
    GameObject footRight;
    GameObject toeRight;

    GameObject leftLeg;
    GameObject hipLeft;
    GameObject kneeLeft;
    GameObject ankleLeft;
    GameObject footLeft;
    GameObject toeLeft;

    GameObject spine3;
    GameObject spine2;
    GameObject spine1;
    GameObject spine;
    GameObject head;

    Byte[] sendBytes = new Byte[5000];
    byte[] readBytes = new byte [5050];
    TcpClient socketForOMProxyServer;
    NetworkStream proxyStream;
    GameObject[] boneArray = new GameObject[21];
    string boneData = "";

    //method for parsing rootbone matrix
    static float[,] ParseBoneData(string boneData, int boneNum)
    {
        string forCurrent = "Bone " + boneNum + "/21: ";
        string forNext = "Bone " + (boneNum+1) + "/21: ";
        print(forCurrent + " " + forNext);
        int rootNodePos = boneData.IndexOf(forCurrent);
        int firstNodePos = boneData.IndexOf(forNext);
        string boneMatrix = boneData.Substring(rootNodePos, firstNodePos - rootNodePos);
        char[] separators = new char [] { ' ', '
', '\r' };
        string[] parts = boneMatrix.Split(separators, StringSplitOptions.RemoveEmptyEntries);
        float[,] rootBoneMat = new float[4,4]
        {
            {float.Parse(parts[ 6]), float.Parse(parts[ 7]), float.Parse(parts[ 8]), float.Parse(parts[ 9])},
            {float.Parse(parts[10]), float.Parse(parts[11]), float.Parse(parts[12]), float.Parse(parts[13])},
            {float.Parse(parts[14]), float.Parse(parts[15]), float.Parse(parts[16]), float.Parse(parts[17])},
            {float.Parse(parts[18]), float.Parse(parts[19]), float.Parse(parts[20]), float.Parse(parts[21])}
        };;

        print("Root bone matrix: " + rootBoneMat[0, 0] + " " + rootBoneMat[0, 1] + " " + rootBoneMat[0, 2] + " " + rootBoneMat[0, 3] + '
'
        +  rootBoneMat[1, 0] + " " + rootBoneMat[1, 1] + " " + rootBoneMat[1, 2] + " "  + rootBoneMat[1, 3] + '
'
        + rootBoneMat[2, 0] + " " + rootBoneMat[2, 1] + " " + rootBoneMat[2, 2] + " " + rootBoneMat[2, 3] + '
'
        + rootBoneMat[3, 0] + " " + rootBoneMat[3, 1] + " " + rootBoneMat[3, 2] + " "+rootBoneMat[3, 3]);

        return rootBoneMat;
    }

    void Start() 
    {
        //socket code from Jack's SLbot

        try
        {
            socketForOMProxyServer = new TcpClient("localHost", 4242);
            print("Connected to OMProxy");
        }
        catch
        {
            print(
            "Failed to connect to server at {0}:4242"+            "localhost");
            return;
        }
        proxyStream = socketForOMProxyServer.GetStream();

        //bind our new GameObjects to the bone structure
        spine3 = GameObject.Find("MidTorso");
        spine2 = GameObject.Find("LowerTorso");
        spine1 = GameObject.Find("UpperChest");
        spine = GameObject.Find("Neck");
        //head = GameObject.Find("Head");

        //spine2.transform.parent = spine3.transform;
        //spine1.transform.parent = spine2.transform;
        //spine.transform.parent = spine1.transform;
        //head.transform.parent = spine.transform;

        boneArray[1] = spine3;
        boneArray[2] = spine2;
        boneArray[3] = spine1;
        boneArray[4] = spine;
        boneArray[5] = head;

        //print(spine3.transform.childCount + " " + spine.transform.childCount + " " + head.transform.root);
    }

    // Update is called once per frame
    void Update () 
    {
        sendBytes = Encoding.ASCII.GetBytes("g");
        proxyStream.Write(sendBytes, 0, sendBytes.Length);
        int count = proxyStream.Read(readBytes, 0, readBytes.Length);

        for (int i = 1; i <= 4; i++)
        {
            string boneData = Encoding.ASCII.GetString(readBytes).Substring(0, count);
            //print(boneData);
            float[,] rootBoneMat = ParseBoneData(boneData, i);

            // Use rootBoneMat to set body rotation
            float T = 1.0f + rootBoneMat[0, 0] + rootBoneMat[1, 1] + rootBoneMat[2, 2];
            float S = (float)Math.Sqrt(T) * 2.0f;
            float X = (rootBoneMat[2, 1] - rootBoneMat[1, 2]) / S;
            float Y = (rootBoneMat[0, 2] - rootBoneMat[2, 0]) / S;
            float Z = (rootBoneMat[1, 0] - rootBoneMat[0, 1]) / S;
            float W = 0.25f * S;
            Quaternion bodyRotation = new Quaternion(X, Y, Z, W);
            print("X: " + X + " Y: " + Y + " Z: " + Z + " W: " + W);

            Vector3  rootPos = new Vector3(rootBoneMat[3, 0], rootBoneMat[3, 2], rootBoneMat[3, 1]);

            //Rotate the bone
            //bodyRotation.eulerAngles =  Vector3(X, Y, Z);
            boneArray*.transform.rotation = bodyRotation;*
 _boneArray*.transform.localPosition = rootPos;*_
 _*}*_
 _*}*_
_*}*_
_*```*_

Have you used debugging to determine if the matrices you get are correct before you convert them to quaternions? You can use Debug.DrawLine to draw lines representing the matrices and see if these look correct.

Unity has a build-in format for 4x4 matrices: Matrix4x4 - you can use that one instead of the float[,] to get easy access to some utility functions.

Where did you get the code for converting to quaternions? It doesn't look the same as the code I use, which I know works:

public static Quaternion QuaternionFromMatrix(Matrix4x4 m) {
    // Adapted from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
    Quaternion q = new Quaternion();
    q.w = Mathf.Sqrt( Mathf.Max( 0, 1 + m[0,0] + m[1,1] + m[2,2] ) ) / 2; 
    q.x = Mathf.Sqrt( Mathf.Max( 0, 1 + m[0,0] - m[1,1] - m[2,2] ) ) / 2; 
    q.y = Mathf.Sqrt( Mathf.Max( 0, 1 - m[0,0] + m[1,1] - m[2,2] ) ) / 2; 
    q.z = Mathf.Sqrt( Mathf.Max( 0, 1 - m[0,0] - m[1,1] + m[2,2] ) ) / 2; 
    q.x *= Mathf.Sign( q.x * ( m[2,1] - m[1,2] ) );
    q.y *= Mathf.Sign( q.y * ( m[0,2] - m[2,0] ) );
    q.z *= Mathf.Sign( q.z * ( m[1,0] - m[0,1] ) );
    return q;
}

Are the rotations in your data local or global? If they are local, you should set the Transform.localRotation rather than the Transform.rotation.

I hope some of these tips can be of help.