svdcmp.c

统计模式识别算法包

/******************************************************************************/
/*                                                                            */
/*  SVDCMP  -  SingularValueDecomp object routines for performing singular    */
/*             value decomposition on a matrix, and using backsubstitution    */
/*             to find least squares solutions to simultaneous equations.     */
/*                                                                            */
/*             The decomposition algorithm is yet another implementation of   */
/*             the classic method of Golub and Reinsch (Wilkinson, J.H. and   */
/*             Reinsch, C., 1971, 'Handbook for Automatic Computation' vol. 2)*/
/*             Some tricks have been taken from later sources.  See (Press    */
/*             et al 'Numerical Recipes in C') for a complete list of         */
/*             references.                                                    */
/*                                                                            */
/* Copyright (c) 1993 by Academic Press, Inc.                                 */
/*                                                                            */
/* All rights reserved.  Permission is hereby granted, until further notice,  */
/* to make copies of this diskette, which are not for resale, provided these  */
/* copies are made from this master diskette only, and provided that the      */
/* following copyright notice appears on the diskette label:                  */
/* (c) 1993 by Academic Press, Inc.                                           */
/*                                                                            */
/* Except as previously stated, no part of the computer program embodied in   */
/* this diskette may be reproduced or transmitted in any form or by any means,*/
/* electronic or mechanical, including input into storage in any information  */
/* system for resale, without permission in writing from the publisher.       */
/*                                                                            */
/* Produced in the United States of America.                                  */
/*                                                                            */
/* ISBN 0-12-479041-0                                                         */
/*                                                                            */
/******************************************************************************/

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <stdlib.h>
#include "const.h"       // System and limitation constants, typedefs, structs
#include "classes.h"     // Includes all class headers
#include "funcdefs.h"    // Function prototypes

/*
   Prototypes for local subroutines
*/

static double bidiag ( double *a , int rows , int cols , double *w ,
                       double *work ) ;

static void cancel ( int rows , int cols , int lower , int index ,
                     double matnorm , double *a , double *w , double *work ) ;

static void qr ( int rows , int cols , int lower , int index ,
                 double *a , double *v , double *w , double *work ) ;

static void transforms ( double *a , int rows , int cols , double *w ,
                         double *v , double *work ) ;

static void verify_nonneg ( int cols , int index , double *w , double *v ) ;

/*
   Local macros.  RSS computes the root of the sum of squares of
   its arguments.  This clever implementation avoids over/underflow.
   SIGN is the old FORTRAN routine which returns the value of its
   first argument with the sign of its second.
   The variables va, vb and vc are local work areas for these macros.
*/

static double va, vb, vc ;

#define RSS(a,b) ((va=fabs(a)) > (vb=fabs(b)) ? \
                 (vc=vb/va , va*sqrt(vc*vc+1.0)) : \
                 ((vb != 0.0) ? (vc=va/vb , vb*sqrt(vc*vc+1.0)) : 0.0))

#define SIGN(a,b) (va=fabs(a) , (b) >= 0.0 ? va : -va)

/*
--------------------------------------------------------------------------------

   Constructor - This allocates memory for the input/output matrix 'a' and
                 any work areas which it will need (including the public
                 outputs of w and v).  It also allocates 'b' which will be
                 input to the backsub routine.  It does not allocate 'x' which
                 is the output of backsub.

                 If there is a problem (rows < cols, or insufficient memory),
                 it leaves public ok=0.  The user should check for this after
                 allocating with new.

--------------------------------------------------------------------------------
*/

SingularValueDecomp::SingularValueDecomp ( int nrows , int ncols , int preserve)
{
   if (nrows < ncols) {
      rows = cols = ok = 0 ;
      return ;
      }

   a = u = w = v = work = b = NULL ;
   if (((a = (double *) MALLOC ( nrows * ncols * sizeof(double) )) == NULL)
    || (preserve  &&
        (u = (double *) MALLOC ( nrows * ncols * sizeof(double)))== NULL)
    || ((w = (double *) MALLOC ( ncols * sizeof(double) )) == NULL)
    || ((v = (double *) MALLOC ( ncols * ncols * sizeof(double) )) == NULL)
    || ((work = (double *) MALLOC ( ncols * sizeof(double) )) == NULL)
    || ((b = (double *) MALLOC ( nrows * sizeof(double) )) == NULL)) {
      if (a != NULL)
         FREE ( a ) ;
      if (u != NULL)
         FREE ( u ) ;
      if (w != NULL)
         FREE ( w ) ;
      if (v != NULL)
         FREE ( v ) ;
      if (work != NULL)
         FREE ( work ) ;
      if (b != NULL)
         FREE ( b ) ;
      rows = cols = ok = 0 ;
      return ;
      }

   rows = nrows ;
   cols = ncols ;
   ok = 1 ;
}

/*
--------------------------------------------------------------------------------

   Destructor - This frees all memory allocated by the constructor.

--------------------------------------------------------------------------------
*/

SingularValueDecomp::~SingularValueDecomp ()
{
   if (! ok)    // If constructor's mallocs failed
      return ;  // there is nothing to free

   FREE ( a ) ;
   if (u != NULL)    // This was allocated only if preserve was nonzero
      FREE ( u ) ;
   FREE ( w ) ;
   FREE ( v ) ;
   FREE ( work ) ;
   FREE ( b ) ;
}


/*
--------------------------------------------------------------------------------

   svdcmp - Perform singular value decomposition on the matrix already stored.

--------------------------------------------------------------------------------
*/

void SingularValueDecomp::svdcmp ()
{
   int cflag, iter, index, lower ;
   double matnorm, *mat ;

   if (u == NULL)    // Do we replace a with u
      mat = a ;
   else {            // or preserve it?
      memcpy ( u , a , rows * cols * sizeof(double) ) ;
      mat = u ;
      }

   matnorm = bidiag ( mat , rows , cols , w , work ) ; // Reduce to bidiagonal
   transforms ( mat , rows , cols , w , v , work ) ;   // Accumulate R&L trans

   for (index=cols-1 ; index>=0 ; index--) { // All singular values
      for (iter=0 ; iter<100 ; iter++) {     // Conservative limit on QR tries
         cflag = 1 ;
         for (lower=index ; lower ; lower--) { // Split?
            if (fabs (work[lower]) + matnorm == matnorm) {
               cflag = 0 ;
               break ;
               }
            if (fabs (w[lower-1]) + matnorm == matnorm)
               break ;
            }
         if (lower && cflag)
            cancel ( rows , cols , lower , index , matnorm , mat , w , work ) ;
         if (lower == index) {  // Converged?
            verify_nonneg ( cols , index , w , v ) ; // Want nonegative singvals
            break ;
            }
         qr ( rows , cols , lower , index , mat , v , w , work ) ; // Another QR
         }
      }
}

/*
--------------------------------------------------------------------------------

   bidiag - Local routine for Householder reduction to bidiagonal form

--------------------------------------------------------------------------------
*/

static double bidiag (
   double *a ,
   int rows ,
   int cols ,
   double *w ,
   double *work
   )

{
   int col, j, k, nextcol ;
   double pp, qq, denom, sum ;
   double matnorm, scale ;

   matnorm = qq = sum = scale = 0.0 ;

   for (col=0 ; col<cols ; col++) {

      nextcol = col + 1 ;
      work[col] = scale * qq ;
      qq = sum = scale = 0.0 ;

      for (k=col ; k<rows ; k++)
         scale += fabs ( a[k*cols+col] ) ;
      if (scale > 0.0) {
         for (k=col ; k<rows ; k++) {
            a[k*cols+col] /= scale ;
            sum += a[k*cols+col] * a[k*cols+col] ;
            }
         pp = a[col*cols+col] ;
         qq = -SIGN ( sqrt(sum) , pp ) ;
         denom = pp * qq - sum ;
         a[col*cols+col] = pp - qq ;

         for (j=nextcol ; j<cols ; j++) {
            sum = 0.0 ;
            for (k=col ; k<rows ; k++)
               sum += a[k*cols+col] * a[k*cols+j] ;
            pp = sum / denom ;
            for (k=col ; k<rows ; k++)
               a[k*cols+j] += pp * a[k*cols+col] ;
            }

         for (k=col ; k<rows ; k++)
            a[k*cols+col] *= scale ;
         } // if scale > 0

      w[col] = scale * qq ;


      qq = sum = scale = 0.0 ;

      for (k=nextcol ; k<cols ; k++)
         scale += fabs ( a[col*cols+k] ) ;

      if (scale > 0.0) {
         for (k=nextcol ; k<cols ; k++) {
            a[col*cols+k] /= scale ;
            sum += a[col*cols+k] * a[col*cols+k] ;
            }
         pp = a[col*cols+nextcol] ;
         qq = -SIGN ( sqrt ( sum ) , pp ) ;
         denom = pp * qq - sum ;
         a[col*cols+nextcol] = pp - qq ;
         for (k=nextcol ; k<cols ; k++)
            work[k] = a[col*cols+k] / denom ;
         if (col != rows-1) {
            for (j=nextcol ; j<rows ; j++) {
               sum = 0.0 ;
               for (k=nextcol ; k<cols ; k++)
                  sum += a[j*cols+k] * a[col*cols+k] ;
               for (k=nextcol ; k<cols ; k++)
                  a[j*cols+k] += sum * work[k] ;
               }
            }
         for (k=nextcol ; k<cols ; k++)
            a[col*cols+k] *= scale ;
         }

      sum = fabs (w[col]) + fabs (work[col]) ;
      if (sum > matnorm)
         matnorm = sum ;
      }
   return matnorm ;
}


/*
--------------------------------------------------------------------------------

   cancel

--------------------------------------------------------------------------------
*/

static void cancel (
   int rows ,
   int cols ,
   int lower ,
   int index ,
   double matnorm ,
   double *a ,
   double *w ,
   double *work
   )
{
   int col, row ;
   double c, rr, ww, hypot, s, pp, qq ;

   s = 1.0 ;
   for (col=lower ; col<=index ; col++) {
      rr = s * work[col] ;
      if (fabs (rr) + matnorm != matnorm) {
         ww = w[col] ;
         hypot = RSS ( rr , ww ) ;
         w[col] = hypot ;
         c =  ww / hypot ;
         s = -rr / hypot ;
         for (row=0 ; row<rows ; row++) {
            pp = a[row*cols+lower-1] ;
            qq = a[row*cols+col] ;
            a[row*cols+lower-1] = qq * s  +  pp * c ;
            a[row*cols+col] = qq * c  -  pp * s ;
            }
         }
      }
}

/*
--------------------------------------------------------------------------------

   Cumulate right and left transforms

--------------------------------------------------------------------------------
*/

static void transforms (
   double *a ,
   int rows ,
   int cols ,
   double *w ,
   double *v ,
   double *work
   )
{