/// <summary>
/// 计算协方差举证
/// https://zhuanlan.zhihu.com/p/37609917
/// </summary>
/// <param name="points"></param>
/// <returns></returns>
public static Matrix2x2 GetCovarianceMatrix(Double2[] points)
{
Matrix2x2 covarianceMatrix = Matrix2x2.zero;
if (points == null || points.Length <= 0)
{
return(covarianceMatrix);
}
int numPoints = points.Length;
//Compute the covariance
for (int col = 0; col < 2; col++)
{
Double2 data;
for (int row = col; row < 2; row++)
{
covarianceMatrix[col, row] = 0.0f;
Double acc = 0.0f;
//cov(X,Y)=E[(X-x)(Y-y)]
for (int i = 0; i < numPoints; i++)
{
data = points[i];
acc += data[col] * data[row];
}
acc /= numPoints;
covarianceMatrix[col, row] = acc;
//symmetric
covarianceMatrix[row, col] = acc;
}
}
return(covarianceMatrix);
}
/// <summary>
/// * 运算
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(System.Object obj)
{
if (obj == null)
{
return(false);
}
Matrix2x2 p = (Matrix2x2)obj;
if ((System.Object)p == null)
{
return(false);
}
return((col1 == p.col1) && (col2 == p.col2));
}
/// <summary>
/// 求特征向量矩阵 通过使用Jacobi 迭代算法
/// </summary>
/// <param name="matrix"></param>
public static Matrix2x2 GetFeatureVectorMatrix(Matrix2x2 matrix)
{
double p, q, spq;
double cosa, sina; //cos(alpha) and sin(alpha)
double temp;
double s1 = 0.0f; //sums of squares of diagonal
double s2; //elements
bool flag = true; //determine whether to iterate again
int iteration = 0; //iteration counter
double[] data = new double[2];
Matrix2x2 t = Matrix2x2.one;//To store the product of the rotation matrices.
do
{
iteration++;
for (int i = 0; i < 2; i++)
{
for (int j = i + 1; j < 2; j++)
{
if (System.Math.Abs(matrix[j, i]) < MathUtil.kEpsilon)
{
matrix[j, i] = 0.0f;
}
else
{
q = System.Math.Abs(matrix[i, i] - matrix[j, j]);
if (q > MathUtil.kEpsilon)
{
p = (2.0f * matrix[j, i] * q) / (matrix[i, i] - matrix[j, j]);
spq = (float)System.Math.Sqrt(p * p + q * q);
cosa = (float)System.Math.Sqrt((1.0f + q / spq) / 2.0f);
sina = p / (2.0f * cosa * spq);
}
else
{
sina = cosa = (float)System.Math.Sqrt(2) * 0.5f;
}
for (int k = 0; k < 2; k++)
{
temp = t[i, k];
t[i, k] = (temp * cosa + t[j, k] * sina);
t[j, k] = (temp * sina - t[j, k] * cosa);
}
for (int k = i; k < 2; k++)
{
if (k > j)
{
temp = matrix[k, i];
matrix[k, i] = (cosa * temp + sina * matrix[k, j]);
matrix[k, j] = (sina * temp - cosa * matrix[k, j]);
}
else
{
data[k] = matrix[k, i];
matrix[k, i] = (cosa * data[k] + sina * matrix[j, k]);
if (k == j)
{
matrix[k, j] = (sina * data[k] - cosa * matrix[k, j]);
}
}
}
data[j] = sina * data[i] - cosa * data[j];
for (int k = 0; k <= j; k++)
{
if (k <= i)
{
temp = matrix[i, k];
matrix[i, k] = (cosa * temp + sina * matrix[j, k]);
matrix[j, k] = (sina * temp - cosa * matrix[j, k]);
}
else
{
matrix[j, k] = (sina * data[k] - cosa * matrix[j, k]);
}
}
}
}
}
s2 = 0.0f;
for (int i = 0; i < 2; i++)
{
s2 += matrix[i, i] * matrix[i, i];
}
if (System.Math.Abs(s2) < MathUtil.kEpsilon || System.Math.Abs(1 - s1 / s2) < MathUtil.kEpsilon)
{
flag = false;
}
else
{
s1 = s2;
}
} while (flag);
return(t);
}
/// <summary>
/// 转置矩阵
/// </summary>
/// <param name="k"></param>
/// <param name="vector"></param>
/// <returns></returns>
public static Matrix2x2 T(Matrix2x2 v)
{
return(new Matrix2x2(v.row1, v.row2));
}
/// <summary>
/// 向量旋转
/// </summary>
/// <param name="angle"></param>
/// <returns></returns>
public static Double2 Rotate(Double2 a, double angle)
{
return(Matrix2x2.RotateMatrix(angle) * a);
}
/// <summary>
/// 返回逆时针旋转90度的向量
/// </summary>
/// <param name="inDirection"></param>
/// <returns></returns>
public static Double2 Perpendicular(Double2 inDirection)
{
return(Matrix2x2.RotateMatrix(MathUtil.kPI / 2) * inDirection);
}
