public int parseItems( Byte[] buffer )
{
segments.Clear();
if( buffer.Length < 3) return 0;
// The first character should be 42, the next should be the number if entries.
if(buffer[0] != 42) throw new Exception("Invalid header");
byte count = buffer[1];
if(count == 0) return 0;
int ptr = 2;
for (int i = 0; i < count && ptr < buffer.Length; i++)
{
byte id = buffer[ptr++];
if (id != 43 && id != 44 && id != 45) throw new Exception("Invalid packet entry");
byte namelen = buffer[ptr++];
byte datalen = buffer[ptr++];
int name = ptr;
ptr += namelen;
int data = ptr;
ptr += datalen;
if (ptr > buffer.Length) throw new Exception("Parser buffer overrun");
if( id == 43 )
{
MocapSegment segment = new MocapSegment();
// Name
for(int q = 0; q < namelen; q++)
{
segment.name += (char)buffer[ name + q ];
}
int n = 0;
int c = 0;
string temp = "";
while(n<7)
{
byte ch = buffer[ data + c ];
c++;
if ( ch != 0 )
{
temp += (char)ch;
continue;
}
if(n==0) float.TryParse ( temp, out segment.tx );
if(n==1) float.TryParse ( temp, out segment.ty );
if(n==2) float.TryParse ( temp, out segment.tz );
if(n==3) float.TryParse ( temp, out segment.rx );
if(n==4) float.TryParse ( temp, out segment.ry );
if(n==5) float.TryParse ( temp, out segment.rz );
if(n==6) float.TryParse ( temp, out segment.rw );
temp = "";
n++;
}
segments.Add(segment);
}
/*if( id == 44 )
{
Marker marker = new Marker();
// Name
memcpy(marker->name, name, namelen);
marker->name[namelen] = 0;
float *fptr = &marker->tx;
size_t dptr = 0;
for (int j = 0; j < 3 && data + dptr < buffer + len; j++)
{
fptr[j] = atof(data + dptr);
dptr += strlen(data + dptr) + 1;
}
items.Add(marker);
}*/
/*if (id == 45)
{
char tmp[64];
memcpy(tmp, data, datalen);
tmp[datalen] = 0;
float f1 = 0.0f;
float f2 = 0.0f;
int ret = sscanf(tmp, "%f %f", &f1, &f2);
if (ret == 2)
{
Marker *marker = new Marker();
// Name
memcpy(marker->name, name, namelen);
marker->name[namelen] = 0;
marker->tx = f1;
marker->ty = f2;
marker->tz = 0.0f;
items->push_back(marker);
}
}*/
}
return ptr;
}
public int parseItems(Byte[] buffer)
{
segments.Clear();
if (buffer.Length < 3)
{
return(0);
}
// The first character should be 42, the next should be the number if entries.
if (buffer[0] != 42)
{
throw new Exception("Invalid header");
}
byte count = buffer[1];
if (count == 0)
{
return(0);
}
int ptr = 2;
for (int i = 0; i < count && ptr < buffer.Length; i++)
{
byte id = buffer[ptr++];
if (id != 43 && id != 44 && id != 45)
{
throw new Exception("Invalid packet entry");
}
byte namelen = buffer[ptr++];
byte datalen = buffer[ptr++];
int name = ptr;
ptr += namelen;
int data = ptr;
ptr += datalen;
if (ptr > buffer.Length)
{
throw new Exception("Parser buffer overrun");
}
if (id == 43)
{
MocapSegment segment = new MocapSegment();
// Name
for (int q = 0; q < namelen; q++)
{
segment.name += (char)buffer[name + q];
}
int n = 0;
int c = 0;
string temp = "";
while (n < 7)
{
byte ch = buffer[data + c];
c++;
if (ch != 0)
{
temp += (char)ch;
continue;
}
if (n == 0)
{
float.TryParse(temp, out segment.tx);
}
if (n == 1)
{
float.TryParse(temp, out segment.ty);
}
if (n == 2)
{
float.TryParse(temp, out segment.tz);
}
if (n == 3)
{
float.TryParse(temp, out segment.rx);
}
if (n == 4)
{
float.TryParse(temp, out segment.ry);
}
if (n == 5)
{
float.TryParse(temp, out segment.rz);
}
if (n == 6)
{
float.TryParse(temp, out segment.rw);
}
temp = "";
n++;
}
segments.Add(segment);
}
/*if( id == 44 )
* {
* Marker marker = new Marker();
*
* // Name
* memcpy(marker->name, name, namelen);
* marker->name[namelen] = 0;
*
* float *fptr = &marker->tx;
* size_t dptr = 0;
*
* for (int j = 0; j < 3 && data + dptr < buffer + len; j++)
* {
* fptr[j] = atof(data + dptr);
* dptr += strlen(data + dptr) + 1;
* }
*
* items.Add(marker);
*
* }*/
/*if (id == 45)
* {
* char tmp[64];
* memcpy(tmp, data, datalen);
* tmp[datalen] = 0;
*
* float f1 = 0.0f;
* float f2 = 0.0f;
* int ret = sscanf(tmp, "%f %f", &f1, &f2);
*
* if (ret == 2)
* {
* Marker *marker = new Marker();
*
* // Name
* memcpy(marker->name, name, namelen);
* marker->name[namelen] = 0;
*
* marker->tx = f1;
* marker->ty = f2;
* marker->tz = 0.0f;
*
* items->push_back(marker);
*
* }
* }*/
}
return(ptr);
}
// Update is called once per frame
void FixedUpdate()
{
if (clientSocket.Available == 0)
{
return;
}
// Frame rate calculator
dt += Time.deltaTime;
if (dt > 1.0 / updateRate)
{
fps = frameCount / dt;
frameCount = 0;
dt -= 1.0 / updateRate;
}
try
{
byte[] bytes = clientSocket.Receive(ref groupEP);
Parser parser = new Parser();
parser.parseItems(bytes);
infoMessage = "Bytes " + bytes.Length;
infoMessage += " Segments: " + parser.segments.Count;
for (int i = 0; i < parser.segments.Count; i++)
{
MocapSegment segment = parser.segments[i];
infoMessage += "\n" + segment.name;
if (objects.ContainsKey(segment.name))
{
Quaternion localOrientation = segment.GetOrientation();
// Zero off the rotation
if (Input.GetKeyDown("1"))
{
rotationOffsets[segment.name] = Quaternion.Inverse(localOrientation);
}
GameObject o = objects[segment.name];
o.transform.localRotation = localOrientation * rotationOffsets[segment.name];
o.transform.localPosition = segment.GetTranslation(0.1f);
String dbg = "SEG:" + segment.name + "\n";
Vector3 eul = localOrientation.eulerAngles;
Vector3 ofs = rotationOffsets[segment.name].eulerAngles;
Vector3 ofi = initialOrientation[segment.name].eulerAngles;
Vector3 eu2 = o.transform.localRotation.eulerAngles;
Vector3 pos = o.transform.localPosition;
float px = pos[0] * 100;
float py = pos[1] * 100;
float pz = pos[2] * 100;
dbg += "POS:" + px.ToString("0.00") + "\t\t" + py.ToString("0.00") + "\t\t" + pz.ToString("0.00") + "\n";
dbg += "L: " + eul.x.ToString("0.00") + "\t\t" + eul.y.ToString("0.00") + "\t\t" + eul.z.ToString("0.00") + "\n";
//Debug.Log (dbg);
infoMessage += " tx: " + segment.tx;
infoMessage += " ty: " + segment.ty;
infoMessage += " tz: " + segment.tz;
//frameDone = true;
}
}
/*
*
* int subjects = 0;
* if(!int.TryParse ( lineItems[0], out subjects))
* {
* infoMessage = "Invalid data while parsing";
* return;
* }
* int line = 1;
* bool frameDone = false;
* for(int i=0; i < subjects && line < lineItems.Length; i++)
* {
* string[] subjectSplit = lineItems[line++].Split ('\t');
* string subjectName = subjectSplit[0];
* int noSegments = Convert.ToInt32 (subjectSplit[1]);
* int noMarkers = Convert.ToInt32 (subjectSplit[2]);
* //infoMessage += "SUB: " + subjectName + "\n";
*
*
* for(int j=0; j < noSegments && line < lineItems.Length; j++)
* {
* string[] segmentSplit = lineItems[line++].Split('\t');
*
* if(segmentSplit.Length != 8)
* {
* infoMessage += "Segments: " + segmentSplit.Length;
* continue;
* }
*
* string segmentName = subjectName + "_" + segmentSplit[0];
* float[] tr = new float[3];
* float[] lo = new float[4];
* for(int k=0; k < 3; k++) tr[k] = float.Parse (segmentSplit[k+1]);
* for(int k=0; k < 4; k++) lo[k] = float.Parse (segmentSplit[k+4]);
*
* if(objects.ContainsKey (segmentName))
* {
* Quaternion localOrientation = new Quaternion(lo[0], lo[1], lo[2], lo[3]);
*
* // Zero off the rotation
* if (Input.GetKeyDown ("1"))
* {
* rotationOffsets[segmentName] = Quaternion.Inverse ( localOrientation );
* }
*
* GameObject o = objects[segmentName];
* o.transform.localRotation = localOrientation * rotationOffsets[segmentName];
* o.transform.localPosition = new Vector3(tr[0] / 10, tr[1] / 10, tr[2] / 10);
*
* String dbg = "SEG:" + segmentName + "\n";
*
* Vector3 eul = localOrientation.eulerAngles;
* Vector3 ofs = rotationOffsets[segmentName].eulerAngles;
* Vector3 ofi = initialOrientation[segmentName].eulerAngles;
* Vector3 eu2 = o.transform.localRotation.eulerAngles;
* Vector3 pos = o.transform.localPosition;
*
* float px = pos[0] * 100;
* float py = pos[1] * 100;
* float pz = pos[2] * 100;
*
* dbg += "POS:" + px.ToString ("0.00") + "\t\t" + py.ToString ("0.00") + "\t\t" + pz.ToString("0.00") + "\n";
* dbg += "L: " + eul.x.ToString ("0.00") + "\t\t" + eul.y.ToString ("0.00") + "\t\t" + eul.z.ToString("0.00") + "\n";
* //Debug.Log (dbg);
*
* frameDone = true;
* }
*
* } // done segments
*
* line += noMarkers;
* }
*
* infoMessage += "FPS:" + fps.ToString ("0.00");
*
*/
//if(frameDone) frameCount++;
/*
* float f = float.Parse(last, System.Globalization.CultureInfo.InvariantCulture);
* Debug.Log (f);
*
* if(objects["LeftArm"] != null)
* {
* objects["LeftArm"].transform.eulerAngles = new Vector3(0, f * 180, 0);
* }*/
}
catch (SocketException e)
{
Debug.Log("Socket Error: " + e.Message);
}
}
