示例#1
0
    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));
    }
示例#2
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;
        }
    }