/// <summary>
/// Maximum of the absolute value of all Eigenvalues of <paramref name="H"/>.
/// </summary>
static public double MaxAbsEigen(double[,] H)
{
var linalg = new ManagedLinearAlgebraProvider();
double Eigen;
int N = H.GetLength(0);
double[] Matrix = H.Resize(false);
double[] EigenVect = new double[Matrix.Length];
double[] diagM = new double[Matrix.Length];
System.Numerics.Complex[] EigenValues = new System.Numerics.Complex[N];
linalg.EigenDecomp(false, N, Matrix, EigenVect, EigenValues, diagM);
Eigen = EigenValues.Select(ev => Complex.Abs(ev)).Max();
return(Eigen);
}
public Vector3D[] identifyMajorAxes()
{
Vector3D[] majorAxes = new Vector3D[3];
double[] covarianceMatrix = covariance();
double[] eigenVectors = new double[9];
Complex[] eigenValues = new Complex[3];
double[] eigenVectorsM = new double[9];
ManagedLinearAlgebraProvider pca = new ManagedLinearAlgebraProvider();
pca.EigenDecomp(true, 3, covarianceMatrix, eigenVectors, eigenValues, eigenVectorsM);
// major axis
majorAxes[0] = new Vector3D();
majorAxes[0].X = eigenVectors[6];
majorAxes[0].Y = eigenVectors[7];
majorAxes[0].Z = eigenVectors[8];
majorAxes[1] = new Vector3D();
majorAxes[1].X = eigenVectors[3];
majorAxes[1].Y = eigenVectors[4];
majorAxes[1].Z = eigenVectors[5];
majorAxes[2] = new Vector3D();
majorAxes[2].X = eigenVectors[0];
majorAxes[2].Y = eigenVectors[1];
majorAxes[2].Z = eigenVectors[2];
_transformMatrix = new Matrix3D(majorAxes[0].X, majorAxes[1].X, majorAxes[2].X, 0,
majorAxes[0].Y, majorAxes[1].Y, majorAxes[2].Y, 0,
majorAxes[0].Z, majorAxes[1].Z, majorAxes[2].Z, 0,
-Centroid.X, -Centroid.Y, -Centroid.Z, 1);
List <Point3D> trOBBV = transformPointSet();
BoundingBox3D trOBB = new BoundingBox3D(trOBBV);
OBB = transformBackPointSet(trOBB.BBVertices);
return(majorAxes);
}
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);
}