/* vi: set tabstop=4 nocindent noautoindent: */


/** 
@brief   マトリックス＆ベクトルライブラリ 
@file    matrix.c
@author  Fumi.Iseki (C)
*/


#include "matrix.h" 





/**
vector  unit_vector(vector a)

ベクトル aの単位ベクトルを返す．

@param  a  対象ベクトル．
@return    a の単位ベクトル．
*/
vector  unit_vector(vector a)
{
    vector c;
    double r;

    r = a.x*a.x+a.y*a.y+a.z*a.z;
    if (Xabs(r)<EPS*EPS) {
		c.x = 0.0;
		c.y = 0.0;
		c.z = 0.0;
		c.n = 1.0;
    }
    else {
		r   = sqrt(r);
    	c.x = a.x/r;
    	c.y = a.y/r;
    	c.z = a.z/r;
    	c.n = 1.0;
    }
    return c;
}




/**
vector  unit_ivector(ivector a)
 
整数ベクトル aの単位ベクトルを返す．返されるベクトルは実数ベクトル．

@param  a  対象整数ベクトル．
@return    a の単位ベクトル(実数ベクトル)．
*/
vector  unit_ivector(ivector a)
{
    vector c;
    double r;

    r = (double)a.x*a.x+a.y*a.y+a.z*a.z;
    if (Xabs(r)<EPS*EPS) {
		c.x = 0.0;
		c.y = 0.0;
		c.z = 0.0;
		c.n = 1.0;
    }
    else {
		r   = sqrt(r);
    	c.x = a.x/r;
    	c.y = a.y/r;
    	c.z = a.z/r;
    	c.n = 1.0;
    }
    return c;
}




/**
vector  set_vector(double x, double y, double z)
 
ベクトルの作成．x方向成分, y方向成分, z方向成分から実数ベクトルを作り,それを返す．

@param  x  ベクトルの x成分．
@param  y  ベクトルの y成分．
@param  z  ベクトルの z成分．
@return    作成されたベクトル．
 */
vector  set_vector(double x, double y, double z)
{
    vector c;

    c.x = x;
    c.y = y;
    c.z = z;
    c.n = sqrt(x*x + y*y + z*z);
    return c;
}




/**
ivector  set_ivector(int x, int y, int z)
 
整数ベクトルの作成．x方向成分, y方向成分, z方向成分から整数ベクトルを作り,それを返す．

@param  x  ベクトルの x成分．
@param  y  ベクトルの y成分．
@param  z  ベクトルの z成分．
@return    作成された整数ベクトル．
 */
ivector  set_ivector(int x, int y, int z)
{
    ivector c;

    c.x = x;
    c.y = y;
    c.z = z;
    c.n = sqrt((double)x*x + y*y + z*z);
    return c;
}




/**
ivector  d2ivector(vector a)

実数ベクトル a から整数ベクトルをつくり出し,それを返す．
各要素は四捨五入される．

@param  a  変換する実数ベクトル．
@return    変換された整数ベクトル．
 */
ivector  d2ivector(vector a)
{
    ivector c;

    c.x = (int)(a.x + 0.5);
    c.y = (int)(a.y + 0.5);
    c.z = (int)(a.z + 0.5);
    c.n = sqrt((double)c.x*c.x + c.y*c.y + c.z*c.z);
    return c;
}




/**
vector  i2vector(ivector a)

整数ベクトル a から実数ベクトルをつくり出し,それを返す．

@param  a  変換する整数ベクトル．
@return    変換された実数ベクトル．
 */
vector  i2vector(ivector a)
{
    vector c;

    c.x = (double)a.x;
    c.y = (double)a.y;
    c.z = (double)a.z;
    c.n = (double)sqrt(c.x*c.x + c.y*c.y + c.z*c.z);
    return c;
}




/**
vector  ex_vector(vector a, vector b)

ベクトルの外積．
実数ベクトル a,b の外積ベクトルを計算し,それを返す．

@param  a  外積を計算するベクトル． 
@param  b  外積を計算するベクトル．
@return    外積ベクトル．
*/
vector  ex_vector(vector a, vector b)
{
    vector c;

    c.x = a.y*b.z - a.z*b.y;
    c.y = a.z*b.x - a.x*b.z;
    c.z = a.x*b.y - a.y*b.x;
    c.n = sqrt(c.x*c.x+c.y*c.y+c.z*c.z);
    return c;
}




/**
matrix  make_matrix1(int n)

1次元の実数行列のバッファ部をつくり出す．
要素自体は 0.0に初期化される． 

@param  n  1次元行列の大きさ．
@return    作成された1次元行列
*/
matrix  make_matrix1(int n)
{
    int i;
    matrix a;

    a.n = a.r = 0;
    a.mx = NULL;
    a.sz = (int*)malloc(sizeof(int));
    if (a.sz==NULL)  return a;
    a.sz[0] = n;

    a.mx = (double*)malloc(n*sizeof(double));
    if (a.mx==NULL) {
        free(a.sz);
        return a;
    }

    a.n = 1;
    a.r = n;
    for (i=0; i<n; i++) a.mx[i] = 0.0;
    return a;
}




/**
imatrix  make_imatrix1(int n)

1次元の整数行列のバッファ部をつくり出す．
要素自体は 0に初期化される．

@param  n  1次元行列の大きさ．
@return    作成された1次元整数行列
*/
imatrix  make_imatrix1(int n)
{
    int i;
    imatrix a;

    a.n = a.r = 0;
    a.mx = NULL;
    a.sz = (int*)malloc(sizeof(int));
    if (a.sz==NULL)  return a;
    a.sz[0] = n;

    a.mx = (int*)malloc(n*sizeof(int));
    if (a.mx==NULL) {
        free(a.sz);
        return a;
    }

    a.n = 1;
    a.r = n;
    for (i=0; i<n; i++) a.mx[i] = 0;
    return a;
}




/**
matrix  make_matrix2(int n, int m)

2次元の実数行列のバッファ部をつくり出す．
要素自体は 0.0に初期化される． 

@param  n  2次元行列の行の数．
@param  m  2次元行列の列の数．
@return    作成された2次元行列
*/
matrix  make_matrix2(int n, int m)
{
    int i, s;
    matrix a;

    a.n = a.r = 0;
    a.mx = NULL;
    a.sz = (int*)malloc(2*sizeof(int));
    if (a.sz==NULL)  return a;
    a.sz[0] = n;
    a.sz[1] = m;
    s = n*m;

    a.mx = (double*)malloc(s*sizeof(double));
    if (a.mx==NULL) {
        free(a.sz);
        return a;
    }

    a.n = 2;
    a.r = s;
    for (i=0; i<s; i++) a.mx[i] = 0.0;
    return a;
}




/**
imatrix  make_imatrix2(int n, int m)

2次元の整数行列のバッファ部をつくり出す．
要素自体は 0に初期化される．

@param  n  2次元行列の行の数．
@param  m  2次元行列の列の数．
@return    作成された2次元整数行列
*/
imatrix  make_imatrix2(int n, int m)
{
    int i, s;
    imatrix a;

    a.n = a.r = 0;
    a.mx = NULL;
    a.sz = (int*)malloc(2*sizeof(int));
    if (a.sz==NULL)  return a;
    a.sz[0] = n;
    a.sz[1] = m;
    s = n*m;

    a.mx = (int*)malloc(s*sizeof(int));
    if (a.mx==NULL) {
        free(a.sz);
        return a;
    }

    a.n = 2;
    a.r = s;
    for (i=0; i<s; i++) a.mx[i] = 0;
    return a;
}




/**
matrix  make_matrix(int n, int* sz)

任意(n)次元の実数マトリックスのバッファ部をつくり出す．
要素自体は 0.0に初期化される．

@param  n   マトリックスの次元数．
@param  sz  sz[0]〜sz[n-1]:  各次元の要素数．
@return     作成されたマトリックス．
*/
matrix  make_matrix(int n, int* sz)
{
    int i, s;
    matrix a;

    a.n = a.r = 0;
    a.sz = (int*)malloc(n*sizeof(int));
    if (a.sz==NULL) {
        a.mx = NULL;
        return a;
    }
    for (s=1,i=0; i<n; i++) {
		a.sz[i] = sz[i];
		s = s*sz[i];
    }

    a.mx = (double*)malloc(s*sizeof(double));
    if (a.mx==NULL) {
        free(a.sz);
        return a;
    }

    a.n = n;
    a.r = s;
    for (i=0; i<s; i++) a.mx[i] = 0.0;
    return a;
}




/**
imatrix  make_imatrix(int n, int* sz)

任意(n)次元の整数マトリックスのバッファ部をつくり出す．
要素自体は 0に初期化される．

@param  n   マトリックスの次元数．
@param  sz  sz[0]〜sz[n-1]: 各次元の要素数．
@return     作成された整数マトリックス．
*/
imatrix  make_imatrix(int n, int* sz)
{
    int i, s;
    imatrix a;

    a.n = a.r = 0;
    a.sz = (int*)malloc(n*sizeof(int));
    if (a.sz==NULL) {
        a.mx = NULL;
        return a;
    }
    for (s=1,i=0; i<n; i++) {
		a.sz[i] = sz[i];
		s = s*sz[i];
    }

    a.mx = (int*)malloc(s*sizeof(int));
    if (a.mx==NULL) {
        free(a.sz);
        return a;
    }

    a.n = n;
    a.r = s;
    for (i=0; i<s; i++) a.mx[i] = 0;
    return a;
}




/**
void  free_matrix(matrix* a)

マトリックスのバッファ部を開放をする．

@param  a  開放するバッファ部を持ったマトリックスへのポインタ．
*/
void  free_matrix(matrix* a)
{
    if(a->sz!=NULL) free(a->sz);
    if(a->mx!=NULL) free(a->mx);
    a->sz = NULL;
    a->mx = NULL;
    a->n  = a->r  = 0;
}




/**
void  free_imatrix(imatrix* a)

整数マトリックスのバッファ部を開放する．

@param  a  開放するバッファ部を持った整数マトリックスへのポインタ．
*/
void  free_imatrix(imatrix* a)
{
    if(a->sz!=NULL) free(a->sz);
    if(a->mx!=NULL) free(a->mx);
    a->sz = NULL;
    a->mx = NULL;
    a->n  = a->r  = 0;
}




/**  
double*  get_matrix(matrix mtx, ...) 

Matrix の要素を返す．次元数に制限はない．インデックスは1から数える（0からではない）．

@code
参考：1次元配列へのアクセスインデックス
	1次元: (i)                                                                    (i-1)
	2次元: (i,j)                                        (j-1) +             sz[1]*(i-1)
	3次元: (i,j,k)                  (k-1) +       sz[2]*(j-1) +       sz[1]*sz[2]*(i-1)
	4次元: (i,j,k,l)  (l-1) + sz[3]*(k-1) + sz[2]*sz[3]*(j-1) + sz[1]*sz[2]*sz[3]*(i-1)
	...................
@endcode
*/
double*  get_matrix(matrix mtx, ...) 
{
	int   m, d;
	int*  args;
	va_list argsptr;
		
	args = (int*)malloc(mtx.n*sizeof(int));
	if (args==NULL) return NULL;

	va_start(argsptr, mtx);
	for (m=0; m<mtx.n; m++) {
		args[m] = (int)va_arg(argsptr, int);
	}
	va_end(argsptr);

	int dx = args[0] - 1;
	for (d=1; d<mtx.n; d++) dx = dx*mtx.sz[d] + args[d] - 1;
	free(args);

	if (dx>=mtx.r || dx<0) return NULL;
	return &mtx.mx[dx];
}




/**  
int*  get_imatrix(imatrix mtx, ...) 

Matrix の要素を返す．次元数に制限はない．インデックスは1から数える（0からではない）．

@code
参考：1次元配列へのアクセスインデックス
	1次元: (i)                                                                    (i-1)
	2次元: (i,j)                                        (j-1) +             sz[1]*(i-1)
	3次元: (i,j,k)                  (k-1) +       sz[2]*(j-1) +       sz[1]*sz[2]*(i-1)
	4次元: (i,j,k,l)  (l-1) + sz[3]*(k-1) + sz[2]*sz[3]*(j-1) + sz[1]*sz[2]*sz[3]*(i-1)
	...................
@endcode
*/
int*  get_imatrix(imatrix mtx, ...) 
{
	int   m, d;
	int*  args;
	va_list argsptr;
		
	args = (int*)malloc(mtx.n*sizeof(int));
	if (args==NULL) return NULL;

	va_start(argsptr, mtx);
	for (m=0; m<mtx.n; m++) {
		args[m] = (int)va_arg(argsptr, int);
	}
	va_end(argsptr);

	int dx = args[0] - 1;
	for (d=1; d<mtx.n; d++) dx = dx*mtx.sz[d] + args[d] - 1;
	free(args);

	if (dx>=mtx.r || dx<0) return NULL;
	return &mtx.mx[dx];
}




/**
void   copy_matrix(matrix src, matrix dst)

マトリックスのコピー．srcの内容を dstへコピーする．@n
マトリックス全体のサイズが合わない場合は何もしない．@n
全体のサイズが合っていればコピーする．

@param  src  コピー元マトリックス．
@param  dst  コピー先マトリックス．
*/
void   copy_matrix(matrix src, matrix dst)
{
    int  i;

    if (src.mx==NULL  || dst.mx==NULL)  return;
    if ((src.r!=dst.r)||(dst.n!=dst.n)) return;

    for (i=0; i<src.n; i++) dst.sz[i] = src.sz[i];
    for (i=0; i<src.r; i++) dst.mx[i] = src.mx[i];
}




/**
void   copy_imatrix(imatrix src, imatrix dst)

整数マトリックスのコピー．srcの内容を dstへコピーする．@n
マトリックス全体のサイズが合わない場合は何もしない．@n
全体のサイズが合っていればコピーする．

@param  src  コピー元マトリックス．
@param  dst  コピー先マトリックス．
*/
void   copy_imatrix(imatrix src, imatrix dst)
{
    int  i;

    if (src.mx==NULL  || dst.mx==NULL)  return;
    if ((src.r!=dst.r)||(dst.n!=dst.n)) return;

    for (i=0; i<src.n; i++) dst.sz[i] = src.sz[i];
    for (i=0; i<src.r; i++) dst.mx[i] = src.mx[i];
}




/**
matrix   add_matrix(matrix a, matrix b)

マトリックスの足し算．
実数マトリックス a, bを足し算して,結果の実数マトリックスを返す．

@param  a  足されるマトリックス．
@param  b  足すマトリックス．
@return    足し算結果のマトリックス．
*/
matrix   add_matrix(matrix a, matrix b)
{
    int  i;
    matrix c;

    c.n = c.r = 0;
    c.sz = NULL;
    c.mx = NULL;
    if (a.mx==NULL || b.mx==NULL) return c;
    if (a.r != b.r)               return c;

    c = make_matrix(a.n, a.sz);
    for (i=0; i<c.r; i++) c.mx[i] = a.mx[i] + b.mx[i];
    return c;
}




/**
imatrix   add_imatrix(imatrix a, imatrix b)

マトリックスの足し算．
整数マトリックス a, bを足し算して,結果の整数マトリックスを返す．

@param  a  足されるマトリックス．
@param  b  足すマトリックス．
@return    足し算結果の整数マトリックス．
*/
imatrix   add_imatrix(imatrix a, imatrix b)
{
    int  i;
    imatrix c;

    c.n = c.r = 0;
    c.sz = NULL;
    c.mx = NULL;
    if (a.mx==NULL || b.mx==NULL) return c;
    if (a.r != b.r)               return c;

    c = make_imatrix(a.n, a.sz);
    for (i=0; i<c.r; i++) c.mx[i] = a.mx[i] + b.mx[i];
    return c;
}





/**
matrix   sub_matrix(matrix a, matrix b)

マトリックスの引き算．
実数マトリックス a, bを引き算して,結果の実数マトリックスを返す．

@param  a  引かれるマトリックス．
@param  b  引くマトリックス．
@return    引き算結果のマトリックス．
*/
matrix   sub_matrix(matrix a, matrix b)
{
    int  i;
    matrix c;

    c.n = c.r = 0;
    c.sz = NULL;
    c.mx = NULL;
    if (a.mx==NULL || b.mx==NULL) return c;
    if (a.r != b.r)               return c;

    c = make_matrix(a.n, a.sz);
    for (i=0; i<c.r; i++) c.mx[i] = a.mx[i] - b.mx[i];
    return c;
}




/**
imatrix   sub_imatrix(imatrix a, imatrix b)

マトリックスの引き算．
整数マトリックス a, bを引き算して,結果の整数マトリックスを返す．

@param  a  引かれるマトリックス．
@param  b  引くマトリックス．
@return    引き算結果の整数マトリックス．
*/
imatrix   sub_imatrix(imatrix a, imatrix b)
{
    int  i;
    imatrix c;

    c.n = c.r = 0;
    c.sz = NULL;
    c.mx = NULL;
    if (a.mx==NULL || b.mx==NULL) return c;
    if (a.r != b.r)               return c;

    c = make_imatrix(a.n, a.sz);
    for (i=0; i<c.r; i++) c.mx[i] = a.mx[i] - b.mx[i];
    return c;
}




/**
matrix   mlt_matrix(matrix a, matrix b)

マトリックスのかけ算．
実数マトリックス a, bをかけ算して,結果の実数マトリックスを返す．

@param  a  かけられるマトリックス．
@param  b  かけるマトリックス．
@return    かけ算結果のマトリックス．
*/
matrix   mlt_matrix(matrix a, matrix b)
{
    int  i, j, k, n, ii, aa, bb;
    int  *sz, *sa, *sb, *sc, *cx;
    double st;
    matrix c;

    c.n = c.r = 0;
    c.sz = NULL;
    c.mx = NULL;
    if (a.mx==NULL || b.mx==NULL) return c;
    if (a.sz[a.n-1]!=b.sz[0])     return c;

    n  = a.n + b.n - 2;
    sz = (int*)malloc(n*sizeof(int));
    if (sz==NULL) return c;
    sa = (int*)malloc(a.n*sizeof(int));
    if (sa==NULL) {free(sz); return c;}
    sb = (int*)malloc(b.n*sizeof(int));
    if (sb==NULL) {free(sz); free(sa); return c;}
    sc = (int*)malloc(n*sizeof(int));
    if (sc==NULL) {free(sz); free(sa); free(sb); return c;}
    cx = (int*)malloc(n*sizeof(int));
    if (cx==NULL) {free(sz); free(sa); free(sb); free(sc); return c;}

    for (i=0; i<a.n-1; i++) sz[i] = a.sz[i];
    for (i=1; i<b.n; i++)   sz[a.n-2+i] = b.sz[i];

    sa[a.n-1] = sb[b.n-1] = sc[n-1] = 1;
    for (i=a.n-2; i>=0; i--) sa[i] = sa[i+1]*a.sz[i+1];
    for (i=b.n-2; i>=0; i--) sb[i] = sb[i+1]*b.sz[i+1];
    for (i=n-2;   i>=0; i--) sc[i] = sc[i+1]*sz[i+1];

    c = make_matrix(n, sz);

    for (i=0; i<c.r; i++) {
		ii = i;
		for (j=0; j<c.n; j++) {
	    	cx[j] = ii / sc[j]; 
	    	ii = ii % sc[j];
		}
		aa = bb = 0;
		for (j=0; j<a.n-1; j++) aa = aa + sa[j]*cx[j]; 
		for (j=1; j<b.n; j++)   bb = bb + sb[j]*cx[j+a.n-2];

		st = 0.0;
		for (k=0; k<b.sz[0]; k++) st = st + a.mx[k+aa]*b.mx[bb+sb[0]*k];
		c.mx[i] = st;
    }
		
    free(sz);
    free(sa);
    free(sb);
    free(sc);
    free(cx);

    return c;
}




/**
imatrix   mlt_imatrix(imatrix a, imatrix b)

マトリックスのかけ算．
整数マトリックス a, bをかけ算して,結果の整数マトリックスを返す．

@param  a  かけられるマトリックス．
@param  b  かけるマトリックス．
@return    かけ算結果の整数マトリックス．
*/
imatrix   mlt_imatrix(imatrix a, imatrix b)
{
    int  i, j, k, n, ii, aa, bb, st;
    int  *sz, *sa, *sb, *sc, *cx;
    imatrix c;

    c.n = c.r = 0;
    c.sz = NULL;
    c.mx = NULL;
    if (a.mx==NULL || b.mx==NULL) return c;
    if (a.sz[a.n-1]!=b.sz[0])     return c;

    n  = a.n + b.n - 2;
    sz = (int*)malloc(n*sizeof(int));
    if (sz==NULL) return c;
    sa = (int*)malloc(a.n*sizeof(int));
    if (sa==NULL) {free(sz); return c;}
    sb = (int*)malloc(b.n*sizeof(int));
    if (sb==NULL) {free(sz); free(sa); return c;}
    sc = (int*)malloc(n*sizeof(int));
    if (sc==NULL) {free(sz); free(sa); free(sb); return c;}
    cx = (int*)malloc(n*sizeof(int));
    if (cx==NULL) {free(sz); free(sa); free(sb); free(sc); return c;}


    for (i=0; i<a.n-1; i++) sz[i] = a.sz[i];
    for (i=1; i<b.n; i++)   sz[a.n-2+i] = b.sz[i];

    sa[a.n-1] = sb[b.n-1] = sc[n-1] = 1;
    for (i=a.n-2; i>=0; i--) sa[i] = sa[i+1]*a.sz[i+1];
    for (i=b.n-2; i>=0; i--) sb[i] = sb[i+1]*b.sz[i+1];
    for (i=n-2;   i>=0; i--) sc[i] = sc[i+1]*sz[i+1];

    c = make_imatrix(n, sz);

    for (i=0; i<c.r; i++) {
		ii = i;
		for (j=0; j<c.n; j++) {
	    	cx[j] = ii / sc[j]; 
	    	ii = ii % sc[j];
		}
		aa = bb = 0;
		for (j=0; j<a.n-1; j++) aa = aa + sa[j]*cx[j]; 
		for (j=1; j<b.n; j++)   bb = bb + sb[j]*cx[j+a.n-2];

		st = 0;
		for (k=0; k<b.sz[0]; k++) st = st + a.mx[k+aa]*b.mx[bb+sb[0]*k];
		c.mx[i] = st;
    }
		
    free(sz);
    free(sa);
    free(sb);
    free(sc);
    free(cx);

    return c;
}




/**
void   print_matrix(FILE* fp, matrix a)

実数マトリックスの要素を標準出力に書き出す．

@param  fp  出力先のファイル記述子
@param  a   プリントするマトリックス．
*/
void   print_matrix(FILE* fp, matrix a)
{
    int  i;
    for (i=0; i<a.r; i++) {
        fprintf(fp, "  %10f", a.mx[i]);
		if ((i+1)%a.sz[a.n-1]==0) fprintf(fp, "\n");
    }
//	fprintf(stdout,"\n");
}




/**
void   print_imatrix(FILE* fp, imatrix a)

整数マトリックスの要素を標準出力に書き出す．

@param  fp  出力先のファイル記述子
@param  a   プリントするマトリックス．
*/
void   print_imatrix(FILE* fp, imatrix a)
{
    int  i;
    for (i=0; i<a.r; i++) {
        fprintf(fp, "  %6d", a.mx[i]);
		if ((i+1)%a.sz[a.n-1]==0) fprintf(fp, "\n");
    }
    fprintf(stdout,"\n");
}




/**
matrix  decompQR(matrix xx, imatrix col)

2次元行列 xxのQR分解を行い,R行列を返す．

<b>xx = Q・R</b>
- xxのQR分解を行った後,係数のマトリックス(上三角行列 R)を返す．
- ピボット操作（列の入れ換え）をした場合は整数ベクトル colに Qの 基底ベクトルが入る．
- colは予め make_imatrix()等によって確保されていなければならない．

@param  xx  QR分解を行う2次元行列．
@param  col メモリを確保し,初期化しておく．Qの基底ベクトルが入る．
@return     上三角行列 R

*/
matrix  decompQR(matrix xx, imatrix col)
{
    int    i, j, r, n, m, sz[2];
    double s, t, u;
    matrix  nsq, isq, x, a;

    a.n = a.r = 0;
    a.sz = NULL;
    a.mx = NULL;
    if (xx.mx==NULL || col.mx==NULL)      return a;
    if (xx.n!=2 || (xx.sz[1]!=col.sz[0])) return a;
	
    nsq.mx = isq.mx = x.mx = NULL;
    n = xx.sz[0];
    m = xx.sz[1];
    sz[0] = sz[1] = m;
    nsq = make_matrix(1, &m);
    isq = make_matrix(1, &m);
    x   = make_matrix(xx.n, xx.sz);
    a   = make_matrix(xx.n, sz);
    if (nsq.mx==NULL || isq.mx==NULL || x.mx==NULL || a.mx==NULL) {
		free_matrix(&nsq); 
		free_matrix(&isq); 
		free_matrix(&x); 
		return a;
    }

    for (i=0; i<xx.r; i++) x.mx[i] = xx.mx[i];

    for (i=1; i<=m; i++) {
		for (s=0.0,j=1; j<=n; j++) s = s + Mx(x, j, i)*Mx(x, j, i);
		Vt(nsq, i) = s;
		Vt(isq, i) = (double)((Vt(nsq, i)!=0) ? Vt(nsq,i) : -1.0);
		Vt(col, i) = i;
    }

    for (r=1; r<=m; r++) {
		if (r!=1) {
	    	j = r; u = 0.0;
	    	for (i=r; i<=m; i++) {
				t = Vt(nsq,i)/Vt(isq,i);
				if (t>u) { u = t; j = i; }
	    	}
	    	i = Vt(col,j); Vt(col,j) = Vt(col,r); Vt(col,r) = i;
	    	t = Vt(nsq,j); Vt(nsq,j) = Vt(nsq,r); Vt(nsq,r) = t;
	    	t = Vt(isq,j); Vt(isq,j) = Vt(isq,r); Vt(isq,r) = t;
  	    	for (i=1; i<=n; i++) {
				t = Mx(x,i,j);
				Mx(x,i,j) = Mx(x,i,r);
				Mx(x,i,r) = t;
	    	}
		}

		for (u=0.0,i=r; i<=n; i++) u = u + Mx(x,i,r)*Mx(x,i,r);
		u = sqrt(u); 
		if (Mx(x,r,r)<0.0) u = -u;
		Mx(x, r, r) = Mx(x,r,r) + u; 
		t = 1.0/(Mx(x,r,r)*u);

  		for (j=1; j<=r-1; j++) Mx(x,r,j)=0.0;
  		for (j=r+1; j<=m; j++) {
	    	for (s=0.0,i=r; i<=n; i++) s = s + Mx(x,i,r)*Mx(x,i,j);
	    	for (i=r; i<=n; i++) Mx(x,i,j) = Mx(x,i,j) - s*t*Mx(x,i,r);
		}
		Mx(x,r,r) = -u;
    }

    for (i=1; i<=m; i++) {
		for (j=1; j<=m; j++) Mx(a,i,j) = Mx(x,i,j);
    }

    free_matrix(&nsq);
    free_matrix(&isq);
    free_matrix(&x);

    return a;
}




/**
matrix minimum2(matrix y, matrix x)

最小2乗法で方程式の近似解を解き,結果を返す．
ただし, x,yは2次元行列のみ．

@param  y  連立方程式の結果の行列 (例を見よ)
@param  x  連立方程式の変数の行列 (例を見よ)
@return    連立方程式の係数の行列 (例を見よ)

@par 例
以下の場合 A = minimum2(Y, X) で解く(a1,a2を求める)． 
@code
Y = X・A   
	y1 = x11*a1 + x12*a2
	y2 = x21*a1 + x22*a2
	y3 = x31*a1 + x32*a2
@endcode
 */
matrix minimum2(matrix y, matrix x)
{
    int  i, n, m;
    imatrix cl;
    matrix  rx, rt, rr, aa, bb, cc;
   
    cc.n = cc.r = 0;
    cc.sz = NULL;
    cc.mx = NULL;
    if (y.mx==NULL || x.mx==NULL) return cc;
    if (y.sz[0]!=x.sz[0])         return cc;

    n  = x.sz[0];
    m  = x.sz[1];       /* n×m 行列 */
    cl = make_imatrix(1, &m);
    if (cl.mx==NULL) return cc;
    cc = make_matrix (1, &m);
    if (cc.mx==NULL) {free_imatrix(&cl); return cc;}

    rx = decompQR(x, cl);
    rr = invrU_matrix(rx), 
    rt = mlt_matrix(rr, trans_matrix(rr)); 
    aa = mlt_matrix(trans_matrix(x), y), 
    bb = mlt_matrix(rt, aa);

    if (bb.mx!=NULL) for (i=0; i<m; i++) cc.mx[cl.mx[i]-1] = bb.mx[i];

    free_imatrix(&cl);
    free_matrix(&rx);
    free_matrix(&rr);
    free_matrix(&rt);
    free_matrix(&aa);
    free_matrix(&bb);

    return cc;
}




/**
matrix  trans_matrix(matrix a)

2次元行列 aの転置行列を返す．

@param  a  操作対象行列．
@return    aの転置行列．
*/
matrix  trans_matrix(matrix a)
{ 
    int    i, j, k;
    matrix c;
    int sz[2]; 

    c.n = c.r = 0;
    c.sz = NULL;
    c.mx = NULL;
    if (a.mx==NULL || a.n!=2) return c; 

    sz[0] = a.sz[1];
    sz[1] = a.sz[0];
    c = make_matrix(a.n, sz); 
 
    for (k=0; k<c.r; k++) {
        i = k/a.sz[1];
        j = k%a.sz[1]; 
        c.mx[j*sz[1]+i] = a.mx[k]; 
   }    
   return c; 
}       




/**
matrix  invrU_matrix(matrix x)

2次元の上三角行列 xの逆行列を求める．

@param  x  操作対象行列(上三角行列)．
@return    xの逆行列．
*/
matrix  invrU_matrix(matrix x)
{
    int    i, j, k, n;
    double t, u, det;
    matrix a;

    a.n = a.r = 0;
    a.sz = NULL;
    a.mx = NULL;
    if (x.mx==NULL)       return a;
    if (x.sz[0]!=x.sz[1]) return a;

    n = x.sz[0];  // 上三角行列のチェック
    for (j=1; j<n; j++) {
        for (i=j+1; i<=n; i++) {
	    	if (Mx(x,i,j) != 0.0) return a;
		}
    }
		
    det = 1.0;
    a = make_matrix(x.n, x.sz);
    if (a.mx==NULL) return a;

    for (i=0; i<a.r; i++) a.mx[i] = x.mx[i];

    for (k=1; k<=n; k++) {
		t = Mx(a,k,k);
		det = det*t;
		for (i=1; i<=n; i++) Mx(a,i,k) = Mx(a,i,k)/t; 
   		Mx(a,k,k) = 1.0/t;

		for (j=1; j<=n; j++) {
	    	if (j!=k) {
				u = Mx(a,k,j);
				for (i=1; i<=n; i++) {
		    		if (i!=k) Mx(a,i,j) = Mx(a,i,j) - Mx(a,i,k)*u;
		    		else      Mx(a,i,j) = - u/t;
				}
	    	}
		}
    }
    return a;
}



