public static double CalculateDistance(NullablePoint3D point1, NullablePoint3D point2)
{
return Math.Sqrt((
Math.Pow(point1.Y - point2.Y, 2) +
Math.Pow(point1.Z - point2.Z, 2) +
Math.Pow(point1.X - point2.X, 2)
));
}
private void ProcessPointCloudStreamMessage(object obj, KinectClient client)
{
bool coloredPointCloud = false;
PointCloudStreamMessage msg = (PointCloudStreamMessage)obj;
if (obj is ColoredPointCloudStreamMessage)
{
coloredPointCloud = true;
}
double[] doubleArray = msg.PointCloud;
NullablePoint3D[] pointArray = new NullablePoint3D[doubleArray.Length / 3];
for (int i = 0; i < doubleArray.Length; i += 3)
{
if (double.IsNegativeInfinity(doubleArray[i]))
{
pointArray[i / 3] = null;
}
else
{
pointArray[i / 3] = new NullablePoint3D(doubleArray[i], doubleArray[i + 1], doubleArray[i + 2]);
}
}
PointCloud pointCloud = new PointCloud() {Points = pointArray};
if (coloredPointCloud)
{
pointCloud.ColorBytes = ((ColoredPointCloudStreamMessage) msg).ColorPixels;
}
dataStore.AddOrUpdatePointCloud(client, pointCloud);
if (PointCloudMessageArrived != null)
{
PointCloudMessageArrived(msg, client);
}
if (coloredPointCloud && ColoredPointCloudMessageArrived != null)
{
ColoredPointCloudMessageArrived(msg, client);
}
}
public ColoredPointCloudStreamMessage(NullablePoint3D[] points, byte[] colors)
: base(points)
{
ColorPixels = colors;
}