matrix.cpp

Go to the documentation of this file.

 
#include "matrix.h" 
 
 
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)
{
    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  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)
{
    ivector c;
 
    c.x = x;
    c.y = y;
    c.z = z;
    c.n = sqrt((double)x*x + y*y + z*z);
    return c;
}
 
 
ivector  f2ivector(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)
{
    vector c;
 
    c.x = (double)a.x;
    c.y = (double)a.y;
    c.z = (double)a.z;
    c.n = sqrt(c.x*c.x + c.y*c.y + c.z*c.z);
    return c;
}
 
 
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)
{
    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);
        a.sz = NULL;
        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)
{
    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);
        a.sz = NULL;
        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)
{
    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);
        a.sz = NULL;
        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)
{
    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);
        a.sz = NULL;
        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)
{
    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);
        a.sz = NULL;
        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)
{
    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);
        a.sz = NULL;
        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)
{
    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)
{
    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, ...) 
{
    int   m, d;
    int*  args;
    va_list argsptr;
        
    if (mtx.n<1) return NULL;
    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, ...) 
{
    int   m, d;
    int*  args;
    va_list argsptr;
        
    if (mtx.n<1) return NULL;
    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)
{
    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)
{
    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)
{
    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)
{
    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)
{
    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)
{
    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)
{
    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;}
 
    memset(sz, 0, n*sizeof(int));
    memset(sa, 0, a.n*sizeof(int));
    memset(sb, 0, b.n*sizeof(int));
    memset(sc, 0, n*sizeof(int));
    memset(cx, 0, n*sizeof(int));
 
    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)
{
    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;}
 
    memset(sz, 0, n*sizeof(int));
    memset(sa, 0, a.n*sizeof(int));
    memset(sb, 0, b.n*sizeof(int));
    memset(sc, 0, n*sizeof(int));
    memset(cx, 0, n*sizeof(int));
 
    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)
{
    int  i;
    for (i=0; i<a.r; i++) {
        fprintf(fp, "  %15lf", 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)
{
    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)
{
    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) = ((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)
{
    int  i, 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 行列 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)
{ 
    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)
{
    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;
}