1 /* This file is part of Shapes.
3 * Shapes is free software: you can redistribute it and/or modify
4 * it under the terms of the GNU General Public License as published by
5 * the Free Software Foundation, either version 3 of the License, or
8 * Shapes is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
13 * You should have received a copy of the GNU General Public License
14 * along with Shapes. If not, see <http://www.gnu.org/licenses/>.
16 * Copyright 2009 Henrik Tidefelt
19 #include "quadratic_programs.h"
21 #include <gsl/gsl_linalg.h>
22 #include <gsl/gsl_blas.h>
23 #include <gsl/gsl_cblas.h>
25 using namespace Shapes
;
32 //#define TRACEFIGS // generate output on stdout that gives a graphical picture of the operation
35 #include "mathematicarepresentation.h"
36 size_t polytope_distance_generator_form_CallCount
= 0;
47 /* Since an active constraint implies that the corresponding variable is zero, the
48 * subproblems are not concerned with these variables at all.
50 void polytope_distance_generator_form_sub( const size_t dim
,
51 const double ** g1Begin
, const double ** g1End
,
52 const double ** g2Begin
, const double ** g2End
,
55 double * d1
, double * d2
,
56 double * work
); /* allocated memory of size max( dim * ( n1 + n2 - 2 ) + ( n1 + n2 - 2 )^2 + 2 * dim + n1 + n2 )*/
58 void polytope_distance_generator_form_sub_Gilbert( const size_t dim
,
59 const double ** g1Begin
, const double ** g1End
,
60 const double ** g2Begin
, const double ** g2End
,
62 double * d1
, double * d2
);
64 void polytope_distance_generator_form_diff( const size_t dim
,
65 const double ** g1Begin
, const double ** g1End
,
66 const double ** g2Begin
, const double ** g2End
,
67 const double * x1
, const double * x2
,
70 void polytope_distance_generator_form_costs( const size_t dim
,
71 const double ** g1Begin
, const double ** g1End
,
72 const double ** g2Begin
, const double ** g2End
,
75 double * dualCostAccum
, /* This value is updated as an accumulated maximum. */
76 double * work
); /* Just needs to be of size (dim) */
78 /* This function will compute orthogonal vectors spanning the nullspaces of vectors of ones.
79 * Memorey will be reused, but never returned.
80 * Given argument n, it returns a pointer to an array of ( n - 1 ) pointers, each pointing to an
81 * array of doubles of length n.
83 const double ** polytope_distance_generator_form_kernel( size_t n
);
92 DeleteOnExit( T
* mem
) : mem_( mem
) { }
93 ~DeleteOnExit( ){ delete mem_
; }
99 Computation::polytope_distance_generator_form( const size_t dim
,
100 const size_t n1
, const double * g1
,
101 const size_t n2
, const double * g2
,
102 double * costLower
, double * costUpper
,
103 double stopLower
, double stopUpper
,
104 double gapTolAbs
, double gapTolRel
,
108 double * y1
, double * y2
,
109 QPSolverStatus
* status
)
111 /* In case we for some reason would return without setting status properly... */
112 status
->code
= QP_ERROR
;
113 status
->error
= QP_ERROR_UNDEFINED
;
117 stopLower
= std::numeric_limits
< double >::max( );
119 /* stopUpper < 0 gives a test that will never be true. */
136 /* Since the dual cost is not strictly increasing, we must compute the accumulative max of all dual costs.
137 * Since the cost cannot be negative, it is safe to initialize with 0.
143 ( costLower
!= 0 && stopLower
< std::numeric_limits
< double >::max( ) )
144 || ( costUpper
!= 0 && stopUpper
> 0 )
145 || ( costLower
!= 0 && costUpper
!= 0 && ( gapTolAbs
>= 0 || gapTolRel
>= 0 ) );
147 /*-------------------------------------------------------------------------*/
148 /* Allocate memory: this section should be removed later and free memory
149 should be passed as a pointer */
150 /*-------------------------------------------------------------------------*/
151 const double ** gMem
= new const double * [ n1
+ n2
];
152 DeleteOnExit
< const double * > gMemDeleter( gMem
);
153 const double ** g1InactiveBegin
= gMem
;
154 const double ** g1InactiveEnd
= g1InactiveBegin
+ n1
;
155 const double ** g1ActiveEnd
= g1InactiveEnd
;
156 const double ** g2InactiveBegin
= gMem
+ n1
;
157 const double ** g2InactiveEnd
= g2InactiveBegin
+ n2
;
158 const double ** g2ActiveEnd
= g2InactiveEnd
;
159 double * xMem
= new double[ 5 * ( n1
+ n2
) ];
160 DeleteOnExit
< double > xMemDeleter( xMem
);
161 /* The x1 and x2 variables correspond to the generators in the order they appear in the sets defined above.
162 * The elements that corresponds to generators with active constraints are zero by definition, and
163 * do not have to be zero in memory.
166 double * x2
= x1
+ n1
;
167 /* Similarly, only the last part of the Lagrange multiplier vectors are actually used.
169 double * lm1
= x2
+ n2
;
170 double * lm2
= lm1
+ n1
;
171 /* ... and only the first part of the subproblem step vectors.
173 double * d1
= lm2
+ n2
;
174 double * d2
= d1
+ n1
;
175 double * x1short
= d2
+ n2
;
176 double * x2short
= x1short
+ n1
;
177 double * x1long
= x2short
+ n2
;
178 double * x2long
= x1long
+ n1
;
180 const double xInit
= 1. / n1
;
182 const double * src
= g1
;
183 for( const double ** dst
= g1InactiveBegin
; dst
!= g1InactiveEnd
; ++dst
, src
+= dim
, ++xDst
)
190 const double xInit
= 1. / n2
;
192 const double * src
= g2
;
193 for( const double ** dst
= g2InactiveBegin
; dst
!= g2InactiveEnd
; ++dst
, src
+= dim
, ++xDst
)
199 double * yDiff
= new double[ dim
];
200 DeleteOnExit
< double > xDiffDeleter( yDiff
);
201 double * yDiffEnd
= yDiff
+ dim
;
203 double * sub_work
= new double[ ( dim
+ n1
+ n2
- 2 ) * ( n1
+ n2
- 2 ) + 2 * dim
+ n1
+ n2
];
204 DeleteOnExit
< double > sub_workDeleter( sub_work
);
207 ++polytope_distance_generator_form_CallCount
;
208 std::cerr
<< "Polytope distance call count: " << polytope_distance_generator_form_CallCount
<< std::endl
;
209 if( polytope_distance_generator_form_CallCount
<= 10 )
211 std::cerr
<< "=== Problem number " << polytope_distance_generator_form_CallCount
<< " ===" << std::endl
;
212 Computation::polytope_distance_generator_form_write_data( std::cerr
,
216 std::cerr
<< "========================" << std::endl
;
222 std::cout
<< "##echo Invalid program, dimension of metric space is not two!" << std::endl
;
224 bool tracefigPageOpen
= false;
225 std::cout
<< "#catalog.[setbboxgroup 'a]" << std::endl
;
227 std::cout
<< std::fixed
<< "g1: ";
228 const double * begin
= g1
;
229 const double * end
= begin
+ dim
* n1
;
230 for( const double * gSrc
= begin
; gSrc
!= end
; gSrc
+= dim
)
237 for( const double * src
= gSrc
; src
!= gSrc
+ dim
; ++src
)
243 std::cout
<< "(" << *src
<< "bp)" ;
247 std::cout
<< std::endl
;
251 const double * begin
= g2
;
252 const double * end
= begin
+ dim
* n2
;
253 for( const double * gSrc
= begin
; gSrc
!= end
; gSrc
+= dim
)
260 for( const double * src
= gSrc
; src
!= gSrc
+ dim
; ++src
)
266 std::cout
<< "(" << *src
<< "bp)" ;
270 std::cout
<< std::endl
;
274 bool checkNextCost
= true;
275 /* The number of iterations is theoretically bounded by the number of possible combinations of activated constraints. Here, we know that
276 * one can never activate all constraints in a polytope, leading to the following bound.
278 const size_t maxIter
= ( (2<<(n1
-1)) - 1 ) * ( (2<<(n2
-1)) - 1 );
279 for( size_t iterCount
= 0; ; ++iterCount
)
281 if( iterCount
> maxIter
)
283 status
->code
= QP_FAIL
;
284 status
->failure
= QP_FAIL_ITER
;
286 std::cerr
<< "Early return: Maximum number of iterations exceeded." << std::endl
;
289 if( tracefigPageOpen
)
291 std::cout
<< ")" << std::endl
;
292 tracefigPageOpen
= false;
298 if( tracefigPageOpen
)
300 std::cout
<< ")" << std::endl
;
301 tracefigPageOpen
= false;
303 std::cout
<< "#catalog << @text_size:5bp | ( newGroup" << std::endl
;
304 tracefigPageOpen
= true;
305 std::cout
<< " << @stroking:RGB_RED&@width:2bp | [[range '0 [duration g1]].foldl \\ p e → p & [spot [g1 1*e].p] null]" << std::endl
306 << " << @stroking:RGB_GREEN&@width:2bp | [[range '0 [duration g2]].foldl \\ p e → p & [spot [g2 1*e].p] null]" << std::endl
307 << " << @dash:[dashpattern 2bp 2bp]&@width:0.3bp | [stroke [controlling_hull g1] & [controlling_hull g2]]" << std::endl
;
308 for( const double ** gSrc
= g1InactiveEnd
; gSrc
!= g2ActiveEnd
; ++gSrc
)
310 if( gSrc
== g1ActiveEnd
)
312 gSrc
= g2InactiveEnd
- 1;
315 std::cout
<< " << [stroke [circle 2bp] >> [shift (" ;
316 for( const double * src
= *gSrc
; src
!= *gSrc
+ dim
; ++src
)
322 std::cout
<< "(" << *src
<< "bp)" ;
324 std::cout
<< ")]]" << std::endl
;
326 std::cout
<< " << @stroking:RGB_BLUE | [stroke (" ;
327 polytope_distance_generator_form_diff( dim
,
328 g1InactiveBegin
, g1InactiveEnd
,
329 g2InactiveBegin
, g2InactiveBegin
,
332 for( const double * src
= yDiff
; src
!= yDiff
+ dim
; ++src
)
338 std::cout
<< "(" << -*src
<< "bp)" ;
340 std::cout
<< ")--(" ;
341 polytope_distance_generator_form_diff( dim
,
342 g1InactiveBegin
, g1InactiveBegin
,
343 g2InactiveBegin
, g2InactiveEnd
,
346 for( const double * src
= yDiff
; src
!= yDiff
+ dim
; ++src
)
352 std::cout
<< "(" << *src
<< "bp)" ;
354 std::cout
<< ")]" << std::endl
;
356 size_t nInactive1
= g1InactiveEnd
- g1InactiveBegin
;
357 size_t nInactive2
= g2InactiveEnd
- g2InactiveBegin
;
358 polytope_distance_generator_form_diff( dim
,
359 g1InactiveBegin
, g1InactiveEnd
,
360 g2InactiveBegin
, g2InactiveEnd
,
363 if( checkCost
&& checkNextCost
)
365 /* All generators are used here, not only those of inactive constraints. */
366 polytope_distance_generator_form_costs( dim
,
367 g1InactiveBegin
, g1ActiveEnd
,
368 g2InactiveBegin
, g2ActiveEnd
,
374 std::cerr
<< "Interation: " << iterCount
+ 1 << std::endl
;
375 std::cerr
<< "Cost: " << *costLower
<< " -- " << *costUpper
<< std::endl
;
378 if( costUpper
!= 0 && costLower
!= 0 )
380 if( *costUpper
- *costLower
< gapTolAbs
)
382 status
->code
= QP_OK
;
383 status
->termination
= QP_OK_GAP_ABS
;
384 status
->iterations
= iterCount
;
387 if( *costUpper
- *costLower
< gapTolRel
* *costUpper
)
389 status
->code
= QP_OK
;
390 status
->termination
= QP_OK_GAP_REL
;
391 status
->iterations
= iterCount
;
395 if( costUpper
!= 0 && *costUpper
< stopUpper
)
397 status
->code
= QP_OK
;
398 status
->termination
= QP_OK_UPPER
;
399 status
->iterations
= iterCount
;
402 if( costLower
!= 0 && *costLower
> stopLower
)
404 status
->code
= QP_OK
;
405 status
->termination
= QP_OK_LOWER
;
406 status
->iterations
= iterCount
;
410 checkNextCost
= true;
411 polytope_distance_generator_form_sub( dim
,
412 g1InactiveBegin
, g1InactiveEnd
,
413 g2InactiveBegin
, g2InactiveEnd
,
419 std::cerr
<< "Delta: " << Helpers::mathematicaFormat( nInactive1
, 1, d1
)
420 << " " << Helpers::mathematicaFormat( nInactive2
, 1, d2
) << std::endl
;
423 /* Compute squared norm of subproblem step. */
424 double stepNorm2
= 0;
427 double * end
= d1
+ nInactive1
;
428 for( ; src
!= end
; ++src
)
430 stepNorm2
+= (*src
) * (*src
);
435 double * end
= d2
+ nInactive2
;
436 for( ; src
!= end
; ++src
)
438 stepNorm2
+= (*src
) * (*src
);
441 if( stepNorm2
< xTol
)
443 /* No step taken in sub-problem. Are there any constraints to drop? */
444 /* Compute lagrange multipliers.
445 * Note that the lagrange multipliers of the inactive constraints are only used for temporary storage.
446 * Normalizing yDiff will only scale the Langrange multipliers, and by normalizing yDiff we give
447 * the Lagrange multiplier tolerance (lmTol) a geometric interpretation; lmTol becomes a gap tolerance which
448 * is linear in the distance (not distance squared).
450 polytope_distance_generator_form_diff( dim
,
451 g1InactiveBegin
, g1InactiveEnd
,
452 g2InactiveBegin
, g2InactiveEnd
,
456 /* Normalize yDiff. */
457 double lengthSquared
= 0;
458 for( const double * src
= yDiff
; src
!= yDiffEnd
; ++src
)
460 lengthSquared
+= (*src
) * (*src
);
462 const double yDiffScaling
= 1. / sqrt( lengthSquared
);
463 for( double * dst
= yDiff
; dst
!= yDiffEnd
; ++dst
)
465 *dst
*= yDiffScaling
;
469 /* Initialize with negative gradient. */
470 double * lmDst
= lm1
;
471 for( const double ** gSrc
= g1InactiveBegin
; gSrc
!= g1ActiveEnd
; ++gSrc
, ++lmDst
)
474 const double * ySrc
= yDiff
;
475 const double * src
= *gSrc
;
476 for( ; ySrc
!= yDiffEnd
; ++ySrc
, ++src
)
478 tmp
-= (*ySrc
) * (*src
);
482 /* Let nu be the dual variable of the given equality constraint (all variables sum to 1). */
484 double nu
= 0; /* Initialize to avoid compiler warnings. */
485 double * lmInactiveEnd
= lm1
+ nInactive1
;
486 double * lmPtr
= lm1
;
487 for( ; lmPtr
!= lmInactiveEnd
; ++lmPtr
)
489 double lmAbs
= fabs( *lmPtr
);
496 /* Then the remaining Lagrange multipliers follow. */
497 double * lmActiveEnd
= lm1
+ n1
;
498 for( ; lmPtr
!= lmActiveEnd
; ++lmPtr
)
505 /* Compute Lagrange multipliers for the second polytope in the same way... up to sign changes. */
506 double * lmDst
= lm2
;
507 for( const double ** gSrc
= g2InactiveBegin
; gSrc
!= g2ActiveEnd
; ++gSrc
, ++lmDst
)
510 const double * ySrc
= yDiff
;
511 const double * src
= *gSrc
;
512 for( ; ySrc
!= yDiffEnd
; ++ySrc
, ++src
)
514 tmp
+= (*ySrc
) * (*src
); /* Note sign! */
519 double nu
= 0; /* Initialize to avoid compiler warnings. */
520 double * lmInactiveEnd
= lm2
+ nInactive2
;
521 double * lmPtr
= lm2
;
522 for( ; lmPtr
!= lmInactiveEnd
; ++lmPtr
)
524 double lmAbs
= fabs( *lmPtr
);
531 double * lmActiveEnd
= lm2
+ n2
;
532 for( ; lmPtr
!= lmActiveEnd
; ++lmPtr
)
539 std::cerr
<< "lambda: " << Helpers::mathematicaFormat( n1
- nInactive1
, 1, lm1
+ nInactive1
)
540 << " " << Helpers::mathematicaFormat( n2
- nInactive2
, 1, lm2
+ nInactive2
) << std::endl
;
544 const double * lambdaSrc
= lm1
+ nInactive1
;
545 for( const double ** gSrc
= g1InactiveEnd
; gSrc
!= g2ActiveEnd
; ++gSrc
, ++lambdaSrc
)
547 if( gSrc
== g1ActiveEnd
)
549 gSrc
= g2InactiveEnd
- 1;
550 lambdaSrc
= lm2
+ nInactive2
- 1;
553 std::cout
<< " << @nonstroking:[gray 0.5] | (newText << `" << std::scientific
<< std::setprecision(1) << *lambdaSrc
<< std::fixed
<< "´) >> [shift (0,3bp)+(" ;
554 for( const double * src
= *gSrc
; src
!= *gSrc
+ dim
; ++src
)
560 std::cout
<< "(" << *src
<< "bp)" ;
562 std::cout
<< ")]" << std::endl
;
567 bool dropped
= false;
569 const double * lmSrc
= lm1
+ nInactive1
;
570 const double * lmEnd
= lm1
+ n1
;
571 const double ** gSrc
= g1InactiveEnd
;
572 double * xDst
= x1
+ nInactive1
;
573 for( ; lmSrc
!= lmEnd
; ++lmSrc
, ++gSrc
)
575 if( *lmSrc
< -lmTol
)
578 * Swap the generator out of the active set, so that it appears at the end
579 * of the inactive set.
581 const double * gTmp
= *g1InactiveEnd
;
582 *g1InactiveEnd
= *gSrc
;
586 /* Since the constraint has been active, the corresponding variable has been zero.
596 const double * lmSrc
= lm2
+ nInactive2
;
597 const double * lmEnd
= lm2
+ n2
;
598 const double ** gSrc
= g2InactiveEnd
;
599 double * xDst
= x2
+ nInactive2
;
600 for( ; lmSrc
!= lmEnd
; ++lmSrc
, ++gSrc
)
602 if( *lmSrc
< -lmTol
)
605 * Swap the generator out of the active set, so that it appears at the end
606 * of the inactive set.
608 const double * gTmp
= *g2InactiveEnd
;
609 *g2InactiveEnd
= *gSrc
;
613 /* Since the constraint has been active, the corresponding variable has been zero.
624 /* We reached optimum!
626 status
->code
= QP_OK
;
627 status
->termination
= QP_OK_LM
;
628 status
->iterations
= iterCount
;
631 /* If we dropped some constraint, we just return to the top of the loop.
632 * No need to re-normalize variables since added variables are exactly zero.
634 checkNextCost
= false; /* No need to check the cost in the next iteration since no step was made. */
638 /* Find inactive constraints that limit the step length in each polytope. */
640 const double ** gStop1
= 0;
642 const double ** gSrc
= g1InactiveBegin
;
643 const double ** gEnd
= g1InactiveEnd
;
644 const double * dSrc
= d1
;
645 const double * xSrc
= x1
;
646 for( ; gSrc
!= gEnd
; ++gSrc
, ++dSrc
, ++xSrc
)
648 double denom
= -*dSrc
;
651 double sTmp
= *xSrc
/ denom
;
661 const double ** gStop2
= 0;
663 const double ** gSrc
= g2InactiveBegin
;
664 const double ** gEnd
= g2InactiveEnd
;
665 const double * dSrc
= d2
;
666 const double * xSrc
= x2
;
667 for( ; gSrc
!= gEnd
; ++gSrc
, ++dSrc
, ++xSrc
)
669 double denom
= -*dSrc
;
672 double sTmp
= *xSrc
/ denom
;
682 /* In a basic active set algorithm, one uses only the smaller of s1 and s2, but here
683 * we actually use both, if that gives further decrease in the objective function.
685 double sShort
= ( s1
< s2
) ? s1
: s2
;
687 const double * xSrc
= x1
;
688 const double * xEnd
= x1
+ nInactive1
;
689 const double * dSrc
= d1
;
690 double * dst
= x1short
;
691 for( ; xSrc
!= xEnd
; ++xSrc
, ++dSrc
, ++dst
)
693 *dst
= *xSrc
+ sShort
* (*dSrc
);
697 const double * xSrc
= x2
;
698 const double * xEnd
= x2
+ nInactive2
;
699 const double * dSrc
= d2
;
700 double * dst
= x2short
;
701 for( ; xSrc
!= xEnd
; ++xSrc
, ++dSrc
, ++dst
)
703 *dst
= *xSrc
+ sShort
* (*dSrc
);
706 double shortCost
= 0;
707 polytope_distance_generator_form_diff( dim
,
708 g1InactiveBegin
, g1InactiveEnd
,
709 g2InactiveBegin
, g2InactiveEnd
,
712 for( const double * src
= yDiff
; src
!= yDiffEnd
; ++src
)
714 shortCost
+= (*src
) * (*src
);
719 const double * xSrc
= x1
;
720 const double * xEnd
= x1
+ nInactive1
;
721 const double * dSrc
= d1
;
722 double * dst
= x1long
;
723 for( ; xSrc
!= xEnd
; ++xSrc
, ++dSrc
, ++dst
)
725 *dst
= *xSrc
+ s1
* (*dSrc
);
728 polytope_distance_generator_form_diff( dim
,
729 g1InactiveBegin
, g1InactiveEnd
,
730 g2InactiveBegin
, g2InactiveEnd
,
733 for( const double * src
= yDiff
; src
!= yDiffEnd
; ++src
)
735 longCost
+= (*src
) * (*src
);
738 if( longCost
> shortCost
)
740 /* Dont make long step. */
747 const double * xSrc
= x2
;
748 const double * xEnd
= x2
+ nInactive2
;
749 const double * dSrc
= d2
;
750 double * dst
= x2long
;
751 for( ; xSrc
!= xEnd
; ++xSrc
, ++dSrc
, ++dst
)
753 *dst
= *xSrc
+ s2
* (*dSrc
);
756 polytope_distance_generator_form_diff( dim
,
757 g1InactiveBegin
, g1InactiveEnd
,
758 g2InactiveBegin
, g2InactiveEnd
,
761 for( const double * src
= yDiff
; src
!= yDiffEnd
; ++src
)
763 longCost
+= (*src
) * (*src
);
766 if( longCost
> shortCost
)
768 /* Dont make long step. */
774 /* Update variables and active constraints.
775 * Important: variables first, or we will get a mess!
778 const double * xSrc
= x1
;
779 const double * xEnd
= x1
+ nInactive1
;
780 const double * dSrc
= d1
;
782 for( ; xSrc
!= xEnd
; ++xSrc
, ++dSrc
, ++dst
)
784 *dst
= *xSrc
+ s1
* (*dSrc
);
789 /* Swap constraints, and swap variables in the same way. New variable is zero by construction. */
791 const double * gTmp
= *gStop1
;
792 *gStop1
= *g1InactiveEnd
;
793 *g1InactiveEnd
= gTmp
;
794 *( x1
+ ( gStop1
- g1InactiveBegin
) ) = *( x1
+ ( g1InactiveEnd
- g1InactiveBegin
) );
797 const double * xSrc
= x2
;
798 const double * xEnd
= x2
+ nInactive2
;
799 const double * dSrc
= d2
;
801 for( ; xSrc
!= xEnd
; ++xSrc
, ++dSrc
, ++dst
)
803 *dst
= *xSrc
+ s2
* (*dSrc
);
808 /* Swap constraints, and swap variables in the same way. New variable is zero by construction. */
810 const double * gTmp
= *gStop2
;
811 *gStop2
= *g2InactiveEnd
;
812 *g2InactiveEnd
= gTmp
;
813 *( x2
+ ( gStop2
- g2InactiveBegin
) ) = *( x2
+ ( g2InactiveEnd
- g2InactiveBegin
) );
819 if( tracefigPageOpen
)
821 std::cout
<< ")" << std::endl
;
822 tracefigPageOpen
= false;
826 /* Compute final cost. */
828 polytope_distance_generator_form_diff( dim
,
829 g1InactiveBegin
, g1InactiveEnd
,
830 g2InactiveBegin
, g2InactiveEnd
,
833 polytope_distance_generator_form_costs( dim
,
834 g1InactiveBegin
, g1ActiveEnd
,
835 g2InactiveBegin
, g2ActiveEnd
,
841 std::cerr
<< "Terminating with final cost: " << *costLower
<< " -- " << *costUpper
<< std::endl
;
845 /* Compute final point in each polytope. */
849 double * yEnd
= y
+ dim
;
850 for( double * dst
= y
; dst
!= yEnd
; ++dst
)
854 const double * xSrc
= x1
;
855 for( const double ** gSrc
= g1InactiveBegin
; gSrc
!= g1InactiveEnd
; ++gSrc
, ++xSrc
)
858 const double * src
= *gSrc
;
859 for( double * dst
= y
; dst
!= yEnd
; ++dst
, ++src
)
861 *dst
+= xVal
* (*src
);
868 double * yEnd
= y
+ dim
;
869 for( double * dst
= y
; dst
!= yEnd
; ++dst
)
873 const double * xSrc
= x2
;
874 for( const double ** gSrc
= g2InactiveBegin
; gSrc
!= g2InactiveEnd
; ++gSrc
, ++xSrc
)
877 const double * src
= *gSrc
;
878 for( double * dst
= y
; dst
!= yEnd
; ++dst
, ++src
)
880 *dst
+= xVal
* (*src
);
887 Computation::polytope_distance_generator_form_diff( const size_t dim
,
888 const double ** g1Begin
, const double ** g1End
,
889 const double ** g2Begin
, const double ** g2End
,
890 const double * x1
, const double * x2
,
893 double * yDiffEnd
= yDiff
+ dim
;
894 for( double * dst
= yDiff
; dst
!= yDiffEnd
; ++dst
)
899 const double * xSrc
= x2
;
900 for( const double ** gSrc
= g2Begin
; gSrc
!= g2End
; ++gSrc
, ++xSrc
)
903 const double * src
= *gSrc
;
904 for( double * dst
= yDiff
; dst
!= yDiffEnd
; ++dst
, ++src
)
906 *dst
+= xVal
* (*src
);
911 const double * xSrc
= x1
;
912 for( const double ** gSrc
= g1Begin
; gSrc
!= g1End
; ++gSrc
, ++xSrc
)
915 const double * src
= *gSrc
;
916 for( double * dst
= yDiff
; dst
!= yDiffEnd
; ++dst
, ++src
)
918 *dst
-= xVal
* (*src
);
925 Computation::polytope_distance_generator_form_costs( const size_t dim
,
926 const double ** g1Begin
, const double ** g1End
,
927 const double ** g2Begin
, const double ** g2End
,
928 const double * yDiff
,
930 double * dualCostAccum
,
933 const double * yDiffEnd
= yDiff
+ dim
;
936 for( const double * src
= yDiff
; src
!= yDiffEnd
; ++src
)
938 pCost
+= (*src
) * (*src
);
941 if( primalCost
!= 0 )
946 if( dualCostAccum
== 0 )
951 double * n
= work
; /* Normalaized normal to planes separating the two polytopes. */
952 double * nEnd
= work
+ dim
;
954 /* Normalize yDiff and store in n. */
955 double yDiffScale
= 1. / sqrt( pCost
);
957 for( const double * src
= yDiff
; src
!= yDiffEnd
; ++src
, ++dst
)
959 *dst
= yDiffScale
* (*src
);
963 /* Find dual cost by projecting generators onto n. */
964 double min2
= std::numeric_limits
< double >::max( );
966 for( const double ** gSrc
= g2Begin
; gSrc
!= g2End
; ++gSrc
)
969 const double * src
= *gSrc
;
970 for( const double * nSrc
= n
; nSrc
!= nEnd
; ++nSrc
, ++src
)
972 tmp
+= (*nSrc
) * (*src
);
974 min2
= std::min( min2
, tmp
);
978 double max1
= -std::numeric_limits
< double >::max( );
980 for( const double ** gSrc
= g1Begin
; gSrc
!= g1End
; ++gSrc
)
983 const double * src
= *gSrc
;
984 for( const double * nSrc
= n
; nSrc
!= nEnd
; ++nSrc
, ++src
)
986 tmp
+= (*nSrc
) * (*src
);
988 max1
= std::max( max1
, tmp
);
992 double diff
= min2
- max1
;
995 *dualCostAccum
= std::max( *dualCostAccum
, diff
* diff
);
1001 Computation::polytope_distance_generator_form_sub( const size_t dim
,
1002 const double ** g1Begin
, const double ** g1End
,
1003 const double ** g2Begin
, const double ** g2End
,
1004 const double * yDiff
,
1006 double * d1
, double * d2
,
1009 size_t n1
= g1End
- g1Begin
;
1010 size_t n2
= g2End
- g2Begin
;
1011 double * d1End
= d1
+ n1
;
1012 double * d2End
= d2
+ n2
;
1013 for( double * dst
= d1
; dst
!= d1End
; ++dst
)
1017 for( double * dst
= d2
; dst
!= d2End
; ++dst
)
1021 if( n1
== 1 && n2
== 1 ) /* They must never be less than 1! */
1023 /* If there is only one inactive constraint in a polytope, the corresponding variable is in fact locked to 1.
1024 * Henece, we cannot move in that polytope. If we can't move in either polytope, there is nothing to optimize.
1025 * We have already set the result to 0.
1029 const double ** N1
= Computation::polytope_distance_generator_form_kernel( n1
);
1030 const double ** N2
= Computation::polytope_distance_generator_form_kernel( n2
);
1033 y_gsl
.data
= const_cast< double * >( yDiff
);
1039 work
+= ( d_gsl
.size
= n1
+ n2
- 2 );
1042 if( dim
>= n1
+ n2
- 2 )
1044 /* Over-determined case. SVD of A can be computed as usual. */
1047 work
+= ( A
.size1
= dim
) * ( A
.size2
= A
.tda
= n1
+ n2
- 2 );
1050 work
+= ( V
.size1
= n1
+ n2
- 2 ) * ( V
.size2
= V
.tda
= n1
+ n2
- 2 );
1053 work
+= ( S
.size
= n1
+ n2
- 2 );
1056 SV_work
.data
= work
;
1057 work
+= ( SV_work
.size
= n1
+ n2
- 2 );
1060 /* Populate A with data.
1061 * The data access here can be improved. */
1064 double * ARowStart
= A
.data
;
1065 for( size_t row
= 0; row
< dim
; ++row
, ARowStart
+= A
.tda
)
1067 const double ** NcolStart
= N1
;
1068 const double ** NcolEnd
= N1
+ n1
- 1;
1069 double * Adst
= ARowStart
;
1070 for( ; NcolStart
!= NcolEnd
; ++NcolStart
, ++Adst
)
1073 const double ** gcolStart
= g1Begin
;
1074 const double ** gcolEnd
= g1End
;
1075 const double * Nsrc
= *NcolStart
;
1076 for( ; gcolStart
!= gcolEnd
; ++gcolStart
, ++Nsrc
)
1078 tmp
+= (*((*gcolStart
) + row
)) * (*Nsrc
);
1086 double * ARowStart
= A
.data
+ ( n1
- 1 );
1087 for( size_t row
= 0; row
< dim
; ++row
, ARowStart
+= A
.tda
)
1089 const double ** NcolStart
= N2
;
1090 const double ** NcolEnd
= N2
+ n2
- 1;
1091 double * Adst
= ARowStart
;
1092 for( ; NcolStart
!= NcolEnd
; ++NcolStart
, ++Adst
)
1095 const double ** gcolStart
= g2Begin
;
1096 const double ** gcolEnd
= g2End
;
1097 const double * Nsrc
= *NcolStart
;
1098 for( ; gcolStart
!= gcolEnd
; ++gcolStart
, ++Nsrc
)
1100 tmp
-= (*((*gcolStart
) + row
)) * (*Nsrc
); /* Note the sign! */
1107 gsl_linalg_SV_decomp( & A
, & V
, & S
, & SV_work
);
1109 /* Truncate small singular values. */
1110 double * dst
= S
.data
;
1111 double * end
= S
.data
+ S
.size
;
1112 const double sigmaCut
= sigmaTol
* (*dst
);
1114 for( ; dst
!= end
; ++dst
)
1116 if( *dst
< sigmaCut
)
1121 for( ; dst
!= end
; ++dst
)
1126 gsl_linalg_SV_solve( & A
, & V
, & S
, & y_gsl
, & d_gsl
);
1130 /* Under-determined case. Need to transpose A to compute SVD. */
1133 work
+= ( A
.size1
= n1
+ n2
- 2 ) * ( A
.size2
= A
.tda
= dim
);
1136 work
+= ( V
.size1
= dim
) * ( V
.size2
= V
.tda
= dim
);
1139 work
+= ( S
.size
= dim
);
1142 SV_work
.data
= work
;
1143 work
+= ( SV_work
.size
= dim
);
1146 /* Populate A with data.
1147 * The data access here can be improved. */
1150 double * AColStart
= A
.data
;
1151 for( size_t row
= 0; row
< dim
; ++row
, ++AColStart
)
1153 const double ** NcolStart
= N1
;
1154 const double ** NcolEnd
= N1
+ n1
- 1;
1155 double * Adst
= AColStart
;
1156 for( ; NcolStart
!= NcolEnd
; ++NcolStart
, Adst
+= A
.tda
)
1159 const double ** gcolStart
= g1Begin
;
1160 const double ** gcolEnd
= g1End
;
1161 const double * Nsrc
= *NcolStart
;
1162 for( ; gcolStart
!= gcolEnd
; ++gcolStart
, ++Nsrc
)
1164 tmp
+= (*((*gcolStart
) + row
)) * (*Nsrc
);
1172 double * AColStart
= A
.data
+ ( n1
- 1 ) * A
.tda
;
1173 for( size_t row
= 0; row
< dim
; ++row
, ++AColStart
)
1175 const double ** NcolStart
= N2
;
1176 const double ** NcolEnd
= N2
+ n2
- 1;
1177 double * Adst
= AColStart
;
1178 for( ; NcolStart
!= NcolEnd
; ++NcolStart
, Adst
+= A
.tda
)
1181 const double ** gcolStart
= g2Begin
;
1182 const double ** gcolEnd
= g2End
;
1183 const double * Nsrc
= *NcolStart
;
1184 for( ; gcolStart
!= gcolEnd
; ++gcolStart
, ++Nsrc
)
1186 tmp
-= (*((*gcolStart
) + row
)) * (*Nsrc
); /* Note the sign! */
1193 gsl_linalg_SV_decomp( & A
, & V
, & S
, & SV_work
);
1195 /* Truncate small singular values. */
1196 double * dst
= S
.data
;
1197 double * end
= S
.data
+ S
.size
;
1198 const double sigmaCut
= sigmaTol
* (*dst
);
1200 for( ; dst
!= end
; ++dst
)
1202 if( *dst
< sigmaCut
)
1207 for( ; dst
!= end
; ++dst
)
1212 /* Now, instead of using
1213 * gsl_linalg_SV_solve( & A, & V, & S, & y_gsl, & d_gsl );
1214 * we must solve the equation ourselves since the decomposition is transposed.
1216 gsl_blas_dgemv( CblasTrans
, 1, & V
, & y_gsl
, 0, & SV_work
);
1218 const double * src
= S
.data
;
1219 const double * srcEnd
= S
.data
+ S
.size
;
1220 double * dst
= SV_work
.data
;
1221 for( ; src
!= srcEnd
; ++src
, ++dst
)
1233 gsl_blas_dgemv( CblasNoTrans
, 1, & A
, & SV_work
, 0, & d_gsl
);
1237 /* Compute output variables. */
1241 double * dstEnd
= d1
+ n1
;
1242 for( ; dst
!= dstEnd
; ++dst
)
1249 double * dstEnd
= d2
+ n2
;
1250 for( ; dst
!= dstEnd
; ++dst
)
1256 /* Then add linear combination of basis vectors. */
1257 const double * dSrc
= d_gsl
.data
;
1259 const double ** NSrc
= N1
;
1260 const double ** NSrcEnd
= N1
+ ( n1
- 1 );
1261 for( ; NSrc
!= NSrcEnd
; ++NSrc
, ++dSrc
)
1265 double * dstEnd
= d1
+ n1
;
1266 const double * src
= *NSrc
;
1267 for( ; dst
!= dstEnd
; ++dst
, ++src
)
1274 const double ** NSrc
= N2
;
1275 const double ** NSrcEnd
= N2
+ ( n2
- 1 );
1276 for( ; NSrc
!= NSrcEnd
; ++NSrc
, ++dSrc
)
1280 double * dstEnd
= d2
+ n2
;
1281 const double * src
= *NSrc
;
1282 for( ; dst
!= dstEnd
; ++dst
, ++src
)
1292 Computation::polytope_distance_generator_form_sub_Gilbert( const size_t dim
,
1293 const double ** g1Begin
, const double ** g1End
,
1294 const double ** g2Begin
, const double ** g2End
,
1295 const double * yDiff
,
1296 double * d1
, double * d2
)
1302 Computation::polytope_distance_generator_form_kernel( size_t n
)
1304 static std::vector
< const double ** > mem( 0 );
1305 if( n
< mem
.size( ) )
1309 mem
.reserve( n
+ 1 );
1310 while( mem
.size( ) <= n
)
1312 size_t m
= mem
.size( );
1315 /* Invalid dimension, just skip. */
1321 /* Trivial case; kernel has dimension 0 */
1325 const double ** res
= new const double * [ m
- 1 ];
1326 gsl_matrix
* qr
= gsl_matrix_alloc( m
, m
);
1327 gsl_vector
* tau
= gsl_vector_alloc( m
);
1328 gsl_matrix_set_zero( qr
);
1329 for( size_t i
= 0; i
< m
; ++i
)
1331 gsl_matrix_set( qr
, i
, 0, 1. );
1333 gsl_matrix
* q
= gsl_matrix_alloc( m
, m
);
1334 gsl_matrix
* r
= gsl_matrix_alloc( m
, m
);
1336 gsl_linalg_QR_decomp( qr
, tau
);
1337 gsl_linalg_QR_unpack( qr
, tau
, q
, r
);
1339 const double ** dst
= res
;
1340 const double ** dstEnd
= res
+ ( m
- 1 );
1341 double * srcQStart
= q
->data
+ 1; /* Skip first column. */
1342 for( ; dst
!= dstEnd
; ++dst
, ++srcQStart
)
1344 double * v
= new double[ m
];
1346 double * vEnd
= v
+ m
;
1347 double * srcQ
= srcQStart
;
1348 for( ; v
!= vEnd
; ++v
, srcQ
+= q
->tda
)
1355 mem
.push_back( res
);
1357 gsl_matrix_free( qr
);
1358 gsl_vector_free( tau
);
1359 gsl_matrix_free( q
);
1360 gsl_matrix_free( r
);