C# DenseVector.SetSubVector方法代码示例

 public double[] transform(double[] displacement, bool isreverse)
 {
     DenseMatrix xform = (isreverse ? (DenseMatrix)transformation.Inverse() : transformation);
     double[] dispcopy = new double[6];
     displacement.CopyTo(dispcopy, 0);
     DenseMatrix2String m2s = new DenseMatrix2String();
     List2String l2s = new List2String();
     DenseVector dispV = new DenseVector(dispcopy);
     log.Debug("original disp: " + l2s.ToString(displacement));
     if (isreverse)
     {
         dispV = (DenseVector)xform.Multiply(dispV);
         log.Debug("transformed Disp: " + l2s.ToString(dispV.Values));
     }
     DenseVector newDisp = translate(dispV.Values, isreverse);
     log.Debug("newDisp: " + l2s.ToString(newDisp.Values));
     DenseMatrix rollM = roll.create(dispV.Values[3]);
     DenseMatrix pitchM = pitch.create(dispV.Values[4]);
     DenseMatrix yawM = yaw.create(dispV.Values[5]);
     DenseMatrix rotation = (DenseMatrix)rollM.Multiply(pitchM.Multiply(yawM));
     log.Debug("rotation: " + m2s.ToString(rotation));
     DenseVector unt = (DenseVector)rotation.Multiply(directionalVector);
     log.Debug("unt: " + l2s.ToString(unt.Values));
     DenseVector newDisp1 = (DenseVector)unt.Add(newDisp);
     log.Debug("newDisp1: " + l2s.ToString(newDisp1.Values));
     dispV.SetSubVector(0, 3, newDisp1);
     if (isreverse == false)
     {
         dispV = (DenseVector)xform.Multiply(dispV);
     }
     log.Debug("resulting Disp: " + l2s.ToString(dispV.Values));
     return dispV.Values;
 }

        public double[] transformMoments(double[] forces)
        {
            double[] fforce = new double[6];
            forces.CopyTo(fforce, 0);
            DenseVector forceV = new DenseVector(fforce);
            DenseVector tforce = (DenseVector)forceV.SubVector(0, 3);
            DenseVector moments = (DenseVector)forceV.SubVector(3, 3);

            DenseVectorCrossProduct crs = new DenseVectorCrossProduct(tforce);
            moments = (DenseVector)crs.crossProduct(directionalVector).Add(moments);

            forceV.SetSubVector(3, 3, moments);
            return forceV.Values;
        }

        public void FitQuadricThroughPoint(int line)
        {
            // | 0 |   |x1^2-x0^2  x1-x0  x1y1-x0y0  y1-y0  y1^2-y0^2| | A |
            // |   |   |                                             | | B |
            // |   | = |                                             | | C |
            // |   |   |                                             | | D |
            // | 0 |   |xn^2-x0^2  xn-x0  xnyn-x0y0  yn-y0  yn^2-y0^2| | E |

            var points = _correctedPf[line];
            Vector2 fp = points[FitPoints[line]];
            Matrix<double> X = new DenseMatrix(LinePoints[line].Count - 1, 5);
            for(int p = 0; p < FitPoints[line]; ++p)
            {
                var point = points[p];
                X[p, 0] = point.X * point.X - fp.X * fp.X;
                X[p, 1] = point.X - fp.X;
                X[p, 2] = point.X * point.Y - fp.X * fp.Y;
                X[p, 3] = point.Y - fp.Y;
                X[p, 4] = point.Y * point.Y - fp.Y * fp.Y;
            }

            for(int p = FitPoints[line] + 1; p < LinePoints[line].Count; ++p)
            {
                var point = points[p];
                X[p - 1, 0] = point.X * point.X - fp.X * fp.X;
                X[p - 1, 1] = point.X - fp.X;
                X[p - 1, 2] = point.X * point.Y - fp.X * fp.Y;
                X[p - 1, 3] = point.Y - fp.Y;
                X[p - 1, 4] = point.Y * point.Y - fp.Y * fp.Y;
            }

            var coeffs = SvdZeroFullrankSolver.Solve(X);

            double F = -(coeffs[0] * fp.X * fp.X + coeffs[1] * fp.X +
                coeffs[2] * fp.X * fp.Y + coeffs[3] * fp.Y + coeffs[4] * fp.Y * fp.Y);

            var coeffs_f = new DenseVector(6);
            coeffs_f.SetSubVector(0, 5, coeffs);
            coeffs_f[5] = F;

            FitQuadrics[line] = coeffs_f;
        }

        /*
         * Layer 4: weighted Layer.
         * 
        */

        public DenseMatrix CalculateGreatPsi(DenseMatrix X, DenseMatrix Psi)
        {
            int N = Psi.RowCount;
            int U = Psi.ColumnCount;
            int R = X.RowCount; //the number of inputs per observation

            var GreatPsi = new DenseMatrix(U*(R + 1), N);

            //foreach observation
            for (int i = 0; i < N; i++)
            {
                var x = new DenseVector(R);
                X.Column(i, x);

                var GreatPsiCol = new DenseVector(U*(R + 1));

                //foreach neuron
                for (int j = 0; j < U; j++)
                {
                    var temp = new DenseVector(x.Count + 1, 1);
                    temp.SetSubVector(1, x.Count, x);
                    GreatPsiCol.SetSubVector(j*(temp.Count), temp.Count, Psi[i, j]*temp);
                }

                GreatPsi.SetColumn(i, GreatPsiCol);
            }

            return GreatPsi;
        }

//.........这里部分代码省略.........
            //}

            // Шаг 3
            var falseIndex = -1;
            var maxFract = 0d;
            for (int i = 0; i < _task.xo.Count(); i++)
            {
                if (Math.Abs(Math.Round(_task.xo[i]) - _task.xo[i]) > Eps)
                {
                    var fract = Math.Abs(_task.xo[i] - Math.Floor(_task.xo[i]));    // Находим базисную переменную
                    if (_task.Jb.Contains(i) && fract > Eps)                        // С максимальной дробной частью
                    {                                                               // и запоминаем ее индекс
                        if (fract > maxFract)
                        {
                            maxFract = fract;
                            falseIndex = i;
                        }
                    }
                }
            }

            if (falseIndex < 0)     // Если все переменные целые - решение найдено
            {
                return false;   // Прерываем выполнение метода
            }
            _writer.WriteLine("Jk = {0}", falseIndex);

            // Шаг 4
            var aB = new DenseMatrix(_task.Jb.Count());
            int index = 0;
            foreach (var j in _task.Jb)
            {
                aB.SetColumn(index, _task.A.Column(j));     // Формируем матрицу Ab из базисных столбцов А
                index++;
            }
            _writer.Write("Jb: ");
            _task.Jb.ForEach(x => _writer.Write("{0} ", x));
            _writer.WriteLine();
            _writer.WriteLine("Basis matrix: {0}", aB);
            var y = DenseMatrix.Identity(_task.A.RowCount).Column(_task.Jb.IndexOf(falseIndex)) * aB.Inverse(); //Находим e'*Ab

            var newRow = new DenseVector(_task.A.ColumnCount + 1);
            newRow.SetSubVector(0, _task.A.ColumnCount, y * _task.A);   // Находим данные для нового отсекающего ограничения

            _writer.WriteLine("Data for new limitation: {0}", newRow);

            for (int i = 0; i < newRow.Count; i++)      // Формируем новое отсекающее ограничение
            {
                if (i < _task.A.ColumnCount)
                {
                    if (Math.Abs(newRow[i]) < Eps)
                    {
                        newRow[i] = 0;
                    }
                    else
                    {
                        newRow[i] = newRow[i] > 0
                                    ? -(newRow[i] - Math.Floor(newRow[i]))
                                    : -(Math.Ceiling(Math.Abs(newRow[i])) - Math.Abs(newRow[i]));
                    }
                }
                else
                {
                    newRow[i] = 1;
                }
            }
            newRow[falseIndex] = 0;
            _writer.WriteLine("New limitation: {0}", newRow);

            var newb = (y * _task.b);   // Находим новый элемент вектора b
            newb = newb > 0 ? -(newb - Math.Floor(newb)) : -(Math.Ceiling(Math.Abs(newb)) - Math.Abs(newb)); // TODO probably need to rewrite this

            _writer.WriteLine("New b = {0}", newb);

            // Шаг 5
            var newMatrix = new DenseMatrix(_task.A.RowCount + 1, _task.A.ColumnCount + 1); // Формируем новую
            newMatrix.SetSubMatrix(0, _task.A.RowCount, 0, _task.A.ColumnCount, _task.A);   // матрицу А
            newMatrix.SetRow(_task.A.RowCount, newRow);
            newMatrix[_task.A.RowCount, _task.A.ColumnCount] = 1;

            var newBVector = new DenseVector(_task.b.Count + 1);    // Формируем новый
            newBVector.SetSubVector(0, _task.b.Count, _task.b);     // вектор b
            newBVector[_task.b.Count] = newb;

            var newCVector = new DenseVector(_task.c.Count + 1);    // Добавляем новую
            newCVector.SetSubVector(0, _task.c.Count, _task.c);     // компоненту вектора с

            var newJb = _task.Jb.ToList();
            newJb.Add(newJb[newJb.Count - 1] + 1);
            _artJ.Add(new ArtJEntry { Column = newMatrix.ColumnCount - 1, Row = newMatrix.RowCount - 1 });

            _task.A = newMatrix.Clone();        // Создаем
            _task.b = newBVector.Clone();       // новую задачу
            _task.c = newCVector.Clone();       // для следующей итерации
            _task.Jb = newJb;

            iterationNumber++;              // Присваиваем новый номер итерации

            return true;
        }

        private void InsertNewRestrictionRow(DenseVector insF, double insB)
        {
            CurrA = (DenseMatrix) CurrA.InsertRow(CurrA.RowCount, -insF); // TODO: test this
            var newColumn = new DenseVector(CurrA.RowCount);
            newColumn[CurrA.RowCount - 1] = 1;
            CurrA = (DenseMatrix) CurrA.InsertColumn(CurrA.ColumnCount, newColumn);

            var newB = new DenseVector(CurrB.Count + 1);
            newB.SetSubVector(0, CurrB.Count, CurrB);
            newB[CurrB.Count] = -insB;
            CurrB = newB;

            var newC = new DenseVector(CurrC.Count + 1);
            newC.SetSubVector(0, CurrC.Count, CurrC);
            CurrC = newC;
        }

        public double[] calcNewDiffs(double[] cartesian)
        {
            DenseMatrix rotTrig = DenseMatrix.Create(3,2,0);
            double theta_x = cartesian[3];
            double theta_y = cartesian[4];
            double theta_z = cartesian[5];
            var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            rotTrig.At(0, 0, Math.Sin(theta_x));
            rotTrig.At(0, 1, Math.Cos(theta_x));
            rotTrig.At(1, 0, Math.Sin(theta_y));
            rotTrig.At(1, 1, Math.Cos(theta_y));
            rotTrig.At(2, 0, Math.Sin(theta_z));
            rotTrig.At(2, 1, Math.Cos(theta_z));
            log.Debug("Rotation Angles " + rotTrig.ToString("#000.00000\t", formatProvider));
            DenseVector basepin = fixedPin;
            DenseVector nominalplatformpin = initialPlatformPin;
            double Ppx = nominalplatformpin[0];
            double Ppy = nominalplatformpin[1];
            double Ppz = nominalplatformpin[2];
            log.Debug("Ppx= " + Ppx + "Ppy= " + Ppy + "Ppz= " + Ppz);
            DenseVector currentplatformpin = platformPin;
            List2String l2s = new List2String();

            DenseVector phai = (DenseVector) currentplatformpin.Subtract(basepin);
            log.Debug("phai " + l2s.ToString(phai.Values));

            if (length == 0.0)
            {
                log.Error("Actuator length is zero.  Caused a divide-by-zero error");
                throw new Exception("Actuator length is zero causing a divide by zero error");
            }

            phai = (DenseVector) phai.Divide(length);
            log.Debug("phai " + l2s.ToString(phai.Values));

            DenseMatrix J = DenseMatrix.Create(3,3,0);
            // Jacobian Element
            J[0, 0] = 0;
            J[0, 1] =  -rotTrig[1, 0] * rotTrig[2, 1] * Ppx + rotTrig[1, 0] * rotTrig[2, 0] * Ppy + rotTrig[1, 1] * Ppz;
            J[0, 2] =  -rotTrig[1, 1] * rotTrig[2, 0] * Ppx - rotTrig[1, 1] * rotTrig[2, 1] * Ppy;
            J[1, 0] = (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 0] * rotTrig[2, 0]) * Ppx +
                    (-rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 0] - rotTrig[0, 0] * rotTrig[0, 1]) * Ppy
                     - rotTrig[0, 1] * rotTrig[1, 1] * Ppz;
            J[1, 1] =  rotTrig[0, 0] * rotTrig[1, 1] * rotTrig[2, 1] * Ppx - rotTrig[0, 0] * rotTrig[1, 1] * rotTrig[2, 0] * Ppy
                     - rotTrig[0, 0] * rotTrig[1, 1] * Ppz;
            J[1, 2] = (-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 0] * rotTrig[0, 1] * rotTrig[2, 1]) * Ppx +
                    (-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 1] * rotTrig[2, 0]) * Ppy;
            J[2, 0] = (rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 1] + rotTrig[0, 1] * rotTrig[2, 0]) * Ppx +
                    (-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 0] + rotTrig[0, 1] * rotTrig[2, 1]) * Ppy
                     - rotTrig[0, 0] * rotTrig[1, 1] * Ppz;
            J[2, 1] =  -rotTrig[0, 1] * rotTrig[1, 1] * rotTrig[2, 1] * Ppx + rotTrig[0, 1] * rotTrig[1, 1] * rotTrig[2, 0] * Ppy
                     - rotTrig[0, 1] * rotTrig[1, 0] * Ppz;
            J[2, 2] = (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 0] + rotTrig[0, 0] * rotTrig[2, 1]) * Ppx
                    + (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 0] * rotTrig[2, 0]) * Ppy;

            log.Debug("Jacobian " + J.ToString("#000.00000\t", formatProvider));
            DenseVector diffs = new DenseVector(6);
            diffs.SetSubVector(0, 3, phai);
            DenseVector rphai = new DenseVector(new[] { (J[0, 0] * phai[0] + J[1, 0] * phai[1] + J[2, 0] * phai[2]),
                    (J[0, 1] * phai[0] + J[1, 1] * phai[1] + J[2, 1] * phai[2]),
                        (J[0, 2] * phai[0] + J[1, 2] * phai[2] + J[2, 2] * phai[2]) });
            log.Debug("rphai " + l2s.ToString(rphai.Values));
            diffs.SetSubVector(3, 3, rphai);
            return diffs.Values;
        }