5 #ifndef MC__ELLIPSOID_HPP
6 #define MC__ELLIPSOID_HPP
13 #include "mclapack.hpp"
17 #undef MC__ELLIPSOID_DEBUG_SQRT
18 #undef MC__ELLIPSOID_DEBUG_HPINTERSECTION
19 #undef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
20 #undef MC__ELLIPSOID_DEBUG
28 template <
typename T> Ellipsoid minksum_ea
29 (
const Ellipsoid&E,
const T*I,
const double&TOL,
const double EPS=machprec() );
40 friend std::ostream&
operator<<
43 (
const unsigned int n );
45 (
const Ellipsoid&E,
const CPPL::dgematrix&A,
const CPPL::dcovector&b );
47 (
const std::vector<Ellipsoid>&E,
const CPPL::dcovector&D );
48 friend std::vector<Ellipsoid> minksum_ea
49 (
const std::vector<Ellipsoid>&E,
const std::vector<CPPL::dcovector>&D );
51 (
const Ellipsoid&E,
const std::pair<CPPL::dcovector,CPPL::dcovector>&I,
52 const double&TOL,
const double EPS );
53 template <
typename T>
friend Ellipsoid minksum_ea
54 (
const Ellipsoid&E,
const T*I,
const double&TOL,
const double EPS );
57 friend std::vector<Ellipsoid> inv
58 (
const std::vector<Ellipsoid>&E );
60 (
const Ellipsoid&E,
const std::pair<CPPL::dcovector,double>&HP );
62 (
const Ellipsoid&E,
const std::pair<CPPL::dcovector,double>&HP );
64 (
const Ellipsoid&E,
const std::vector< std::pair<CPPL::dcovector,double> >&HP );
66 (
const Ellipsoid&E,
const std::pair<CPPL::dcovector,double>&HP,
69 (
const Ellipsoid&E,
const std::vector< std::pair<CPPL::dcovector,double> >&HP,
79 std::pair< CPPL::dcovector, CPPL::dgematrix > _eigQ;
81 CPPL::dsymatrix _sqrtQ;
83 std::pair< CPPL::dcovector, std::pair<CPPL::dgematrix,CPPL::dgematrix> > _svdQ;
85 CPPL::dsymatrix _invQ;
91 void _reset_auxiliary()
93 _eigQ.first.clear(); _eigQ.second.clear();
95 _svdQ.first.clear(); _svdQ.second.first.clear(); _svdQ.second.second.clear();
107 (
const CPPL::dcovector& d )
114 (
const CPPL::dcovector& d )
171 return "Not positive semi-definite shape matrix";
173 return "Failure in LAPACK components";
175 return "Failure in root-finding method";
186 _Q(), _c(), _PSDchecked(
false)
191 (
const CPPL::dsymatrix&
Q,
const CPPL::dcovector&
c=CPPL::dcovector() ):
192 _Q(Q), _c(
c), _PSDchecked(
false)
194 #ifdef MC__ELLIPSOID_DEBUG
195 assert( !
c.l || Q.n ==
c.l );
198 bool PSD; _isPSD( Q, _eigQ.first, PSD );
200 std::cout <<
"Min. eigenvalue: " << _eigQ.first(0) <<
" < 0 !" << std::endl;
205 if( _c.l != _Q.n ){ _c.resize(_Q.n); _c.zero(); }
210 (
const unsigned int n,
const double*Q,
const double*
c=0 ):
213 #ifdef MC__ELLIPSOID_DEBUG
217 for(
unsigned int j=0,k=0; j<
n; j++ )
218 for(
unsigned int i=j; i<
n; i++ )
221 for(
unsigned int i=0; i<
n; i++ )
222 _c(i) = (
c?
c[i]: 0. );
225 bool PSD; _isPSD( _Q, _eigQ.first, PSD );
227 std::cout <<
"Min. eigenvalue: " << _eigQ.first(0) <<
" < 0 !" << std::endl;
236 (
const CPPL::dcovector&
r,
const CPPL::dcovector&
c=CPPL::dcovector() ):
237 _Q(r.l), _c(
c), _PSDchecked(
false)
239 #ifdef MC__ELLIPSOID_DEBUG
240 assert( !
c.l || r.l ==
c.l );
243 for(
unsigned int i=0; i<r.l; i++ )
if(
r(i) < 0 ){
244 std::cout <<
"Interval radius: " <<
r(i) <<
" < 0 !" << std::endl;
250 double nrm2_r = CPPL::nrm2( r );
251 for(
unsigned int i=0; i<r.l; i++ ) _Q(i,i) =
r(i)*nrm2_r;
252 if( _c.l != _Q.n ){ _c.resize(_Q.n); _c.zero(); }
258 _Q(E._Q), _c(E._c), _PSDchecked(
false)
267 (
const CPPL::dsymatrix& Q=CPPL::dsymatrix(),
const CPPL::dcovector&
c=CPPL::dcovector() )
269 #ifdef MC__ELLIPSOID_DEBUG
270 assert( !
c.l || Q.n ==
c.l );
273 if( Q.n && options.
PSDCHK ){
274 bool PSD; _isPSD( Q, _eigQ.first, PSD );
276 std::cout <<
"Min. eigenvalue: " << _eigQ.first(0) <<
" < 0 !" << std::endl;
283 if( _c.l != _Q.n ){ _c.resize(_Q.n); _c.zero(); }
289 (
const unsigned int n,
const double*Q,
const double*
c=0 )
291 #ifdef MC__ELLIPSOID_DEBUG
295 for(
unsigned int j=0,k=0; j<
n; j++ )
296 for(
unsigned int i=j; i<
n; i++ )
299 for(
unsigned int i=0; i<
n; i++ )
300 _c(i) = (
c?
c[i]: 0. );
304 bool PSD; _isPSD( _Q, _eigQ.first, PSD );
306 std::cout <<
"Min. eigenvalue: " << _eigQ.first(0) <<
" < 0 !" << std::endl;
316 (
const CPPL::dcovector& r,
const CPPL::dcovector&
c=CPPL::dcovector() )
318 #ifdef MC__ELLIPSOID_DEBUG
319 assert( !
c.l || r.l ==
c.l );
323 for(
unsigned int i=0; i<r.l; i++ )
if(
r(i) < 0 ){
324 std::cout <<
"Interval radius: " <<
r(i) <<
" < 0 !" << std::endl;
331 double nrm2_r = nrm2( r );
332 for(
unsigned int i=0; i<r.l; i++ ) _Q(i,i) =
r(i)*nrm2_r;
334 if( _c.l != _Q.n ){ _c.resize(_Q.n); _c.zero(); }
340 (
const unsigned n,
const T*B )
342 #ifdef MC__ELLIPSOID_DEBUG
345 CPPL::dcovector
r( n ),
c( n );
346 for(
unsigned i=0; i<
n; i++ ){
364 (
const CPPL::drovector& Qi,
const double& ci=0. )
366 #ifdef MC__ELLIPSOID_DEBUG
367 assert( Qi.l == _Q.n+1 );
370 CPPL::dsymatrix Qext(_Q.n+1);
371 CPPL::dcovector cext(_Q.n+1);
372 for(
unsigned int i=0; i<_Q.n; i++ ){
374 for(
unsigned int j=0; j<=i; j++ )
378 for(
unsigned int i=0; i<=_Q.n; i++ )
379 Qext(_Q.n,i) = Qi(i);
391 unsigned int n()
const
396 const CPPL::dcovector&
c()
const
407 (
unsigned int i )
const
409 #ifdef MC__ELLIPSOID_DEBUG
415 const CPPL::dsymatrix&
Q()
const
421 (
unsigned int i,
unsigned int j )
const
423 #ifdef MC__ELLIPSOID_DEBUG
424 assert( i<_Q.n && j<_Q.n );
430 (
unsigned int i,
unsigned int j )
432 #ifdef MC__ELLIPSOID_DEBUG
433 assert( i<_Q.n && j<_Q.n );
441 if( !_Q.n )
return 0.;
443 for(
unsigned int i=1; i<_Q.n; i++ ) tr += _Q(i,i);
448 const std::pair< CPPL::dcovector, CPPL::dgematrix >&
eigQ()
450 if( !_eigQ.first.l || !_eigQ.second.n )
451 _eigen( _Q, _eigQ.first, _eigQ.second );
458 bool PSD; _isPSD( _Q, _eigQ.first, PSD );
463 const CPPL::dsymatrix&
sqrtQ
464 (
const bool complete=
false )
467 if( !_eigQ.first.l || !_eigQ.second.n )
468 _eigen( _Q, _eigQ.first, _eigQ.second );
469 _sqrt( _Q, _eigQ.first, _eigQ.second, _sqrtQ, _PSDchecked );
471 if( complete ) _sqrtQ.complete();
479 unsigned int rank = _Q.n;
480 for(
unsigned int i=0; i<_Q.n; i++, rank-- )
481 if( _svdQ.first(_Q.n-i-1) > options.
RKTOLA
482 && _svdQ.first(_Q.n-i-1) > options.
RKTOLR*_svdQ.first(0) )
break;
487 const std::pair< CPPL::dcovector, std::pair<CPPL::dgematrix,CPPL::dgematrix> >&
svdQ()
489 if( !_svdQ.first.l || !_svdQ.second.first.n || !_svdQ.second.second.n )
490 _svd( _Q, _svdQ.first, _svdQ.second.first, _svdQ.second.second );
497 const unsigned int r =
rankQ();
498 #ifdef MC__ELLIPSOID_DEBUG_REGQ
499 std::cout <<
"***Ellipsoid::regQ -- r = " << r << std::endl;
501 if( r == _Q.n )
return _Q;
503 CPPL::dgbmatrix E(_Q.n,_Q.n,0,0); E.zero();
504 const double eps = std::max( options.
RKTOLA, options.
RKTOLR*
svdQ().first(0) );
505 for(
unsigned int i=0; i<_Q.n-
r; i++ ) E(r+i,r+i) = eps;
506 CPPL::dgematrix UEUT =
svdQ().second.first * E * t(
svdQ().second.first);
507 #ifdef MC__ELLIPSOID_DEBUG_REGQ
508 std::cout <<
"***Ellipsoid::regQ -- UEUT = " << UEUT << std::endl;
510 for(
unsigned int i=r; i<_Q.n; i++ )
511 _svdQ.first(i) += eps;
512 for(
unsigned int i=0; i<_Q.n; i++ ){
513 for(
unsigned int j=0; j<=i; j++ )
514 _Q(i,j) += 0.5*(UEUT(i,j)+UEUT(j,i));
516 #ifdef MC__ELLIPSOID_DEBUG_REGQ
525 if( !_invQ.n ) _inv( _Q, _invQ );
530 CPPL::dgematrix
align
531 (
const CPPL::dcovector&v,
const CPPL::dcovector&x )
const
534 return CPPL::dgematrix();
536 CPPL::dgematrix vmat( v.l, 1 ), xmat( x.l, 1 );
537 for(
unsigned int i=0; i<v.l; i++ ){
538 vmat( i, 0 ) = v( i );
539 xmat( i, 0 ) = x( i );
541 CPPL::dcovector Sv, Sx;
542 CPPL::dgematrix Uv, Ux, VTv, VTx;
543 _svd( vmat, Sv, Uv, VTv );
544 _svd( xmat, Sx, Ux, VTx );
545 return (Uv * VTv(0,0)) * t( Ux * VTx(0,0) );
550 (
const unsigned int i )
const
552 #ifdef MC__ELLIPSOID_DEBUG
553 assert( i>=0 && i<_Q.n );
555 return _c(i) - std::sqrt(std::max(0.,_Q(i,i)));
559 (
const unsigned int i )
const
561 #ifdef MC__ELLIPSOID_DEBUG
562 assert( i>=0 && i<_Q.n );
564 return _c(i) + std::sqrt(std::max(0.,_Q(i,i)));
568 (
const unsigned int i )
const
570 #ifdef MC__ELLIPSOID_DEBUG
571 assert( i>=0 && i<_Q.n );
573 return std::sqrt(std::max(0.,_Q(i,i)));
581 (
const CPPL::dsymatrix&Q, CPPL::dcovector&D, CPPL::dgematrix&U );
585 (
const CPPL::dsymatrix&Q, CPPL::dcovector&D );
589 (
const CPPL::dsymatrix&Q, CPPL::dcovector&D,
bool&PSD );
593 (
const CPPL::dsymatrix&Q, CPPL::dcovector&D, CPPL::dgematrix&U,
594 CPPL::dsymatrix&R,
bool&PSDchecked );
598 (
const CPPL::dgematrix&Q, CPPL::dcovector&S, CPPL::dgematrix&U,
599 CPPL::dgematrix&VT );
603 (
const CPPL::dsymatrix&Q, CPPL::dcovector&S, CPPL::dgematrix&U,
604 CPPL::dgematrix&VT );
608 (
const CPPL::dsymatrix&Q, CPPL::dsymatrix&Qinv );
611 typedef double (puniv)
612 (
const double x,
const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
613 const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
const unsigned int n );
615 static double _secant
616 (
const double x0,
const double x1,
const double xL,
const double xU,
617 puniv f,
const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
618 const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
const unsigned int n );
620 static double _goldsect
621 (
const double xL,
const double xU, puniv f,
const CPPL::dcovector&c1,
622 const CPPL::dsymatrix&W1,
const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
623 const unsigned int n );
625 static double _goldsect_iter
626 (
const bool init,
const double a,
const double fa,
const double b,
627 const double fb,
const double c,
const double fc, puniv f,
628 const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
const CPPL::dcovector&c2,
629 const CPPL::dsymatrix&W2,
const unsigned int n );
631 static double _ell_fusionlambda
632 (
const double a,
const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
633 const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
const unsigned int n );
635 static double _ell_get_lambda
636 (
const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
637 const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
const bool isHP );
640 template <
typename M>
static double _trace
644 for(
unsigned int i=0; i<W.n; i++ ) trace += W(i,i);
650 (
const CPPL::dcovector&eigW )
653 for(
unsigned int i=0; i<eigW.l; i++ ) det *= eigW(i);
660 std::cout <<
"ENTER <1> TO CONTINUE" << std::endl;
666 Ellipsoid::Options Ellipsoid::options;
670 (
const CPPL::dsymatrix&Q, CPPL::dcovector&D )
676 CPPL::dsymatrix S =
Q;
677 std::vector<double> vD;
680 typename std::vector<double>::const_iterator Di = vD.begin();
681 for(
unsigned int i=0; Di!=vD.end(); ++Di, i++ ) D(i) = *Di;
687 (
const CPPL::dsymatrix&Q, CPPL::dcovector&D, CPPL::dgematrix&U )
690 D.clear(); U.clear();
693 CPPL::dsymatrix S =
Q;
694 std::vector<double> vD;
695 std::vector<CPPL::dcovector> vU;
698 typename std::vector<double>::const_iterator Di = vD.begin();
699 for(
unsigned int i=0; Di!=vD.end(); ++Di, i++ ) D(i) = *Di;
700 U.resize( Q.n, Q.m );
701 typename std::vector<CPPL::dcovector>::const_iterator Uj = vU.begin();
702 for(
unsigned int j=0; Uj!=vU.end(); ++Uj, j++ )
703 for(
unsigned int i=0; i<(*Uj).l; i++ ) U(i,j) = (*Uj)(i);
709 (
const CPPL::dsymatrix&Q, CPPL::dcovector&D,
bool&PSD )
720 if( D(0) < -std::fabs( options.
PSDTOL ) ){
721 #ifdef MC__ELLIPSOID_DEBUG
722 std::cout <<
"Min. eigenvalue: " << D(0) <<
" < 0 !" << std::endl;
727 for(
unsigned int i=0; i<D.l; i++ ){
728 if( D(i) < 0. ) D(i) = 0.;
736 (
const CPPL::dsymatrix&Q, CPPL::dcovector&D, CPPL::dgematrix&U,
737 CPPL::dsymatrix&sqrtQ,
bool&PSDchecked )
739 bool PSD; _isPSD( Q, D, PSD );
740 if( options.
PSDCHK && !PSDchecked && !PSD ){
744 if( !D.l || !U.n || !U.m ){ sqrtQ.clear();
return; }
746 CPPL::dgbmatrix sqrtD(D.l,D.l,0,0);
747 for(
unsigned int i=0; i<sqrtD.n; i++ )
748 sqrtD(i,i) = std::sqrt( D(i) );
749 CPPL::dgematrix sqrtQ_ge = U*sqrtD*t(U);
750 sqrtQ.resize( sqrtQ_ge.n );
751 for(
unsigned int i=0; i<sqrtQ_ge.n; i++ )
752 for(
unsigned int j=0; j<=i; j++ )
753 sqrtQ(i,j) = 0.5*(sqrtQ_ge(i,j)+sqrtQ_ge(j,i));
755 #ifdef MC__ELLIPSOID_DEBUG_SQRT
756 std::cout <<
"sqrtQ*sqrtQ:\n" << sqrtQ*
sqrtQ;
763 (
const CPPL::dgematrix&Q, CPPL::dcovector&S, CPPL::dgematrix&U,
767 S.clear(); U.clear(); VT.clear();
770 CPPL::dgematrix R( Q );
777 (
const CPPL::dsymatrix&Q, CPPL::dcovector&S, CPPL::dgematrix&U,
780 return _svd( Q.to_dgematrix(), S, U, VT );
785 (
const CPPL::dsymatrix&Q, CPPL::dsymatrix&Qinv )
794 #ifdef MC__ELLIPSOID_DEBUG_INV
795 std::cout <<
"\n***Ellispoid: Q\n" <<
Q;
796 std::cout <<
"***Ellispoid: inv(Q) (ill-conditioned)\n" << R;
798 CPPL::dgematrix invRQ_R = i( R * Q ) * R;
799 #ifdef MC__ELLIPSOID_DEBUG_INV
800 std::cout <<
"***Ellispoid: inv(R*Q)*R\n" << invRQ_R;
804 for(
unsigned int i=0; i<Q.n; i++ )
805 for(
unsigned int j=0; j<=i; j++ )
806 Qinv(i,j) = 0.5 * ( invRQ_R(i,j) + invRQ_R(j,i) );
807 #ifdef MC__ELLIPSOID_DEBUG_INV
808 std::cout <<
"***Ellispoid: inv(Q) (proper)\n" << invRQ_R;
814 inline Ellipsoid mtimes
815 (
const Ellipsoid&E,
const CPPL::dgematrix&A,
const CPPL::dcovector&b=CPPL::dcovector() )
817 #ifdef MC__ELLIPSOID_DEBUG
818 assert( E._Q.n == A.n );
821 if( !A.m )
return AE;
825 if( b.l == A.m ) AE._c += b;
828 CPPL::dgematrix AQAT = A * E._Q * t(A);
830 for(
unsigned int i=0; i<A.m; i++ )
831 for(
unsigned int j=0; j<=i; j++ )
832 AE._Q(i,j) = 0.5*(AQAT(i,j)+AQAT(j,i));
833 #ifdef MC__ELLIPSOID_DEBUG
834 bool PSD; AE._isPSD( AE._Q, AE._eigQ.first, PSD ); assert( PSD );
839 inline Ellipsoid minksum_ea
840 (
const std::vector<Ellipsoid>&E,
const CPPL::dcovector&D )
843 if( !E.size() )
return EA;
846 const unsigned int n = (*E.begin())._Q.
n;
847 typename std::vector<Ellipsoid>::const_iterator Ei = E.begin();
848 #ifdef MC__ELLIPSOID_DEBUG
849 for( ; Ei!=E.end(); ++Ei ) assert( (*Ei)._Q.n == n );
854 Ei = E.begin(); CPPL::dcovector c_EA( (*Ei)._c );
855 for( ++Ei; Ei!=E.end(); ++Ei ) c_EA += (*Ei)._c;
856 CPPL::dsymatrix Q_EA( n );
857 double sum_sqrt_lTQil = 0.;
858 for( Ei=E.begin(); Ei!=E.end(); ++Ei ){
859 double sqrt_lTQil = std::sqrt(D%((*Ei)._Q*D));
860 sum_sqrt_lTQil += sqrt_lTQil;
861 if( Ei==E.begin() ) Q_EA = (*Ei)._Q/sqrt_lTQil;
862 else Q_EA += (*Ei)._Q/sqrt_lTQil;
864 Q_EA *= sum_sqrt_lTQil;
870 inline std::vector<Ellipsoid> minksum_ea
871 (
const std::vector<Ellipsoid>&E,
const std::vector<CPPL::dcovector>&D )
873 std::vector<Ellipsoid> EA;
874 if( !E.size() || !D.size() )
return EA;
877 const unsigned int n = (*E.begin())._Q.
n;
878 typename std::vector<CPPL::dcovector>::const_iterator Di = D.begin();
879 typename std::vector<Ellipsoid>::const_iterator Ei = E.begin();
880 #ifdef MC__ELLIPSOID_DEBUG
881 for( ; Ei!=E.end(); ++Ei ) assert( (*Ei)._Q.n == n );
882 for( ; Di!=D.end(); ++Di ) assert( (*Di).l == n );
886 Ei = E.begin(); CPPL::dcovector c_EA( (*Ei)._c );
887 for( ++Ei; Ei!=E.end(); ++Ei ) c_EA += (*Ei)._c;
888 CPPL::dsymatrix Q_EA( n );
889 for( Di=D.begin(); Di!=D.end(); ++Di ){
890 double sum_sqrt_lTQil = 0.;
891 for( Ei=E.begin(); Ei!=E.end(); ++Ei ){
892 double sqrt_lTQil = std::sqrt((*Di)%((*Ei)._Q*(*Di)));
893 sum_sqrt_lTQil += sqrt_lTQil;
894 if( Ei==E.begin() ) Q_EA = (*Ei)._Q/sqrt_lTQil;
895 else Q_EA += (*Ei)._Q/sqrt_lTQil;
897 Q_EA *= sum_sqrt_lTQil;
898 EA.push_back( Ellipsoid(Q_EA,c_EA) );
904 template <
typename T>
905 inline Ellipsoid minksum_ea
906 (
const Ellipsoid&E,
const T*I,
const double&TOL,
const double EPS )
908 if( !E._Q.n )
return E;
910 CPPL::dcovector c(E._Q.n), r(E._Q.n);
911 for(
int i=0; i<E._Q.n; i++ ){
912 c(i) = Op<T>::mid( I[i] );
913 r(i) = 0.5 * Op<T>::diam( I[i] );
915 return minksum_ea( E, std::make_pair(r,c), TOL, EPS );
918 inline Ellipsoid minksum_ea
919 (
const Ellipsoid&E,
const std::pair<CPPL::dcovector,CPPL::dcovector>&I,
920 const double&TOL,
const double EPS=machprec() )
922 #ifdef MC__ELLIPSOID_DEBUG
923 assert( E._Q.n == I.first.l && ( !I.second.l || E._Q.n == I.second.l ) );
926 if( !E._Q.n )
return EA;
929 CPPL::dcovector c_EA( E._c );
930 if( I.second.l ) c_EA += I.second;
933 CPPL::dcovector sqrR(E._Q.n);
934 for(
int i=0; i<E._Q.n; i++ ){
935 trQ += E._Q(i,i)/(E._Q(i,i)+TOL);
936 sqrR(i) = I.first(i)/std::sqrt(E._Q(i,i)+TOL);
938 trQ = std::sqrt(trQ);
940 for(
int i=0; i<E._Q.n; i++ ) kappa += sqrR(i);
942 CPPL::dsymatrix Q_EA( E._Q*kappa/(trQ+EPS) );
943 for(
int i=0; i<E._Q.n; i++ )
944 Q_EA(i,i) += I.first(i)*I.first(i)*kappa/(sqrR(i)+sqr(EPS))+sqr(EPS);
946 return EA.set(Q_EA,c_EA);
950 (
const Ellipsoid&E )
953 if( !E._Q.n )
return Einv;
955 Einv._Q = Einv.invQ();
956 Einv._reset_auxiliary();
960 inline std::vector<Ellipsoid> inv
961 (
const std::vector<Ellipsoid>&E )
963 std::vector<Ellipsoid> Einv;
964 typename std::vector<Ellipsoid>::const_iterator Ei = E.begin();
965 for( ; Ei!=E.end(); ++Ei ) Einv.push_back( inv(*Ei) );
970 (
const Ellipsoid&E,
const std::pair<CPPL::dcovector,double>&HP )
972 if( E._Q.n != HP.first.l ){
973 std::cerr <<
"mc::dist: ellipsoid and hyperplane must be of the same dimension.\n";
977 return ( std::fabs( HP.second - E._c % HP.first )
978 - std::sqrt( HP.first % ( E._Q * HP.first ) ) )
979 / ( std::sqrt( HP.first % HP.first ) );
982 inline Ellipsoid intersection_ea
983 (
const Ellipsoid&E,
const std::pair<CPPL::dcovector,double>&HP,
984 const double&TOL=machprec() )
986 if( E._Q.n != HP.first.l ){
987 std::cerr <<
"mc::intersection_ea: ellipsoid and hyperplane must be of the same dimension.\n";
990 const unsigned int n = E._Q.n;
991 if( !n )
return Ellipsoid();
993 CPPL::dcovector v = -HP.first / std::sqrt( HP.first % HP.first );
994 double c = -HP.second / std::sqrt( HP.first % HP.first );
995 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
996 std::cout <<
"dist(E,HP): " << dist( E, HP ) << std::endl;
997 std::cout <<
"HP.vT E.c > 0 ? " << ( v % E._c > c?
'T':
'F' ) << std::endl;
999 if( dist( E, HP ) > 0 && v % E._c > c )
1001 if( dist( E, HP ) > 0 && v % E._c < c )
1003 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
1004 std::cout <<
"E:\n" << E << std::endl;
1005 std::cout <<
"HP:\n" << HP.first <<
"; " << HP.second << std::endl;
1009 CPPL::dsymatrix W2(n);
1010 CPPL::dgematrix vmat(n,1);
1011 for(
unsigned int i=0; i<n; i++ ) vmat(i,0) = v(i);
1012 CPPL::dgematrix vmatvmatT = vmat * t(vmat);
1013 for(
unsigned int i=0; i<n; i++ )
1014 for(
unsigned int j=0; j<=i; j++ )
1015 W2(i,j) = 0.5 * ( vmatvmatT(i,j) + vmatvmatT(j,i) )
1016 / ( 4.*EM.eigQ().first(n-1) );
1017 Ellipsoid Einter = hpintersection( EM, HP );
1018 double h = 2.*std::sqrt( EM.eigQ().first(n-1) );
1019 CPPL::dcovector c2 = Einter._c + h*v;
1020 Ellipsoid EM2( W2, c2 ); EM2.regQ(); W2 = EM2._Q;
1021 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
1022 std::cout <<
"W2:\n" << W2 << std::endl;
1023 std::cout <<
"c2:\n" << c2 << std::endl;
1026 const CPPL::dcovector& c1 = EM._c;
1027 EM.regQ();
const CPPL::dsymatrix& W1 = EM.invQ();
1028 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
1029 std::cout <<
"E (reg):\n" << EM << std::endl;
1030 std::cout <<
"rank(E._Q):\n" << EM.rankQ() << std::endl;
1031 std::cout <<
"inv(E._Q):\n" << EM.invQ() << std::endl;
1034 double a = Ellipsoid::_ell_get_lambda( c1, W1, c2, W2,
true );
1035 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
1036 std::cout <<
"a (ell_get_lambda):" << a << std::endl;
1038 CPPL::dsymatrix W = a*W1 + (1-a)*W2;
1039 CPPL::dsymatrix Winv( n ); Ellipsoid::_inv( W, Winv );
1040 double k = 1. - a*(1-a)*(c2-c1)%(W2*Winv*W1*(c2-c1));
1041 CPPL::dcovector q = Winv*(a*W1*c1 + (1-a)*W2*c2);
1042 CPPL::dsymatrix Q = (1+TOL)*k*Winv;
1043 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
1044 std::cout <<
"W:\n" << W << std::endl;
1045 std::cout <<
"Winv:\n" << Winv << std::endl;
1046 std::cout <<
"k:" << k << std::endl;
1047 std::cout <<
"q:" << q << std::endl;
1048 std::cout <<
"Q:\n" << Q << std::endl;
1050 return Ellipsoid( Q, q );
1054 Ellipsoid::_ell_get_lambda
1055 (
const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
1056 const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
const bool isHP )
1058 assert( c1.l == W1.n && c2.l == W2.n && c1.l == c2.l );
1059 const unsigned int n = c1.l;
1063 a= _goldsect( -.1, 1., _ell_fusionlambda, c1, W1, c2, W2, n );
1064 return( a<=0.? ( isHP? 1.: 0. ): a );
1068 a = _secant( .9, 1., -.1, 1., _ell_fusionlambda, c1, W1, c2, W2, n );
1070 catch( Ellipsoid::Exceptions ){
1071 a = _goldsect( -.1, 1., _ell_fusionlambda, c1, W1, c2, W2, n );
1073 return( a<=0.? ( isHP? 1.: 0. ): a );
1078 (
const double x0,
const double x1,
const double xL,
const double xU,
1079 puniv f,
const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
1080 const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
const unsigned int n )
1082 double xkm = std::max(xL,std::min(xU,x0));
1083 double fkm = f(xkm,c1,W1,c2,W2,n);
1084 double xk = std::max(xL,std::min(xU,x1));
1087 double fk = f(xk,c1,W1,c2,W2,n);
1088 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
1089 std::cout <<
"xk (fk):" << xk <<
" (" << fk <<
")" << std::endl;
1091 if( std::fabs(fk) < options.
ROOTTOL )
return xk;
1092 double Bk = (fk-fkm)/(xk-xkm);
1094 if( isequal(xk,xL) && fk/Bk>0 )
return xk;
1095 if( isequal(xk,xU) && fk/Bk<0 )
return xk;
1098 xk = std::max(xL,std::min(xU,xk-fk/Bk));
1105 Ellipsoid::_goldsect
1106 (
const double xL,
const double xU, puniv f,
const CPPL::dcovector&c1,
1107 const CPPL::dsymatrix&W1,
const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
1108 const unsigned int n )
1110 const double phi = 2.-(1.+std::sqrt(5.))/2.;
1111 const double fL = f(xL,c1,W1,c2,W2,n), fU = f(xU,c1,W1,c2,W2,n);
1112 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
1113 std::cout <<
"x (f):" << xL <<
" (" << fL <<
")" << std::endl;
1114 std::cout <<
"x (f):" << xU <<
" (" << fU <<
")" << std::endl;
1117 const double xm = xU-phi*(xU-xL), fm = f(xm,c1,W1,c2,W2,n);
1118 return _goldsect_iter(
true, xL, fL, xm, fm, xU, fU, f, c1, W1, c2, W2, n );
1122 Ellipsoid::_goldsect_iter
1123 (
const bool init,
const double a,
const double fa,
const double b,
1124 const double fb,
const double c,
const double fc, puniv f,
1125 const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
const CPPL::dcovector&c2,
1126 const CPPL::dsymatrix&W2,
const unsigned int n )
1130 static unsigned int iter;
1131 iter = ( init? 1: iter+1 );
1132 const double phi = 2.-(1.+std::sqrt(5.))/2.;
1133 bool b_then_x = ( c-b > b-a );
1134 double x = ( b_then_x? b+phi*(c-b): b-phi*(b-a) );
1137 double fx = f(x,c1,W1,c2,W2,n);
1138 #ifdef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
1139 std::cout <<
"x (f):" << x <<
" (" << fx <<
")" << std::endl;
1144 return( fa*fx<0? _goldsect_iter(
false, a, fa, b, fb, x, fx, f, c1, W1, c2, W2, n ):
1145 _goldsect_iter(
false, b, fb, x, fx, c, fc, f, c1, W1, c2, W2, n ) );
1146 return( fa*fb<0? _goldsect_iter(
false, a, fa, x, fx, b, fb, f, c1, W1, c2, W2, n ):
1147 _goldsect_iter(
false, x, fx, b, fb, c, fc, f, c1, W1, c2, W2, n ) );
1151 Ellipsoid::_ell_fusionlambda
1152 (
const double a,
const CPPL::dcovector&c1,
const CPPL::dsymatrix&W1,
1153 const CPPL::dcovector&c2,
const CPPL::dsymatrix&W2,
const unsigned int n )
1155 CPPL::dsymatrix W = a*W1 + (1-a)*W2;
1156 CPPL::dsymatrix Winv( n ); Ellipsoid::_inv( W, Winv );
1157 double k = 1. - a*(1-a)*(c2-c1)%(W2*Winv*W1*(c2-c1));
1158 CPPL::dcovector c = Winv*(a*W1*c1 + (1-a)*W2*c2);
1160 CPPL::dcovector eigW; _eigen( W, eigW );
1161 double detW = Ellipsoid::_det(eigW);
1162 double traW = Ellipsoid::_trace(detW*Winv*(W1-W2));
1163 return k*detW*traW - n*(detW*detW) * ( c%(2*W1*c1-2*W2*c2+(W2-W1)*c)
1164 - c1%(W1*c1) + c2%(W2*c2) );
1167 inline Ellipsoid hpintersection
1168 (
const Ellipsoid&E,
const std::pair<CPPL::dcovector,double>&HP )
1170 if( E._Q.n != HP.first.l ){
1171 std::cerr <<
"mc::intersection_ea: ellipsoid and hyperplane must be of the same dimension.\n";
1174 const unsigned int n = E._Q.n;
1175 if( !n )
return Ellipsoid();
1177 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1178 std::cout <<
"dist(E,HP): " << dist( E, HP ) << std::endl;
1180 if( dist( E, HP ) > 0 )
return Ellipsoid();
1181 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1182 std::cout <<
"E:\n" << E << std::endl;
1183 std::cout <<
"HP:\n" << HP.first <<
"; " << HP.second << std::endl;
1186 CPPL::dcovector e1( n ); e1.zero(); e1(0) = 1.;
1187 CPPL::dgematrix T = E.align( e1, HP.first );
1188 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1189 std::cout <<
"T:\n" << T << std::endl;
1190 std::cout <<
"T*d:\n" << T*HP.first << std::endl;
1192 CPPL::dcovector f = ( HP.second / ( HP.first % HP.first ) ) * T * HP.first;
1193 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1194 std::cout <<
"f: " << f << std::endl;
1196 Ellipsoid EM = mtimes( E, T, -f );
1197 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1198 std::cout <<
"EM:\n" << EM << std::endl;
1199 std::cout <<
"rank(EM._Q):\n" << EM.rankQ() << std::endl;
1200 std::cout <<
"svd(EM._Q):\n" << EM.svdQ().first << std::endl;
1203 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1204 std::cout <<
"EM (reg):\n" << EM << std::endl;
1205 std::cout <<
"rank(EM._Q):\n" << EM.rankQ() << std::endl;
1206 std::cout <<
"svd(EM._Q):\n" << EM.svdQ().first << std::endl;
1207 std::cout <<
"inv(EM._Q):\n" << EM.invQ() << std::endl;
1210 CPPL::dsymatrix W( n-1 );
1211 CPPL::dcovector w( n-1 );
1212 for(
unsigned int i=1; i<n; i++ ){
1213 for(
unsigned int j=i; j<n; j++ )
1214 W(i-1,j-1) = EM.invQ()(i,j);
1215 w(i-1) = EM.invQ()(i,0);
1217 const double w11 = EM.invQ()(0,0);
1218 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1219 std::cout <<
"W:\n" << W << std::endl;
1220 std::cout <<
"w:\n" << w << std::endl;
1221 std::cout <<
"w11:\n" << w11 << std::endl;
1223 CPPL::dsymatrix Winv( n-1 );
1224 Ellipsoid::_inv( W, Winv );
1225 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1226 std::cout <<
"Winv:\n" << Winv << std::endl;
1229 CPPL::dcovector Winv_w( Winv * w );
1230 const double h = EM.c(0)*EM.c(0) * ( w11 - w % Winv_w );
1231 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1232 std::cout <<
"h: " << h << std::endl;
1235 CPPL::dsymatrix Z( n ); Z.zero();
1236 CPPL::dcovector z( EM._c ); z(0) = 0.;
1237 for(
unsigned int i=1; i<n; i++ ){
1238 for(
unsigned int j=i; j<n; j++ )
1239 Z(i,j) = (1.-h) * Winv(i-1,j-1);
1240 z(i) += EM._c(0) * Winv_w(i-1);
1242 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1243 std::cout <<
"Z:\n" << Z << std::endl;
1244 std::cout <<
"z:\n" << z << std::endl;
1246 Ellipsoid I( Z, z );
1247 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1248 std::cout <<
"I: " << I;
1249 std::cout <<
"eig(I): " << I.eigQ().first;
1252 return mtimes( I, t(T) );
1255 inline Ellipsoid hpintersection
1256 (
const Ellipsoid&E,
const std::vector< std::pair<CPPL::dcovector,double> >&HP )
1259 typename std::vector< std::pair<CPPL::dcovector,double> >::const_iterator HPi = HP.begin();
1260 for( ; HPi!=HP.end(); ++HPi )
1261 Ei = hpintersection( Ei, *HPi );
1265 inline Ellipsoid ell_unitball
1266 (
const unsigned int n )
1269 Eunit._c.resize(n); Eunit._c.zero();
1270 Eunit._Q.resize(n); Eunit._Q.identity();
1274 inline std::ostream&
1276 ( std::ostream&out,
const Ellipsoid&E)
1278 if( !E.n() )
return out;
1279 const int iprec = 5;
1280 out << std::scientific << std::setprecision(iprec);
1281 out <<
"\ncenter:\n" << E._c;
1282 out <<
"shape:\n" << E._Q;
double r(const unsigned int i) const
Return maximum radius for for index .
Definition: ellipsoid.hpp:568
virtual ~Ellipsoid()
Destructor.
Definition: ellipsoid.hpp:262
const std::pair< CPPL::dcovector, std::pair< CPPL::dgematrix, CPPL::dgematrix > > & svdQ()
Return singular value decomposition of shape matrix.
Definition: ellipsoid.hpp:487
const CPPL::dsymatrix & Q() const
Return shape matrix of ellipsoid.
Definition: ellipsoid.hpp:415
Exceptions(TYPE ierr)
Constructor for error ierr
Definition: ellipsoid.hpp:164
bool PSDCHK
Whether or not to check positive semi-definiteness of shape matrices (default=false) ...
Definition: ellipsoid.hpp:138
const CPPL::dsymatrix & regQ()
Return pointer to regularized shape matrix.
Definition: ellipsoid.hpp:495
const std::pair< CPPL::dcovector, CPPL::dgematrix > & eigQ()
Return eigenvalues and eigenvectors of shape matrix.
Definition: ellipsoid.hpp:448
const CPPL::dcovector & c() const
Return center of ellipsoid.
Definition: ellipsoid.hpp:396
double l(const unsigned int i) const
Return lower bound for for index .
Definition: ellipsoid.hpp:550
int ierr()
Return error flag.
Definition: ellipsoid.hpp:166
Non positive-semi definite shape matrix.
Definition: ellipsoid.hpp:159
Ellipsoid options.
Definition: ellipsoid.hpp:130
bool ROOTSECANT
Whether to use the secant method for root finding (default=false)
Definition: ellipsoid.hpp:148
Ellipsoid exceptions.
Definition: ellipsoid.hpp:154
double trQ() const
Return trace of shape matrix.
Definition: ellipsoid.hpp:439
double PSDTOL
Absolute tolerance for positive semi-definiteness check of shape matrix (default=1e2*mc::machprec()) ...
Definition: ellipsoid.hpp:140
Options()
Constructor.
Definition: ellipsoid.hpp:133
const CPPL::dsymatrix & invQ()
Return pointer to inverse shape matrix.
Definition: ellipsoid.hpp:523
double RKTOLA
Absolute tolerance for rank and regularization of shape matrix (default=mc::machprec()) ...
Definition: ellipsoid.hpp:142
double u(const unsigned int i) const
Return upper bound for for index .
Definition: ellipsoid.hpp:559
Ellipsoid O() const
Recenter ellipsoid at the origin by canceling out the centre.
Definition: ellipsoid.hpp:386
TYPE
Enumeration type for Interval exception handling.
Definition: ellipsoid.hpp:158
C++ class for ellipsoidal calculus.
Definition: ellipsoid.hpp:37
double ROOTTOL
Absolute stopping tolerance for root-finding method (objective function value less than ROOTTOL; defa...
Definition: ellipsoid.hpp:146
double RKTOLR
Relative tolerance for rank and regularization of shape matrix (default=1e6*mc::machprec()) ...
Definition: ellipsoid.hpp:144
Root-finding routine failed.
Definition: ellipsoid.hpp:161
Ellipsoid & set(const CPPL::dsymatrix &Q=CPPL::dsymatrix(), const CPPL::dcovector &c=CPPL::dcovector())
Define ellipsoid of dimension with center and shape matrix .
Definition: ellipsoid.hpp:267
CPPL::dcovector & c()
Return center of ellipsoid.
Definition: ellipsoid.hpp:401
std::string what()
Return error description.
Definition: ellipsoid.hpp:168
bool psdQ()
Return square root of shape matrix.
Definition: ellipsoid.hpp:456
unsigned int rankQ()
Return rank of shape matrix.
Definition: ellipsoid.hpp:476
Ellipsoid()
Default constructor (needed to declare arrays of Ellipsoid class)
Definition: ellipsoid.hpp:185
CPPL::dgematrix align(const CPPL::dcovector &v, const CPPL::dcovector &x) const
Computes an orthogonal matrix rotating the vector x so that it is parallel to the vector v ...
Definition: ellipsoid.hpp:531
unsigned int n() const
Return dimension of ellipsoid.
Definition: ellipsoid.hpp:391
unsigned int ROOTMAXIT
Maximum number of iteration for root-finding method (default=0 - no maximum)
Definition: ellipsoid.hpp:150
const CPPL::dsymatrix & sqrtQ(const bool complete=false)
Return square root of shape matrix.
Definition: ellipsoid.hpp:464
Ellipsoid & extend(const CPPL::drovector &Qi, const double &ci=0.)
Extend dimension by one, by appending row Qi to shape matrix and entry ci to center.
Definition: ellipsoid.hpp:364
Ellipsoid & reset()
Reset ellipsoid.
Definition: ellipsoid.hpp:354
Linear algebra routine in LAPACK failed.
Definition: ellipsoid.hpp:160
C++ structure to allow usage of various template parameters in the types mc::McCormick, mc::TModel, mc::TVar, and mc::SpecBnd of MC++ _ Specialization of this structure is required for the template parameters can be found in the header files defining the types mc::McCormick, mc::TModel, mc::TVar, and mc::SpecBnd.
Definition: mcop.hpp:13