Update procedures

[shapes.git] / source / solve_qp_dist_gen.cc

/* This file is part of Shapes.
 *
 * Shapes is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * Shapes is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Shapes.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Copyright 2009 Henrik Tidefelt
 */

#include "quadratic_programs.h"

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_cblas.h>

using namespace Shapes;

#include <vector>
#include <cstdlib>
#include <cmath>
#include <limits>

//#define DISPLAY
//#define TRACEFIGS // generate output on stdout that gives a graphical picture of the operation

#ifdef DISPLAY
#include "mathematicarepresentation.h"
size_t polytope_distance_generator_form_CallCount = 0;
#endif
#ifdef TRACEFIGS
#include <iomanip>
#endif


namespace Shapes
{
        namespace Computation
        {
                /* Since an active constraint implies that the corresponding variable is zero, the
                 * subproblems are not concerned with these variables at all.
                 */
                void polytope_distance_generator_form_sub( const size_t dim,
                                                                                                                                                                                         const double ** g1Begin, const double ** g1End,
                                                                                                                                                                                         const double ** g2Begin, const double ** g2End,
                                                                                                                                                                                         const double * yDiff,
                                                                                                                                                                                         double sigmaTol,
                                                                                                                                                                                         double * d1, double * d2,
                                                                                                                                                                                         double * work ); /* allocated memory of size max( dim * ( n1 + n2 - 2 ) + ( n1 + n2 - 2 )^2 + 2 * dim + n1 + n2 )*/

                void polytope_distance_generator_form_sub_Gilbert( const size_t dim,
                                                                                                                                                                                                                         const double ** g1Begin, const double ** g1End,
                                                                                                                                                                                                                         const double ** g2Begin, const double ** g2End,
                                                                                                                                                                                                                         const double * yDiff,
                                                                                                                                                                                                                         double * d1, double * d2 );

                void polytope_distance_generator_form_diff( const size_t dim,
                                                                                                                                                                                                const double ** g1Begin, const double ** g1End,
                                                                                                                                                                                                const double ** g2Begin, const double ** g2End,
                                                                                                                                                                                                const double * x1, const double * x2,
                                                                                                                                                                                                double * yDiff );

                void polytope_distance_generator_form_costs( const size_t dim,
                                                                                                                                                                                                 const double ** g1Begin, const double ** g1End,
                                                                                                                                                                                                 const double ** g2Begin, const double ** g2End,
                                                                                                                                                                                                 const double * yDiff,
                                                                                                                                                                                                 double * primalCost,
                                                                                                                                                                                                 double * dualCostAccum, /* This value is updated as an accumulated maximum. */
                                                                                                                                                                                                 double * work ); /* Just needs to be of size (dim)  */

                /* This function will compute orthogonal vectors spanning the nullspaces of vectors of ones.
                 * Memorey will be reused, but never returned.
                 * Given argument n, it returns a pointer to an array of ( n - 1 ) pointers, each pointing to an
                 * array of doubles of length n.
                 */
                const double ** polytope_distance_generator_form_kernel( size_t n );
        }
}

template<class T>
class DeleteOnExit
{
  T * mem_;
public:
  DeleteOnExit( T * mem ) : mem_( mem ) { }
  ~DeleteOnExit( ){ delete mem_; }
};



void
Computation::polytope_distance_generator_form( const size_t dim,
                                                                                                                                                                                         const size_t n1, const double * g1,
                                                                                                                                                                                         const size_t n2, const double * g2,
                                                                                                                                                                                         double * costLower, double * costUpper,
                                                                                                                                                                                         double stopLower, double stopUpper,
                                                                                                                                                                                         double gapTolAbs, double gapTolRel,
                                                                                                                                                                                         double lmTol,
                                                                                                                                                                                         double xTol,
                                                                                                                                                                                         double sigmaTol,
                                                                                                                                                                                         double * y1, double * y2,
                                                                                                                                                                                         QPSolverStatus * status )
{
        /* In case we for some reason would return without setting status properly... */
        status->code = QP_ERROR;
        status->error = QP_ERROR_UNDEFINED;

        if( stopLower < 0 )
                {
                        stopLower = std::numeric_limits< double >::max( );
                }
        /* stopUpper < 0 gives a test that will never be true. */

        if( lmTol <= 0 )
                {
                        lmTol = 1e-14;
                }
        if( xTol <= 0 )
                {
                        xTol = 1e-8;
                }
        if( sigmaTol <= 0 )
                {
                        sigmaTol = 1e-3;
                }

        if( costLower != 0 )
                {
                        /* Since the dual cost is not strictly increasing, we must compute the accumulative max of all dual costs.
                         * Since the cost cannot be negative, it is safe to initialize with 0.
                         */
                        *costLower = 0;
                }

        bool checkCost =
                ( costLower != 0 && stopLower < std::numeric_limits< double >::max( ) )
                || ( costUpper != 0 && stopUpper > 0 )
                || ( costLower != 0 && costUpper != 0 && ( gapTolAbs >= 0 || gapTolRel >= 0 ) );

        /*-------------------------------------------------------------------------*/
        /* Allocate memory: this section should be removed later and free memory
                 should be passed as a pointer */
        /*-------------------------------------------------------------------------*/
        const double ** gMem = new const double * [ n1 + n2 ];
        DeleteOnExit< const double * > gMemDeleter( gMem );
        const double ** g1InactiveBegin = gMem;
        const double ** g1InactiveEnd = g1InactiveBegin + n1;
        const double ** g1ActiveEnd = g1InactiveEnd;
        const double ** g2InactiveBegin = gMem + n1;
        const double ** g2InactiveEnd = g2InactiveBegin + n2;
        const double ** g2ActiveEnd = g2InactiveEnd;
        double * xMem = new double[ 5 * ( n1 + n2 ) ];
        DeleteOnExit< double > xMemDeleter( xMem );
        /* The x1 and x2 variables correspond to the generators in the order they appear in the sets defined above.
         * The elements that corresponds to generators with active constraints are zero by definition, and
         * do not have to be zero in memory.
         */
        double * x1 =      xMem;
        double * x2 =      x1 + n1;
        /* Similarly, only the last part of the Lagrange multiplier vectors are actually used.
         */
        double * lm1 =     x2 + n2;
        double * lm2 =     lm1 + n1;
        /* ... and only the first part of the subproblem step vectors.
         */
        double * d1 =      lm2 + n2;
        double * d2 =      d1 + n1;
        double * x1short = d2 + n2;
        double * x2short = x1short + n1;
        double * x1long =  x2short + n2;
        double * x2long =  x1long + n1;
        {
                const double xInit = 1. / n1;
                double * xDst = x1;
                const double * src = g1;
                for( const double ** dst = g1InactiveBegin; dst != g1InactiveEnd; ++dst, src += dim, ++xDst )
                        {
                                *dst = src;
                                *xDst = xInit;
                        }
        }
        {
                const double xInit = 1. / n2;
                double * xDst = x2;
                const double * src = g2;
                for( const double ** dst = g2InactiveBegin; dst != g2InactiveEnd; ++dst, src += dim, ++xDst )
                        {
                                *dst = src;
                                *xDst = xInit;
                        }
        }
        double * yDiff = new double[ dim ];
        DeleteOnExit< double > xDiffDeleter( yDiff );
        double * yDiffEnd = yDiff + dim;

        double * sub_work = new double[ ( dim + n1 + n2 - 2 ) * ( n1 + n2 - 2 ) + 2 * dim + n1 + n2 ];
        DeleteOnExit< double > sub_workDeleter( sub_work );

#ifdef DISPLAY
        ++polytope_distance_generator_form_CallCount;
        std::cerr << "Polytope distance call count: " << polytope_distance_generator_form_CallCount << std::endl ;
        if( polytope_distance_generator_form_CallCount <= 10 )
                {
                        std::cerr << "=== Problem number " << polytope_distance_generator_form_CallCount << " ===" << std::endl ;
                        Computation::polytope_distance_generator_form_write_data( std::cerr,
                                                                                                                                                                                                                                                                dim,
                                                                                                                                                                                                                                                                n1, g1,
                                                                                                                                                                                                                                                                n2, g2 );
                        std::cerr << "========================" << std::endl ;
                }
#endif
#ifdef TRACEFIGS
        if( dim != 2 )
                {
                        std::cout << "##echo Invalid program, dimension of metric space is not two!" << std::endl ;
                }
        bool tracefigPageOpen = false;
        std::cout << "#catalog.[setbboxgroup 'a]" << std::endl ;
        {
                std::cout << std::fixed << "g1: ";
                const double * begin = g1;
                const double * end = begin + dim * n1;
                for( const double * gSrc = begin; gSrc != end; gSrc += dim )
                        {
                                if( gSrc != begin )
                                        {
                                                std::cout << "--" ;
                                        }
                                std::cout << "(" ;
                                for( const double * src = gSrc; src != gSrc + dim; ++src )
                                        {
                                                if( src != gSrc )
                                                        {
                                                                std::cout << "," ;
                                                        }
                                                std::cout << "(" << *src << "bp)" ;
                                        }
                                std::cout << ")" ;
                        }
                std::cout << std::endl ;
        }
        {
                std::cout << "g2: ";
                const double * begin = g2;
                const double * end = begin + dim * n2;
                for( const double * gSrc = begin; gSrc != end; gSrc += dim )
                        {
                                if( gSrc != begin )
                                        {
                                                std::cout << "--" ;
                                        }
                                std::cout << "(" ;
                                for( const double * src = gSrc; src != gSrc + dim; ++src )
                                        {
                                                if( src != gSrc )
                                                        {
                                                                std::cout << "," ;
                                                        }
                                                std::cout << "(" << *src << "bp)" ;
                                        }
                                std::cout << ")" ;
                        }
                std::cout << std::endl ;
        }
#endif

        bool checkNextCost = true;
        /* The number of iterations is theoretically bounded by the number of possible combinations of activated constraints.  Here, we know that
         * one can never activate all constraints in a polytope, leading to the following bound.
         */
        const size_t maxIter = ( (2<<(n1-1)) - 1 ) * ( (2<<(n2-1)) - 1 );
        for( size_t iterCount = 0; ; ++iterCount )
                {
                        if( iterCount > maxIter )
                                {
                                        status->code = QP_FAIL;
                                        status->failure = QP_FAIL_ITER;
#ifdef DISPLAY
                                        std::cerr << "Early return: Maximum number of iterations exceeded." << std::endl ;
#endif
#ifdef TRACEFIGS
                                        if( tracefigPageOpen )
                                                {
                                                        std::cout << ")" << std::endl ;
                                                        tracefigPageOpen = false;
                                                }
#endif
                                        return;
                                }
#ifdef TRACEFIGS
                        if( tracefigPageOpen )
                                {
                                        std::cout << ")" << std::endl ;
                                        tracefigPageOpen = false;
                                }
                        std::cout << "#catalog << @text_size:5bp | ( newGroup" << std::endl ;
                        tracefigPageOpen = true;
                        std::cout << " << @stroking:RGB_RED&@width:2bp | [[range '0 [duration g1]].foldl \\ p e → p & [spot [g1 1*e].p] null]" << std::endl
                                                                << " << @stroking:RGB_GREEN&@width:2bp | [[range '0 [duration g2]].foldl \\ p e → p & [spot [g2 1*e].p] null]" << std::endl
                                                                << " << @dash:[Traits..dashpattern 2bp 2bp]&@width:0.3bp | [stroke [controlling_hull g1] & [controlling_hull g2]]" << std::endl ;
                        for( const double ** gSrc = g1InactiveEnd; gSrc != g2ActiveEnd; ++gSrc )
                                {
                                        if( gSrc == g1ActiveEnd )
                                                {
                                                        gSrc = g2InactiveEnd - 1;
                                                        continue;
                                                }
                                        std::cout << " << [stroke [circle 2bp] >> [shift (" ;
                                        for( const double * src = *gSrc; src != *gSrc + dim; ++src )
                                                {
                                                        if( src != *gSrc )
                                                                {
                                                                        std::cout << "," ;
                                                                }
                                                        std::cout << "(" << *src << "bp)" ;
                                                }
                                        std::cout << ")]]" << std::endl ;
                                }
                        std::cout << " << @stroking:RGB_BLUE | [stroke (" ;
                        polytope_distance_generator_form_diff( dim,
                                                                                                                                                                                 g1InactiveBegin, g1InactiveEnd,
                                                                                                                                                                                 g2InactiveBegin, g2InactiveBegin,
                                                                                                                                                                                 x1, x2,
                                                                                                                                                                                 yDiff );
                        for( const double * src = yDiff; src != yDiff + dim; ++src )
                                {
                                        if( src != yDiff )
                                                {
                                                        std::cout << "," ;
                                                }
                                        std::cout << "(" << -*src << "bp)" ;
                                }
                        std::cout << ")--(" ;
                        polytope_distance_generator_form_diff( dim,
                                                                                                                                                                                 g1InactiveBegin, g1InactiveBegin,
                                                                                                                                                                                 g2InactiveBegin, g2InactiveEnd,
                                                                                                                                                                                 x1, x2,
                                                                                                                                                                                 yDiff );
                        for( const double * src = yDiff; src != yDiff + dim; ++src )
                                {
                                        if( src != yDiff )
                                                {
                                                        std::cout << "," ;
                                                }
                                        std::cout << "(" << *src << "bp)" ;
                                }
                        std::cout << ")]" << std::endl ;
#endif
                        size_t nInactive1 = g1InactiveEnd - g1InactiveBegin;
                        size_t nInactive2 = g2InactiveEnd - g2InactiveBegin;
                        polytope_distance_generator_form_diff( dim,
                                                                                                                                                                                 g1InactiveBegin, g1InactiveEnd,
                                                                                                                                                                                 g2InactiveBegin, g2InactiveEnd,
                                                                                                                                                                                 x1, x2,
                                                                                                                                                                                 yDiff );
                        if( checkCost && checkNextCost )
                                {
                                        /* All generators are used here, not only those of inactive constraints. */
                                        polytope_distance_generator_form_costs( dim,
                                                                                                                                                                                                        g1InactiveBegin, g1ActiveEnd,
                                                                                                                                                                                                        g2InactiveBegin, g2ActiveEnd,
                                                                                                                                                                                                        yDiff,
                                                                                                                                                                                                        costUpper,
                                                                                                                                                                                                        costLower,
                                                                                                                                                                                                        sub_work );
#ifdef DISPLAY
                                        std::cerr << "Interation: " << iterCount + 1 << std::endl ;
                                        std::cerr << "Cost: " << *costLower << " -- " << *costUpper << std::endl ;
#endif

                                        if( costUpper != 0 && costLower != 0 )
                                                {
                                                        if( *costUpper - *costLower < gapTolAbs )
                                                                {
                                                                        status->code = QP_OK;
                                                                        status->termination = QP_OK_GAP_ABS;
                                                                        status->iterations = iterCount;
                                                                        break;
                                                                }
                                                        if( *costUpper - *costLower < gapTolRel * *costUpper )
                                                                {
                                                                        status->code = QP_OK;
                                                                        status->termination = QP_OK_GAP_REL;
                                                                        status->iterations = iterCount;
                                                                        break;
                                                                }
                                                }
                                        if( costUpper != 0 && *costUpper < stopUpper )
                                                {
                                                        status->code = QP_OK;
                                                        status->termination = QP_OK_UPPER;
                                                        status->iterations = iterCount;
                                                        break;
                                                }
                                        if( costLower != 0 && *costLower > stopLower )
                                                {
                                                        status->code = QP_OK;
                                                        status->termination = QP_OK_LOWER;
                                                        status->iterations = iterCount;
                                                        break;
                                                }
                                }
                        checkNextCost = true;
                        polytope_distance_generator_form_sub( dim,
                                                                                                                                                                                g1InactiveBegin, g1InactiveEnd,
                                                                                                                                                                                g2InactiveBegin, g2InactiveEnd,
                                                                                                                                                                                yDiff,
                                                                                                                                                                                sigmaTol,
                                                                                                                                                                                d1, d2,
                                                                                                                                                                                sub_work );
#ifdef DISPLAY
                        std::cerr << "Delta: " << Helpers::mathematicaFormat( nInactive1, 1, d1 )
                                                                << "   " << Helpers::mathematicaFormat( nInactive2, 1, d2 ) << std::endl ;
#endif

                        /* Compute squared norm of subproblem step. */
                        double stepNorm2 = 0;
                        {
                                double * src = d1;
                                double * end = d1 + nInactive1;
                                for( ; src != end; ++src )
                                        {
                                                stepNorm2 += (*src) * (*src);
                                        }
                        }
                        {
                                double * src = d2;
                                double * end = d2 + nInactive2;
                                for( ; src != end; ++src )
                                        {
                                                stepNorm2 += (*src) * (*src);
                                        }
                        }
                        if( stepNorm2 < xTol )
                                {
                                        /* No step taken in sub-problem.  Are there any constraints to drop? */
                                        /* Compute lagrange multipliers.
                                         * Note that the lagrange multipliers of the inactive constraints are only used for temporary storage.
                                         * Normalizing yDiff will only scale the Langrange multipliers, and by normalizing yDiff we give
                                         * the Lagrange multiplier tolerance (lmTol) a geometric interpretation; lmTol becomes a gap tolerance which
                                         * is linear in the distance (not distance squared).
                                         */
                                        polytope_distance_generator_form_diff( dim,
                                                                                                                                                                                                 g1InactiveBegin, g1InactiveEnd,
                                                                                                                                                                                                 g2InactiveBegin, g2InactiveEnd,
                                                                                                                                                                                                 x1short, x2short,
                                                                                                                                                                                                 yDiff );
                                        {
                                                /* Normalize yDiff. */
                                                double lengthSquared = 0;
                                                for( const double * src = yDiff; src != yDiffEnd; ++src )
                                                        {
                                                                lengthSquared += (*src) * (*src);
                                                        }
                                                const double yDiffScaling = 1. / sqrt( lengthSquared );
                                                for( double * dst = yDiff; dst != yDiffEnd; ++dst )
                                                        {
                                                                *dst *= yDiffScaling;
                                                        }
                                        }
                                        {
                                                /* Initialize with negative gradient. */
                                                double * lmDst = lm1;
                                                for( const double ** gSrc = g1InactiveBegin; gSrc != g1ActiveEnd; ++gSrc, ++lmDst )
                                                        {
                                                                double tmp = 0;
                                                                const double * ySrc = yDiff;
                                                                const double * src = *gSrc;
                                                                for( ; ySrc != yDiffEnd; ++ySrc, ++src )
                                                                        {
                                                                                tmp -= (*ySrc) * (*src);
                                                                        }
                                                                *lmDst = tmp;
                                                        }
                                                /* Let nu be the dual variable of the given equality constraint (all variables sum to 1). */
                                                double nuMax = -1;
                                                double nu = 0; /* Initialize to avoid compiler warnings. */
                                                double * lmInactiveEnd = lm1 + nInactive1;
                                                double * lmPtr = lm1;
                                                for( ; lmPtr != lmInactiveEnd; ++lmPtr )
                                                        {
                                                                double lmAbs = fabs( *lmPtr );
                                                                if( lmAbs > nuMax )
                                                                        {
                                                                                nuMax = lmAbs;
                                                                                nu = *lmPtr;
                                                                        }
                                                        }
                                                /* Then the remaining Lagrange multipliers follow. */
                                                double * lmActiveEnd = lm1 + n1;
                                                for( ; lmPtr != lmActiveEnd; ++lmPtr )
                                                        {
                                                                *lmPtr -= nu;
                                                        }
                                        }

                                        {
                                                /* Compute Lagrange multipliers for the second polytope in the same way... up to sign changes. */
                                                double * lmDst = lm2;
                                                for( const double ** gSrc = g2InactiveBegin; gSrc != g2ActiveEnd; ++gSrc, ++lmDst )
                                                        {
                                                                double tmp = 0;
                                                                const double * ySrc = yDiff;
                                                                const double * src = *gSrc;
                                                                for( ; ySrc != yDiffEnd; ++ySrc, ++src )
                                                                        {
                                                                                tmp += (*ySrc) * (*src); /* Note sign! */
                                                                        }
                                                                *lmDst = tmp;
                                                        }
                                                double nuMax = -1;
                                                double nu = 0; /* Initialize to avoid compiler warnings. */
                                                double * lmInactiveEnd = lm2 + nInactive2;
                                                double * lmPtr = lm2;
                                                for( ; lmPtr != lmInactiveEnd; ++lmPtr )
                                                        {
                                                                double lmAbs = fabs( *lmPtr );
                                                                if( lmAbs > nuMax )
                                                                        {
                                                                                nuMax = lmAbs;
                                                                                nu = *lmPtr;
                                                                        }
                                                        }
                                                double * lmActiveEnd = lm2 + n2;
                                                for( ; lmPtr != lmActiveEnd; ++lmPtr )
                                                        {
                                                                *lmPtr -= nu;
                                                        }
                                        }

#ifdef DISPLAY
                                        std::cerr << "lambda: " << Helpers::mathematicaFormat( n1 - nInactive1, 1, lm1 + nInactive1 )
                                                                                << "   " << Helpers::mathematicaFormat( n2 - nInactive2, 1, lm2 + nInactive2 ) << std::endl ;
#endif
#ifdef TRACEFIGS
                                        {
                                                const double * lambdaSrc = lm1 + nInactive1;
                                                for( const double ** gSrc = g1InactiveEnd; gSrc != g2ActiveEnd; ++gSrc, ++lambdaSrc )
                                                        {
                                                                if( gSrc == g1ActiveEnd )
                                                                        {
                                                                                gSrc = g2InactiveEnd - 1;
                                                                                lambdaSrc = lm2 + nInactive2 - 1;
                                                                                continue;
                                                                        }
                                                                std::cout << " << @nonstroking:[gray 0.5] | (newText << `" << std::scientific << std::setprecision(1) << *lambdaSrc << std::fixed << "´) >> [shift (0,3bp)+(" ;
                                                                for( const double * src = *gSrc; src != *gSrc + dim; ++src )
                                                                        {
                                                                                if( src != *gSrc )
                                                                                        {
                                                                                                std::cout << "," ;
                                                                                        }
                                                                                std::cout << "(" << *src << "bp)" ;
                                                                        }
                                                                std::cout << ")]" << std::endl ;
                                                        }
                                        }
#endif

                                        bool dropped = false;
                                        {
                                                const double * lmSrc = lm1 + nInactive1;
                                                const double * lmEnd = lm1 + n1;
                                                const double ** gSrc = g1InactiveEnd;
                                                double * xDst = x1 + nInactive1;
                                                for( ; lmSrc != lmEnd; ++lmSrc, ++gSrc )
                                                        {
                                                                if( *lmSrc < -lmTol )
                                                                        {
                                                                                /* Drop constraint.
                                                                                 * Swap the generator out of the active set, so that it appears at the end
                                                                                 * of the inactive set.
                                                                                 */
                                                                                const double * gTmp = *g1InactiveEnd;
                                                                                *g1InactiveEnd = *gSrc;
                                                                                *gSrc = gTmp;
                                                                                ++g1InactiveEnd;

                                                                                /* Since the constraint has been active, the corresponding variable has been zero.
                                                                                 */
                                                                                *xDst = 0;
                                                                                ++xDst;

                                                                                dropped = true;
                                                                        }
                                                        }
                                        }
                                        {
                                                const double * lmSrc = lm2 + nInactive2;
                                                const double * lmEnd = lm2 + n2;
                                                const double ** gSrc = g2InactiveEnd;
                                                double * xDst = x2 + nInactive2;
                                                for( ; lmSrc != lmEnd; ++lmSrc, ++gSrc )
                                                        {
                                                                if( *lmSrc < -lmTol )
                                                                        {
                                                                                /* Drop constraint.
                                                                                 * Swap the generator out of the active set, so that it appears at the end
                                                                                 * of the inactive set.
                                                                                 */
                                                                                const double * gTmp = *g2InactiveEnd;
                                                                                *g2InactiveEnd = *gSrc;
                                                                                *gSrc = gTmp;
                                                                                ++g2InactiveEnd;

                                                                                /* Since the constraint has been active, the corresponding variable has been zero.
                                                                                 */
                                                                                *xDst = 0;
                                                                                ++xDst;

                                                                                dropped = true;
                                                                        }
                                                        }
                                        }
                                        if( ! dropped )
                                                {
                                                        /* We reached optimum!
                                                         */
                                                        status->code = QP_OK;
                                                        status->termination = QP_OK_LM;
                                                        status->iterations = iterCount;
                                                        break;
                                                }
                                        /* If we dropped some constraint, we just return to the top of the loop.
                                         * No need to re-normalize variables since added variables are exactly zero.
                                         */
                                        checkNextCost = false; /* No need to check the cost in the next iteration since no step was made. */
                                }
                        else
                                {
                                        /* Find inactive constraints that limit the step length in each polytope. */
                                        double s1 = 1;
                                        const double ** gStop1 = 0;
                                        {
                                                const double ** gSrc = g1InactiveBegin;
                                                const double ** gEnd = g1InactiveEnd;
                                                const double * dSrc = d1;
                                                const double * xSrc = x1;
                                                for( ; gSrc != gEnd; ++gSrc, ++dSrc, ++xSrc )
                                                        {
                                                                double denom = -*dSrc;
                                                                if( denom > 0 )
                                                                        {
                                                                                double sTmp = *xSrc / denom;
                                                                                if( sTmp < s1 )
                                                                                        {
                                                                                                s1 = sTmp;
                                                                                                gStop1 = gSrc;
                                                                                        }
                                                                        }
                                                        }
                                        }
                                        double s2 = 1;
                                        const double ** gStop2 = 0;
                                        {
                                                const double ** gSrc = g2InactiveBegin;
                                                const double ** gEnd = g2InactiveEnd;
                                                const double * dSrc = d2;
                                                const double * xSrc = x2;
                                                for( ; gSrc != gEnd; ++gSrc, ++dSrc, ++xSrc )
                                                        {
                                                                double denom = -*dSrc;
                                                                if( denom > 0 )
                                                                        {
                                                                                double sTmp = *xSrc / denom;
                                                                                if( sTmp < s2 )
                                                                                        {
                                                                                                s2 = sTmp;
                                                                                                gStop2 = gSrc;
                                                                                        }
                                                                        }
                                                        }
                                        }

                                        /* In a basic active set algorithm, one uses only the smaller of s1 and s2, but here
                                         * we actually use both, if that gives further decrease in the objective function.
                                         */
                                        double sShort = ( s1 < s2 ) ? s1 : s2;
                                        {
                                                const double * xSrc = x1;
                                                const double * xEnd = x1 + nInactive1;
                                                const double * dSrc = d1;
                                                double * dst = x1short;
                                                for( ; xSrc != xEnd; ++xSrc, ++dSrc, ++dst )
                                                        {
                                                                *dst = *xSrc + sShort * (*dSrc);
                                                        }
                                        }
                                        {
                                                const double * xSrc = x2;
                                                const double * xEnd = x2 + nInactive2;
                                                const double * dSrc = d2;
                                                double * dst = x2short;
                                                for( ; xSrc != xEnd; ++xSrc, ++dSrc, ++dst )
                                                        {
                                                                *dst = *xSrc + sShort * (*dSrc);
                                                        }
                                        }
                                        double shortCost = 0;
                                        polytope_distance_generator_form_diff( dim,
                                                                                                                                                                                                 g1InactiveBegin, g1InactiveEnd,
                                                                                                                                                                                                 g2InactiveBegin, g2InactiveEnd,
                                                                                                                                                                                                 x1short, x2short,
                                                                                                                                                                                                 yDiff );
                                        for( const double * src = yDiff; src != yDiffEnd; ++src )
                                                {
                                                        shortCost += (*src) * (*src);
                                                }

                                        if( s1 > s2 )
                                                {
                                                        const double * xSrc = x1;
                                                        const double * xEnd = x1 + nInactive1;
                                                        const double * dSrc = d1;
                                                        double * dst = x1long;
                                                        for( ; xSrc != xEnd; ++xSrc, ++dSrc, ++dst )
                                                                {
                                                                        *dst = *xSrc + s1 * (*dSrc);
                                                                }
                                                        double longCost = 0;
                                                        polytope_distance_generator_form_diff( dim,
                                                                                                                                                                                                                 g1InactiveBegin, g1InactiveEnd,
                                                                                                                                                                                                                 g2InactiveBegin, g2InactiveEnd,
                                                                                                                                                                                                                 x1long, x2short,
                                                                                                                                                                                                                 yDiff );
                                                        for( const double * src = yDiff; src != yDiffEnd; ++src )
                                                                {
                                                                        longCost += (*src) * (*src);
                                                                }

                                                        if( longCost > shortCost )
                                                                {
                                                                        /* Dont make long step. */
                                                                        gStop1 = 0;
                                                                        s1 = s2;
                                                                }
                                                }
                                        else if( s2 > s1 )
                                                {
                                                        const double * xSrc = x2;
                                                        const double * xEnd = x2 + nInactive2;
                                                        const double * dSrc = d2;
                                                        double * dst = x2long;
                                                        for( ; xSrc != xEnd; ++xSrc, ++dSrc, ++dst )
                                                                {
                                                                        *dst = *xSrc + s2 * (*dSrc);
                                                                }
                                                        double longCost = 0;
                                                        polytope_distance_generator_form_diff( dim,
                                                                                                                                                                                                                 g1InactiveBegin, g1InactiveEnd,
                                                                                                                                                                                                                 g2InactiveBegin, g2InactiveEnd,
                                                                                                                                                                                                                 x1short, x2long,
                                                                                                                                                                                                                 yDiff );
                                                        for( const double * src = yDiff; src != yDiffEnd; ++src )
                                                                {
                                                                        longCost += (*src) * (*src);
                                                                }

                                                        if( longCost > shortCost )
                                                                {
                                                                        /* Dont make long step. */
                                                                        gStop2 = 0;
                                                                        s2 = s1;
                                                                }
                                                }

                                        /* Update variables and active constraints.
                                         * Important: variables first, or we will get a mess!
                                         */
                                        {
                                                const double * xSrc = x1;
                                                const double * xEnd = x1 + nInactive1;
                                                const double * dSrc = d1;
                                                double * dst = x1;
                                                for( ; xSrc != xEnd; ++xSrc, ++dSrc, ++dst )
                                                        {
                                                                *dst = *xSrc + s1 * (*dSrc);
                                                        }
                                        }
                                        if( gStop1 != 0 )
                                                {
                                                        /* Swap constraints, and swap variables in the same way.  New variable is zero by construction. */
                                                        --g1InactiveEnd;
                                                        const double * gTmp = *gStop1;
                                                        *gStop1 = *g1InactiveEnd;
                                                        *g1InactiveEnd = gTmp;
                                                        *( x1 + ( gStop1 - g1InactiveBegin ) ) = *( x1 + ( g1InactiveEnd - g1InactiveBegin ) );
                                                }
                                        {
                                                const double * xSrc = x2;
                                                const double * xEnd = x2 + nInactive2;
                                                const double * dSrc = d2;
                                                double * dst = x2;
                                                for( ; xSrc != xEnd; ++xSrc, ++dSrc, ++dst )
                                                        {
                                                                *dst = *xSrc + s2 * (*dSrc);
                                                        }
                                        }
                                        if( gStop2 != 0 )
                                                {
                                                        /* Swap constraints, and swap variables in the same way.  New variable is zero by construction. */
                                                        --g2InactiveEnd;
                                                        const double * gTmp = *gStop2;
                                                        *gStop2 = *g2InactiveEnd;
                                                        *g2InactiveEnd = gTmp;
                                                        *( x2 + ( gStop2 - g2InactiveBegin ) ) = *( x2 + ( g2InactiveEnd - g2InactiveBegin ) );
                                                }
                                }
                }

#ifdef TRACEFIGS
        if( tracefigPageOpen )
                {
                        std::cout << ")" << std::endl ;
                        tracefigPageOpen = false;
                }
#endif

        /* Compute final cost. */
        {
                polytope_distance_generator_form_diff( dim,
                                                                                                                                                                         g1InactiveBegin, g1InactiveEnd,
                                                                                                                                                                         g2InactiveBegin, g2InactiveEnd,
                                                                                                                                                                         x1, x2,
                                                                                                                                                                         yDiff );
                polytope_distance_generator_form_costs( dim,
                                                                                                                                                                                g1InactiveBegin, g1ActiveEnd,
                                                                                                                                                                                g2InactiveBegin, g2ActiveEnd,
                                                                                                                                                                                yDiff,
                                                                                                                                                                                costUpper,
                                                                                                                                                                                costLower,
                                                                                                                                                                                sub_work );
#ifdef DISPLAY
                std::cerr << "Terminating with final cost: " << *costLower << " -- " << *costUpper << std::endl ;
#endif
        }

        /* Compute final point in each polytope. */
        if( y1 != 0 )
                {
                        double * y = y1;
                        double * yEnd = y + dim;
                        for( double * dst = y; dst != yEnd; ++dst )
                                {
                                        *dst = 0;
                                }
                        const double * xSrc = x1;
                        for( const double ** gSrc = g1InactiveBegin; gSrc != g1InactiveEnd; ++gSrc, ++xSrc )
                                {
                                        double xVal = *xSrc;
                                        const double * src = *gSrc;
                                        for( double * dst = y; dst != yEnd; ++dst, ++src )
                                                {
                                                        *dst += xVal * (*src);
                                                }
                                }
                }
        if( y2 != 0 )
                {
                        double * y = y2;
                        double * yEnd = y + dim;
                        for( double * dst = y; dst != yEnd; ++dst )
                                {
                                        *dst = 0;
                                }
                        const double * xSrc = x2;
                        for( const double ** gSrc = g2InactiveBegin; gSrc != g2InactiveEnd; ++gSrc, ++xSrc )
                                {
                                        double xVal = *xSrc;
                                        const double * src = *gSrc;
                                        for( double * dst = y; dst != yEnd; ++dst, ++src )
                                                {
                                                        *dst += xVal * (*src);
                                                }
                                }
                }
}

void
Computation::polytope_distance_generator_form_diff( const size_t dim,
                                                                                                                                                                                                                const double ** g1Begin, const double ** g1End,
                                                                                                                                                                                                                const double ** g2Begin, const double ** g2End,
                                                                                                                                                                                                                const double * x1, const double * x2,
                                                                                                                                                                                                                double * yDiff )
{
        double * yDiffEnd = yDiff + dim;
        for( double * dst = yDiff; dst != yDiffEnd; ++dst )
                {
                        *dst = 0;
                }
        {
                const double * xSrc = x2;
                for( const double ** gSrc = g2Begin; gSrc != g2End; ++gSrc, ++xSrc )
                        {
                                double xVal = *xSrc;
                                const double * src = *gSrc;
                                for( double * dst = yDiff; dst != yDiffEnd; ++dst, ++src )
                                        {
                                                *dst += xVal * (*src);
                                        }
                        }
        }
        {
                const double * xSrc = x1;
                for( const double ** gSrc = g1Begin; gSrc != g1End; ++gSrc, ++xSrc )
                        {
                                double xVal = *xSrc;
                                const double * src = *gSrc;
                                for( double * dst = yDiff; dst != yDiffEnd; ++dst, ++src )
                                        {
                                                *dst -= xVal * (*src);
                                        }
                        }
        }
}

void
Computation::polytope_distance_generator_form_costs( const size_t dim,
                                                                                                                                                                                                                 const double ** g1Begin, const double ** g1End,
                                                                                                                                                                                                                 const double ** g2Begin, const double ** g2End,
                                                                                                                                                                                                                 const double * yDiff,
                                                                                                                                                                                                                 double * primalCost,
                                                                                                                                                                                                                 double * dualCostAccum,
                                                                                                                                                                                                                 double * work )
{
        const double * yDiffEnd = yDiff + dim;

        double pCost = 0;
        for( const double * src = yDiff; src != yDiffEnd; ++src )
                {
                        pCost += (*src) * (*src);
                }

        if( primalCost != 0 )
                {
                        *primalCost = pCost;
                }

        if( dualCostAccum == 0 )
                {
                        return;
                }

        double * n = work; /* Normalaized normal to planes separating the two polytopes. */
        double * nEnd = work + dim;
        {
                /* Normalize yDiff and store in n. */
                double yDiffScale = 1. / sqrt( pCost );
                double * dst = n;
                for( const double * src = yDiff; src != yDiffEnd; ++src, ++dst )
                        {
                                *dst = yDiffScale * (*src);
                        }
        }

        /* Find dual cost by projecting generators onto n. */
        double min2 = std::numeric_limits< double >::max( );
        {
                for( const double ** gSrc = g2Begin; gSrc != g2End; ++gSrc )
                        {
                                double tmp = 0;
                                const double * src = *gSrc;
                                for( const double * nSrc = n; nSrc != nEnd; ++nSrc, ++src )
                                        {
                                                tmp += (*nSrc) * (*src);
                                        }
                                min2 = std::min( min2, tmp );
                        }
        }

        double max1 = -std::numeric_limits< double >::max( );
        {
                for( const double ** gSrc = g1Begin; gSrc != g1End; ++gSrc )
                        {
                                double tmp = 0;
                                const double * src = *gSrc;
                                for( const double * nSrc = n; nSrc != nEnd; ++nSrc, ++src )
                                        {
                                                tmp += (*nSrc) * (*src);
                                        }
                                max1 = std::max( max1, tmp );
                        }
        }

        double diff = min2 - max1;
        if( diff > 0 )
                {
                        *dualCostAccum = std::max( *dualCostAccum, diff * diff );
                }
}


void
Computation::polytope_distance_generator_form_sub( const size_t dim,
                                                                                                                                                                                                         const double ** g1Begin, const double ** g1End,
                                                                                                                                                                                                         const double ** g2Begin, const double ** g2End,
                                                                                                                                                                                                         const double * yDiff,
                                                                                                                                                                                                         double sigmaTol,
                                                                                                                                                                                                         double * d1, double * d2,
                                                                                                                                                                                                         double * work )
{
        size_t n1 = g1End - g1Begin;
        size_t n2 = g2End - g2Begin;
        double * d1End = d1 + n1;
        double * d2End = d2 + n2;
        for( double * dst = d1; dst != d1End; ++dst )
                {
                        *dst = 0;
                }
        for( double * dst = d2; dst != d2End; ++dst )
                {
                        *dst = 0;
                }
        if( n1 == 1 && n2 == 1 ) /* They must never be less than 1! */
                {
                        /* If there is only one inactive constraint in a polytope, the corresponding variable is in fact locked to 1.
                         * Henece, we cannot move in that polytope.  If we can't move in either polytope, there is nothing to optimize.
                         * We have already set the result to 0.
                         */
                        return;
                }
        const double ** N1 = Computation::polytope_distance_generator_form_kernel( n1 );
        const double ** N2 = Computation::polytope_distance_generator_form_kernel( n2 );

        gsl_vector y_gsl;
        y_gsl.data = const_cast< double * >( yDiff );
        y_gsl.size = dim;
        y_gsl.stride = 1;

        gsl_vector d_gsl;
        d_gsl.data = work;
        work += ( d_gsl.size = n1 + n2 - 2 );
        d_gsl.stride = 1;

        if( dim >= n1 + n2 - 2 )
                {
                        /* Over-determined case.  SVD of A can be computed as usual. */
                        gsl_matrix A;
                        A.data = work;
                        work += ( A.size1 = dim ) * ( A.size2 = A.tda = n1 + n2 - 2 );
                        gsl_matrix V;
                        V.data = work;
                        work += ( V.size1 = n1 + n2 - 2 ) * ( V.size2 = V.tda = n1 + n2 - 2 );
                        gsl_vector S;
                        S.data = work;
                        work += ( S.size = n1 + n2 - 2 );
                        S.stride = 1;
                        gsl_vector SV_work;
                        SV_work.data = work;
                        work += ( SV_work.size = n1 + n2 - 2 );
                        SV_work.stride = 1;

                        /* Populate A with data.
                         * The data access here can be improved. */
                        {
                                /* Polytope 1 */
                                double * ARowStart = A.data;
                                for( size_t row = 0; row < dim; ++row, ARowStart += A.tda )
                                        {
                                                const double ** NcolStart = N1;
                                                const double ** NcolEnd = N1 + n1 - 1;
                                                double * Adst = ARowStart;
                                                for( ; NcolStart != NcolEnd; ++NcolStart, ++Adst )
                                                        {
                                                                double tmp = 0;
                                                                const double ** gcolStart = g1Begin;
                                                                const double ** gcolEnd = g1End;
                                                                const double * Nsrc = *NcolStart;
                                                                for( ; gcolStart != gcolEnd; ++gcolStart, ++Nsrc )
                                                                        {
                                                                                tmp += (*((*gcolStart) + row)) * (*Nsrc);
                                                                        }
                                                                *Adst = tmp;
                                                        }
                                        }
                        }
                        {
                                /* Polytope 2 */
                                double * ARowStart = A.data + ( n1 - 1 );
                                for( size_t row = 0; row < dim; ++row, ARowStart += A.tda )
                                        {
                                                const double ** NcolStart = N2;
                                                const double ** NcolEnd = N2 + n2 - 1;
                                                double * Adst = ARowStart;
                                                for( ; NcolStart != NcolEnd; ++NcolStart, ++Adst )
                                                        {
                                                                double tmp = 0;
                                                                const double ** gcolStart = g2Begin;
                                                                const double ** gcolEnd = g2End;
                                                                const double * Nsrc = *NcolStart;
                                                                for( ; gcolStart != gcolEnd; ++gcolStart, ++Nsrc )
                                                                        {
                                                                                tmp -= (*((*gcolStart) + row)) * (*Nsrc); /* Note the sign! */
                                                                        }
                                                                *Adst = tmp;
                                                        }
                                        }
                        }

                        gsl_linalg_SV_decomp( & A, & V, & S, & SV_work );
                        {
                                /* Truncate small singular values. */
                                double * dst = S.data;
                                double * end = S.data + S.size;
                                const double sigmaCut = sigmaTol * (*dst);
                                ++dst;
                                for( ; dst != end; ++dst )
                                        {
                                                if( *dst < sigmaCut )
                                                        {
                                                                break;
                                                        }
                                        }
                                for( ; dst != end; ++dst )
                                        {
                                                *dst = 0;
                                        }
                        }
                        gsl_linalg_SV_solve( & A, & V, & S, & y_gsl, & d_gsl );
                }
        else
                {
                        /* Under-determined case.  Need to transpose A to compute SVD. */
                        gsl_matrix A;
                        A.data = work;
                        work += ( A.size1 = n1 + n2 - 2 ) * ( A.size2 = A.tda = dim );
                        gsl_matrix V;
                        V.data = work;
                        work += ( V.size1 = dim ) * ( V.size2 = V.tda = dim );
                        gsl_vector S;
                        S.data = work;
                        work += ( S.size = dim );
                        S.stride = 1;
                        gsl_vector SV_work;
                        SV_work.data = work;
                        work += ( SV_work.size = dim );
                        SV_work.stride = 1;

                        /* Populate A with data.
                         * The data access here can be improved. */
                        {
                                /* Polytope 1 */
                                double * AColStart = A.data;
                                for( size_t row = 0; row < dim; ++row, ++AColStart )
                                        {
                                                const double ** NcolStart = N1;
                                                const double ** NcolEnd = N1 + n1 - 1;
                                                double * Adst = AColStart;
                                                for( ; NcolStart != NcolEnd; ++NcolStart, Adst += A.tda )
                                                        {
                                                                double tmp = 0;
                                                                const double ** gcolStart = g1Begin;
                                                                const double ** gcolEnd = g1End;
                                                                const double * Nsrc = *NcolStart;
                                                                for( ; gcolStart != gcolEnd; ++gcolStart, ++Nsrc )
                                                                        {
                                                                                tmp += (*((*gcolStart) + row)) * (*Nsrc);
                                                                        }
                                                                *Adst = tmp;
                                                        }
                                        }
                        }
                        {
                                /* Polytope 2 */
                                double * AColStart = A.data + ( n1 - 1 ) * A.tda;
                                for( size_t row = 0; row < dim; ++row, ++AColStart )
                                        {
                                                const double ** NcolStart = N2;
                                                const double ** NcolEnd = N2 + n2 - 1;
                                                double * Adst = AColStart;
                                                for( ; NcolStart != NcolEnd; ++NcolStart, Adst += A.tda )
                                                        {
                                                                double tmp = 0;
                                                                const double ** gcolStart = g2Begin;
                                                                const double ** gcolEnd = g2End;
                                                                const double * Nsrc = *NcolStart;
                                                                for( ; gcolStart != gcolEnd; ++gcolStart, ++Nsrc )
                                                                        {
                                                                                tmp -= (*((*gcolStart) + row)) * (*Nsrc); /* Note the sign! */
                                                                        }
                                                                *Adst = tmp;
                                                        }
                                        }
                        }

                        gsl_linalg_SV_decomp( & A, & V, & S, & SV_work );
                        {
                                /* Truncate small singular values. */
                                double * dst = S.data;
                                double * end = S.data + S.size;
                                const double sigmaCut = sigmaTol * (*dst);
                                ++dst;
                                for( ; dst != end; ++dst )
                                        {
                                                if( *dst < sigmaCut )
                                                        {
                                                                break;
                                                        }
                                        }
                                for( ; dst != end; ++dst )
                                        {
                                                *dst = 0;
                                        }
                        }
                        /* Now, instead of using
                         *   gsl_linalg_SV_solve( & A, & V, & S, & y_gsl, & d_gsl );
                         * we must solve the equation ourselves since the decomposition is transposed.
                         */
                        gsl_blas_dgemv( CblasTrans, 1, & V, & y_gsl, 0, & SV_work );
                        {
                                const double * src = S.data;
                                const double * srcEnd = S.data + S.size;
                                double * dst = SV_work.data;
                                for( ; src != srcEnd; ++src, ++dst )
                                        {
                                                if( *src > 0 )
                                                        {
                                                                *dst /= *src;
                                                        }
                                                else
                                                        {
                                                                *dst = 0;
                                                        }
                                        }
                        }
                        gsl_blas_dgemv( CblasNoTrans, 1, & A, & SV_work, 0, & d_gsl );
                }

        {
                /* Compute output variables. */
                /* First clear. */
                {
                        double * dst = d1;
                        double * dstEnd = d1 + n1;
                        for( ; dst != dstEnd; ++dst )
                                {
                                        *dst = 0;
                                }
                }
                {
                        double * dst = d2;
                        double * dstEnd = d2 + n2;
                        for( ; dst != dstEnd; ++dst )
                                {
                                        *dst = 0;
                                }
                }

                /* Then add linear combination of basis vectors. */
                const double * dSrc = d_gsl.data;
                {
                        const double ** NSrc = N1;
                        const double ** NSrcEnd = N1 + ( n1 - 1 );
                        for( ; NSrc != NSrcEnd; ++NSrc, ++dSrc )
                                {
                                        double w = *dSrc;
                                        double * dst = d1;
                                        double * dstEnd = d1 + n1;
                                        const double * src = *NSrc;
                                        for( ; dst != dstEnd; ++dst, ++src )
                                                {
                                                        *dst += w * *src;
                                                }
                                }
                }
                {
                        const double ** NSrc = N2;
                        const double ** NSrcEnd = N2 + ( n2 - 1 );
                        for( ; NSrc != NSrcEnd; ++NSrc, ++dSrc )
                                {
                                        double w = *dSrc;
                                        double * dst = d2;
                                        double * dstEnd = d2 + n2;
                                        const double * src = *NSrc;
                                        for( ; dst != dstEnd; ++dst, ++src )
                                                {
                                                        *dst += w * *src;
                                                }
                                }
                }
        }
}

void
Computation::polytope_distance_generator_form_sub_Gilbert( const size_t dim,
                                                                                                                                                                                                                                         const double ** g1Begin, const double ** g1End,
                                                                                                                                                                                                                                         const double ** g2Begin, const double ** g2End,
                                                                                                                                                                                                                                         const double * yDiff,
                                                                                                                                                                                                                                         double * d1, double * d2 )
{

}

const double **
Computation::polytope_distance_generator_form_kernel( size_t n )
{
        static std::vector< const double ** > mem( 0 );
        if( n < mem.size( ) )
                {
                        return mem[ n ];
                }
        mem.reserve( n + 1 );
        while( mem.size( ) <= n )
                {
                        size_t m = mem.size( );
                        if( m == 0 )
                                {
                                        /* Invalid dimension, just skip. */
                                        mem.push_back( 0 );
                                        continue;
                                }
                        if( m == 1 )
                                {
                                        /* Trivial case; kernel has dimension 0 */
                                        mem.push_back( 0 );
                                        continue;
                                }
                        const double ** res = new const double * [ m - 1 ];
                        gsl_matrix * qr = gsl_matrix_alloc( m, m );
                        gsl_vector * tau = gsl_vector_alloc( m );
                        gsl_matrix_set_zero( qr );
                        for( size_t i = 0; i < m; ++i )
                                {
                                        gsl_matrix_set( qr, i, 0, 1. );
                                }
                        gsl_matrix * q = gsl_matrix_alloc( m, m );
                        gsl_matrix * r = gsl_matrix_alloc( m, m );

                        gsl_linalg_QR_decomp( qr, tau );
                        gsl_linalg_QR_unpack( qr, tau, q, r );
                        {
                                const double ** dst = res;
                                const double ** dstEnd = res + ( m - 1 );
                                double * srcQStart = q->data + 1; /* Skip first column. */
                                for( ; dst != dstEnd; ++dst, ++srcQStart )
                                        {
                                                double * v = new double[ m ];
                                                *dst = v;
                                                double * vEnd = v + m;
                                                double * srcQ = srcQStart;
                                                for( ; v != vEnd; ++v, srcQ += q->tda )
                                                        {
                                                                *v = *srcQ;
                                                        }
                                        }
                        }

                        mem.push_back( res );

                        gsl_matrix_free( qr );
                        gsl_vector_free( tau );
                        gsl_matrix_free( q );
                        gsl_matrix_free( r );
                }
        return mem[ n ];
}