static public RawJointState FromInputLine(string line) { string[] words = line.Split(); int num = (words.Length - 1) / 3; var rjs = new RawJointState(); rjs.Joints = new Vector3[num]; try { rjs.Timestamp = float.Parse(words[0]); for (int i = 0; i < num; i++) { int ri = (i * 3) + 1; rjs.Joints[i].X = float.Parse(words[ri]); rjs.Joints[i].Y = float.Parse(words[ri + 1]); rjs.Joints[i].Z = float.Parse(words[ri + 2]); } } catch (FormatException) { Console.WriteLine("Input line not properly formatted: {0}", line); return(null); } return(rjs); }
public RawJointState ToRawJointState() { var output = new RawJointState(); output.Timestamp = Timestamp; output.Joints = new Vector3[RelativeJoints.Length]; output.Joints[0] = NeckPos; for (int i = 1; i < output.Joints.Length; i++) { output.Joints[i] = NeckPos + RelativeJoints[i]; } return(output); }
static public JointState FromRawJointState(RawJointState rjs) { JointState rel = new JointState(); rel.Timestamp = rjs.Timestamp; rel.NeckPos = rjs.Joints[0]; rel.RelativeJoints = rjs.Joints.Select(x => x - rel.NeckPos).ToArray(); rel.RelativeAngles = new float[rjs.Joints.Length]; for (int i = 1; i < rel.RelativeJoints.Length; i++) { Vector3 thisVec = rel.RelativeJoints[i] - rel.RelativeJoints[JointParents[i]]; Vector3 parentVec = (JointParents[JointParents[i]] == -1) ? Vector3.UnitY : rel.RelativeJoints[JointParents[i]] - rel.RelativeJoints[JointParents[JointParents[i]]]; rel.RelativeAngles[i] = Vector3.CalculateAngle(thisVec, parentVec); } return(rel); }
void ThreadRun() { try { mListener.Start(1); Console.WriteLine("Listening on {0}", mListener.LocalEndpoint); using (var client = mListener.AcceptTcpClient()) { // Synchronous? Console.WriteLine("Accepted connection from {0}", client.Client.RemoteEndPoint); var rawstream = client.GetStream(); var stream = new StreamReader(rawstream); while (mRunning) { try { var line = stream.ReadLine(); if (line == null) { break; } var rjs = RawJointState.FromInputLine(line); lock ( mQueue ) { mQueue.Enqueue(rjs); } } catch (IOException e) { Console.WriteLine("IO exception while receiving network data:\n{0}\n{1}", e, e.StackTrace); mRunning = false; break; } } client.Close(); } } finally { mListener.Stop(); mRunning = false; } }
static public RawJointState FromInputLine(string line) { string[] words = line.Split(); int num = (words.Length-1) / 3; var rjs = new RawJointState(); rjs.Joints = new Vector3[num]; try { rjs.Timestamp = float.Parse(words[0]); for ( int i = 0; i < num; i++ ) { int ri = (i*3) + 1; rjs.Joints[i].X = float.Parse(words[ri]); rjs.Joints[i].Y = float.Parse(words[ri+1]); rjs.Joints[i].Z = float.Parse(words[ri+2]); } } catch ( FormatException ) { Console.WriteLine("Input line not properly formatted: {0}", line); return null; } return rjs; }
static public JointState FromRawJointState(RawJointState rjs) { JointState rel = new JointState(); rel.Timestamp = rjs.Timestamp; rel.NeckPos = rjs.Joints[0]; rel.RelativeJoints = rjs.Joints.Select(x => x - rel.NeckPos).ToArray(); rel.RelativeAngles = new float[rjs.Joints.Length]; for ( int i = 1; i < rel.RelativeJoints.Length; i++ ) { Vector3 thisVec = rel.RelativeJoints[i] - rel.RelativeJoints[JointParents[i]]; Vector3 parentVec = (JointParents[JointParents[i]] == -1) ? Vector3.UnitY : rel.RelativeJoints[JointParents[i]] - rel.RelativeJoints[JointParents[JointParents[i]]]; rel.RelativeAngles[i] = Vector3.CalculateAngle(thisVec, parentVec); } return rel; }
public RawJointState ToRawJointState() { var output = new RawJointState(); output.Timestamp = Timestamp; output.Joints = new Vector3[RelativeJoints.Length]; output.Joints[0] = NeckPos; for ( int i = 1; i < output.Joints.Length; i++ ) { output.Joints[i] = NeckPos + RelativeJoints[i]; } return output; }