urotorder.c

matlab中uwb波形优化算法经常会使用的工具包:SeDuMi_1_1R3.

/*
%                         [u,perm,gjc,g] = urotorder(u,K, maxu,permIN)
% UROTORDER  Stable reORDERing of triu U-factor by Givens ROTations.
%
% SEE ALSO sedumi
% **********  INTERNAL FUNCTION OF SEDUMI **********
function [u,perm,gjc,g] = urotorder(u,K, maxu,permIN)

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program 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 2 of the License, or
% (at your option) any later version.
%
% This program 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 this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA

 */

#include <string.h>
#include <math.h>
#include "mex.h"
#include "blksdp.h"
#include "givens.h"

#define U_OUT myplhs[0]
#define PERM_OUT myplhs[1]
#define GJC_OUT myplhs[2]
#define G_OUT myplhs[3]
#define NPAROUT 4

#define U_IN prhs[0]
#define K_IN prhs[1]
#define MAXU_IN prhs[2]
#define NPARINMIN 3
#define PERM_IN prhs[3]
#define NPARIN 4

/* ============================================================
   TYPE DEFINITIONS:
   ============================================================ */
/* controls when d's are recomputed from scratch. Not critical, since
   d's are used only for SELECTING pivots. Just need avoid full underflow. */
#define DRELTOL 1E-10

/* ************************************************************
   PROCEDURE rotorder
   UPDATED
     u - full n x n matrix. On input, triu(u) is possibly unstable factor.
        On output, triu(u(:,perm)) is a stable factor. U_OUT = Q*U_IN,
        where Q is a sequence of givens rotations, given in g.
   OUTPUT
     perm - length n stable (column) pivot ordering.
     gjc - The givens rotations at step k are g[gjc[k]:gjc[k+1]-1].
       The order in each column is bottom up.
     g - length gjc[n] <= n(n-1)/2 array of givens rotations.
      At worst we need n-1-k rotations in iter k=0:n-2.
   ************************************************************ */
void rotorder(int *perm, double *u, int *gjc, twodouble *g, double *d,
              const double maxusqr, const int n)
{
  int i,j,k,inz, pivk, m;
  double *uj, *rowuk;
  double dk,y,nexty, h, uki,ukmax;
  twodouble gi;
/* ------------------------------------------------------------
   Initialize:
   Let perm = 1:n, inz = 0. (inz points into rotation list r)
   Let d(0) = 0, h = 1: this will let us compute all d's (since d(0)<h).
   ------------------------------------------------------------ */
  for(j = 0; j < n; j++)
    perm[j] = j;
  inz = 0;
  d[0] = 0.0; h = 1.0;
  for(k = 0, rowuk = u; k < n-1; k++, rowuk++){
    gjc[k] = inz;
/* ------------------------------------------------------------
   If current d's are not reliable then
   compute d(i) = sum(u(k:n-1,i).^2) from scratch.
   ------------------------------------------------------------ */
    if(d[perm[k]] <= h){
      for(j = k; j < n; j++){
        i = perm[j];
        d[i] = realssqr(rowuk + i*n,j+1-k);
      }
      h = d[perm[k]] * DRELTOL;
    }
/* ------------------------------------------------------------
   Let ukmax = max(U(k,perm(k+1:n)).^2)
   ------------------------------------------------------------ */
    ukmax = 0.0;
    for(j = k + 1; j < n; j++){
      uki = rowuk[perm[j] * n];
      uki *= uki;
      ukmax = MAX(ukmax, uki);
    }
/* ------------------------------------------------------------
   If ukmax >  maxusqr * d(k), then pivot k is unstable.
   If so, find best pivot: (pivk, dk) = max(perm(d(k:n))).
   ------------------------------------------------------------ */
    if(ukmax > maxusqr * d[perm[k]]){
      dk = 0.0;
      for(j = k+1; j < n; j++)
        if(d[perm[j]] > dk){
          pivk = j;
          dk = d[perm[j]];
        }
/* ------------------------------------------------------------
   Pivot on column pivk, and make U(:,perm)
   upper-triangular by pivk - k givens rotations on U(:,perm(k:n)).
   Givens at row i is {u(i,j), norm( u(i+1:pivk,j) )} for
   j=perm[pivk] and i = k:pivk-1.
   ------------------------------------------------------------ */
      m = pivk - k;                    /* number of Givens rotations needed */
      j = perm[pivk];                  /* uj(1:m) should become 0 */
      uj = rowuk + j * n;
      nexty = uj[m];                   /* last nonzero in col uj */
      y = SQR(nexty);
      for(i = m-1; i >= 0; i--){
        gi.x = uj[i];
        gi.y = nexty;
        y += SQR(gi.x);
        nexty = sqrt(y);
        gi.x /= nexty;                  /* Normalize to rotation [x,y; y,-x] */
        gi.y /= nexty;
        g[i] = gi;
      }                                /* y == d[j] after loop */
      uj[0] = nexty;                   /* New pivotal diagonal entry */
/* ------------------------------------------------------------
   move pivot j=perm[pivk] to head of perm (shifting old k:pivk-1)
   ------------------------------------------------------------ */
      memmove(perm+k+1, perm+k, m * sizeof(int));     /* move 1-> */
      perm[k] = j;                     /* inserted at k */
/* ------------------------------------------------------------
   Apply rotations to columns perm(k+1:n-1).
   Apply 1,2,...,m rotations on column k+1,..,k+m=pivk,
   and m rotations on cols pivk+1:n-1.
   ------------------------------------------------------------ */
      for(i = 1; i <= m; i++)
        givensrotuj(rowuk + perm[k+i] * n, g,i);
      for(i += k; i < n; i++)
        givensrot(rowuk + perm[i] * n, g,m);
      inz += m;                         /* point to next avl. place */
      g += m;
/* ------------------------------------------------------------
   Update d(perm(k+1:n)) -= u(k,perm(k+1:n)).^2.
   ------------------------------------------------------------ */
      for(j = k + 1; j < n; j++){
        i = perm[j];
        d[i] -= SQR(rowuk[i * n]);
      }
    }
  }
/* ------------------------------------------------------------
   We have reordered n-1 columns of U using inz Givens-rotations.
   ------------------------------------------------------------ */
  mxAssert(n > 0,"");
  gjc[n-1] = inz;
}

/* ************************************************************
   PROCEDURE prpirotorder
   UPDATED
     u,upi - full n x n matrix. On input, triu(u) is possibly unstable factor.
        u is triu, real diagonal.
        On output, triu(u(:,perm)) is a stable factor. U_OUT = Q*U_IN,
        where Q is a sequence of givens rotations, given in g.
        u remains triu, real diagonal.
   OUTPUT
     perm - length n stable (column) pivot ordering.
     gjc - The givens rotations at step k are g[gjc[k]:gjc[k+1]-1].
       The order in each column is bottom up.
     g - length gjc[n] <= n(n-1)/2 array of givens rotations.
      At worst we need n-1-k rotations in iter k=0:n-2.
   ************************************************************ */
void prpirotorder(int *perm, double *u,double *upi, int *gjc,
                  tridouble *g, double *d,
                  const double maxusqr, const int n)
{
  int i,j,k,inz, pivk, m;
  double *uj,*ujpi, *rowuk, *rowukpi;
  double dk,y,nexty, h, uki,ukiim,ukmax;
  tridouble gi;
/* ------------------------------------------------------------
   Initialize:
   Let perm = 1:n, inz = 0. (inz points into rotation list r)
   Let d(0) = 0, h = 1: this will let us compute all d's (since d(0)<h).
   ------------------------------------------------------------ */
  for(j = 0; j < n; j++)
    perm[j] = j;
  inz = 0;
  d[0] = 0.0; h = 1.0;
  for(k = 0, rowuk = u, rowukpi = upi; k < n-1; k++, rowuk++, rowukpi++){
    gjc[k] = inz;
/* ------------------------------------------------------------
   If current d's are not reliable then
   compute d(i) = sum(u(k:n-1,i).^2) from scratch.
   ------------------------------------------------------------ */
    if(d[perm[k]] <= h){
      for(j = k; j < n; j++){
        i = perm[j];                /* diag entry u(j,i) is real */
        d[i] = realssqr(rowuk + i*n,j+1-k) + realssqr(rowukpi + i*n,j-k);
      }
      h = d[perm[k]] * DRELTOL;
    }
/* ------------------------------------------------------------
   Let ukmax = max(abs(U(k,perm(k+1:n))).^2)
   ------------------------------------------------------------ */
    ukmax = 0.0;
    for(j = k + 1; j < n; j++){
      uki = rowuk[perm[j] * n];
      ukiim = rowukpi[perm[j] * n];
      ukmax = MAX(ukmax, SQR(uki) + SQR(ukiim));
    }
/* ------------------------------------------------------------
   If ukmax >  maxusqr * d(k), then pivot k is unstable.
   If so, find best pivot: (pivk, dk) = max(perm(d(k:n))).
   ------------------------------------------------------------ */
    if(ukmax > maxusqr * d[perm[k]]){
      dk = 0.0;
      for(j = k+1; j < n; j++)
        if(d[perm[j]] > dk){
          pivk = j;
          dk = d[perm[j]];
        }