private BitmapSource Vector4ArrayToBitmapSource(Float4[,] source, float maxDepth) { int width = source.GetLength(0), height = source.GetLength(1); byte[] target = new byte[width * height]; unsafe { fixed(Float4 *sSource = &source[0, 0]) fixed(byte *sTarget = &target[0]) { Float4 *pSource = sSource; byte * pTarget = sTarget, eTarget = sTarget + width * height; float depth; while (pTarget < eTarget) { depth = pSource->Z; if (depth > maxDepth) { *pTarget = 255; } else if (depth < 0) { *pTarget = 0; } else { *pTarget = (byte)(depth * 255 / maxDepth); } pTarget++; pSource++; } } } return(BitmapSource.Create(width, height, 96d, 96d, PixelFormats.Gray8, BitmapPalettes.Gray256, target, width)); }
public unsafe bool InputFloat4(string label, Float4 *value) { return(InputFloat4(label, &value->X)); }
public static Plane PlaneFromDepthData(Float4[,] Data, Region region) { int width = Data.GetLength(0); int height = Data.GetLength(1); Plane P = new Plane(); //Area selection int minX = (region.MinX < 0 ? 0 : region.MinX); int maxX = (region.MaxX > width ? width : region.MaxX); int minY = (region.MinY < 0 ? 0 : region.MinY); int maxY = (region.MaxY > height ? height : region.MaxY); int jumpX = width - (maxX - minX); int startX = minY * width + minX; int count = 0; Vector3 pos = Vector3.Zero; unsafe { fixed(Float4 *sData = &Data[0, 0]) { //Calculate origin Float4 *pData = sData + startX; for (int y = minY; y < maxY; y++) { for (int x = minX; x < maxX; x++) { pData++; if (region.Covered(x, y) && pData->W != 0f) { pos.X += pData->X; pos.Y += pData->Y; pos.Z += pData->Z; count++; } } pData += jumpX; } pos /= count; P.Origin = new Vector3(pos.X, pos.Y, pos.Z); //Calculate normal double x2, y2, z2, xy, xz, yz; x2 = y2 = z2 = xy = xz = yz = 0; double rx, ry, rz; pData = sData + startX; for (int y = minY; y < maxY; y++) { for (int x = minX; x < maxX; x++) { pData++; if (region.Covered(x, y) && pData->W != 0f) { pos = new Vector3(pData->X, pData->Y, pData->Z); rx = pos.X - P.Origin.X; ry = pos.Y - P.Origin.Y; rz = pos.Z - P.Origin.Z; x2 += rx * rx; y2 += ry * ry; z2 += rz * rz; xy += rx * ry; xz += rx * rz; yz += ry * rz; } } pData += jumpX; } ManagedLinearAlgebraProvider linear = new ManagedLinearAlgebraProvider(); double[] matrix = new double[] { x2, xy, xz, xy, y2, yz, xz, yz, z2 }; double[] eigenVectors = new double[9]; Complex[] eigenValues = new Complex[3]; double[] matrixD = new double[9]; linear.EigenDecomp(true, 3, matrix, eigenVectors, eigenValues, matrixD); P.Normal = new Vector3(eigenVectors[0], eigenVectors[1], eigenVectors[2]); } } return(P); }