All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
CPG.h
Go to the documentation of this file.
1 //=================================================================================================
20 //=================================================================================================
21 
22 #ifndef _BLAZE_MATH_SOLVERS_CPG_H_
23 #define _BLAZE_MATH_SOLVERS_CPG_H_
24 
25 
26 //*************************************************************************************************
27 // Includes
28 //*************************************************************************************************
29 
35 #include <blaze/math/shims/IsNaN.h>
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 CPG : public Solver
60 {
61  public:
62  //**Constructors********************************************************************************
65  explicit CPG();
67  //**********************************************************************************************
68 
69  //**Utility functions***************************************************************************
72  template< typename CP > bool solve( CP& cp );
74  //**********************************************************************************************
75 
76  private:
77  //**Member variables****************************************************************************
80  VecN r_;
81  VecN w_;
82  VecN p_;
84 
85  //**********************************************************************************************
86 };
87 //*************************************************************************************************
88 
89 
90 
91 
92 //=================================================================================================
93 //
94 // UTILITY FUNCTIONS
95 //
96 //=================================================================================================
97 
98 //*************************************************************************************************
106 template< typename CP > // Type of the complementarity problem
107 bool CPG::solve( CP& cp )
108 {
109  const size_t n( cp.size() );
110  const CMatMxN& A( cp.A_ );
111  const VecN& b( cp.b_ );
112 
113  bool converged( false );
114  VecN& x( cp.x_ );
115  size_t activeSetChanges( 0 );
116  real alpha( 0 ), alpha_nom( 0 ), alpha_denom( 1 );
117  real beta( 0 ), beta_nom( 0 ), beta_denom( 0 );
118  real tmp( 0 );
119 
120  BLAZE_INTERNAL_ASSERT( A.isSymmetric(), "The CPG solver requires that the system matrix is symmetric" );
121 
122  // Allocating helper data
123  r_.resize( n, false );
124  w_.resize( n, false );
125  p_.resize( n, false );
126  activity_.resize( n, false );
127 
128  // Determining activity and project initial solution to feasible region
129  for( size_t i=0; i<n; ++i ) {
130  if( x[i] <= cp.lbound( i ) ) {
131  x[i] = cp.lbound( i );
132  ++activeSetChanges;
133  activity_[i] = -1;
134  }
135  else if( x[i] >= cp.ubound( i ) ) {
136  x[i] = cp.ubound( i );
137  ++activeSetChanges;
138  activity_[i] = 1;
139  }
140  else {
141  activity_[i] = 0;
142  }
143  }
144 
145  // Computing the initial residual
146  lastPrecision_ = cp.residual();
147  if( lastPrecision_ < threshold_ )
148  converged = true;
149 
150  // Choosing the initial values such that the descent direction conjugation process is disabled
151  p_ = real(0);
152  w_ = real(0);
153 
154  size_t it( 0 );
155  for( ; !converged && it < maxIterations_; ++it )
156  {
157  // Computing the steepest descent direction
158  r_ = -( A*x + b );
159 
160  // Projecting the gradient and the previous descent direction
161  beta_nom = real(0);
162  beta_denom = alpha_denom;
163 
164  for( size_t i=0; i<n; ++i ) {
165  tmp = r_[i];
166  if( activity_[i] == -1 ) {
167  tmp = max( tmp, 0 );
168  p_[i] = max( p_[i], 0 );
169  }
170  else if( activity_[i] == 1 ) {
171  tmp = min( tmp, 0 );
172  p_[i] = min( p_[i], 0 );
173  }
174  beta_nom += w_[i] * tmp;
175  w_[i] = tmp;
176  }
177 
178  if( beta_denom == 0 ) {
179  // No conjugation can be performed, fallback to steepest descent
180  beta = 0;
181  }
182  else {
183  beta = -beta_nom / beta_denom;
184  }
185 
186  BLAZE_INTERNAL_ASSERT( !isnan( beta ), "Conjugation coefficient is nan" );
187 
188  // Choosing the next descent direction conjugated to all previous directions
189  p_ = w_ + beta * p_;
190 
191  // Finding the minimum along the descent direction p
192  alpha_nom = trans(r_) * p_;
193  alpha_denom = trans(p_) * A * p_;
194 
195  if( alpha_denom == 0 )
196  // In case p^T A p is zero, no reduction of the objective function can be obtained
197  // in direction p no matter which alpha we choose
198  alpha = 0;
199  else
200  alpha = alpha_nom / alpha_denom;
201 
202  if( alpha == 0 ) {
203  if( beta == 0 ) {
204  // p is the steepest descent direction since beta = 0 but we still cannot
205  // make any progress along p => minimum
206  break;
207  }
208  else {
209  // Retry with steepest descent direction
210  continue;
211  }
212  }
213 
214  // Descending along p and projecting
215  activeSetChanges = 0;
216 
217  for( size_t i=0; i<n; ++i ) {
218  if( activity_[i] != 0 && p_[i] == real(0) ) {
219  // In case the bounds are depending on the unknowns this ensures that
220  // the unknowns stay at the bounds
221  if( activity_[i] == -1 )
222  x[i] = cp.lbound( i );
223  else
224  x[i] = cp.ubound( i );
225  }
226  else {
227  x[i] += alpha * p_[i];
228 
229  if( x[i] <= cp.lbound( i ) ) {
230  x[i] = cp.lbound( i );
231  if( activity_[i] != -1 )
232  ++activeSetChanges;
233  activity_[i] = -1;
234  }
235  else if( x[i] >= cp.ubound( i ) ) {
236  x[i] = cp.ubound( i );
237  if( activity_[i] != +1 )
238  ++activeSetChanges;
239  activity_[i] = +1;
240  }
241  else {
242  if( activity_[i] != 0 )
243  ++activeSetChanges;
244  activity_[i] = 0;
245  }
246  }
247  }
248 
249  // Computing the residual (TODO we should improve this)
250  lastPrecision_ = cp.residual();
251  if( lastPrecision_ < threshold_ )
252  converged = true;
253  }
254 
255  BLAZE_LOG_DEBUG_SECTION( log ) {
256  if( converged )
257  log << " Solved the quadratic program in " << it << " CPG iterations.";
258  else
259  log << BLAZE_YELLOW << " WARNING: Did not solve the quadratic program within accuracy. (" << lastPrecision_ << ")" << BLAZE_OLDCOLOR;
260  }
261 
262  lastIterations_ = it;
263 
264  return converged;
265 }
266 //*************************************************************************************************
267 
268 
269 
270 
271 //=================================================================================================
272 //
273 // EXPLICIT TEMPLATE INSTANTIATIONS
274 //
275 //=================================================================================================
276 
277 //*************************************************************************************************
279 #if !defined(_MSC_VER)
280 extern template bool CPG::solve<LCP>( LCP& );
281 extern template bool CPG::solve<BoxLCP>( BoxLCP& );
282 extern template bool CPG::solve<ContactLCP>( ContactLCP& );
283 #endif
284 
285 //*************************************************************************************************
286 
287 } // namespace blaze
288 
289 #endif