symmetric_conjugate_gradient_method.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00034 #include <acado/sparse_solver/sparse_solver.hpp>
00035 
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 
00046 SymmetricConjugateGradientMethod::SymmetricConjugateGradientMethod( )
00047                                  :ConjugateGradientMethod( ){
00048 
00049     index     = 0;
00050     nIndex    = 0;
00051     diag      = 0;
00052 }
00053 
00054 
00055 SymmetricConjugateGradientMethod::SymmetricConjugateGradientMethod( const SymmetricConjugateGradientMethod &arg )
00056                                  :ConjugateGradientMethod(arg){
00057 
00058     int run1, run2;
00059 
00060     if( arg.nIndex != 0 ){
00061         nIndex = new int[dim];
00062         for( run1 = 0; run1 < dim; run1++ ){
00063              nIndex[run1] = arg.nIndex[run1];
00064         }
00065     }
00066     else  nIndex = 0;
00067 
00068     if( arg.index != 0 ){
00069         index = new int*[dim];
00070         for( run1 = 0; run1 < dim; run1++ ){
00071              index[run1] = new int[nIndex[run1]];
00072              for( run2 = 0; run2 < nIndex[run1]; run2++ )
00073                  index[run1][run2] = arg.index[run1][run2];
00074         }
00075     }
00076     else index = 0;
00077 
00078     if( arg.diag != 0 ){
00079         diag = new int[dim];
00080         for( run1 = 0; run1 < dim; run1++ ){
00081              diag[run1] = arg.diag[run1];
00082         }
00083     }
00084     else  diag = 0;
00085 }
00086 
00087 
00088 SymmetricConjugateGradientMethod::~SymmetricConjugateGradientMethod( ){
00089 
00090     int run1;
00091 
00092     if( nIndex != 0 )  delete[] nIndex;
00093 
00094     if( diag != 0 )  delete[] diag;
00095 
00096     if( index != 0 ){
00097         for( run1 = 0; run1 < dim; run1++ )
00098             delete[] index[run1];
00099         delete[] index;
00100     }
00101 }
00102 
00103 
00104 
00105 SparseSolver* SymmetricConjugateGradientMethod::clone() const{
00106 
00107     return new SymmetricConjugateGradientMethod(*this);
00108 }
00109 
00110 
00111 returnValue SymmetricConjugateGradientMethod::setIndices( const int *rowIdx_, const int *colIdx_  ){
00112 
00113      return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00114 }
00115 
00116 
00117 returnValue SymmetricConjugateGradientMethod::setIndices( const int *indices ){
00118 
00119 
00120     // CONSISTENCY CHECK:
00121     // ------------------
00122 
00123     if( dim    <= 0 )  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00124     if( nDense <= 0 )  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00125 
00126 
00127     // ALLOCATE MEMORY:
00128     // ------------------
00129 
00130     if( nIndex != 0 )  delete[] nIndex;
00131     nIndex = new int[dim];
00132 
00133     if( diag != 0 )  delete[] diag;
00134     diag = new int[dim];
00135 
00136 
00137     // LOCAL AUXILIARY VARIABLE:
00138     // -------------------------
00139     int run1,  run2;
00140     int counter0   ;
00141     int bound      ;
00142     int bound2     ;
00143     int counter = 0;
00144 
00145     BooleanType diagFound;
00146 
00147 
00148     // DETERMINE THE INDEX LENGHTS:
00149     // ----------------------------
00150 
00151 
00152     for( run1 = 0; run1 < dim; run1++ ){
00153 
00154         counter0 = counter;
00155         bound    = dim*(run1+1);
00156         bound2   = run1*(dim+1);
00157 
00158         diagFound = BT_FALSE;
00159 
00160         while( counter < nDense ){
00161 
00162             if( diagFound == BT_FALSE && indices[counter] >= bound2 ){
00163                  diag[run1] = counter;
00164                  diagFound  = BT_TRUE;
00165             }
00166 
00167             if( indices[counter] >= bound ) break;
00168             counter++;
00169         }
00170 
00171         nIndex[run1] = counter - counter0;
00172     }
00173 
00174     if( index != 0 ){
00175         for( run1 = 0; run1 < dim; run1++ )
00176             delete[] index[run1];
00177         delete[] index;
00178     }
00179     index     = new int*[dim];
00180 
00181     counter = 0;
00182 
00183     for( run1 = 0; run1 < dim; run1++ ){
00184         index[run1]     = new int[nIndex[run1]];
00185         for( run2 = 0; run2 < nIndex[run1]; run2++ ){
00186             index    [run1][run2] = indices[counter];
00187             counter++;
00188         }
00189     }
00190 
00191     return SUCCESSFUL_RETURN;
00192 }
00193 
00194 
00195 
00196 
00197 
00198 
00199 
00200 
00201 //
00202 // PROTECTED MEMBER FUNCTIONS:
00203 //
00204 
00205 
00206 
00207 void SymmetricConjugateGradientMethod::multiply( double *xx , double *result ){
00208 
00209 
00210     int i,j,counter, offset;
00211 
00212     counter = 0;
00213 
00214     for( i = 0; i < dim; i++ ){
00215         result[i] = 0.0;
00216         offset    = dim*i;
00217         for( j = 0; j < nIndex[i]; j++ ){
00218             result[i] += A[counter]*xx[index[i][j]-offset];
00219             counter++;
00220         }
00221     }
00222 }
00223 
00224 
00225 
00226 
00227 returnValue SymmetricConjugateGradientMethod::computePreconditioner( double* A_ ){
00228 
00229     int i,j,counter, offset;
00230 
00231     for( i = 0; i < dim; i++ ){
00232         condScale[i] = sqrt(A_[diag[i]]);
00233     }
00234 
00235     counter = 0;
00236 
00237     for( i = 0; i < dim; i++ ){
00238         offset = dim*i;
00239         for( j = 0; j < nIndex[i]; j++ ){
00240             A[counter] = A_[counter]/(condScale[i]*condScale[index[i][j]-offset]);
00241             counter++;
00242         }
00243     }
00244 
00245     return SUCCESSFUL_RETURN;
00246 }
00247 
00248 
00249 
00250 returnValue SymmetricConjugateGradientMethod::applyPreconditioner( double *b ){
00251 
00252     int run1;
00253 
00254     for( run1 = 0; run1 < dim; run1++ )
00255         r[run1] = b[run1]/condScale[run1];
00256 
00257     return SUCCESSFUL_RETURN;
00258 }
00259 
00260 
00261 
00262 
00263 returnValue SymmetricConjugateGradientMethod::applyInversePreconditioner( double *x_ ){
00264 
00265     int run1;
00266 
00267     for( run1 = 0; run1 < dim; run1++ )
00268         x_[run1] = x[run1]/condScale[run1];
00269 
00270     return SUCCESSFUL_RETURN;
00271 }
00272 
00273 
00274 
00275 
00276 CLOSE_NAMESPACE_ACADO
00277 
00278 
00279 /*
00280  *   end of file
00281  */


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:39:55