void
SolverUnconstrained<Data,Problem>::lambda_LS( vector_type & _x, vector_type & _lambda_l, vector_type & _lambda_u )
{
//symmetric_matrix_type __A ( _E_nL, _E_nL );
matrix_type __A ( _E_nL, _E_nL );
vector_type __b ( _E_nL );
// At = [ -I (l-x) 0; I 0 (x-u) ] --> AtA = [ I+(l-x)^2 -I; -I I+(x-u)^2 ]
__A = zero_matrix<double>( _E_nL, _E_nL );
for ( int __i = 0; __i < _E_n; ++__i )
{
__A( __i, __i ) = 1.0 + ( M_prob.x_l( __i )-_x ( __i ) )*( M_prob.x_l( __i )-_x ( __i ) );
__A( __i, _E_n+__i ) = -1.0;
__A( _E_n+__i,__i ) = -1.0;
__A( ( _E_n+__i ), _E_n+__i ) = 1.0+( _x ( __i )-M_prob.x_u( __i ) )*( _x ( __i )-M_prob.x_u( __i ) );
}
// b = [ \nabla f; 0; 0 ] --> Atb = [ -\nabla f; \nabla f ]
f_type __f_x;
M_prob.evaluate( _x, __f_x, diff_order<1>() );
for ( int i = 0; i < _E_n; i++ )
{
value_type val = __f_x.gradient( 0, i );
__b ( i ) = val;
__b ( _E_n+i ) = -val;
}
// after solve __b = [ _lambda_l; _lambda_] = A\b
//char __uplo = 'U';
int __info = 0;
int __nrhs = 1;
int __N = _E_nL;
//dppsv_ ( &__uplo, &__N, &__nrhs, __A.data(), __b.data(), &__ldb, &__info );
//dppsv_ ( &__uplo, &__N, &__nrhs, __A.data().begin(), __b.data().begin(), &__N, &__info );
ublas::vector<int> __itype( __N );
//dspsv_( &__uplo, &__N, &__nrhs, __A.data().begin(), __itype.data().begin(), __b.data().begin(), &__N, &__info );
dgesv_( &__N,&__nrhs,__A.data().begin(),&__N,__itype.data().begin(),__b.data().begin(),&__N,&__info );
if ( __info!= 0 )
{
std::ostringstream __err;
__err << "[" << __PRETTY_FUNCTION__ << "] dppsv failed: " << __info << "\n"
<< "A = " << __A << "\n"
<< "B = " << __b << "\n";
throw std::out_of_range( __err.str() );
}
for ( int i = 0; i < _E_n; i++ )
{
_lambda_l ( i ) = __b ( i );
_lambda_u ( i ) = __b ( _E_n+i );
}
}
void Line::Draw (CDC *pDC, Camera *camera, unsigned int color)
{
Matrix a = Matrix::HomogeneousCoordinate3D (A.x, A.y, A.z) * transform;
Matrix b = Matrix::HomogeneousCoordinate3D (B.x, B.y, B.z) * transform;
if(camera->CullLine (&a, &b))
{
Matrix mat = camera->Get ();
Matrix aa = a * mat;
Matrix bb = b * mat;
Point3D aaa = camera->World2Screen (aa);
Point3D bbb = camera->World2Screen (bb);
Matrix __a = a*camera->Get2();
Matrix __b = b*camera->Get2();
aaa.z = __a(1,3)/__a(1,4);
bbb.z = __b(1,3)/__b(1,4);
DrawLine(pDC, aaa, bbb, color, flag);
}
}
