matrix.java

Java实现的各种数学算法

    add( m1, m2, mr );

    return mr;
  } //add(double)


  public static void add(final double[][] m1, final double[][] m2,
			 double[][] mr)
  {
    array.assertCongruent(m1, m2);
    array.assertCongruent(m1, mr);
    int nr = m1.length;
    int nc = m1[0].length;

    for( int ir = 0; ir < nr; ir++ ) {
      double[] m1row = m1[ir];
      double[] m2row = m2[ir];
      double[] mrrow = mr[ir];
      for( int ic = 0; ic < nc; ic++ ) {
	mrrow[ic] = m1row[ic] + m2row[ic];
      }
    }
  } //add(double)


  public static void add(final double[] v1, final double[] v2,
			 double[] v3)
  {
    int len = v1.length;
    zliberror._assert(v2.length == len);
    zliberror._assert(v3.length == len);

    for( int i = 0; i < len; i++ ) {
      v3[i] = v1[i] + v2[i];
    }
  } //add


  // dont like the 'addfun' naming
  public static double[] add(final double[] v1, final double[] v2)
  {
    int len = v1.length;
    zliberror._assert(v2.length == len);
    double[] v3 = new double[len];

    add(v1, v2, v3);

    return v3;
  } //add

  //----------------------------------------------------------------
  // BLAS-like  naming conventions:
  // saxpy = scalar a x + y   = ax+y
  // gaxpy = general a x + y =  Ax+y
  // sscal = single scale = a x
  // dscal = double scale = a x
  // (zilla)
  // funsaxpy = functional version, returns 
  //----------------------------------------------------------------

  /**
   */
  public static double[][] alloc(int nr, int nc)
  {
    return new double[nr][nc];
  }

  /**
   */
  public static double[] alloc(int nr)
  {
    return new double[nr];
  }

  //----------------------------------------------------------------

  /**
   * todo is there a better algorithm?
   * maybe not, jama uses this algorithm
   */
  public static double[][] transpose(final double[][] A)
  {
    int N = A.length;
    zliberror._assert(A[0].length == N, "transpose needs sqauare matrix");
    double[][] B = alloc(N,N);
    for( int r=0; r < N; r++ ) {
	for( int c=0; c < N; c++ ) {
	    B[r][c] = A[c][r];
	}
    }
    return B;
  } //transpose

  //----------------------------------------------------------------
  // matrix-matrix multiply
  //----------------------------------------------------------------

  /**
   * Conformal matrix multiply M3[n1,n3] = M1[n1,n2] * M2[n2,n3]; 
   * M3 must be distinct from M1,M2.
   */
  public static void multiply(final double[][] A, final double[][] B,
			      double[][] C)
  {
    int nr = A.length;
    int nc = B[0].length;
    int ni = A[0].length;
    zliberror._assert(ni == B.length, "matrices do not conform");

    for( int r=0; r < nr; r++ ) {
      for( int c=0; c < nc; c++ ) {
	double sum = 0.0;
	for( int i=0; i < ni; i++ ) {
	  sum += (A[r][i] * B[i][c]);
	}
	C[r][c] = sum;
      }
    }
  } //multiply

  public static double[][] multiply(final double[][] A, final double[][] B)
  {
    int nr = A.length;
    int nc = B[0].length;
    double[][] C = new double[nr][nc];

    multiply(A,B, C);

    return C;
  } //multiply

  //----------------------------------------------------------------
  // matrix-vector multiply
  //----------------------------------------------------------------

  /**
   * multiply M,v
   */
  public static double[] multiply(final double[][] M, final double[] v1)
  {
    int nr = M.length;
    int nc = M[0].length;
    zliberror._assert(v1.length == nc);

    double[] v2 = new double[nr];

    for( int r=0; r < nr; r++ ) {
	double sum = 0.0;
	for( int c=0; c < nc; c++ ) sum += (M[r][c] * v1[c]);
	v2[r] = sum;
    }

    return v2;
  } /*multiply*/


  /**
   * multiply M,v1 -> v2
   */
  public static void multiply(final double[][] M, final double[] v1,
			      double[] v2)
  {
    int nr = M.length;
    int nc = M[0].length;
    zliberror._assert(v1.length == nc);
    zliberror._assert(v2.length == nr);

    for( int r=0; r < nr; r++ ) {
	double sum = 0.0;
	for( int c=0; c < nc; c++ ) sum += (M[r][c] * v1[c]);
	v2[r] = sum;
    }

  } /*multiply*/

} //matrix


/* matrix.c -3 - a few general matrix routines
 * modified
 * 9apr/2       ansi update
 *
 * package: Mat
 * todo:
 * include solve(), renamed to MatSolve() for conflicts.
 */

/****************************************************************

  /+* Copies M1 to M2.
   +/

  final public static void MatCopy(M1,M2,m,n)
  Flotype *M1,*M2;
  int4 m,n;
{
    Zbcopy((char *)M1,(char *)M2,m*n*sizeof(Flotype));
} /+Copy+/


/+OLDENTRY
MatMul(M1,M2,M3,N)
Square matrix multiply M3 = M1*M2; M3 must be distinct from M1,M2.
+/

final public static void MatMul(A_,B_,C_,N)
  Flotype *A_,*B_,*C_;
  int4 N;
{
    register int4 r,c,i;
    register Flotype sum;

#   define A(i,j) A_[(i)*N+(j)]
#   define B(i,j) B_[(i)*N+(j)]
#   define C(i,j) C_[(i)*N+(j)]

    for( r=0; r < N; r++ ) {
	for( c=0; c < N; c++ ) {
	    sum = 0.0;
	    for( i=0; i < N; i++ ) {
		sum += A(r,i) * B(i,c);
	    }
	    C(r,c) = sum;
	}
    }

#   undef A
#   undef B
#   undef C

} /+matmul+/





/+OLDENTRY
double MatInvert(M,N)
Inverts M in place using Gauss-Jordan elimination, returning the determinant.
Code from 'invert' in IBM scientific subroutine library (public domain).
+/

double
MatInvert( mat, n )
  register Flotype mat[];
  register int4 n;
{
#   define MAXMAT 100
    double det;
    register Flotype biga, hold;
    int4 l[MAXMAT], m[MAXMAT];
    register int4 i,j,k;
    int4 ij,jk,ik,ji, iz, ki, kj;
    int4 jp,jq,jr;
    int4 nk,kk;
    extern double fabs();
 
    if( n >= MAXMAT ) {
      fprintf( stderr, "Matrix Invert : %d too large \n", n);
      return 0.0;
    }

    det = 1.0;
    nk = -n;
    for(k=0; k<n; k++ ) {
	nk=nk+n; kk=nk+k;
	/+ 
	 *  Search for maximum value
	 +/
	l[k] = k;
	m[k] = k;
	biga = mat[kk];
	for(j=k; j<n; j++ ) {
	    iz=n*j;
	    for(i=k; i<n; i++) {
		ij=iz+i;
		if( fabs(biga)-fabs(mat[ij]) < 0.0 ) {
		    biga=mat[ij];
		    l[k]=i;
		    m[k]=j;
		 }
             }
	 }

	/+
	 *  Interchange rows  
	 +/
	j = l[k];
	if(j>k) {
	    ki=k-n;
	    for(i=0; i<n; i++ ) {
		ki=ki+n; ji=ki-k+j;
		hold= -mat[ki]; mat[ki]=mat[ji]; mat[ji]=hold;
	      }
	 }

	/+
	 *  Interchange columns 
	 +/
	i=m[k];
	if(i>k) {
	    jp=n*i;
	    for(j=0; j<n; j++ ) {
		jk=nk+j; ji=jp+j;
		hold = -mat[jk]; mat[jk]=mat[ji]; mat[ji]=hold;
	     }
	 }

	/+
	 *	Divide column by minus pivot. The value of 
	 *	pivot is contained in biga.
	 +/
	if(biga==0.0) {
	    fprintf(stderr,"Matrix Invert : singular matrix \n");
	    return 0.0;
	 }
	for(i=0; i<n; i++ ) {
	    if(i!=k) {
		ik = nk + i;
		mat[ik]=mat[ik]/(-biga);
	     }
	 }

	/+
	 *	Reduce matrix
	 +/
	for(i=0; i<n; i++) {
	    ik=nk+i; hold = mat[ik]; ij=i-n;
	    for(j=0; j<n; j++ ) {
		ij=ij+n;
		if(i!=k) if(j!=k) {
		    kj=ij-i+k;
		    mat[ij]=hold*mat[kj]+mat[ij];
		 }
	     }
	 }

	/+
	 *  Divide row by pivot  
	 +/
	kj=k-n;
	for(j=0; j<n; j++) {
	    kj=kj+n;
	    if(k!=j) mat[kj] = mat[kj]/biga;
	 }

	/+
	 *  Product of pivots  
	 +/
	det *= biga;

	/+
	 *  Replace pivot by reciprocal 
	 +/
	mat[kk]=1.0/biga;
    }

  /+  Final row and column interchange +/
  for(k=n-2; k>=0; k--) {
      i=l[k];
      if(i>k) {
          jq=n*k; jr=n*i;
          for(j=0; j<n; j++ ) {
            jk=jq+j; ji=jr+j; 
            hold=mat[jk]; mat[jk] = -mat[ji]; mat[ji]=hold;
           }
       }

       j=m[k];
       if(j>k) {
	   ki=k-n;
           for(i=0; i<n; i++ ) {
	       ki=ki+n; ji=ki-k+j;
	       hold=mat[ki]; mat[ki]= -mat[ji]; mat[ji]=hold;
            }
        }
    }

  return( (Flotype)det );
} /+matinvert+/


/+OLDENTRY
double MatInvert2(M1,M2,N)
Inverts M1 into M2, returning the determinant.
+/

double
MatInvert2( a, b, n )
  register Flotype *a, *b;
  int4 n;
{
  register int4 i, j;
  register Flotype *bb;
  bb = b;
  for( i=0; i<n; i++ ) for( j=0; j<n; j++ ) *bb++ = *a++;
  return( MatInvert( b, n ) );
} /+matinvert2+/


/+OLDENTRY
double MatSolve(n,M,b)
Solve an nxn system of linear equations Mx=b
using Gaussian elimination with partial pivoting.
leave solution x in b array (destroying original A and b in the process)
Returns determinant.
(code from Paul Heckbert).
+/

double
MatSolve(n,A,b)
  register Flotype *A,*b;
  int4 n;
{
   register int4 i,j,k;
   double max,t,det,sum,pivot;	/+ keep these double +/
   extern double fabs();

#  define swap(a,b,t) {t=a; a=b; b=t;}
#  define a(i,j) A[(i)*n+(j)]

   /+---------- forward elimination ----------+/

   det = 1.;
   for (i=0; i<n; i++) {		/+ eliminate in column i +/
      max = -1.;
      for (k=i; k<n; k++)		/+ find pivot for column i +/
         if (fabs(a(k,i))>max) {
            max = fabs(a(k,i));
            j = k;
         }
      if (max<=0.) return(0.);		/+ if no nonzero pivot, PUNT +/
      if (j!=i) {			/+ swap rows i and j +/
         for (k=i; k<n; k++)
            swap(a(i,k),a(j,k),t);
         det = -det;
         swap(b[i],b[j],t);		/+ swap elements of column vector +/
      }
      pivot = a(i,i);
      det *= pivot;
      for (k=i+1; k<n; k++)		/+ only do elems to right of pivot +/
         a(i,k) /= pivot;

      /+ we know that a(i,i) will be set to 1, so why bother to do it? +/
      b[i] /= pivot;
      for (j=i+1; j<n; j++) {		/+ eliminate in rows below i +/
         t = a(j,i);			/+ we're gonna zero this guy +/
         for (k=i+1; k<n; k++)		/+ subtract scaled row i from row j +/
            a(j,k) -= a(i,k)*t;		/+ (ignore k<=i, we know they're 0) +/
         b[j] -= b[i]*t;
      }
   }

   /+---------- back substitution ----------+/

   for (i=n-1; i>=0; i--) {		/+ solve for x[i] (put it in b[i]) +/
      sum = b[i];
      for (k=i+1; k<n; k++)		/+ really a(i,k)*x[k] +/
         sum -= a(i,k)*b[k];
      b[i] = sum;
   }

   return(det);

#  undef swap
#  undef a

} /+solve+/


/+OLDENTRY
MatRandom(M,m,n)
Fills M with random numbers between -1..1 
(for testing matrix inversion routines).
+/

final public static void MatRandom(M,m,n)
  Flotype *M;
  int4 m,n;
{
    int4 i,l;

    l = n*m;
    for( i=0; i < l; i++ ) *M++ = rndf11();
}


/+***************************************************************
 ************************  Vector  ******************************
 ***************************************************************+/

#define VecKey 'VEC!'

/+OLDENTRY
Flotype *V = VecAlloc(int4 n)
Allocates storage and a type-checking field for
a vector of length n.
+/

Flotype *
VecAlloc(n)
  int4 n;
{
    Flotype *V;

    V = (Flotype *)malloc((n+3) * sizeof(Flotype));

    /+ set type-checking keys before,after the data
     * [key,length,...data...,key]
     +/
    *((int4 *)(V+0)) = (int4)VecKey;
    *((int4 *)(V+1)) = n;	/+ length +/
    *((int4 *)(V+2+n)) = (int4)VecKey;

    return(V+2);
} /+Alloc+/


/+OLDENTRY
VecFree(V)
Frees V; complains if V was not allocated with VecAlloc.
+/

final public static void VecFree(V)
  Flotype *V;
{
    int4 n;

    V -= 2;
    if (*((int4 *)(V+0)) != VecKey)  Zcodeerror("VecFree");

    n = *((int4 *)(V+1));
    Ztrace(("VecFree recovered length %d\n",n));

    if (*((int4 *)(V+2+n)) != (int4)VecKey)
	Zcodeerror("VecFree:corrupted matrix");

    free((char *)V);
} /+Free+/






/+OLDENTRY
VecCopy(V1,V2,n)
Copies V1 to V2.
+/

final public static void VecCopy(V1,V2,n)
  Flotype *V1,*V2;
  int4 n;
{
    Zbcopy((char *)V1,(char *)V2,n*sizeof(Flotype));
} /+Copy+/



/+OLDENTRY
MatVecMul(M,V1, V2,N)
Postmultiplies matrix M by vector V1, result in V2.
+/

final public static void MatVecMul(M,V1, V2,N)
  Flotype *M,*V1,*V2;
  register int4 N;
{
    register int4 i,j;
    register Flotype sum;

    for( i=0; i < N; i++ ) {
	sum = 0.0;
	for( j=0; j < N; j++ ) sum += *M++ * V1[j];
	V2[i] = sum;
    }

} /+MatVecMul+/



/+OLDENTRY
VecRandom(V,n)
Fills V with random numbers between -1..1 
(for testing purposes).
+/

final public static void VecRandom(V,n)
  Flotype *V;
  int4 n;
{
    int4 i;

    for( i=0; i < n; i++ ) *V++ = rndf11();
}



#ifdef TESTIT /+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%+/

/+% cc -DTESTIT -DFLTPREC -Dztrace % -lZ -lm -o matrixTST %+/

main()
{
    Flotype *m,*v,*v2;
    m = MatAlloc(5,5);
    MatFree(m);

    v = VecAlloc(2);
    v2 = VecAlloc(2);

    VecRandom(v,2);
    VecCopy(v,v2,2);
    VecPrint("v: ",v,2); VecPrint("copied: ",v2,2);

    VecFree(v);
    VecFree(v2);
}

#endif /+TESTIT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%+/

****************************************************************/