matrixn.cpp

basic mathematic classes used for math programming

	for( int j=0; j<n; j++ )
	{
		for( int i=0; i<n; i++ ) b[i] = 0;
		b[j] = 1.0;

		LUsubstitute( index, b );

		for( i=0; i<n; i++ )
			mat[i][j] = b[i];
	}
}

matrixN&
matrixN::mergeUpDown( const matrixN& a, const matrixN& b )
{
	assert( a.column()==b.column() );
	matrixN &c = (*this);
	c.setSize( a.row()+b.row(), a.column() );

	for( int j=0; j<a.column(); j++ )
	{
		for( int i=0; i<a.row(); i++ )
			c[i][j] = a[i][j];

		for( i=0; i<b.row(); i++ )
			c[i+a.row()][j] = b[i][j];
	}

	return c;
}

matrixN&
matrixN::mergeLeftRight( const matrixN& a, const matrixN& b )
{
	assert( a.row()==b.row() );
	matrixN &c = (*this);
	c.setSize( a.row(), a.column()+b.column() );

	for( int i=0; i<a.row(); i++ )
	{
		for( int j=0; j<a.column(); j++ )
			c[i][j] = a[i][j];

		for( j=0; j<b.column(); j++ )
			c[i][j+a.column()] = b[i][j];
	}

	return c;
}

void
matrixN::splitUpDown( matrixN& a, matrixN& b )
{
	assert( this->row()%2 == 0 );
	matrixN &c = (*this);
	a.setSize( c.row()/2, c.column() );
	b.setSize( c.row()/2, c.column() );

	for( int j=0; j<a.column(); j++ )
	{
		for( int i=0; i<a.row(); i++ )
			a[i][j] = c[i][j];

		for( i=0; i<b.row(); i++ )
			b[i][j] = c[i+a.row()][j];
	}
}

void
matrixN::splitLeftRight( matrixN& a, matrixN& b )
{
	assert( this->column()%2 == 0 );
	matrixN &c = (*this);
	a.setSize( c.row(), c.column()/2 );
	b.setSize( c.row(), c.column()/2 );

	for( int i=0; i<a.row(); i++ )
	{
		for( int j=0; j<a.column(); j++ )
			a[i][j] = b[i][j];

		for( j=0; j<b.column(); j++ )
			b[i][j] = c[i][j+a.column()];
	}
}

void
matrixN::splitUpDown( matrixN& a, matrixN& b, int num )
{
	assert( this->row()>num );
	matrixN &c = (*this);
	a.setSize( num, c.column() );
	b.setSize( c.row()-num, c.column() );

	for( int j=0; j<a.column(); j++ )
	{
		for( int i=0; i<a.row(); i++ )
			a[i][j] = c[i][j];

		for( i=0; i<b.row(); i++ )
			b[i][j] = c[i+a.row()][j];
	}
}

void
matrixN::splitLeftRight( matrixN& a, matrixN& b, int num )
{
	assert( this->column()>num );
	matrixN &c = (*this);
	a.setSize( c.row(), num );
	b.setSize( c.row(), c.column()-num );

	for( int i=0; i<a.row(); i++ )
	{
		for( int j=0; j<a.column(); j++ )
			a[i][j] = b[i][j];

		for( j=0; j<b.column(); j++ )
			b[i][j] = c[i][j+a.column()];
	}
}

m_real
pythag( m_real a, m_real b )
{
	m_real pa = fabs( a );
	m_real pb = fabs( b );

	if ( pa > pb ) return pa * sqrt( 1.0f + SQR(pb / pa) );
	else return (pb==0.0f ? 0.0f : pb * sqrt(1.0f + SQR(pa / pb)));
}

void
matrixN::SVdecompose( vectorN& w, matrixN& v )
{
	// This version does't get your odered singular values. 
	// so liu ren put a code in the end of this function which is  from
	// NR before.
	matrixN &a = (*this);
	int m = a.row();
	int n = a.column();

	w.setSize( n );
	v.setSize( n, n );

	int flag, i, its, j, jj, k, l, nm;
	m_real anorm, c, f, g, h, s, scale, x, y, z;

  	static vectorN rv1; rv1.setSize( n );
	g = scale = anorm = 0.0;

	for( i=0; i<n; i++ )
	{
		l = i + 1;
		rv1[i] = scale * g;
		g = s = scale = 0.0;

		if ( i<m )
		{
			for ( k=i; k<m; k++ )
				scale += fabs(a[k][i]);

			if ( scale )
			{
				for ( k=i; k<m; k++ )
				{
					a[k][i] /= scale;
					s += a[k][i] * a[k][i];
				}

				f = a[i][i];
				g = -SIGN(sqrt(s), f);
				h = f * g - s;
				a[i][i] = f - g;

				for( j=l; j<n; j++ )
				{
					for ( s=0.0, k=i; k<m; k++ )
						s += a[k][i] * a[k][j];
					f = s / h;

					for ( k=i; k<m; k++ )
						a[k][j] += f * a[k][i];
				}

				for( k=i; k<m; k++ )
					a[k][i] *= scale;
			}
		}

		w[i] = scale * g;
		g = s = scale = 0.0;

		if ( i<m && i != n-1)
		{
			for( k=l; k<n; k++)
				scale += fabs(a[i][k]);

			if ( scale )
			{
				for( k=l; k<n; k++ )
				{
					a[i][k] /= scale;
					s += a[i][k] * a[i][k];
				}

				f = a[i][l];
				g = -SIGN(sqrt(s), f);
				h = f * g - s;
				a[i][l] = f - g;

				for ( k=l; k<n; k++ )
					rv1[k] = a[i][k] / h;

				for( j=l; j<m; j++ )
				{
					for( s=0.0, k=l; k<n; k++ )
						s += a[j][k] * a[i][k];

					for( k=l; k<n; k++ )
						a[j][k] += s * rv1[k];
				}

				for( k=l; k<n; k++ )
					a[i][k] *= scale;
			}
		}

		anorm = MAX(anorm, (fabs(w[i]) + fabs(rv1[i])));
	}

	for( i=n-1; i>=0; i-- )
	{
		if ( i<n-1 )
		{
			if ( g )
			{
				for( j=l; j<n; j++ )
					v[j][i] = (a[i][j] / a[i][l]) / g;

				for ( j=l; j<n; j++ )
				{
					for( s=0.0, k=l; k<n; k++ )
						s += a[i][k] * v[k][j];

					for( k=l; k<n; k++ )
						v[k][j] += s * v[k][i];
				}
			}

			for( j=l; j<n; j++ )
				v[i][j] = v[j][i] = 0.0;
		}

		v[i][i] = 1.0;
		g = rv1[i];
		l = i;
	}

	for( i=MIN(m, n)-1; i>=0; i-- )
	{
		l = i + 1;
		g = w[i];
		for( j=l; j<n; j++ )
		a[i][j] = 0.0;

		if ( g )
		{
			g = 1.0 / g;
			for( j=l; j<n; j++ )
			{
				for ( s=0.0, k=l; k<m; k++ )
					s += a[k][i] * a[k][j];

				f = (s / a[i][i]) * g;

				for( k=i; k<m; k++ )
					a[k][j] += f * a[k][i];
			}

			for( j=i; j<m; j++ )
				a[j][i] *= g;
		}
		else
			for( j=i; j<m; j++ )
				a[j][i] = 0.0;

		++a[i][i];
	}

	for( k=n-1; k>=0; k-- )
	{
		for( its=1; its<30; its++ )
		{
			flag = 1;
			for( l=k; l>=0; l-- )
			{
				nm = l - 1;
				if ((m_real) (fabs(rv1[l]) + anorm) == anorm)
				{
					flag = 0;
					break;
				}

				if ((m_real) (fabs(w[nm]) + anorm) == anorm) break;
			}

			if ( flag )
			{
				c = 0.0;
				s = 1.0;

				for( i=l; i<= k; i++ )
				{
					f = s * rv1[i];
					rv1[i] = c * rv1[i];

					if ((m_real) (fabs(f) + anorm) == anorm) break;

					g = w[i];
					h = pythag(f, g);
					w[i] = h;
					h = 1.0f / h;
					c = g * h;
					s = -f * h;

					for( j=0; j<m; j++ )
					{
						y = a[j][nm];
						z = a[j][i];
						a[j][nm] = y * c + z * s;
						a[j][i] = z * c - y * s;
					}
				}
			}

			z = w[k];
			if ( l == k )
			{
				if ( z < 0.0 )
				{
					w[k] = -z;
					for( j=0; j<n; j++ )
						v[j][k] = -v[j][k];
				}
				break;
			}

			if (its == 29)
				cerr << "no convergence in 30 svdcmp iterations" << endl;

			x = w[l];
			nm = k - 1;
			y = w[nm];
			g = rv1[nm];
			h = rv1[k];
			f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0f * h * y);
			g = pythag(f, 1.0f);
			f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
			c = s = 1.0f;

			for( j=l; j<=nm; j++ )
			{
				i = j + 1;
				g = rv1[i];
				y = w[i];
				h = s * g;
				g = c * g;
				z = pythag(f, h);
				rv1[j] = z;
				c = f / z;
				s = h / z;
				f = x * c + g * s;
				g = g * c - x * s;
				h = y * s;
				y *= c;

				for( jj=0; jj<n; jj++ )
				{
					x = v[jj][j];
					z = v[jj][i];
					v[jj][j] = x * c + z * s;
					v[jj][i] = z * c - x * s;
				}

				z = pythag(f, h);
				w[j] = z;

				if ( z )
				{
					z = 1.0f / z;
					c = f * z;
					s = h * z;
				}

				f = c * g + s * y;
				x = c * y - s * g;

				for( jj=0; jj<m; jj++ )
				{
					y = a[jj][j];
					z = a[jj][i];
					a[jj][j] = y * c + z * s;
					a[jj][i] = z * c - y * s;
				}
			}

			rv1[l] = 0.0;
			rv1[k] = f;
			w[k] = x;
		}
	}

	// sorting sigular values:

	int dim;
	dim = a.column();

	m_real maxval;
    int    index;

	for(i=0;i<dim;i++){

		maxval = w[i];
		index  = i;
	
		for(int j=i+1;j<dim;j++){

			if(w[j]>maxval){
			
				maxval = w[j];
				index = j;
			}
		
		}

		if(i!=index){

			w.exchange(i,index);
			a.exchangeColumn(i,index);
			v.exchangeColumn(i,index);
		}
	}


}

void
matrixN::SVsubstitute( const vectorN& w, const matrixN& v,
            const vectorN& b, vectorN &x )
{
    assert( this->column() == w.getSize() );
    assert( this->column() == v.column() );
    assert( this->column() == v.row() );
    assert( this->row() == b.getSize() );
    assert( this->column() == x.getSize() );

    int m = this->row();
    int n = this->column();

    int jj,j,i;
    m_real s;
    static vectorN tmp; tmp.setSize(n);
    matrixN& u = *this;

    for (j=0;j<n;j++) {
        s=0.0;
        if (w[j]>EPS) {
            for (i=0;i<m;i++) s += u[i][j]*b[i];
            s /= w[j];
        }
        tmp[j]=s;
    }
    for (j=0;j<n;j++) {
        s=0.0;
        for (jj=0;jj<n;jj++) s += v[j][jj]*tmp[jj];
        x[j]=s;
    }
}

void
matrixN::SVinverse( matrixN &mat )
{
    int m = this->row();
    int n = this->column();

    static matrixN V; V.setSize( n, n );
    static vectorN w; w.setSize( n );

    static vectorN b; b.setSize( m );
    static vectorN x; x.setSize( n );

	mat.setSize( n, m );

    SVdecompose( w, V );
    for( int j=0; j<m; j++ )
    {
        for( int i=0; i<m; i++ ) b[i] = 0;
        b[j] = 1.0;

        SVsubstitute( w, V, b, x );

        for( i=0; i<n; i++ )
            mat[i][j] = x[i];
    }
}