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