irtc.c
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 #include "eus.h"
00026 #include "nr.h"
00027 #include <math.h>
00028 extern pointer ___irtc();
00029 static register_irtc()
00030 { add_module_initializer("___irtc", ___irtc);}
00031 
00032 #define colsize(p) (intval(p->c.ary.dim[1]))
00033 #define rowsize(p) (intval(p->c.ary.dim[0]))
00034 #define ismatrix(p) ((isarray(p) && \
00035                       p->c.ary.rank==makeint(2) && \
00036                       elmtypeof(p->c.ary.entity)==ELM_FLOAT))
00037 
00038 /*
00039  *
00040  */
00041 
00042 pointer SV_SOLVE(ctx,n,argv)
00043 register context *ctx;
00044 int n;
00045 pointer argv[];
00046 /* (SV_SOLVE mat vec &optional ret) */
00047 { 
00048   pointer a,b,x;
00049   eusfloat_t **aa, *bb, *xx;
00050   int i, j, s;
00051 
00052   ckarg2(2,3);
00053   a=argv[0];  b=argv[1];
00054   if (!ismatrix(a)) error(E_NOVECTOR);
00055   s=colsize(a);
00056   if (!isfltvector(b)) error(E_NOVECTOR);
00057   if (s!=vecsize(b)) error(E_VECSIZE);
00058   if (n==3) {
00059     x=argv[2];
00060     if (!isvector(x)) error(E_NOVECTOR);
00061     if (s!=vecsize(x)) error(E_VECSIZE); }
00062   else  x=(pointer)makefvector(s);
00063 
00064   aa = nr_matrix(1,s,1,s);
00065   bb = nr_vector(1,s);
00066   xx = nr_vector(1,s);
00067   for (i = 0; i < s; i++){
00068     for (j = 0; j < s; j++){
00069       aa[j+1][i+1]=a->c.ary.entity->c.fvec.fv[j*s+i];
00070     }
00071   }
00072   for (i = 0; i < s; i++){
00073     bb[i+1] = b->c.fvec.fv[i];
00074   }
00075   if ( svdsolve(aa, s, s, bb, xx) < 0 ) {
00076     return NIL;
00077   }
00078   
00079   for (i = 0; i < s; i++){
00080     x->c.fvec.fv[i] = xx[i+1];
00081   }
00082 
00083   free_nr_matrix(aa,1,s,1,s);
00084   free_nr_vector(bb,1,s);
00085   free_nr_vector(xx,1,s);
00086 
00087   return(x);}
00088 
00089 pointer SV_DECOMPOSE(ctx,n,argv)
00090 register context *ctx;
00091 int n;
00092 pointer argv[];
00093 /* (SV_DECOMPOSE mat) */
00094 { 
00095   pointer a,ru,rv,rw, rr;
00096   eusfloat_t **u, **v, *w, y;
00097   int c, r, i, j, *idx, k, pc=0;;
00098 
00099   ckarg(1);
00100   a=argv[0];
00101   if (!ismatrix(a)) error(E_NOVECTOR);
00102   c = colsize(a);
00103   r = rowsize(a);
00104 
00105   u = nr_matrix(1,r,1,c);
00106   v = nr_matrix(1,c,1,c);
00107   w = nr_vector(1,c);
00108   for (i = 0; i < c; i++){
00109     for (j = 0; j < r; j++){
00110       u[j+1][i+1]=a->c.ary.entity->c.fvec.fv[j*c+i];
00111     }
00112   }
00113   if ( svdcmp(u, r, c, w, v) < 0 ) {
00114     free_nr_matrix(u,1,r,1,c);
00115     free_nr_matrix(v,1,c,1,c);
00116     free_nr_vector(w,1,c);
00117     return NIL;
00118   }
00119 
00120   ru = makematrix(ctx,r,c); vpush(ru); pc++;
00121   rw = makefvector(c);      vpush(rw); pc++;
00122   rv = makematrix(ctx,c,c); vpush(rv); pc++;
00123 
00124   idx = malloc(sizeof(int)*(c+1));
00125 
00126   for (i = 0; i < c; i++){ idx[i+1] = i+1 ;}
00127   for (i = 0; i < c; i++) {
00128     for (j = i+1; j < c; j++) {
00129       if ( w[i+1] < w[j+1] ) {
00130         SWAP(w[i+1], w[j+1]);
00131         k = idx[i+1]; idx[i+1] = idx[j+1]; idx[j+1] = k;
00132       }
00133     }
00134   }
00135   
00136   for (i = 0; i < c; i++){
00137     for (j = 0; j < r; j++){
00138       ru->c.ary.entity->c.fvec.fv[j*c+i] = u[j+1][idx[i+1]];
00139     }
00140   }
00141   for (i = 0; i < c; i++){
00142     rw->c.fvec.fv[i] = w[i+1];
00143   }
00144   for (i = 0; i < c; i++){
00145     for (j = 0; j < c; j++){
00146       rv->c.ary.entity->c.fvec.fv[j*c+i] = v[j+1][idx[i+1]];
00147     }
00148   }
00149 
00150   free_nr_matrix(u,1,r,1,c);
00151   free_nr_matrix(v,1,c,1,c);
00152   free_nr_vector(w,1,c);
00153 
00154   free(idx);
00155   
00156   while(pc-->0) vpop();
00157   return(cons(ctx,ru,cons(ctx,rw,(cons(ctx,rv,NIL)))));}
00158   
00159 /*
00160  *
00161  */
00162 
00163 pointer LU_SOLVE2(ctx,n,argv) /* re-definition */
00164 register context *ctx;
00165 int n;
00166 pointer argv[];
00167 /* (LU-SOLVE mat perm bvector [result]) */
00168 { pointer a,p,b,x;
00169   int i, j, s;
00170   eusfloat_t **aa, *cols;
00171   int *indx;
00172 
00173   ckarg2(3,4);
00174   a=argv[0];  p=argv[1]; b=argv[2];
00175   if (!ismatrix(a)) error(E_NOVECTOR);
00176   s=colsize(a);
00177   if (!isvector(p) || !isfltvector(b)) error(E_NOVECTOR);
00178   if (s!=vecsize(p) || s!=vecsize(b)) error(E_VECSIZE);
00179   if (n==4) {
00180     x=argv[3];
00181     if (!isvector(x)) error(E_NOVECTOR);
00182     if (s!=vecsize(x)) error(E_VECSIZE); }
00183   else  x=(pointer)makefvector(s);
00184 
00185   aa = nr_matrix(1,s,1,s);
00186   cols = nr_vector(1,s);
00187   indx = malloc(sizeof(int)*(s+1));
00188   for (i=0; i<s; i++)
00189     for (j=0; j<s; j++)
00190       aa[i+1][j+1]=a->c.ary.entity->c.fvec.fv[i*s+j];
00191   for (i=0; i<s; i++) indx[i+1] = intval(p->c.vec.v[i]);
00192   for (i=0; i<s; i++) cols[i+1] = b->c.fvec.fv[i];
00193   lubksb(aa,s,indx,cols);
00194   for (i=0; i<s; i++) x->c.fvec.fv[i] = cols[i+1];
00195   
00196   free_nr_matrix(aa,1,s,1,s);
00197   free_nr_vector(cols,1,s);
00198   free(indx);
00199 
00200   return(x);}
00201 
00202 pointer LU_DECOMPOSE2(ctx,n,argv) /* re-definition */
00203 register context *ctx;
00204 int n;
00205 pointer argv[];
00206 /* (LU-DECOMPOSE mat [result] [tmp-vector]) */
00207 { pointer a,result,pv;
00208   eusfloat_t **aa, d;
00209   int i, j, s, stat, *indx;
00210 
00211   ckarg2(1,3);
00212   a=argv[0];
00213   if (!ismatrix(a)) error(E_NOVECTOR);
00214   s=colsize(a);
00215   if (s!=rowsize(a)) error(E_VECSIZE);
00216   if (n==1) result=a;
00217   else {
00218     result=argv[1];
00219     if (!ismatrix(result)) error(E_NOVECTOR);
00220     if (s!=colsize(result)) error(E_VECSIZE);
00221     copymat(result,a,s); 
00222   }
00223   if (n==3) {
00224     pv=argv[2];
00225     if (!isvector(pv)) error(E_NOVECTOR);
00226     if (s!=vecsize(pv)) error(E_VECSIZE);
00227   }else{
00228     pv=makevector(C_VECTOR,s);
00229   }
00230 
00231   aa = nr_matrix(1,s,1,s);
00232   indx = malloc(sizeof(int)*(s+1));
00233 
00234   for (i=0; i<s; i++)
00235     for (j=0; j<s; j++)
00236       aa[i+1][j+1]=a->c.ary.entity->c.fvec.fv[i*s+j];
00237   stat = ludcmp(aa, s, indx, &d);
00238   for (i=0; i<s; i++) pv->c.vec.v[i]=makeint(indx[i+1]);
00239   for (i=0; i<s; i++)
00240     for (j=0; j<s; j++)
00241       result->c.ary.entity->c.fvec.fv[i*s+j] = aa[i+1][j+1];
00242 
00243   free_nr_matrix(aa,1,s,1,s);
00244   free(indx);
00245 
00246   if (stat<0) return(NIL);
00247   else return(pv);}
00248 
00249 pointer MATRIX_DETERMINANT(ctx,n,argv)
00250 register context *ctx;
00251 int n;
00252 pointer argv[];
00253 { pointer a,result;
00254   numunion nu;
00255   eusfloat_t **aa, d;
00256   int i, j, s, stat, *indx;
00257 
00258   ckarg2(1,2);
00259   a=argv[0];
00260   if (!ismatrix(a)) error(E_NOVECTOR);
00261   s=colsize(a);
00262   if (s!=rowsize(a)) error(E_VECSIZE);
00263   if (n==1) result=a;
00264   else {
00265     result=argv[1];
00266     if (!ismatrix(result)) error(E_NOVECTOR);
00267     if (s!=colsize(result)) error(E_VECSIZE);
00268     copymat(result,a,s); 
00269   }
00270 
00271   aa = nr_matrix(1,s,1,s);
00272   indx = malloc(sizeof(int)*(s+1));
00273 
00274   for (i=0; i<s; i++)
00275     for (j=0; j<s; j++)
00276       aa[i+1][j+1]=a->c.ary.entity->c.fvec.fv[i*s+j];
00277   stat = ludcmp(aa, s, indx, &d);
00278   for (i=0; i<s; i++) d = d*aa[i+1][i+1];
00279   if ( ((-1 * TINY) <= d) && (d <= TINY) ) d = 0.0;
00280 
00281   free_nr_matrix(aa,1,s,1,s);
00282   free(indx);
00283 
00284   if (stat<0) return(makeflt(0.0));
00285   else return(makeflt(d));}
00286 
00287 pointer PSEUDO_INVERSE2(ctx,n,argv)
00288 register context *ctx;
00289 int n;
00290 pointer argv[];
00291 { pointer a,result;
00292   numunion nu;
00293   eusfloat_t **u, **v, *w, y;
00294   int c, r, i, j, k, *idx;
00295 
00296   ckarg2(1,2);
00297   a=argv[0];
00298   if (!ismatrix(a)) error(E_NOVECTOR);
00299   c=colsize(a);
00300   r=rowsize(a);
00301   if (n==1) {
00302     result=makematrix(ctx,c,r); vpush(result);
00303   }else {
00304     result=argv[1];
00305     if (!ismatrix(result)) error(E_NOVECTOR);
00306     if (r!=colsize(result)||c!=rowsize(result)) error(E_VECSIZE);
00307   }
00308 
00309   u = nr_matrix(1,r,1,c);
00310   v = nr_matrix(1,c,1,c);
00311   w = nr_vector(1,c);
00312   for (i = 0; i < c; i++){
00313     for (j = 0; j < r; j++){
00314       u[j+1][i+1]=a->c.ary.entity->c.fvec.fv[j*c+i];
00315     }
00316   }
00317   if ( svdcmp(u, r, c, w, v) < 0 ) {
00318     nrerror("svdcmp() returns error"); 
00319     free_nr_matrix(u,1,r,1,c);
00320     free_nr_matrix(v,1,c,1,c);
00321     free_nr_vector(w,1,c);
00322     return NIL;
00323   }
00324   idx = malloc(sizeof(int)*(c+1));
00325 
00326   for (i = 0; i < c; i++){ idx[i+1] = i+1 ;}
00327   for (i = 0; i < c; i++) {
00328     for (j = i+1; j < c; j++) {
00329       if ( w[i+1] < w[j+1] ) {
00330         SWAP(w[i+1], w[j+1]);
00331         k = idx[i+1]; idx[i+1] = idx[j+1]; idx[j+1] = k;
00332       }
00333     }
00334   }
00335   
00336   // A* = v w ut
00337   for (i=1;i<=c;i++) {
00338     if (w[i]>0.0001) w[i] = 1.0/w[i];
00339   }
00340   for (i=0;i<c;i++) {
00341     for (j=0;j<r;j++) {
00342       result->c.ary.entity->c.fvec.fv[(i)*r+(j)]=0.0;
00343       for (k=0;k<c;k++) {
00344         result->c.ary.entity->c.fvec.fv[(i)*r+(j)]+=
00345           v[(i+1)][idx[(k+1)]]*w[(k+1)]*u[(j+1)][idx[(k+1)]];
00346       }
00347     }
00348   }
00349 
00350   free_nr_matrix(u,1,r,1,c);
00351   free_nr_matrix(v,1,c,1,c);
00352   free_nr_vector(w,1,c);
00353 
00354   free(idx);
00355 
00356   vpop(); // vpush(result)
00357   return(result);}
00358 
00359 /*
00360  *
00361  */
00362 
00363 int matrix2quaternion(eusfloat_t *c, eusfloat_t *q){
00364   eusfloat_t q02, q12, q22, q32;
00365   q02 = (1 + c[0*3+0] + c[1*3+1] + c[2*3+2]) / 4;
00366   q12 = (1 + c[0*3+0] - c[1*3+1] - c[2*3+2]) / 4;
00367   q22 = (1 - c[0*3+0] + c[1*3+1] - c[2*3+2]) / 4;
00368   q32 = (1 - c[0*3+0] - c[1*3+1] + c[2*3+2]) / 4;
00369 
00370   if ( (q02 >= q12) && (q02 >= q22) && (q02 >= q32) ) {
00371     q[0] = sqrt(q02);
00372     q[1] = (c[2*3+1] - c[1*3+2]) / (4 * q[0]);
00373     q[2] = (c[0*3+2] - c[2*3+0]) / (4 * q[0]);
00374     q[3] = (c[1*3+0] - c[0*3+1]) / (4 * q[0]);
00375   } else if ( (q12 >= q02) && (q12 >= q22) && (q12 >= q32) ) {
00376     q[1] = sqrt(q12);
00377     q[0] = (c[2*3+1] - c[1*3+2]) / (4 * q[1]);
00378     q[2] = (c[0*3+1] + c[1*3+0]) / (4 * q[1]);
00379     q[3] = (c[0*3+2] + c[2*3+0]) / (4 * q[1]);
00380   } else if ( (q22 >= q02) && (q22 >= q12) && (q22 >= q32) ) {
00381     q[2] = sqrt(q22);
00382     q[0] = (c[0*3+2] - c[2*3+0]) / (4 * q[2]);
00383     q[1] = (c[0*3+1] + c[1*3+0]) / (4 * q[2]);
00384     q[3] = (c[1*3+2] + c[2*3+1]) / (4 * q[2]);
00385   } else if ( (q32 >= q02) && (q32 >= q12) && (q32 >= q22) ) {
00386     q[3] = sqrt(q32);
00387     q[0] = (c[1*3+0] - c[0*3+1]) / (4 * q[3]);
00388     q[1] = (c[0*3+2] + c[2*3+0]) / (4 * q[3]);
00389     q[2] = (c[1*3+2] + c[2*3+1]) / (4 * q[3]);
00390   } else {
00391     fprintf(stderr, ";; matrix2quaternion q02=%f,q12=%f,q22=%f,q32=%f\n",
00392             q02,q12,q22,q32);
00393     error(E_USER,(pointer)";; matrix2quaternion\n");
00394   }
00395 }
00396 
00397 int quaternion2matrix(eusfloat_t *q, eusfloat_t *c){
00398   eusfloat_t q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
00399   // (+ (* q0 q0) (* q1 q1) (- (* q2 q2)) (- (* q3 q3)))
00400   c[0*3+0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
00401   // (* 2 (- (* q1 q2) (* q0 q3)))
00402   c[0*3+1] = 2 * (q1*q2 - q0*q3);
00403   // (* 2 (+ (* q1 q3) (* q0 q2)))
00404   c[0*3+2] = 2 * (q1*q3 + q0*q2);
00405   // (* 2 (+ (* q1 q2) (* q0 q3)))
00406   c[1*3+0] = 2 * (q1*q2 + q0*q3);
00407   // (+ (* q0 q0) (- (* q1 q1)) (* q2 q2) (- (* q3 q3)))
00408   c[1*3+1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
00409   // (* 2 (- (* q2 q3) (* q0 q1)))
00410   c[1*3+2] = 2 * (q2*q3 - q0*q1);
00411   // (* 2 (- (* q1 q3) (* q0 q2)))
00412   c[2*3+0] = 2 * (q1*q3 - q0*q2);
00413   // (* 2 (+ (* q2 q3) (* q0 q1)))
00414   c[2*3+1] = 2 * (q2*q3 + q0*q1);
00415   // (+ (* q0 q0) (- (* q1 q1)) (- (* q2 q2)) (* q3 q3))
00416   c[2*3+2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
00417 }
00418 
00419 
00420 int quaternion_multiply(eusfloat_t *q1, eusfloat_t *q2, eusfloat_t *q3){
00421   eusfloat_t q10 = q1[0], q11 = q1[1], q12 = q1[2], q13 = q1[3];
00422   eusfloat_t q20 = q2[0], q21 = q2[1], q22 = q2[2], q23 = q2[3];
00423   // (+ (* q10 q20) (- (* q11 q21)) (- (* q12 q22)) (- (* q13 q23)))
00424   q3[0] = q10*q20 - q11*q21 - q12*q22 - q13*q23;
00425   // (+ (* q10 q21)    (* q11 q20)     (* q12 q23)  (- (* q13 q22)))
00426   q3[1] = q10*q21 + q11*q20 + q12*q23 - q13*q22;
00427   // (+ (* q10 q22) (- (* q11 q23))    (* q12 q20)     (* q13 q21))
00428   q3[2] = q10*q22 - q11*q23 + q12*q20 + q13*q21;
00429   // (+ (* q10 q23)    (* q11 q22)  (- (* q12 q21))    (* q13 q20))
00430   q3[3] = q10*q23 + q11*q22 - q12*q21 + q13*q20;
00431 }
00432 
00433 pointer MATTIMES3(ctx,n,argv)
00434      register context *ctx;
00435      int n;
00436      register pointer *argv;
00437 {
00438   register int i;
00439   register pointer p,result;
00440   eusfloat_t *c1,*c2,*c3;
00441   eusfloat_t q1[4], q2[4], q3[4], q;
00442   
00443   ckarg2(2,3);
00444   c1 = argv[0]->c.ary.entity->c.fvec.fv;
00445   c2 = argv[1]->c.ary.entity->c.fvec.fv;
00446   if (n==3) result = argv[2];
00447   else result = makematrix(ctx,3,3);
00448   c3 = result->c.ary.entity->c.fvec.fv;
00449 
00450   /*
00451      (setf c3 (quaternion2matrix 
00452                (normalize-vector (quaternion*
00453                                   (matrix2quaternion c1) 
00454                                   (matrix2quaternion c2)))))
00455   */
00456   matrix2quaternion(c1, q1);
00457   matrix2quaternion(c2, q2);
00458   quaternion_multiply(q1, q2, q3);
00459   //noromalize-vector
00460   q = sqrt(q3[0]*q3[0]+q3[1]*q3[1]+q3[2]*q3[2]+q3[3]*q3[3]);
00461   q3[0] /= q; q3[1] /= q; q3[2] /= q; q3[3] /= q;
00462   quaternion2matrix(q3, c3);
00463 
00464   return(result);
00465 }
00466 
00467 pointer MATPLUS(ctx,n,argv)
00468      register context *ctx;
00469      int n;
00470      register pointer *argv;
00471 {
00472   register int i, j, row, col;
00473   register pointer p,result;
00474   eusfloat_t *c1,*c2,*c3;
00475   
00476   ckarg2(2,3);
00477   if (!ismatrix(argv[0]) || !ismatrix(argv[1])) error(E_NOVECTOR);
00478   c1 = argv[0]->c.ary.entity->c.fvec.fv;
00479   c2 = argv[1]->c.ary.entity->c.fvec.fv;
00480   row = rowsize(argv[0]); col = colsize(argv[0]); 
00481   if (!((row==rowsize(argv[1])) && (col==colsize(argv[1]))) )
00482     error(E_VECINDEX);
00483   if (n==3) {
00484     if (!((row==rowsize(argv[2])) &&
00485           (col==colsize(argv[2])))) error(E_VECINDEX);
00486     result = argv[2];
00487   } else {
00488     result = makematrix(ctx,row,col);
00489   }
00490   c3 = result->c.ary.entity->c.fvec.fv;
00491 
00492   for (i = 0; i< row; i++ ) {
00493     for (j = 0; j< col; j++ ) {
00494       c3[i*col+j] = c1[i*col+j] + c2[i*col+j];
00495     }
00496   }
00497 
00498   return(result);
00499 }
00500 
00501 pointer MATMINUS(ctx,n,argv)
00502      register context *ctx;
00503      int n;
00504      register pointer *argv;
00505 {
00506   register int i, j, row, col;
00507   register pointer p,result;
00508   eusfloat_t *c1,*c2,*c3;
00509   
00510   ckarg2(2,3);
00511   if (!ismatrix(argv[0]) || !ismatrix(argv[1])) error(E_NOVECTOR);
00512   c1 = argv[0]->c.ary.entity->c.fvec.fv;
00513   c2 = argv[1]->c.ary.entity->c.fvec.fv;
00514   row = rowsize(argv[0]); col = colsize(argv[0]); 
00515   if (!((row==rowsize(argv[1])) && (col==colsize(argv[1]))) )
00516     error(E_VECINDEX);
00517   if (n==3) {
00518     if (!((row==rowsize(argv[2])) &&
00519           (col==colsize(argv[2])))) error(E_VECINDEX);
00520     result = argv[2];
00521   } else {
00522     result = makematrix(ctx,row,col);
00523   }
00524   c3 = result->c.ary.entity->c.fvec.fv;
00525 
00526   for (i = 0; i< row; i++ ) {
00527     for (j = 0; j< col; j++ ) {
00528       c3[i*col+j] = c1[i*col+j] - c2[i*col+j];
00529     }
00530   }
00531 
00532   return(result);
00533 }
00534 
00535 void balanc(eusfloat_t **a, int n)
00536 {
00537   eusfloat_t RADIX = 2.0;
00538   int last,j,i;
00539   eusfloat_t s,r,g,f,c,sqrdx;
00540   sqrdx=RADIX*RADIX;
00541   last=0;
00542   while (last == 0) {
00543     last=1;
00544     for (i=1;i<=n;i++) { // Calculate row and column norms.
00545       r=c=0.0;
00546       for (j=1;j<=n;j++)
00547         if (j != i) {
00548           c += fabs(a[j][i]);
00549           r += fabs(a[i][j]);
00550         }
00551       if (c && r) { // If both are nonzero,
00552         g=r/RADIX;
00553         f=1.0;
00554         s=c+r;
00555         while (c<g) { // find the integer power of the machine radix that comes closest to balancing the matrix.
00556           f *= RADIX;
00557           c *= sqrdx;
00558         }
00559         g=r*RADIX;
00560         while (c>g) {
00561           f /= RADIX;
00562           c /= sqrdx;
00563         }
00564         if ((c+r)/f < 0.95*s) {
00565           last=0;
00566           g=1.0/f;
00567           for (j=1;j<=n;j++) a[i][j] *= g; // Apply similarity transformation.
00568           for (j=1;j<=n;j++) a[j][i] *= f;
00569         }
00570       }
00571     }
00572   }
00573 }
00574 
00575 #define SWAP(g,h) {y=(g);(g)=(h);(h)=y;}
00576 void elmhes(eusfloat_t **a, int n)
00577 {
00578   int m,j,i;
00579   eusfloat_t y,x;
00580   for (m=2;m<n;m++) { // m is called r + 1 in the text.
00581     x=0.0;
00582     i=m;
00583     for (j=m;j<=n;j++) { // Find the pivot.
00584       if (fabs(a[j][m-1]) > fabs(x)) {
00585         x=a[j][m-1];
00586         i=j;
00587       }
00588     }
00589     if (i != m) { // Interchange rows and columns.
00590       for (j=m-1;j<=n;j++) SWAP(a[i][j],a[m][j]);
00591       for (j=1;j<=n;j++) SWAP(a[j][i],a[j][m]);
00592     }
00593     if (x) { // Carry out the elimination.
00594       for (i=m+1;i<=n;i++) {
00595         if ((y=a[i][m-1]) != 0.0) {
00596           y /= x;
00597           a[i][m-1]=y;
00598           for (j=m;j<=n;j++)
00599             a[i][j] -= y*a[m][j];
00600           for (j=1;j<=n;j++)
00601             a[j][m] += y*a[j][i];
00602         }
00603       }
00604     }
00605   }
00606 }
00607 
00608 int hqr(eusfloat_t **a, int n, eusfloat_t wr[], eusfloat_t wi[])
00609 {
00610   int nn,m,l,k,j,its,i,mmin;
00611   eusfloat_t z,y,x,w,v,u,t,s,r,q,p,anorm;
00612   anorm=0.0; // Compute matrix norm for possible use inlocating  single small subdiagonal element. 
00613   for (i=1;i<=n;i++)
00614     for (j=max(i-1,1);j<=n;j++)
00615       anorm += fabs(a[i][j]);
00616   nn=n;
00617   t=0.0; //Gets changed only by an exceptional shift.
00618   while (nn >= 1) { // Begin search for next eigenvalue.
00619     its=0;
00620     do {
00621       for (l=nn;l>=2;l--) { // Begin iteration: look for single small subdiagonal element. 
00622         s=fabs(a[l-1][l-1])+fabs(a[l][l]);
00623         if (s == 0.0) s=anorm;
00624         if ((eusfloat_t)(fabs(a[l][l-1]) + s) == s) {
00625           a[l][l-1]=0.0;
00626           break;
00627         }
00628       }
00629       x=a[nn][nn];
00630       if (l == nn) { // One root found.
00631         wr[nn]=x+t;
00632         wi[nn--]=0.0;
00633       } else {
00634         y=a[nn-1][nn-1];
00635         w=a[nn][nn-1]*a[nn-1][nn];
00636         if (l == (nn-1)) { // Two roots found...
00637           p=0.5*(y-x);
00638           q=p*p+w;
00639           z=sqrt(fabs(q));
00640           x += t;
00641           if (q >= 0.0) { // ...a real pair.
00642             z=p+SIGN(z,p);
00643             wr[nn-1]=wr[nn]=x+z;
00644             if (z) wr[nn]=x-w/z;
00645             wi[nn-1]=wi[nn]=0.0;
00646           } else { // ...a complex pair.
00647             wr[nn-1]=wr[nn]=x+p;
00648             wi[nn-1]= -(wi[nn]=z);
00649           }
00650           nn -= 2;
00651         } else { // No roots found. Continue iteration.
00652           if (its == 30) {nrerror("Too many iterations in hqr"); return -1;}
00653           if (its == 10 || its == 20) { // Form exceptional shift.
00654             t += x;
00655             for (i=1;i<=nn;i++) a[i][i] -= x;
00656             s=fabs(a[nn][nn-1])+fabs(a[nn-1][nn-2]);
00657             y=x=0.75*s;
00658             w = -0.4375*s*s;
00659           }
00660           ++its;
00661           for (m=(nn-2);m>=l;m--) { // Form shift and then look for 2 consecutive small subdiagonal elements.
00662             z=a[m][m];
00663             r=x-z;
00664             s=y-z;
00665             p=(r*s-w)/a[m+1][m]+a[m][m+1]; // Equation (11.6.23).
00666             q=a[m+1][m+1]-z-r-s;
00667             r=a[m+2][m+1];
00668             s=fabs(p)+fabs(q)+fabs(r); // Scale to prevent overflow or underflow.
00669             p /= s;
00670             q /= s;
00671             r /= s;
00672             if (m == l) break;
00673             u=fabs(a[m][m-1])*(fabs(q)+fabs(r));
00674             v=fabs(p)*(fabs(a[m-1][m-1])+fabs(z)+fabs(a[m+1][m+1]));
00675             if ((eusfloat_t)(u+v) == v) break; // Equation (11.6.26).
00676           }
00677           for (i=m+2;i<=nn;i++) {
00678             a[i][i-2]=0.0;
00679             if (i != (m+2)) a[i][i-3]=0.0;
00680           }
00681           for (k=m;k<=nn-1;k++) {
00682             // Double QR step on rows l to nn and columns m to nn.
00683             if (k != m) {
00684               p=a[k][k-1]; // Begin setup of Householder vector.
00685               q=a[k+1][k-1];
00686               r=0.0;
00687               if (k != (nn-1)) r=a[k+2][k-1];
00688               if ((x=fabs(p)+fabs(q)+fabs(r)) != 0.0) {
00689                 p /= x; // Scale to prevent overflow or underflow.
00690                 q /= x;
00691                 r /= x;
00692               }
00693             }
00694             if ((s=SIGN(sqrt(p*p+q*q+r*r),p)) != 0.0) {
00695               if (k == m) {
00696                 if (l != m)
00697                   a[k][k-1] = -a[k][k-1];
00698               } else
00699                 a[k][k-1] = -s*x;
00700               p += s; // Equations (11.6.24).
00701               x=p/s;
00702               y=q/s;
00703               z=r/s;
00704               q /= p;
00705               r /= p;
00706               for (j=k;j<=nn;j++) { // Row modification.
00707                 p=a[k][j]+q*a[k+1][j];
00708                 if (k != (nn-1)) {
00709                   p += r*a[k+2][j];
00710                   a[k+2][j] -= p*z;
00711                 }
00712                 a[k+1][j] -= p*y;
00713                 a[k][j] -= p*x;
00714               }
00715               mmin = nn<k+3 ? nn : k+3;
00716               for (i=l;i<=mmin;i++) { // Column modification.
00717                 p=x*a[i][k]+y*a[i][k+1];
00718                 if (k != (nn-1)) {
00719                   p += z*a[i][k+2];
00720                   a[i][k+2] -= p*r;
00721                 }
00722                 a[i][k+1] -= p*q;
00723                 a[i][k] -= p;
00724               }
00725             }
00726           }
00727         }
00728       }
00729     } while (l < nn-1);
00730   }
00731   return 1;
00732 }
00733 
00734 eusfloat_t pythag(eusfloat_t a, eusfloat_t b)
00735 {
00736   eusfloat_t absa, absb;
00737   absa=fabs(a);
00738   absb=fabs(b);
00739   if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa));
00740   else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
00741 }
00742 
00743 pointer QL_DECOMPOSE(ctx,n,argv)
00744 register context *ctx;
00745 int n;
00746 pointer argv[];
00747 /* (QL_DECOMPOSE mat) */
00748 {
00749   pointer a,re,rv;
00750   eusfloat_t **aa, *d, *e;
00751   int c, i, j;
00752 
00753   ckarg(1);
00754   a=argv[0];
00755   if (!ismatrix(a)) error(E_NOVECTOR);
00756   c = colsize(a);
00757   if(c != rowsize(a)) error(E_VECSIZE);
00758 
00759   aa = nr_matrix(1,c,1,c);
00760   d = nr_vector(1,c);
00761   e = nr_vector(1,c);
00762   re = makefvector(c);
00763   rv = makematrix(ctx,c,c);
00764 
00765   for (i = 0; i < c; i++){
00766     for (j = 0; j < c; j++){
00767       aa[j+1][i+1]=a->c.ary.entity->c.fvec.fv[j*c+i];
00768     }
00769   }
00770 
00771   tred2(aa, c, d, e);
00772   if ( tqli(d, e, c, aa) < 0 ) {
00773     free_nr_matrix(aa,1,c,1,c);
00774     free_nr_vector(d,1,c);
00775     free_nr_vector(e,1,c);
00776     return NIL;
00777   }
00778 
00779   for (i = 0; i < c; i++){
00780     re->c.fvec.fv[i] = d[i+1];
00781   }
00782   for (i = 0; i < c; i++){
00783     for (j = 0; j < c; j++){
00784       rv->c.ary.entity->c.fvec.fv[j*c+i] = aa[j+1][i+1];
00785     }
00786   }
00787 
00788   free_nr_matrix(aa,1,c,1,c);
00789   free_nr_vector(d,1,c);
00790   free_nr_vector(e,1,c);
00791   return (cons(ctx,re,cons(ctx,rv,NIL)));}
00792 
00793 pointer QR_DECOMPOSE(ctx,n,argv)
00794 register context *ctx;
00795 int n;
00796 pointer argv[];
00797 /* (QR_DECOMPOSE mat) */
00798 {
00799   pointer a,rr,ri, r;
00800   eusfloat_t **aa, *wr, *wi;
00801   int c, i, j, pc=0;
00802 
00803   ckarg(1);
00804   a=argv[0];
00805   if (!ismatrix(a)) error(E_NOVECTOR);
00806   c = colsize(a);
00807   if(c != rowsize(a)) error(E_VECSIZE);
00808 
00809   aa = nr_matrix(1,c,1,c);
00810   wr = nr_vector(1,c);
00811   wi = nr_vector(1,c);
00812   rr = makefvector(c); vpush(rr); pc++;
00813   ri = makefvector(c); vpush(ri); pc++;
00814 
00815   for (i = 0; i < c; i++){
00816     for (j = 0; j < c; j++){
00817       aa[j+1][i+1]=a->c.ary.entity->c.fvec.fv[j*c+i];
00818     }
00819   }
00820 
00821   balanc(aa, c);
00822   elmhes(aa, c);
00823   if ( hqr(aa, c, wr, wi) < 0 ) {
00824     free_nr_matrix(aa,1,c,1,c);
00825     free_nr_vector(wr,1,c);
00826     free_nr_vector(wi,1,c);
00827     while(pc-->0) vpop();
00828     return NIL;
00829   }
00830 
00831   for (i = 0; i < c; i++){
00832     rr->c.fvec.fv[i] = wr[i+1];
00833     ri->c.fvec.fv[i] = wi[i+1];
00834   }
00835 
00836   free_nr_matrix(aa,1,c,1,c);
00837   free_nr_vector(wr,1,c);
00838   free_nr_vector(wi,1,c);
00839 
00840   while(pc-->0) vpop();
00841   return (cons(ctx,rr,cons(ctx,ri,NIL)));};
00842 
00843 pointer ___irtc(ctx,n,argv, env)
00844 register context *ctx;
00845 int n;
00846 pointer argv[];
00847 pointer env;
00848 {
00849   pointer mod=argv[0];
00850   defun(ctx,"ROTM3*",mod,MATTIMES3);
00851   defun(ctx,"M+",mod,MATPLUS);
00852   defun(ctx,"M-",mod,MATMINUS);
00853   defun(ctx,"SV-SOLVE",mod,SV_SOLVE);
00854   defun(ctx,"SV-DECOMPOSE",mod,SV_DECOMPOSE);
00855   defun(ctx,"LU-SOLVE2",mod,LU_SOLVE2);
00856   defun(ctx,"LU-DECOMPOSE2",mod,LU_DECOMPOSE2);
00857   defun(ctx,"MATRIX-DETERMINANT",mod,MATRIX_DETERMINANT);
00858   defun(ctx,"PSEUDO-INVERSE2",mod,PSEUDO_INVERSE2);
00859   defun(ctx,"QL-DECOMPOSE",mod,QL_DECOMPOSE);
00860   defun(ctx,"QR-DECOMPOSE",mod,QR_DECOMPOSE);
00861 
00862   /* irteus-version */
00863   extern pointer QVERSION;
00864   pointer p, v = speval(QVERSION);
00865   p=cons(ctx,makestring(SVNVERSION,strlen(SVNVERSION)),NIL);
00866   vpush(v); vpush(p);
00867   v=NCONC(ctx,2,ctx->vsp-2);
00868 }
00869 


jskeus
Author(s): JSK Alumnis
autogenerated on Fri Aug 28 2015 11:15:08