MC++
ellipsoid.hpp
1 // Copyright (C) 2012, 2013 Benoit Chachuat, Imperial College London.
2 // All Rights Reserved.
3 // This code is published under the Eclipse Public License.
4 
5 #ifndef MC__ELLIPSOID_HPP
6 #define MC__ELLIPSOID_HPP
7 
8 #include <iostream>
9 #include <iomanip>
10 #include <stdarg.h>
11 #include <cassert>
12 
13 #include "mclapack.hpp"
14 #include "mcfunc.hpp"
15 #include "mcop.hpp"
16 
17 #undef MC__ELLIPSOID_DEBUG_SQRT
18 #undef MC__ELLIPSOID_DEBUG_HPINTERSECTION
19 #undef MC__ELLIPSOID_DEBUG_INTERSECTION_EA
20 #undef MC__ELLIPSOID_DEBUG
21 
22 
23 namespace mc
24 {
25 
26 class Ellipsoid;
27 
28 template <typename T> Ellipsoid minksum_ea
29  ( const Ellipsoid&E, const T*I, const double&TOL, const double EPS=machprec() );
30 
37 class Ellipsoid
39 {
40  friend std::ostream& operator<<
41  ( std::ostream&, const Ellipsoid& );
42  friend Ellipsoid ell_unitball
43  ( const unsigned int n );
44  friend Ellipsoid mtimes
45  ( const Ellipsoid&E, const CPPL::dgematrix&A, const CPPL::dcovector&b );
46  friend Ellipsoid minksum_ea
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 );
50  friend Ellipsoid minksum_ea
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 );
55  friend Ellipsoid inv
56  ( const Ellipsoid&E );
57  friend std::vector<Ellipsoid> inv
58  ( const std::vector<Ellipsoid>&E );
59  friend double dist
60  ( const Ellipsoid&E, const std::pair<CPPL::dcovector,double>&HP );
61  friend Ellipsoid hpintersection
62  ( const Ellipsoid&E, const std::pair<CPPL::dcovector,double>&HP );
63  friend Ellipsoid hpintersection
64  ( const Ellipsoid&E, const std::vector< std::pair<CPPL::dcovector,double> >&HP );
65  friend Ellipsoid intersection_ea
66  ( const Ellipsoid&E, const std::pair<CPPL::dcovector,double>&HP,
67  const double&TOL );
68  friend Ellipsoid intersection_ea
69  ( const Ellipsoid&E, const std::vector< std::pair<CPPL::dcovector,double> >&HP,
70  const double&TOL );
71 
72 private:
73 
75  CPPL::dsymatrix _Q;
77  CPPL::dcovector _c;
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;
86 
88  bool _PSDchecked;
89 
91  void _reset_auxiliary()
92  {
93  _eigQ.first.clear(); _eigQ.second.clear();
94  _sqrtQ.clear();
95  _svdQ.first.clear(); _svdQ.second.first.clear(); _svdQ.second.second.clear();
96  _invQ.clear();
97  _PSDchecked = false;
98  }
99 
100 public:
101 
105  Ellipsoid& operator+=
107  ( const CPPL::dcovector& d )
108  {
109  _c += d;
110  return *this;
111  }
113  Ellipsoid& operator-=
114  ( const CPPL::dcovector& d )
115  {
116  _c -= d;
117  return *this;
118  }
120  Ellipsoid& operator=
121  ( const Ellipsoid&E )
122  {
123  _Q = E._Q;
124  _c = E._c;
125  _reset_auxiliary();
126  return *this;
127  }
128 
130  static struct Options
131  {
134  PSDCHK(true), PSDTOL(machprec()*1e2), RKTOLA(machprec()), RKTOLR(1e6*mc::machprec()),
135  ROOTTOL(1e-10), ROOTSECANT(false), ROOTMAXIT(0)
136  {}
138  bool PSDCHK;
140  double PSDTOL;
142  double RKTOLA;
144  double RKTOLR;
146  double ROOTTOL;
150  unsigned int ROOTMAXIT;
151  } options;
152 
155  {
156  public:
158  enum TYPE{
159  NONPSD=1,
162  };
164  Exceptions( TYPE ierr ) : _ierr( ierr ){}
166  int ierr(){ return _ierr; }
168  std::string what(){
169  switch( _ierr ){
170  case NONPSD:
171  return "Not positive semi-definite shape matrix";
172  case LAPACK:
173  return "Failure in LAPACK components";
174  case ROOT: default:
175  return "Failure in root-finding method";
176  }
177  }
178 
179  private:
180  TYPE _ierr;
181  };
182 
184  Ellipsoid
185  ():
186  _Q(), _c(), _PSDchecked(false)
187  {}
188 
190  Ellipsoid
191  ( const CPPL::dsymatrix& Q, const CPPL::dcovector& c=CPPL::dcovector() ):
192  _Q(Q), _c(c), _PSDchecked(false)
193  {
194 #ifdef MC__ELLIPSOID_DEBUG
195  assert( !c.l || Q.n == c.l );
196 #endif
197  if( options.PSDCHK ){
198  bool PSD; _isPSD( Q, _eigQ.first, PSD );
199  if( !PSD ){
200  std::cout << "Min. eigenvalue: " << _eigQ.first(0) << " < 0 !" << std::endl;
202  }
203  _PSDchecked = true;
204  }
205  if( _c.l != _Q.n ){ _c.resize(_Q.n); _c.zero(); }
206  }
207 
209  Ellipsoid
210  ( const unsigned int n, const double*Q, const double*c=0 ):
211  _PSDchecked(false)
212  {
213 #ifdef MC__ELLIPSOID_DEBUG
214  assert( n && Q );
215 #endif
216  _Q.resize(n);
217  for( unsigned int j=0,k=0; j<n; j++ )
218  for( unsigned int i=j; i<n; i++ )
219  _Q(i,j) = Q[k++];
220  _c.resize(_Q.n);
221  for( unsigned int i=0; i<n; i++ )
222  _c(i) = ( c? c[i]: 0. );
223 
224  if( options.PSDCHK ){
225  bool PSD; _isPSD( _Q, _eigQ.first, PSD );
226  if( !PSD ){
227  std::cout << "Min. eigenvalue: " << _eigQ.first(0) << " < 0 !" << std::endl;
229  }
230  _PSDchecked = true;
231  }
232  }
233 
235  Ellipsoid
236  ( const CPPL::dcovector& r, const CPPL::dcovector& c=CPPL::dcovector() ):
237  _Q(r.l), _c(c), _PSDchecked(false)
238  {
239 #ifdef MC__ELLIPSOID_DEBUG
240  assert( !c.l || r.l == c.l );
241 #endif
242  if( options.PSDCHK ){
243  for( unsigned int i=0; i<r.l; i++ ) if( r(i) < 0 ){
244  std::cout << "Interval radius: " << r(i) << " < 0 !" << std::endl;
246  }
247  _PSDchecked = true;
248  }
249  _Q.zero();
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(); }
253  }
254 
256  Ellipsoid
257  ( const Ellipsoid&E ):
258  _Q(E._Q), _c(E._c), _PSDchecked(false)
259  {}
260 
262  virtual ~Ellipsoid()
263  {}
264 
266  Ellipsoid& set
267  ( const CPPL::dsymatrix& Q=CPPL::dsymatrix(), const CPPL::dcovector& c=CPPL::dcovector() )
268  {
269 #ifdef MC__ELLIPSOID_DEBUG
270  assert( !c.l || Q.n == c.l );
271 #endif
272  _reset_auxiliary();
273  if( Q.n && options.PSDCHK ){
274  bool PSD; _isPSD( Q, _eigQ.first, PSD );
275  if( !PSD ){
276  std::cout << "Min. eigenvalue: " << _eigQ.first(0) << " < 0 !" << std::endl;
278  }
279  _PSDchecked = true;
280  }
281  _Q = Q;
282  _c = c;
283  if( _c.l != _Q.n ){ _c.resize(_Q.n); _c.zero(); }
284  return *this;
285  }
286 
288  Ellipsoid& set
289  ( const unsigned int n, const double*Q, const double*c=0 )
290  {
291 #ifdef MC__ELLIPSOID_DEBUG
292  assert( n && Q );
293 #endif
294  _Q.resize(n);
295  for( unsigned int j=0,k=0; j<n; j++ )
296  for( unsigned int i=j; i<n; i++ )
297  _Q(i,j) = Q[k++];
298  _c.resize(_Q.n);
299  for( unsigned int i=0; i<n; i++ )
300  _c(i) = ( c? c[i]: 0. );
301 
302  _reset_auxiliary();
303  if( options.PSDCHK ){
304  bool PSD; _isPSD( _Q, _eigQ.first, PSD );
305  if( !PSD ){
306  std::cout << "Min. eigenvalue: " << _eigQ.first(0) << " < 0 !" << std::endl;
308  }
309  _PSDchecked = true;
310  }
311  return *this;
312  }
313 
315  Ellipsoid& set
316  ( const CPPL::dcovector& r, const CPPL::dcovector& c=CPPL::dcovector() )
317  {
318 #ifdef MC__ELLIPSOID_DEBUG
319  assert( !c.l || r.l == c.l );
320 #endif
321  _reset_auxiliary();
322  if( options.PSDCHK ){
323  for( unsigned int i=0; i<r.l; i++ ) if( r(i) < 0 ){
324  std::cout << "Interval radius: " << r(i) << " < 0 !" << std::endl;
326  }
327  _PSDchecked = true;
328  }
329  _Q.resize( r.l );
330  _Q.zero();
331  double nrm2_r = nrm2( r );
332  for( unsigned int i=0; i<r.l; i++ ) _Q(i,i) = r(i)*nrm2_r;
333  _c = c;
334  if( _c.l != _Q.n ){ _c.resize(_Q.n); _c.zero(); }
335  return *this;
336  }
337 
339  template <typename T> Ellipsoid& set
340  ( const unsigned n, const T*B )
341  {
342 #ifdef MC__ELLIPSOID_DEBUG
343  assert( !n || B );
344 #endif
345  CPPL::dcovector r( n ), c( n );
346  for( unsigned i=0; i<n; i++ ){
347  r(i) = 0.5*Op<T>::diam(B[i]);
348  c(i) = Op<T>::mid(B[i]);
349  }
350  return set( r, c );
351  }
352 
355  {
356  _Q.clear();
357  _c.clear();
358  _reset_auxiliary();
359  return *this;
360  }
361 
364  ( const CPPL::drovector& Qi, const double& ci=0. )
365  {
366 #ifdef MC__ELLIPSOID_DEBUG
367  assert( Qi.l == _Q.n+1 );
368 #endif
369  _reset_auxiliary();
370  CPPL::dsymatrix Qext(_Q.n+1);
371  CPPL::dcovector cext(_Q.n+1);
372  for( unsigned int i=0; i<_Q.n; i++ ){
373  cext(i) = _c(i);
374  for( unsigned int j=0; j<=i; j++ )
375  Qext(i,j) = _Q(i,j);
376  }
377  cext(_Q.n) = ci;
378  for( unsigned int i=0; i<=_Q.n; i++ )
379  Qext(_Q.n,i) = Qi(i);
380  _c = cext;
381  _Q = Qext;
382  return *this;
383  }
384 
386  Ellipsoid O() const
387  {
388  return Ellipsoid(_Q);
389  }
391  unsigned int n() const
392  {
393  return _Q.n;
394  }
396  const CPPL::dcovector& c() const
397  {
398  return _c;
399  }
401  CPPL::dcovector& c()
402  {
403  return _c;
404  }
406  double c
407  ( unsigned int i ) const
408  {
409 #ifdef MC__ELLIPSOID_DEBUG
410  assert( i<_Q.n );
411 #endif
412  return _c(i);
413  }
415  const CPPL::dsymatrix& Q() const
416  {
417  return _Q;
418  }
420  double Q
421  ( unsigned int i, unsigned int j ) const
422  {
423 #ifdef MC__ELLIPSOID_DEBUG
424  assert( i<_Q.n && j<_Q.n );
425 #endif
426  return _Q(i,j);
427  }
429  double& Q
430  ( unsigned int i, unsigned int j )
431  {
432 #ifdef MC__ELLIPSOID_DEBUG
433  assert( i<_Q.n && j<_Q.n );
434 #endif
435  return _Q(i,j);
436  }
437 
439  double trQ() const
440  {
441  if( !_Q.n ) return 0.;
442  double tr(_Q(0,0));
443  for( unsigned int i=1; i<_Q.n; i++ ) tr += _Q(i,i);
444  return tr;
445  }
446 
448  const std::pair< CPPL::dcovector, CPPL::dgematrix >& eigQ()
449  {
450  if( !_eigQ.first.l || !_eigQ.second.n )
451  _eigen( _Q, _eigQ.first, _eigQ.second );
452  return _eigQ;
453  }
454 
456  bool psdQ()
457  {
458  bool PSD; _isPSD( _Q, _eigQ.first, PSD );
459  return PSD;
460  }
461 
463  const CPPL::dsymatrix& sqrtQ
464  ( const bool complete=false )
465  {
466  if( !_sqrtQ.n ){
467  if( !_eigQ.first.l || !_eigQ.second.n )
468  _eigen( _Q, _eigQ.first, _eigQ.second );
469  _sqrt( _Q, _eigQ.first, _eigQ.second, _sqrtQ, _PSDchecked );
470  }
471  if( complete ) _sqrtQ.complete();
472  return _sqrtQ;
473  }
474 
476  unsigned int rankQ()
477  {
478  svdQ();
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;
483  return rank;
484  }
485 
487  const std::pair< CPPL::dcovector, std::pair<CPPL::dgematrix,CPPL::dgematrix> >& svdQ()
488  {
489  if( !_svdQ.first.l || !_svdQ.second.first.n || !_svdQ.second.second.n )
490  _svd( _Q, _svdQ.first, _svdQ.second.first, _svdQ.second.second );
491  return _svdQ;
492  }
493 
495  const CPPL::dsymatrix& regQ()
496  {
497  const unsigned int r = rankQ();
498 #ifdef MC__ELLIPSOID_DEBUG_REGQ
499  std::cout << "***Ellipsoid::regQ -- r = " << r << std::endl;
500 #endif
501  if( r == _Q.n ) return _Q;
502 
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;
509 #endif
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));
515  }
516 #ifdef MC__ELLIPSOID_DEBUG_REGQ
517  _reset_auxiliary();
518 #endif
519  return _Q;
520  }
521 
523  const CPPL::dsymatrix& invQ()
524  {
525  if( !_invQ.n ) _inv( _Q, _invQ );
526  return _invQ;
527  }
528 
530  CPPL::dgematrix align
531  ( const CPPL::dcovector&v, const CPPL::dcovector&x ) const
532  {
533  if( v.l != x.l )
534  return CPPL::dgematrix();
535 
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 );
540  }
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) );
546  }
547 
549  double l
550  ( const unsigned int i ) const
551  {
552 #ifdef MC__ELLIPSOID_DEBUG
553  assert( i>=0 && i<_Q.n );
554 #endif
555  return _c(i) - std::sqrt(std::max(0.,_Q(i,i)));
556  }
558  double u
559  ( const unsigned int i ) const
560  {
561 #ifdef MC__ELLIPSOID_DEBUG
562  assert( i>=0 && i<_Q.n );
563 #endif
564  return _c(i) + std::sqrt(std::max(0.,_Q(i,i)));
565  }
567  double r
568  ( const unsigned int i ) const
569  {
570 #ifdef MC__ELLIPSOID_DEBUG
571  assert( i>=0 && i<_Q.n );
572 #endif
573  return std::sqrt(std::max(0.,_Q(i,i)));
574  }
577 private:
578 
580  static void _eigen
581  ( const CPPL::dsymatrix&Q, CPPL::dcovector&D, CPPL::dgematrix&U );
582 
584  static void _eigen
585  ( const CPPL::dsymatrix&Q, CPPL::dcovector&D );
586 
588  static void _isPSD
589  ( const CPPL::dsymatrix&Q, CPPL::dcovector&D, bool&PSD );
590 
592  static void _sqrt
593  ( const CPPL::dsymatrix&Q, CPPL::dcovector&D, CPPL::dgematrix&U,
594  CPPL::dsymatrix&R, bool&PSDchecked );
595 
597  static void _svd
598  ( const CPPL::dgematrix&Q, CPPL::dcovector&S, CPPL::dgematrix&U,
599  CPPL::dgematrix&VT );
600 
602  static void _svd
603  ( const CPPL::dsymatrix&Q, CPPL::dcovector&S, CPPL::dgematrix&U,
604  CPPL::dgematrix&VT );
605 
607  static void _inv
608  ( const CPPL::dsymatrix&Q, CPPL::dsymatrix&Qinv );
609 
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 );
638 
640  template <typename M> static double _trace
641  ( const M&W )
642  {
643  double trace = 0.;
644  for( unsigned int i=0; i<W.n; i++ ) trace += W(i,i);
645  return trace;
646  }
647 
649  static double _det
650  ( const CPPL::dcovector&eigW )
651  {
652  double det = 1.;
653  for( unsigned int i=0; i<eigW.l; i++ ) det *= eigW(i);
654  return det;
655  }
656 
658  static void _pause()
659  { double tmp;
660  std::cout << "ENTER <1> TO CONTINUE" << std::endl;
661  std::cin >> tmp; }
662 };
663 
665 
666 Ellipsoid::Options Ellipsoid::options;
667 
668 inline void
669 Ellipsoid::_eigen
670 ( const CPPL::dsymatrix&Q, CPPL::dcovector&D )
671 {
672  if( !Q.n ){
673  D.clear();
674  return;
675  }
676  CPPL::dsymatrix S = Q;
677  std::vector<double> vD;
678  if( S.dsyev( vD ) ) throw Exceptions( Exceptions::LAPACK );
679  D.resize( Q.n );
680  typename std::vector<double>::const_iterator Di = vD.begin();
681  for( unsigned int i=0; Di!=vD.end(); ++Di, i++ ) D(i) = *Di;
682  return;
683 }
684 
685 inline void
686 Ellipsoid::_eigen
687 ( const CPPL::dsymatrix&Q, CPPL::dcovector&D, CPPL::dgematrix&U )
688 {
689  if( !Q.n ){
690  D.clear(); U.clear();
691  return;
692  }
693  CPPL::dsymatrix S = Q;
694  std::vector<double> vD;
695  std::vector<CPPL::dcovector> vU;
696  if( S.dsyev( vD, vU ) ) throw Exceptions( Exceptions::LAPACK );
697  D.resize( Q.n );
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);
704  return;
705 }
706 
707 inline void
708 Ellipsoid::_isPSD
709 ( const CPPL::dsymatrix&Q, CPPL::dcovector&D, bool&PSD )
710 {
711  if( !Q.n ){
712  PSD = false;
713  D.clear();
714  return;
715  }
716  D.resize( Q.n );
717  _eigen( Q, D );
718 
719  PSD = true;
720  if( D(0) < -std::fabs( options.PSDTOL ) ){
721 #ifdef MC__ELLIPSOID_DEBUG
722  std::cout << "Min. eigenvalue: " << D(0) << " < 0 !" << std::endl;
723  _pause();
724 #endif
725  PSD = false;
726  }
727  for( unsigned int i=0; i<D.l; i++ ){
728  if( D(i) < 0. ) D(i) = 0.;
729  else break; // b/c eigenvalues are returned in increasing order
730  }
731  return;
732 }
733 
734 inline void
735 Ellipsoid::_sqrt
736 ( const CPPL::dsymatrix&Q, CPPL::dcovector&D, CPPL::dgematrix&U,
737  CPPL::dsymatrix&sqrtQ, bool&PSDchecked )
738 {
739  bool PSD; _isPSD( Q, D, PSD );
740  if( options.PSDCHK && !PSDchecked && !PSD ){
741  PSDchecked = true;
742  throw Exceptions( Exceptions::NONPSD );
743  }
744  if( !D.l || !U.n || !U.m ){ sqrtQ.clear(); return; }
745 
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));
754 
755 #ifdef MC__ELLIPSOID_DEBUG_SQRT
756  std::cout << "sqrtQ*sqrtQ:\n" << sqrtQ*sqrtQ;
757 #endif
758  return;
759 }
760 
761 inline void
762 Ellipsoid::_svd
763 ( const CPPL::dgematrix&Q, CPPL::dcovector&S, CPPL::dgematrix&U,
764  CPPL::dgematrix&VT )
765 {
766  if( !Q.n || !Q.m ){
767  S.clear(); U.clear(); VT.clear();
768  return;
769  }
770  CPPL::dgematrix R( Q ); // local copy; otherwise Q entries are overwritten...
771  if( R.dgesvd( S, U, VT ) ) throw Exceptions( Exceptions::LAPACK );
772  return;
773 }
774 
775 inline void
776 Ellipsoid::_svd
777 ( const CPPL::dsymatrix&Q, CPPL::dcovector&S, CPPL::dgematrix&U,
778  CPPL::dgematrix&VT )
779 {
780  return _svd( Q.to_dgematrix(), S, U, VT );
781 }
782 
783 inline void
784 Ellipsoid::_inv
785 ( const CPPL::dsymatrix&Q, CPPL::dsymatrix&Qinv )
786 {
787  if( !Q.n ){
788  Qinv.clear();
789  return;
790  }
791  //CPPL::dsymatrix R = i( Q );
792  CPPL::dsymatrix R;
793  if( CPPL::dsysv( Q, R ) ) throw Exceptions( Exceptions::LAPACK );
794 #ifdef MC__ELLIPSOID_DEBUG_INV
795  std::cout << "\n***Ellispoid: Q\n" << Q;
796  std::cout << "***Ellispoid: inv(Q) (ill-conditioned)\n" << R;
797 #endif
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;
801 #endif
802  //Qinv = R;
803  Qinv.resize( Q.n );
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;
809 #endif
810 
811  return;
812 }
813 
814 inline Ellipsoid mtimes
815 ( const Ellipsoid&E, const CPPL::dgematrix&A, const CPPL::dcovector&b=CPPL::dcovector() )
816 {
817 #ifdef MC__ELLIPSOID_DEBUG
818  assert( E._Q.n == A.n );
819 #endif
820  Ellipsoid AE;
821  if( !A.m ) return AE;
822 
823  // Transformed center
824  AE._c = A * E._c;
825  if( b.l == A.m ) AE._c += b;
826 
827  // Transformed shape
828  CPPL::dgematrix AQAT = A * E._Q * t(A);
829  AE._Q.resize( A.m );
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 );
835 #endif
836  return AE;
837 }
838 
839 inline Ellipsoid minksum_ea
840 ( const std::vector<Ellipsoid>&E, const CPPL::dcovector&D )
841 {
842  Ellipsoid EA;
843  if( !E.size() ) return EA;
844 
845  // Check size
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 );
850  assert( D.l == n );
851 #endif
852 
853  // Construct center and shape matrix
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;
863  }
864  Q_EA *= sum_sqrt_lTQil;
865  EA.set(Q_EA,c_EA);
866 
867  return EA;
868 }
869 
870 inline std::vector<Ellipsoid> minksum_ea
871 ( const std::vector<Ellipsoid>&E, const std::vector<CPPL::dcovector>&D )
872 {
873  std::vector<Ellipsoid> EA;
874  if( !E.size() || !D.size() ) return EA;
875 
876  // Check size
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 );
883 #endif
884 
885  // Construct centers and shape matrices
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;
896  }
897  Q_EA *= sum_sqrt_lTQil;
898  EA.push_back( Ellipsoid(Q_EA,c_EA) );
899  }
900 
901  return EA;
902 }
903 
904 template <typename T>
905 inline Ellipsoid minksum_ea
906 ( const Ellipsoid&E, const T*I, const double&TOL, const double EPS )
907 {
908  if( !E._Q.n ) return E;
909 
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] );
914  }
915  return minksum_ea( E, std::make_pair(r,c), TOL, EPS );
916 }
917 
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() )
921 {
922 #ifdef MC__ELLIPSOID_DEBUG
923  assert( E._Q.n == I.first.l && ( !I.second.l || E._Q.n == I.second.l ) );
924 #endif
925  Ellipsoid EA;
926  if( !E._Q.n ) return EA;
927 
928  // Construct center and shape matrix
929  CPPL::dcovector c_EA( E._c );
930  if( I.second.l ) c_EA += I.second;
931 
932  double trQ = 0.0;
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);
937  }
938  trQ = std::sqrt(trQ);
939  double kappa = trQ;
940  for( int i=0; i<E._Q.n; i++ ) kappa += sqrR(i);
941 
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);
945 
946  return EA.set(Q_EA,c_EA);
947 }
948 
949 inline Ellipsoid inv
950 ( const Ellipsoid&E )
951 {
952  Ellipsoid Einv( E );
953  if( !E._Q.n ) return Einv;
954  Einv.regQ();
955  Einv._Q = Einv.invQ();
956  Einv._reset_auxiliary();
957  return Einv;
958 }
959 
960 inline std::vector<Ellipsoid> inv
961 ( const std::vector<Ellipsoid>&E )
962 {
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) );
966  return Einv;
967 }
968 
969 inline double dist
970 ( const Ellipsoid&E, const std::pair<CPPL::dcovector,double>&HP )
971 {
972  if( E._Q.n != HP.first.l ){
973  std::cerr << "mc::dist: ellipsoid and hyperplane must be of the same dimension.\n";
974  return 0./0.; // return NaN
975  }
976 
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 ) );
980 }
981 
982 inline Ellipsoid intersection_ea
983 ( const Ellipsoid&E, const std::pair<CPPL::dcovector,double>&HP,
984  const double&TOL=machprec() )
985 {
986  if( E._Q.n != HP.first.l ){
987  std::cerr << "mc::intersection_ea: ellipsoid and hyperplane must be of the same dimension.\n";
988  return Ellipsoid();
989  }
990  const unsigned int n = E._Q.n;
991  if( !n ) return Ellipsoid();
992 
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;
998 #endif
999  if( dist( E, HP ) > 0 && v % E._c > c )
1000  return E;
1001  if( dist( E, HP ) > 0 && v % E._c < c )
1002  return Ellipsoid();
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;
1006 #endif
1007 
1008  Ellipsoid EM( E );
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;
1024 #endif
1025 
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;
1032 #endif
1033 
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;
1037 #endif
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;
1049 #endif
1050  return Ellipsoid( Q, q );
1051 }
1052 
1053 inline double
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 )
1057 {
1058  assert( c1.l == W1.n && c2.l == W2.n && c1.l == c2.l );
1059  const unsigned int n = c1.l;
1060 
1061  double a;
1062  if( !options.ROOTSECANT ){
1063  a= _goldsect( -.1, 1., _ell_fusionlambda, c1, W1, c2, W2, n );
1064  return( a<=0.? ( isHP? 1.: 0. ): a );
1065  }
1066 
1067  try{
1068  a = _secant( .9, 1., -.1, 1., _ell_fusionlambda, c1, W1, c2, W2, n );
1069  }
1070  catch( Ellipsoid::Exceptions ){
1071  a = _goldsect( -.1, 1., _ell_fusionlambda, c1, W1, c2, W2, n );
1072  }
1073  return( a<=0.? ( isHP? 1.: 0. ): a );
1074 }
1075 
1076 inline double
1077 Ellipsoid::_secant
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 )
1081 {
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));
1085 
1086  for( unsigned int it=0; !options.ROOTMAXIT || it<options.ROOTMAXIT; it++ ){
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;
1090 #endif
1091  if( std::fabs(fk) < options.ROOTTOL ) return xk;
1092  double Bk = (fk-fkm)/(xk-xkm);
1093  if( Bk == 0 ) throw Exceptions( Exceptions::ROOT );
1094  if( isequal(xk,xL) && fk/Bk>0 ) return xk;
1095  if( isequal(xk,xU) && fk/Bk<0 ) return xk;
1096  xkm = xk;
1097  fkm = fk;
1098  xk = std::max(xL,std::min(xU,xk-fk/Bk));
1099  }
1100 
1101  throw Exceptions( Exceptions::ROOT );
1102 }
1103 
1104 inline double
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 )
1109 {
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;
1115 #endif
1116  if( fL*fU > 0 ) throw Exceptions( Exceptions::ROOT );
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 );
1119 }
1120 
1121 inline double
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 )
1127 // a and c are the current bounds; the minimum is between them.
1128 // b is a center point
1129 {
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) );
1135  //if( std::fabs(c-a) < options.ROOTTOL*(std::fabs(b)+std::fabs(x))
1136  // || iter > options.ROOTMAXIT ) return (c+a)/2.;
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;
1140 #endif
1141  if( std::fabs(fx) < options.ROOTTOL || ( options.ROOTMAXIT && iter > options.ROOTMAXIT ) )
1142  return x;
1143  if( b_then_x )
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 ) );
1148 }
1149 
1150 inline double
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 )
1154 {
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);
1159 
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) );
1165 }
1166 
1167 inline Ellipsoid hpintersection
1168 ( const Ellipsoid&E, const std::pair<CPPL::dcovector,double>&HP )
1169 {
1170  if( E._Q.n != HP.first.l ){
1171  std::cerr << "mc::intersection_ea: ellipsoid and hyperplane must be of the same dimension.\n";
1172  return Ellipsoid();
1173  }
1174  const unsigned int n = E._Q.n;
1175  if( !n ) return Ellipsoid();
1176 
1177 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1178  std::cout << "dist(E,HP): " << dist( E, HP ) << std::endl;
1179 #endif
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;
1184 #endif
1185 
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;
1191 #endif
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;
1195 #endif
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;
1201 #endif
1202  EM.regQ();
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;
1208 #endif
1209 
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);
1216  }
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;
1222 #endif
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;
1227 #endif
1228 
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;
1233 #endif
1234 
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);
1241  }
1242 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1243  std::cout << "Z:\n" << Z << std::endl;
1244  std::cout << "z:\n" << z << std::endl;
1245 #endif
1246  Ellipsoid I( Z, z );
1247 #ifdef MC__ELLIPSOID_DEBUG_HPINTERSECTION
1248  std::cout << "I: " << I;
1249  std::cout << "eig(I): " << I.eigQ().first;
1250 #endif
1251  I += f;
1252  return mtimes( I, t(T) );
1253 }
1254 
1255 inline Ellipsoid hpintersection
1256 ( const Ellipsoid&E, const std::vector< std::pair<CPPL::dcovector,double> >&HP )
1257 {
1258  Ellipsoid Ei( E );
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 );
1262  return Ei;
1263 }
1264 
1265 inline Ellipsoid ell_unitball
1266 ( const unsigned int n )
1267 {
1268  Ellipsoid Eunit;
1269  Eunit._c.resize(n); Eunit._c.zero();
1270  Eunit._Q.resize(n); Eunit._Q.identity();
1271  return Eunit;
1272 }
1273 
1274 inline std::ostream&
1275 operator<<
1276 ( std::ostream&out, const Ellipsoid&E)
1277 {
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;
1283  return out;
1284 }
1285 
1286 } // namespace mc
1287 
1288 #endif
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