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); } }