void HandleDiscoveryPacket(byte[] data)
{
BinaryReader reader = new BinaryReader(new MemoryStream(data));
//header
reader.ReadByte();
byte kinectId = reader.ReadByte();
string ipStr = reader.ReadString();
int commandPort = reader.ReadInt32();
Vector3 pos = ReadVector3(reader);
Quaternion rot = Quaternion.Euler(ReadVector3(reader));
Vector3 scale = ReadVector3(reader);
KinectInfo ki = null;
if (kinectInfoTable.ContainsKey(kinectId))
{
ki = kinectInfoTable[kinectId];
ki.UpdateMatrix();
}
else if (!disregardUnknownKinects)
{
ki = new KinectInfo();
kinectInfo.Add(ki);
kinectInfoTable.Add(kinectId, ki);
ki.id = kinectId;
ki.useReportedLocation = true;
ki.UseReportedLocation(transform);
}
if (ki != null)
{
ki.id = kinectId;
ki.address = IPAddress.Parse(ipStr);
ki.port = commandPort;
ki.endPoint = new IPEndPoint(ki.address, ki.port);
ki.position = pos;
ki.rotation = rot;
ki.scale = scale;
if (ki.useReportedLocation)
{
ki.UseReportedLocation(ki.location == null ? transform : ki.location.parent);
}
//ki.ipString = ki.address.ToString() + ":" + ki.port;
//TODO: auto subscribe toggle?
if (!ki.active)
{
Subscribe(ki);
}
}
}
void Unsubscribe(KinectInfo info)
{
discoveryPayload[0] = CMD_UNSUBSCRIBE;
discoveryClient.Send(discoveryPayload, discoveryPayload.Length, info.endPoint);
}
