Пример #1
0
        private double[] CalculateDirection(
            LinearProblemProperties input,
            double[] Xk1,
            double[] deltaK,
            ref double[] searchDirection,
            double betak)
        {
            double[] result = new double[Xk1.Length];

            for (int i = 0; i < Xk1.Length; i++)
            {
                double bDirection = betak * searchDirection[i];

                result[i] = Xk1[i] + bDirection;

                searchDirection[i] = bDirection - deltaK[i];

                result[i] = ClampSolution.Clamp(input, result[i], ref result, i);
            }

            return(result);
        }
        private double ElaborateUpperTriangularMatrix(
            LinearProblemProperties input,
            double[] sum,
            int[] solverOrder,
            ref double[] x,
            ref double[] oldX)
        {
            double error = 0.0;

            for (int i = 0; i < input.Count; i++)
            {
                int index = solverOrder[i];

                double sumBuffer = sum[index];

                SparseVector m = input.M.Rows[index];
                ShapeDefinition.ConstraintType cType = input.ConstraintType[index];

                double xValue = x[index];

                //Avoid first row elaboration
                if (index != 0 && m.Index.Length > 0)
                {
                    var bufValue = m.Value;
                    var bufIndex = m.Index;

                    for (int j = 0; j < bufIndex.Length; j++)
                    {
                        int idx = bufIndex[j];
                        if (idx < index)
                        {
                            sumBuffer += bufValue[j] * x[idx];
                        }
                    }
                }

                sumBuffer = (input.B[index] - sumBuffer) * input.InvD[index];

                double sor = SolverParameters.SOR;

                // Constraints with limit diverge with sor > 1.0
                if (sor > 1.0 &&
                    (cType == ShapeDefinition.ConstraintType.Friction ||
                     cType == ShapeDefinition.ConstraintType.JointMotor ||
                     cType == ShapeDefinition.ConstraintType.JointLimit))
                {
                    sor = 1.0;
                }

                xValue += (sumBuffer - xValue) * sor;

                x[index] = ClampSolution.Clamp(input, xValue, ref x, index);

                //Compute error
                double diff = x[index] - oldX[index];
                error      += diff * diff;
                oldX[index] = x[index];
            }

            return(error);
        }