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