All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
PGS.h
Go to the documentation of this file.
1 //=================================================================================================
20 //=================================================================================================
21 
22 #ifndef _BLAZE_MATH_SOLVERS_PGS_H_
23 #define _BLAZE_MATH_SOLVERS_PGS_H_
24 
25 
26 //*************************************************************************************************
27 // Includes
28 //*************************************************************************************************
29 
30 #include <cmath>
37 #include <blaze/util/Assert.h>
38 #include <blaze/util/ColorMacros.h>
40 #include <blaze/util/Types.h>
41 
42 
43 namespace blaze {
44 
45 //=================================================================================================
46 //
47 // CLASS DEFINITION
48 //
49 //=================================================================================================
50 
51 //*************************************************************************************************
59 class PGS : public Solver
60 {
61  public:
62  //**Constructors********************************************************************************
65  explicit PGS();
67  //**********************************************************************************************
68 
69  //**Utility functions***************************************************************************
72  template< typename CP > bool solve( CP& cp );
74  //**********************************************************************************************
75 
76  private:
77  //**Utility functions***************************************************************************
80  template< typename CP > inline real sweep( CP& cp ) const;
82  //**********************************************************************************************
83 
84  //**Member variables****************************************************************************
88 
91  //**********************************************************************************************
92 };
93 //*************************************************************************************************
94 
95 
96 
97 
98 //=================================================================================================
99 //
100 // UTILITY FUNCTIONS
101 //
102 //=================================================================================================
103 
104 //*************************************************************************************************
112 template< typename CP > // Type of the complementarity problem
113 bool PGS::solve( CP& cp )
114 {
115  const size_t n( cp.size() );
116  const CMatMxN& A( cp.A_ );
117  bool converged( false );
118 
119  // Allocating the helper data
120  diagonal_.resize( n, false );
121 
122  // Locating the diagonal entries in system matrix and precomputing the inverse values
123  for( size_t i=0; i<n; ++i ) {
124  const real tmp( A(i,i) );
125  BLAZE_INTERNAL_ASSERT( tmp != real(0), "Invalid diagonal element in the LCP matrix" );
126  diagonal_[i] = real(1) / tmp;
127  }
128 
129  // Projecting the initial solution to a feasible region
130  for( size_t i=0; i<n; ++i ) {
131  cp.project( i );
132  }
133 
134  // Computing the initial residual
135  lastPrecision_ = cp.residual();
136  if( lastPrecision_ < threshold_ )
137  converged = true;
138 
139  // The main iteration loop
140  size_t it( 0 );
141 
142  for( ; !converged && it<maxIterations_; ++it ) {
143  lastPrecision_ = sweep( cp );
144  if( lastPrecision_ < threshold_ )
145  converged = true;
146  }
147 
148  BLAZE_LOG_DEBUG_SECTION( log ) {
149  if( converged )
150  log << " Solved the complementarity problem in " << it << " PGS iterations.";
151  else
152  log << BLAZE_YELLOW << " WARNING: Did not solve the complementarity problem within accuracy. (" << lastPrecision_ << ")" << BLAZE_OLDCOLOR;
153  }
154 
155  lastIterations_ = it;
156 
157  return converged;
158 }
159 //*************************************************************************************************
160 
161 
162 //*************************************************************************************************
170 template< typename CP > // Type of the complementarity problem
171 inline real PGS::sweep( CP& cp ) const
172 {
173  real maxResidual( 0 ), xold( 0 );
174  const size_t n( cp.size() );
175 
176  const CMatMxN& A( cp.A_ );
177  const VecN& b( cp.b_ );
178  VecN& x( cp.x_ );
179 
180  for( size_t i=0; i<n; ++i )
181  {
182  const real residual( - b[i] - ( A * x )[i] );
183 
184  // Updating and projecting the unknown
185  xold = x[i];
186  x[i] += diagonal_[i] * residual;
187  cp.project( i );
188  maxResidual = max( maxResidual, std::fabs( xold - x[i] ) );
189  }
190 
191  return maxResidual;
192 }
193 //*************************************************************************************************
194 
195 
196 //*************************************************************************************************
205 template<>
206 inline real PGS::sweep( ContactLCP& cp ) const
207 {
208  const size_t N( cp.size() / 3 );
209  real rmax( 0 ), residual, flimit, aux;
210  size_t j;
211 
212  const CMatMxN& A( cp.A_ );
213  const VecN& b( cp.b_ );
214  VecN& x( cp.x_ );
215 
216  for( size_t i=0; i<N; ++i )
217  {
218  j = i * 3;
219  residual = -b[j] - ( A * x )[j];
220  aux = max( 0, x[j] + diagonal_[j] * residual );
221  rmax = max( rmax, std::fabs( x[j] - aux ) );
222  x[j] = aux;
223 
224  flimit = cp.cof_[i] * x[j];
225 
226  ++j;
227  residual = -b[j] - ( A * x )[j];
228  aux = max( -flimit, min( flimit, x[j] + diagonal_[j] * residual ) );
229  rmax = max( rmax, std::fabs( x[j] - aux ) );
230  x[j] = aux;
231 
232  ++j;
233  residual = -b[j] - ( A * x )[j];
234  aux = max( -flimit, min( flimit, x[j] + diagonal_[j] * residual ) );
235  rmax = max( rmax, std::fabs( x[j] - aux ) );
236  x[j] = aux;
237  }
238 
239  return rmax;
240 }
241 //*************************************************************************************************
242 
243 
244 
245 
246 //=================================================================================================
247 //
248 // EXPLICIT TEMPLATE INSTANTIATIONS
249 //
250 //=================================================================================================
251 
252 //*************************************************************************************************
254 #if !defined(_MSC_VER)
255 extern template bool PGS::solve<LCP>( LCP& );
256 extern template bool PGS::solve<BoxLCP>( BoxLCP& );
257 extern template bool PGS::solve<ContactLCP>( ContactLCP& );
258 #endif
259 
260 //*************************************************************************************************
261 
262 } // namespace blaze
263 
264 #endif