public bool TestRank(out int dof)
{
EvalJacobian(J, ref A, clearDrag: false);
int rank = GaussianMethod.Rank(A);
dof = A.GetLength(1) - rank;
return(rank == A.GetLength(0));
}
public void SolveLeastSquares(double[,] A, double[] B, ref double[] X)
{
// A^T * A * X = A^T * B
var rows = A.GetLength(0);
var cols = A.GetLength(1);
UnityEngine.Profiling.Profiler.BeginSample("SolveLeastSquares: A^T * A");
var time = Time.realtimeSinceStartup;
for (int r = 0; r < rows; r++)
{
for (int c = 0; c < rows; c++)
{
double sum = 0.0;
for (int i = 0; i < cols; i++)
{
if (A[c, i] == 0 || A[r, i] == 0)
{
continue;
}
sum += A[r, i] * A[c, i];
}
AAT[r, c] = sum;
}
}
//Debug.Log("AAT time " + (Time.realtimeSinceStartup - time) * 1000);
UnityEngine.Profiling.Profiler.EndSample();
GaussianMethod.Solve(AAT, B, ref Z);
for (int c = 0; c < cols; c++)
{
double sum = 0.0;
for (int r = 0; r < rows; r++)
{
sum += Z[r] * A[r, c];
}
X[c] = sum;
}
}