private OnePC powerMethod(double[,] covar)
{
double sqr3 = 1.0f / Mathf.Sqrt(3.0f);
double[] pc1 = { sqr3, sqr3, sqr3 };
double shrink = 0.0;
for (int k = 0; k < 20; k++)
{
double[] pc1a = { 0.0f, 0.0f, 0.0f };
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
pc1a[i] += covar[i, j] * pc1[j];
}
}
double shrink2 = pc1a[0] * pc1a[0] + pc1a[1] * pc1a[1] + pc1a[2] * pc1a[2];
float shrink2f = (float)shrink2;
shrink = Mathf.Sqrt(shrink2f);
for (int i = 0; i < 3; i++)
{
pc1[i] = pc1a[i] / shrink;
}
}
OnePC pc = new OnePC(pc1, shrink);
return(pc);
}
private double[] fitNearestSurface(List <DistanceAndPoint> distancesAndPoints, int n)
{
if (DEBUG)
{
Debug.Log("TangoUpdate pca , for " + n + " points");
}
MatrixStats mc = getCovar(distancesAndPoints, n);
OnePC pc1 = powerMethod(mc.covar);
if (DEBUG)
{
Debug.Log("TangoUpdate pca1 " + pc1.pc[0] + ", " + pc1.pc[1] + ", " + pc1.pc[2]);
}
double[,] covar2 = new double[3, 3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
covar2[i, j] = mc.covar[i, j] - (pc1.shrink * pc1.pc[i] * pc1.pc[j]);
}
}
OnePC pc2 = powerMethod(covar2);
if (DEBUG)
{
Debug.Log("TangoUpdate pca2 " + pc2.pc[0] + ", " + pc2.pc[1] + ", " + pc2.pc[2]);
}
// c(x=y1 * z2 - z1 * y2, y=z1 * x2 - x1 * z2, z=x1 * y2 - y1 * x2)
double[] planeCoefficients =
{
pc1.pc [1] * pc2.pc [2] - pc1.pc [2] * pc2.pc [1],
pc1.pc [2] * pc2.pc [0] - pc1.pc [0] * pc2.pc [2],
pc1.pc [0] * pc2.pc [1] - pc1.pc [1] * pc2.pc [0],
0.0f
};
planeCoefficients[3] = planeCoefficients[0] * mc.mean[0] + planeCoefficients[1] * mc.mean[1] + planeCoefficients[2] * mc.mean[2];
return(planeCoefficients);
}
private OnePC powerMethod(double[,] covar)
{
double sqr3 = 1.0f / Mathf.Sqrt (3.0f);
double[] pc1 = {sqr3, sqr3, sqr3};
double shrink = 0.0;
for (int k = 0; k < 20; k++) {
double[] pc1a = {0.0f, 0.0f, 0.0f};
for (int i=0; i < 3; i++) {
for (int j=0; j < 3; j++) {
pc1a[i] += covar[i,j] * pc1[j];
}
}
double shrink2 = pc1a[0] * pc1a[0] + pc1a[1] * pc1a[1] + pc1a[2] * pc1a[2];
float shrink2f = (float) shrink2;
shrink = Mathf.Sqrt(shrink2f);
for (int i=0; i < 3; i++) {
pc1[i] = pc1a[i] / shrink;
}
}
OnePC pc = new OnePC (pc1, shrink);
return pc;
}