22 #ifndef _BLAZE_MATH_SOLVERS_CPG_H_
23 #define _BLAZE_MATH_SOLVERS_CPG_H_
72 template<
typename CP >
bool solve( CP& cp );
106 template<
typename CP >
109 const size_t n( cp.size() );
111 const VecN& b( cp.b_ );
113 bool converged(
false );
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 );
120 BLAZE_INTERNAL_ASSERT( A.isSymmetric(),
"The CPG solver requires that the system matrix is symmetric" );
129 for(
size_t i=0; i<n; ++i ) {
130 if( x[i] <= cp.lbound( i ) ) {
131 x[i] = cp.lbound( i );
135 else if( x[i] >= cp.ubound( i ) ) {
136 x[i] = cp.ubound( i );
162 beta_denom = alpha_denom;
164 for(
size_t i=0; i<n; ++i ) {
174 beta_nom +=
w_[i] * tmp;
178 if( beta_denom == 0 ) {
183 beta = -beta_nom / beta_denom;
193 alpha_denom =
trans(p_) * A *
p_;
195 if( alpha_denom == 0 )
200 alpha = alpha_nom / alpha_denom;
215 activeSetChanges = 0;
217 for(
size_t i=0; i<n; ++i ) {
222 x[i] = cp.lbound( i );
224 x[i] = cp.ubound( i );
227 x[i] += alpha * p_[i];
229 if( x[i] <= cp.lbound( i ) ) {
230 x[i] = cp.lbound( i );
235 else if( x[i] >= cp.ubound( i ) ) {
236 x[i] = cp.ubound( i );
257 log <<
" Solved the quadratic program in " << it <<
" CPG iterations.";
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& );