private void ParseClustersPacket(BinaryReader br)
{
ClusterMsg msg = new ClusterMsg ();
// read the header
byte packetType = br.ReadByte();
if (packetType != CLUSTER_PACKET_TYPE)
{
Debug.WriteLine("ERROR: cluster parser got packet starting with " + packetType.ToString());
return;
}
UInt16 packetNum = br.ReadUInt16();
UInt16 tsSeconds = br.ReadUInt16();
UInt32 tsTicks = br.ReadUInt32();
UInt16 numPts = br.ReadUInt16();
UInt16 numClusters = br.ReadUInt16();
msg.timestamp = (double)tsSeconds + (double)tsTicks / 10000.0;
// now for the red meat..
v3f[] pts = new v3f[numPts];
for (int i = 0; i < numPts; i++)
{
pts[i].x = (float)(br.ReadInt16())/100.0f;
pts[i].y = (float)(br.ReadInt16())/100.0f;
pts[i].z = (float)(br.ReadInt16())/100.0f;
}
msg.clusters = new List<LidarCluster>();
for (int i = 0; i < numClusters; i++)
{
LidarCluster lc = new LidarCluster();
UInt16 firstPtIndex, lastPtIndex;
byte flags;
firstPtIndex = br.ReadUInt16();
lastPtIndex = br.ReadUInt16();
flags = br.ReadByte();
lc.pts = new v3f[lastPtIndex - firstPtIndex + 1];
lc.stable = (flags & 0x01)==0;
lc.leftOccluded = (flags & 0x02)!=0;
lc.rightOccluded = (flags & 0x04)!=0;
lc.highObstacle = (flags & 0x08)!=0;
lc.bb_low.x = lc.bb_high.x = pts[firstPtIndex].x;
lc.bb_low.y = lc.bb_high.y = pts[firstPtIndex].y;
lc.bb_low.z = lc.bb_high.z = pts[firstPtIndex].z;
for (int j = firstPtIndex; j <= lastPtIndex; j++)
{
lc.pts[j - firstPtIndex] = pts[j];
lc.bb_low.x = Math.Min(lc.bb_low.x, pts[j].x);
lc.bb_low.y = Math.Min(lc.bb_low.y, pts[j].y);
lc.bb_low.z = Math.Min(lc.bb_low.z, pts[j].z);
lc.bb_high.x = Math.Max(lc.bb_high.x, pts[j].x);
lc.bb_high.y = Math.Max(lc.bb_high.y, pts[j].y);
lc.bb_high.z = Math.Max(lc.bb_high.z, pts[j].z);
}
msg.clusters.Add(lc);
}
if (ClustersReceived != null) ClustersReceived(this, new ClusterRXEventArgs(msg));
}
public ClusterRXEventArgs(ClusterMsg c)
{
this.c = c;
}
public ClusterRXEventArgs(ClusterMsg c)
{
this.c = c;
}
private void ParseClustersPacket(BinaryReader br)
{
ClusterMsg msg = new ClusterMsg();
// read the header
byte packetType = br.ReadByte();
if (packetType != CLUSTER_PACKET_TYPE)
{
Debug.WriteLine("ERROR: cluster parser got packet starting with " + packetType.ToString());
return;
}
UInt16 packetNum = br.ReadUInt16();
UInt16 tsSeconds = br.ReadUInt16();
UInt32 tsTicks = br.ReadUInt32();
UInt16 numPts = br.ReadUInt16();
UInt16 numClusters = br.ReadUInt16();
msg.timestamp = (double)tsSeconds + (double)tsTicks / 10000.0;
// now for the red meat..
v3f[] pts = new v3f[numPts];
for (int i = 0; i < numPts; i++)
{
pts[i].x = (float)(br.ReadInt16()) / 100.0f;
pts[i].y = (float)(br.ReadInt16()) / 100.0f;
pts[i].z = (float)(br.ReadInt16()) / 100.0f;
}
msg.clusters = new List <LidarCluster>();
for (int i = 0; i < numClusters; i++)
{
LidarCluster lc = new LidarCluster();
UInt16 firstPtIndex, lastPtIndex;
byte flags;
firstPtIndex = br.ReadUInt16();
lastPtIndex = br.ReadUInt16();
flags = br.ReadByte();
lc.pts = new v3f[lastPtIndex - firstPtIndex + 1];
lc.stable = (flags & 0x01) == 0;
lc.leftOccluded = (flags & 0x02) != 0;
lc.rightOccluded = (flags & 0x04) != 0;
lc.highObstacle = (flags & 0x08) != 0;
lc.bb_low.x = lc.bb_high.x = pts[firstPtIndex].x;
lc.bb_low.y = lc.bb_high.y = pts[firstPtIndex].y;
lc.bb_low.z = lc.bb_high.z = pts[firstPtIndex].z;
for (int j = firstPtIndex; j <= lastPtIndex; j++)
{
lc.pts[j - firstPtIndex] = pts[j];
lc.bb_low.x = Math.Min(lc.bb_low.x, pts[j].x);
lc.bb_low.y = Math.Min(lc.bb_low.y, pts[j].y);
lc.bb_low.z = Math.Min(lc.bb_low.z, pts[j].z);
lc.bb_high.x = Math.Max(lc.bb_high.x, pts[j].x);
lc.bb_high.y = Math.Max(lc.bb_high.y, pts[j].y);
lc.bb_high.z = Math.Max(lc.bb_high.z, pts[j].z);
}
msg.clusters.Add(lc);
}
if (ClustersReceived != null)
{
ClustersReceived(this, new ClusterRXEventArgs(msg));
}
}
