koobas.hobune.stream
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 [] { ' ', '\n', '\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] + '\n'
+ rootBoneMat[1, 0] + " " + rootBoneMat[1, 1] + " " + rootBoneMat[1, 2] + " " + rootBoneMat[1, 3] + '\n'
+ rootBoneMat[2, 0] + " " + rootBoneMat[2, 1] + " " + rootBoneMat[2, 2] + " " + rootBoneMat[2, 3] + '\n'
+ 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[i].transform.rotation = bodyRotation;
boneArray[i].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.
I've made these changes and redid my armature to match the data exactly. For some reason the new armature is not reacting to the script commands. Unity is not showing the bones as null, so it is identifying them as a GameObject, but they aren't moving in any way.
Here is my model/armature. I tried to follow a rigging guide but I don't think it was unity specific.
If your new armature is not reacting on script adjustments at all, that should be easy for you to debug. Just test that it's even referenced correctly by using some dummy movement, e.g. hipRight.Rotate(90*Time.deltaTime,0,0) or something. If nothing happens, you probably didn't drag the bone transforms into the slots in the Inspector correctly.
