src/jp/nyatla/nyartoolkit/core/NyARMat.java

   1 /* \r
   2  * PROJECT: NyARToolkit\r
   3  * --------------------------------------------------------------------------------\r
   4  * This work is based on the original ARToolKit developed by\r
   5  *   Hirokazu Kato\r
   6  *   Mark Billinghurst\r
   7  *   HITLab, University of Washington, Seattle\r
   8  * http://www.hitl.washington.edu/artoolkit/\r
   9  *\r
  10  * The NyARToolkit is Java version ARToolkit class library.\r
  11  * Copyright (C)2008 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (at your option) any later version.\r
  17  * \r
  18  * This program is distributed in the hope that it will be useful,\r
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  21  * GNU General Public License for more details.\r
  22  * \r
  23  * You should have received a copy of the GNU General Public License\r
  24  * along with this framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.core;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.NyARException;\r
  35 \r
  36  \r
  37 \r
  38 /**\r
  39  *      ARMat構造体に対応するクラス\r
  40  *      typedef struct {\r
  41  *              double *m;\r
  42  *              int row;\r
  43  *              int clm;\r
  44  *      }ARMat;\r
  45  *\r
  46  */\r
  47 public class NyARMat{\r
  48     private double[][] m;\r
  49     private int clm,row;\r
  50     public NyARMat(int i_row,int i_clm)\r
  51     {\r
  52         m=new double[i_row][i_clm];\r
  53         clm=i_clm;\r
  54         row=i_row;\r
  55     }\r
  56     public int getClm()\r
  57     {\r
  58         return clm;\r
  59     }\r
  60     public int getRow()\r
  61     {\r
  62         return row;\r
  63     }\r
  64     /**\r
  65      * 行列をゼロクリアする。\r
  66      */\r
  67     public void zeroClear()\r
  68     {\r
  69         for(int i=0;i<row;i++){\r
  70             for(int i2=0;i2<clm;i2++){\r
  71                 m[i][i2]=0.0;\r
  72             }\r
  73         }\r
  74     }\r
  75     public double[] getRowArray(int i_row)\r
  76     {\r
  77         return m[i_row];\r
  78     }\r
  79     public double[][] getArray()\r
  80     {\r
  81         return m;\r
  82     }\r
  83     public NyARVec getRowVec(int i_row)\r
  84     {\r
  85         return NyARVec.wrap(m[i_row]);\r
  86     }\r
  87     /**\r
  88      * aとbの積をdestに出力する。arMatrixMul()の代替品\r
  89      * @param dest\r
  90      * @param a\r
  91      * @param b\r
  92      * @throws NyARException\r
  93      */\r
  94     public static void matrixMul(NyARMat dest, NyARMat a, NyARMat b) throws NyARException\r
  95     {\r
  96         if(a.clm != b.row || dest.row != a.row || dest.clm != b.clm){\r
  97             throw new NyARException();\r
  98         }\r
  99         for(int r = 0; r < dest.row; r++){\r
 100             for(int c = 0; c < dest.getClm(); c++){\r
 101                 dest.m[r][c]=0.0;//dest.setARELEM0(r, c,0.0);\r
 102                 for(int i = 0; i < a.getClm(); i++){\r
 103                     dest.m[r][c]+=a.m[r][i]*b.m[i][c];//ARELEM0(dest, r, c) += ARELEM0(a, r, i) * ARELEM0(b, i, c);\r
 104                 }\r
 105             }\r
 106         }\r
 107     }\r
 108     /**\r
 109      * i_targetを逆行列に変換する。arMatrixSelfInv()と、arMatrixSelfInv_minv()関数を合成してあります。\r
 110      * @param i_target\r
 111      * 逆行列にする行列\r
 112      * @throws NyARException\r
 113      */\r
 114     public static void matrixSelfInv(NyARMat i_target) throws NyARException\r
 115     {\r
 116         double[][] ap=i_target.m;\r
 117         int dimen=ap.length;\r
 118         double[] wcp,wap,wbp;\r
 119         int i,j,ip,nwork;\r
 120         int[] nos=new int[50];\r
 121         double epsl;\r
 122         double p,pbuf,work;\r
 123 \r
 124         epsl = 1.0e-10;         /* Threshold value      */\r
 125         /* check size */\r
 126         switch(dimen){\r
 127         case 0:\r
 128             throw new NyARException();\r
 129         case 1:\r
 130             ap[0][0]=1.0/ap[0][0];//*ap = 1.0 / (*ap);\r
 131             return;/* 1 dimension */\r
 132         }\r
 133 \r
 134         for(int n = 0; n < dimen ; n++){\r
 135             nos[n] = n;\r
 136         }\r
 137 \r
 138         /* nyatla memo\r
 139          * ipが定まらないで計算が行われる場合があるので挿入。\r
 140          * ループ内で0初期化していいかが判らない。\r
 141          */\r
 142         ip=0;\r
 143         int wap_ptr,wbp_ptr;\r
 144         for(int n=0; n<dimen;n++)\r
 145         {\r
 146             wcp =ap[n];//wcp = ap + n * rowa;\r
 147             p=0.0;\r
 148             wap_ptr=0;//wap = DoublePointer.wrap(wcp);\r
 149             for(i = n; i<dimen ; i++){//for(i = n, wap = wcp, p = 0.0; i < dimen ; i++, wap += rowa)\r
 150                 wap=ap[i];\r
 151                 if( p < ( pbuf = Math.abs(wap[0]))) {\r
 152                     p = pbuf;\r
 153                     ip = i;\r
 154                 }\r
 155             }\r
 156             if (p <= epsl){\r
 157                 return;\r
 158             }\r
 159 \r
 160             nwork  = nos[ip];\r
 161             nos[ip]= nos[n];\r
 162             nos[n] = nwork;\r
 163             \r
 164             wap=ap[ip];\r
 165             wbp=wcp;\r
 166             wap_ptr=0;\r
 167             wbp_ptr=0;\r
 168             for(j=0; j< dimen ; j++){//for(j = 0, wap = ap + ip * rowa, wbp = wcp; j < dimen ; j++) {\r
 169                 work = wap[wap_ptr];               //work = *wap;\r
 170                 wap[wap_ptr]=wbp[wbp_ptr];wap_ptr++;//*wap++ = *wbp;\r
 171                 wbp[wbp_ptr]=work;wbp_ptr++;     //*wbp++ = work;\r
 172             }\r
 173             \r
 174             wap=wcp;\r
 175             wap_ptr=0;\r
 176             work=wcp[0];\r
 177             for(j = 1; j < dimen ; j++){//for(j = 1, wap = wcp, work = *wcp; j < dimen ; j++, wap++)\r
 178                 wap[wap_ptr]=wap[wap_ptr+1]/work;//*wap = *(wap + 1) / work;\r
 179                 wap_ptr++;\r
 180             }\r
 181             wap[wap_ptr]=1.0/work;//*wap = 1.0 / work;\r
 182 \r
 183             for(i = 0; i < dimen ; i++) {\r
 184                 if(i != n) {\r
 185                     wap =ap[i];//wap = ap + i * rowa;\r
 186                     wbp =wcp;\r
 187                     wap_ptr=0;\r
 188                     wbp_ptr=0;\r
 189                     work=wap[0];\r
 190                     for(j = 1;j < dimen ; j++){//for(j = 1, wbp = wcp, work = *wap;j < dimen ; j++, wap++, wbp++)\r
 191                         wap[wap_ptr]=wap[wap_ptr+1]-work*wbp[wbp_ptr];//wap = *(wap + 1) - work * (*wbp);\r
 192                         wap_ptr++;\r
 193                         wbp_ptr++;\r
 194                     }\r
 195                     wap[wap_ptr]=-work*wbp[wbp_ptr];//*wap = -work * (*wbp);\r
 196                 }\r
 197             }\r
 198         }\r
 199 \r
 200         for(int n = 0; n < dimen ; n++) {\r
 201             for(j = n; j < dimen ; j++){\r
 202                 if( nos[j] == n){\r
 203                     break;\r
 204                 }\r
 205             }\r
 206             nos[j] = nos[n];\r
 207             for(i = 0; i < dimen ;i++){//for(i = 0, wap = ap + j, wbp = ap + n; i < dimen ;i++, wap += rowa, wbp += rowa) {\r
 208                 wap=ap[i];\r
 209                 wbp=ap[i];\r
 210                 work  =wap[j];//work = *wap;\r
 211                 wap[j]=wbp[n];//*wap = *wbp;\r
 212                 wbp[n]=work;//*wbp = work;\r
 213             }\r
 214         }\r
 215         return;\r
 216     }\r
 217     /**\r
 218      * sourceの転置行列をdestに得る。arMatrixTrans()の代替品\r
 219      * @param dest\r
 220      * @param source\r
 221      * @return\r
 222      */\r
 223     public static void matrixTrans(NyARMat dest,NyARMat source) throws NyARException\r
 224     {\r
 225         if(dest.row != source.clm || dest.clm != source.row){\r
 226             throw new NyARException();\r
 227         }\r
 228         NyARException.trap("未チェックのパス");\r
 229         for(int r=0;r< dest.row;r++){\r
 230             for(int c=0;c<dest.clm;c++){\r
 231                 dest.m[r][c]=source.m[c][r];\r
 232             }\r
 233         }\r
 234     }\r
 235     /**\r
 236      * unitを単位行列に初期化する。arMatrixUnitの代替品\r
 237      * @param unit\r
 238      */\r
 239     public static void matrixUnit(NyARMat unit) throws NyARException\r
 240     {\r
 241         if(unit.row != unit.clm){\r
 242             throw new NyARException();\r
 243         }\r
 244         NyARException.trap("未チェックのパス");\r
 245         for(int r = 0; r < unit.getRow(); r++) {\r
 246             for(int c = 0; c < unit.getClm(); c++) {\r
 247                 if(r == c) {\r
 248                     unit.m[r][c]=1.0;\r
 249                 }else{\r
 250                     unit.m[r][c]=0.0;\r
 251                 }\r
 252             }\r
 253         }\r
 254     }\r
 255     /**\r
 256      * destにsourceと同じ内容をコピーする。arMatrixDupの代替品\r
 257      * @param dest\r
 258      * @param source\r
 259      * @return\r
 260      */\r
 261     public static void matrixDup(NyARMat dest, NyARMat source) throws NyARException\r
 262     {\r
 263         if(dest.row != source.row || dest.clm != source.clm)\r
 264         {\r
 265             throw new NyARException();\r
 266         }\r
 267         for(int r = 0; r < source.getRow(); r++) {\r
 268             for(int c = 0; c < source.getClm(); c++)\r
 269             {\r
 270                 dest.m[r][c]=source.m[r][c];\r
 271             }\r
 272         }\r
 273     }\r
 274     public NyARMat matrixAllocDup() throws NyARException\r
 275     {\r
 276         NyARMat result=new NyARMat(row,clm);\r
 277         matrixDup(result,this);\r
 278         return result;\r
 279     }    \r
 280     /**\r
 281      * arMatrixInv関数の代替品です。\r
 282      * destにsourceの逆行列を返します。\r
 283      * @param dest\r
 284      * @param source\r
 285      * @throws NyARException\r
 286      */\r
 287     public static void matrixInv(NyARMat dest,NyARMat source) throws NyARException\r
 288     {\r
 289         NyARException.trap("未チェックのパス");\r
 290         matrixDup(dest, source);\r
 291 \r
 292         NyARException.trap("未チェックのパス");\r
 293         matrixSelfInv(dest);\r
 294     }\r
 295     public NyARMat matrixAllocInv() throws NyARException\r
 296     {\r
 297         NyARException.trap("未チェックのパス");\r
 298         NyARMat result=matrixAllocDup();\r
 299 \r
 300         NyARException.trap("未チェックのパス");\r
 301         matrixSelfInv(result);\r
 302         return result;\r
 303     }\r
 304     /**\r
 305      * dim x dim の単位行列を作る。\r
 306      * @param dim\r
 307      * @return\r
 308      * @throws NyARException\r
 309      */\r
 310     public static NyARMat matrixAllocUnit(int dim) throws NyARException\r
 311     {\r
 312         NyARException.trap("未チェックのパス");\r
 313         NyARMat result = new NyARMat(dim, dim);\r
 314         NyARException.trap("未チェックのパス");\r
 315         NyARMat.matrixUnit(result);\r
 316         return result;\r
 317     }\r
 318     /**\r
 319      * arMatrixDispの代替品\r
 320      * @param m\r
 321      * @return\r
 322      */\r
 323     public int matrixDisp() throws NyARException\r
 324     {\r
 325         NyARException.trap("未チェックのパス");\r
 326         System.out.println(" === matrix ("+row+","+clm+") ===");//printf(" === matrix (%d,%d) ===\n", m->row, m->clm);\r
 327         for(int r = 0; r < row; r++){//for(int r = 0; r < m->row; r++) {\r
 328         System.out.print(" |");//printf(" |");\r
 329         for(int c = 0; c < clm; c++) {//for(int c = 0; c < m->clm; c++) {\r
 330             System.out.print(" "+m[r][c]);//printf(" %10g", ARELEM0(m, r, c));\r
 331         }\r
 332         System.out.println(" |");//printf(" |\n");\r
 333         }\r
 334         System.out.println(" ======================");//printf(" ======================\n");\r
 335         return 0;\r
 336     }\r
 337     private final static double PCA_EPS=1e-6;           //#define     EPS             1e-6\r
 338     private final static int            PCA_MAX_ITER=100;       //#define     MAX_ITER        100\r
 339     private final static double PCA_VZERO=1e-16;        //#define     VZERO           1e-16\r
 340     /**\r
 341      * static int EX( ARMat *input, ARVec *mean )の代替関数\r
 342      * @param input\r
 343      * @param mean\r
 344      * @return\r
 345      * @throws NyARException\r
 346      */\r
 347     private static void PCA_EX(NyARMat input, NyARVec mean) throws NyARException\r
 348     {\r
 349         double[] v;\r
 350 \r
 351         int     row, clm;\r
 352         \r
 353         row = input.row;\r
 354         clm = input.clm;\r
 355         if(row <= 0 || clm <= 0){\r
 356             throw new NyARException();\r
 357         }\r
 358         if( mean.getClm() != clm ){\r
 359             throw new NyARException();\r
 360         }\r
 361         double[] mean_array=mean.getArray();\r
 362         for(int i = 0; i < clm; i++ ){\r
 363             mean_array[i]=0.0;//mean->v[i] = 0.0;\r
 364         }\r
 365         \r
 366         v=mean.getArray();\r
 367         for(int i = 0; i < row; i++ ) {\r
 368             for(int j = 0; j < clm; j++ ){\r
 369                 //*(v++) += *(m++);\r
 370                 v[j]+=input.m[i][j];\r
 371             }\r
 372         }\r
 373         \r
 374         for(int i = 0; i < clm; i++ ){\r
 375         mean_array[i]/=row;//mean->v[i] /= row;\r
 376         }\r
 377     }\r
 378     /**\r
 379      * static int CENTER( ARMat *inout, ARVec *mean )の代替関数\r
 380      * @param inout\r
 381      * @param mean\r
 382      * @return\r
 383      */\r
 384     private static void PCA_CENTER(NyARMat inout, NyARVec mean) throws NyARException\r
 385     {\r
 386         double[] v;\r
 387         int     row, clm;\r
 388         \r
 389         row = inout.getRow();\r
 390         clm = inout.getClm();\r
 391         if(mean.getClm()!= clm){\r
 392             throw new NyARException();\r
 393         }\r
 394         \r
 395         v = mean.getArray();\r
 396         for(int i = 0; i < row; i++ ) {\r
 397             for(int j = 0; j < clm; j++ ){\r
 398                 //*(m++) -= *(v++);\r
 399                 inout.m[i][j]-=v[j];\r
 400             }\r
 401         }\r
 402     }\r
 403     /**\r
 404      * int x_by_xt( ARMat *input, ARMat *output )の代替関数\r
 405      * @param input\r
 406      * @param output\r
 407      * @throws NyARException\r
 408      */\r
 409     private static void PCA_x_by_xt( NyARMat input, NyARMat output) throws NyARException\r
 410     {\r
 411         NyARException.trap("動作未チェック/配列化未チェック");\r
 412         int     row, clm;\r
 413 //        double[][] out;\r
 414         double[] in1,in2;\r
 415         \r
 416         NyARException.trap("未チェックのパス");\r
 417         row = input.row;\r
 418         clm = input.clm;\r
 419         NyARException.trap("未チェックのパス");\r
 420         if( output.row != row || output.clm != row ){\r
 421             throw new NyARException();\r
 422         }\r
 423         \r
 424 //        out = output.getArray();\r
 425         for(int i = 0; i < row; i++ ) {\r
 426             for(int j = 0; j < row; j++ ) {\r
 427                 if( j < i ) {\r
 428                     NyARException.trap("未チェックのパス");{\r
 429                     output.m[i][j]=output.m[j][i];//*out = output->m[j*row+i];\r
 430                     }\r
 431                 }else{\r
 432                     in1=input.getRowArray(i);//in1 = &(input->m[clm*i]);\r
 433                     in2=input.getRowArray(j);//in2 = &(input->m[clm*j]);\r
 434                     output.m[i][j]=0;//*out = 0.0;\r
 435                     for(int k = 0; k < clm; k++ ){\r
 436                         output.m[i][j]+=(in1[k]*in2[k]);//*out += *(in1++) * *(in2++);\r
 437                     }\r
 438                 }\r
 439         //                  out.incPtr();\r
 440             }\r
 441         }\r
 442     }\r
 443     /**\r
 444      * static int xt_by_x( ARMat *input, ARMat *output )の代替関数\r
 445      * @param input\r
 446      * @param output\r
 447      * @throws NyARException\r
 448      */\r
 449     private static void PCA_xt_by_x(NyARMat input, NyARMat output) throws NyARException\r
 450     {\r
 451         double[] in;\r
 452         int     row, clm;\r
 453     \r
 454         row = input.row;\r
 455         clm = input.clm;\r
 456         if(output.row!= clm || output.clm != clm ){\r
 457             throw new NyARException();\r
 458         }\r
 459     \r
 460         for(int i = 0; i < clm; i++ ) {\r
 461             for(int j = 0; j < clm; j++ ) {\r
 462                 if( j < i ) {\r
 463                    output.m[i][j]=output.m[j][i];//*out = output->m[j*clm+i];\r
 464                 }else{\r
 465                     output.m[i][j]=0.0;//*out = 0.0;\r
 466                     for(int k = 0; k < row; k++ ){\r
 467                         in=input.getRowArray(k);\r
 468                         output.m[i][j]+=(in[i]*in[j]);//*out += *in1 * *in2;\r
 469                     }\r
 470                 }\r
 471             }\r
 472         }\r
 473     }\r
 474     /**\r
 475      * static int QRM( ARMat *a, ARVec *dv )の代替関数\r
 476      * @param a\r
 477      * @param dv\r
 478      * @throws NyARException\r
 479      */\r
 480     private static void PCA_QRM(NyARMat a, NyARVec dv) throws NyARException\r
 481     {\r
 482         double  w, t, s, x, y, c;\r
 483         int     dim, iter;\r
 484         double[] dv_array=dv.getArray();\r
 485         \r
 486         dim = a.row;\r
 487         if( dim != a.clm || dim < 2 ){\r
 488             throw new NyARException();\r
 489         }\r
 490         if( dv.getClm() != dim ){\r
 491             throw new NyARException();\r
 492         }\r
 493     \r
 494         NyARVec ev = new NyARVec( dim );\r
 495         double[] ev_array=ev.getArray();\r
 496         if( ev == null ){\r
 497             throw new NyARException();\r
 498         }\r
 499 \r
 500         NyARVec.vecTridiagonalize(a,dv,ev,1);\r
 501     \r
 502         ev_array[0]=0.0;//ev->v[0] = 0.0;\r
 503         for(int h = dim-1; h > 0; h-- ) {\r
 504             int j = h;\r
 505             while(j>0 && Math.abs(ev_array[j]) > PCA_EPS*(Math.abs(dv_array[j-1])+Math.abs(dv_array[j]))){// while(j>0 && fabs(ev->v[j]) > EPS*(fabs(dv->v[j-1])+fabs(dv->v[j]))) j--;\r
 506                 j--;\r
 507             }\r
 508             if( j == h ){\r
 509                 continue;\r
 510             }\r
 511             iter = 0;\r
 512             do{\r
 513                 iter++;\r
 514                 if( iter > PCA_MAX_ITER ){\r
 515                         break;\r
 516                 }\r
 517                 w = (dv_array[h-1] - dv_array[h]) / 2;//w = (dv->v[h-1] - dv->v[h]) / 2;//ここ？\r
 518                 t = ev_array[h] * ev_array[h];//t = ev->v[h] * ev->v[h];\r
 519                 s = Math.sqrt(w*w+t);\r
 520                 if( w < 0 ){\r
 521                         s = -s;\r
 522                 }\r
 523                 x=dv_array[j] - dv_array[h] + t/(w+s);//x = dv->v[j] - dv->v[h] + t/(w+s);\r
 524                 y=ev_array[j+1];//y = ev->v[j+1];\r
 525                 for(int k = j; k < h; k++ ){\r
 526                     if( Math.abs(x) >= Math.abs(y)){\r
 527                         if( Math.abs(x) > PCA_VZERO ) {\r
 528                         t = -y / x;\r
 529                         c = 1 / Math.sqrt(t*t+1);\r
 530                         s = t * c;\r
 531                         }else{\r
 532                         c = 1.0;\r
 533                         s = 0.0;\r
 534                         }\r
 535                     }else{\r
 536                         t = -x / y;\r
 537                         s = 1.0 / Math.sqrt(t*t+1);\r
 538                         c = t * s;\r
 539                     }\r
 540                     w = dv_array[k] - dv_array[k+1];//w = dv->v[k] - dv->v[k+1];\r
 541                     t = (w * s + 2 * c * ev_array[k+1]) * s;//t = (w * s + 2 * c * ev->v[k+1]) * s;\r
 542                     dv_array[k]-=t;//dv->v[k]   -= t;\r
 543                     dv_array[k+1]+=t;//dv->v[k+1] += t;\r
 544                     if( k > j){\r
 545                         NyARException.trap("未チェックパス");{\r
 546                         ev_array[k]=c * ev_array[k] - s * y;//ev->v[k] = c * ev->v[k] - s * y;\r
 547                         }\r
 548                     }\r
 549                     ev_array[k+1]+=s * (c * w - 2 * s * ev_array[k+1]);//ev->v[k+1] += s * (c * w - 2 * s * ev->v[k+1]);\r
 550     \r
 551                     for(int i = 0; i < dim; i++ ){\r
 552                         x = a.m[k][i];//x = a->m[k*dim+i];\r
 553                         y = a.m[k+1][i];//y = a->m[(k+1)*dim+i];\r
 554                         a.m[k][i]=c * x - s * y;//a->m[k*dim+i]     = c * x - s * y;\r
 555                         a.m[k+1][i]=s * x + c * y;//a->m[(k+1)*dim+i] = s * x + c * y;\r
 556                     }\r
 557                     if( k < h-1 ) {\r
 558                         NyARException.trap("未チェックパス");{\r
 559                         x = ev_array[k+1];//x = ev->v[k+1];\r
 560                         y =-s*ev_array[k+2];//y = -s * ev->v[k+2];\r
 561                         ev_array[k+2]*=c;//ev->v[k+2] *= c;\r
 562                         }\r
 563                     }\r
 564                 }\r
 565             }while(Math.abs(ev_array[h]) > PCA_EPS*(Math.abs(dv_array[h-1])+Math.abs(dv_array[h])));\r
 566         }\r
 567         double[] v1,v2;\r
 568         for(int k = 0; k < dim-1; k++ ) {\r
 569             int h = k;\r
 570             t=dv_array[h];//t = dv->v[h];\r
 571             for(int i = k+1; i < dim; i++ ){\r
 572                 if(dv_array[i] > t ){//if( dv->v[i] > t ) {\r
 573                     h = i;\r
 574                     t=dv_array[h];//t = dv->v[h];\r
 575                 }\r
 576             }\r
 577             dv_array[h]=dv_array[k];//dv->v[h] = dv->v[k];\r
 578             dv_array[k]=t;//dv->v[k] = t;\r
 579             v1=a.getRowArray(h);//v1 = &(a->m[h*dim]);\r
 580             v2=a.getRowArray(k);//v2 = &(a->m[k*dim]);\r
 581             for(int i = 0; i < dim; i++ ) {\r
 582                 w=v1[i];//w = *v1;\r
 583                 v1[i]=v2[i];//*v1 = *v2;\r
 584                 v2[i]=w;//*v2 = w;\r
 585             }\r
 586         }\r
 587     }\r
 588     /**\r
 589      * static int EV_create( ARMat *input, ARMat *u, ARMat *output, ARVec *ev )の代替関数\r
 590      * @param input\r
 591      * @param u\r
 592      * @param output\r
 593      * @param ev\r
 594      * @throws NyARException\r
 595      */\r
 596     private static void PCA_EV_create(NyARMat input, NyARMat u, NyARMat output, NyARVec ev) throws NyARException\r
 597     {\r
 598         NyARException.trap("未チェックのパス");\r
 599         int     row, clm;\r
 600         row = input.row;//row = input->row;\r
 601         clm = input.clm;//clm = input->clm;\r
 602         if( row <= 0 || clm <= 0 ){\r
 603             throw new NyARException();\r
 604        }\r
 605         if( u.row != row || u.clm != row ){//if( u->row != row || u->clm != row ){\r
 606             throw new NyARException();\r
 607         }\r
 608         if( output.row != row || output.clm != clm ){//if( output->row != row || output->clm != clm ){\r
 609             throw new NyARException();\r
 610         }\r
 611         if( ev.getClm()!= row ){//if( ev->clm != row ){\r
 612             throw new NyARException();\r
 613         }\r
 614         double[][] m,in;\r
 615         double[]  m1,ev_array;\r
 616         double  sum, work;\r
 617     \r
 618         m =output.getArray();//m = output->m;\r
 619         in=input.getArray();\r
 620         int i;\r
 621         ev_array=ev.getArray();\r
 622         for(i = 0; i < row; i++ ) {\r
 623             NyARException.trap("未チェックのパス");\r
 624             if( ev_array[i]<PCA_VZERO ){//if( ev->v[i] < VZERO ){\r
 625                 break;\r
 626             }\r
 627             NyARException.trap("未チェックのパス");\r
 628             work = 1 / Math.sqrt(Math.abs(ev_array[i]));//work = 1 / sqrt(fabs(ev->v[i]));\r
 629             for(int j = 0; j < clm; j++ ) {\r
 630                 sum = 0.0;\r
 631                 m1=u.getRowArray(i);//m1 = &(u->m[i*row]);\r
 632     //              m2=input.getPointer(j);//m2 = &(input->m[j]);\r
 633                 for(int k = 0; k < row; k++ ) {\r
 634                     sum+=m1[k]+in[k][j];//sum += *m1 * *m2;\r
 635     //                  m1.incPtr();   //m1++;\r
 636     //                  m2.addPtr(clm);//m2 += clm;\r
 637                 }\r
 638                 m1[j]=sum * work;//*(m++) = sum * work;\r
 639     //          {//*(m++) = sum * work;\r
 640     //          m.set(sum * work);\r
 641     //          m.incPtr();}\r
 642             }\r
 643         }\r
 644         for( ; i < row; i++ ) {\r
 645         NyARException.trap("未チェックのパス");\r
 646             ev_array[i]=0.0;//ev->v[i] = 0.0;\r
 647             for(int j = 0; j < clm; j++ ){\r
 648                 m[i][j]=0.0;\r
 649     //          m.set(0.0);//*(m++) = 0.0;\r
 650     //          m.incPtr();\r
 651             }\r
 652         }\r
 653     }\r
 654     /*static int PCA( ARMat *input, ARMat *output, ARVec *ev )*/\r
 655     private static void PCA_PCA(NyARMat input, NyARMat output, NyARVec ev) throws NyARException\r
 656     {\r
 657         NyARMat    u;\r
 658         int     row, clm, min;\r
 659         double[] ev_array=ev.getArray();\r
 660 \r
 661         row =input.row;//row = input->row;\r
 662         clm =input.clm;//clm = input->clm;\r
 663         min =(clm < row)? clm: row;\r
 664         if( row < 2 || clm < 2 ){\r
 665             throw new NyARException();\r
 666         }\r
 667         if( output.clm != input.clm){//if( output->clm != input->clm ){\r
 668             throw new NyARException();\r
 669         }\r
 670         if( output.row!= min ){//if( output->row != min ){\r
 671             throw new NyARException();\r
 672         }\r
 673         if( ev.getClm() != min ){//if( ev->clm != min ){\r
 674             throw new NyARException();\r
 675         }\r
 676         u =new NyARMat( min, min );\r
 677     \r
 678         if( row < clm ){\r
 679             NyARException.trap("未チェックのパス");\r
 680             PCA_x_by_xt( input, u );//if(x_by_xt( input, u ) < 0 ) {\r
 681         }else{\r
 682             PCA_xt_by_x( input, u );//if(xt_by_x( input, u ) < 0 ) {\r
 683         }\r
 684         PCA_QRM( u, ev );\r
 685 \r
 686         double[][] m1,m2;\r
 687         if( row < clm ) {\r
 688             NyARException.trap("未チェックのパス");{\r
 689             PCA_EV_create( input, u, output, ev );\r
 690             }\r
 691         }else{\r
 692             m1=u.m;//m1 = u->m;\r
 693             m2=output.m;//m2 = output->m;\r
 694             int i;\r
 695             for(i = 0; i < min; i++){\r
 696                 if( ev_array[i] < PCA_VZERO){//if( ev->v[i] < VZERO ){\r
 697                     break;\r
 698                 }\r
 699                 for(int j = 0; j < min; j++ ){\r
 700                     m2[i][j]=m1[i][j];//*(m2++) = *(m1++);\r
 701                 }\r
 702             }\r
 703             for( ; i < min; i++){//for( ; i < min; i++){\r
 704                 //コードを見た限りあってそうだからコメントアウト(2008/03/26)NyARException.trap("未チェックのパス");\r
 705                 ev_array[i]=0.0;//ev->v[i] = 0.0;\r
 706                 for(int j = 0; j < min; j++ ){\r
 707                     m2[i][j]=0.0;//*(m2++) = 0.0;\r
 708                 }\r
 709             }\r
 710         }\r
 711     }\r
 712         \r
 713     /*int    arMatrixPCA( ARMat *input, ARMat *evec, ARVec *ev, ARVec *mean );*/\r
 714     public static void matrixPCA(NyARMat input, NyARMat evec, NyARVec ev, NyARVec mean) throws NyARException\r
 715     {\r
 716         NyARMat   work;\r
 717         double srow, sum;\r
 718         int     row, clm;\r
 719         int     check;\r
 720     \r
 721         row=input.row;//row = input->row;\r
 722         clm=input.clm;//clm = input->clm;\r
 723         check = (row < clm)? row: clm;\r
 724         if( row < 2 || clm < 2 ){\r
 725             throw new NyARException();\r
 726         }\r
 727         if( evec.clm != input.clm || evec.row != check ){//if( evec->clm != input->clm || evec->row != check ){\r
 728             throw new NyARException();\r
 729         }\r
 730         if( ev.getClm()   != check ){//if( ev->clm   != check ){\r
 731             throw new NyARException();\r
 732         }\r
 733         if( mean.getClm() != input.clm){//if( mean->clm != input->clm ){\r
 734             throw new NyARException();\r
 735         }\r
 736     \r
 737         work =input.matrixAllocDup();//arMatrixAllocDup( input );work = arMatrixAllocDup( input );\r
 738     \r
 739         srow = Math.sqrt((double)row);\r
 740         PCA_EX( work, mean );\r
 741 \r
 742         PCA_CENTER(work,mean);\r
 743 \r
 744 \r
 745         for(int i=0; i<row; i++){\r
 746             for(int j=0;j<clm;j++){\r
 747                 work.m[i][j]/=srow;//work->m[i] /= srow;\r
 748             }\r
 749         }\r
 750     \r
 751         PCA_PCA( work, evec, ev );\r
 752     \r
 753         sum = 0.0;\r
 754         double[] ev_array=ev.getArray();\r
 755         for(int i = 0; i < ev.getClm(); i++ ){//for(int i = 0; i < ev->clm; i++ ){\r
 756                 sum+=ev_array[i];//sum += ev->v[i];\r
 757         }\r
 758         for(int i = 0; i < ev.getClm(); i++ ){//for(int i = 0; i < ev->clm; i++ ){\r
 759                 ev_array[i]/=sum;//ev->v[i] /= sum;\r
 760         }\r
 761     }\r
 762 \r
 763     /*int    arMatrixPCA2( ARMat *input, ARMat *evec, ARVec *ev );*/\r
 764     public static void arMatrixPCA2( NyARMat input, NyARMat evec, NyARVec ev) throws NyARException\r
 765     {\r
 766         NyARException.trap("未チェックのパス");\r
 767         NyARMat   work;\r
 768         // double  srow; // unreferenced\r
 769         double  sum;\r
 770         int     row, clm;\r
 771         int     check;\r
 772 \r
 773         row=input.row;//row = input->row;\r
 774         clm=input.clm;//clm = input->clm;\r
 775         check = (row < clm)? row: clm;\r
 776         if( row < 2 || clm < 2 ){\r
 777             throw new NyARException();\r
 778         }\r
 779         if( evec.getClm()!= input.clm|| evec.row!=check){//if( evec->clm != input->clm || evec->row != check ){\r
 780             throw new NyARException();\r
 781         }\r
 782         if( ev.getClm() != check ){//if( ev->clm   != check ){\r
 783             throw new NyARException();\r
 784         }\r
 785         \r
 786         NyARException.trap("未チェックのパス");\r
 787         work =input.matrixAllocDup();\r
 788 \r
 789         NyARException.trap("未チェックパス");\r
 790         PCA_PCA( work, evec, ev );//rval = PCA( work, evec, ev );\r
 791         sum = 0.0;\r
 792         double[] ev_array=ev.getArray();\r
 793         for(int i = 0; i < ev.getClm(); i++ ){//for( i = 0; i < ev->clm; i++ ){\r
 794             NyARException.trap("未チェックパス");\r
 795             sum+=ev_array[i];//sum += ev->v[i];\r
 796         }\r
 797         for(int i = 0; i < ev.getClm(); i++ ){//for(int i = 0; i < ev->clm; i++ ){\r
 798             NyARException.trap("未チェックパス");\r
 799                 ev_array[i]/=sum;//ev->v[i] /= sum;\r
 800         }\r
 801         return;\r
 802     }\r
 803     public static NyARMat matrixAllocMul(NyARMat a, NyARMat b) throws NyARException\r
 804     {\r
 805         NyARException.trap("未チェックのパス");\r
 806         NyARMat dest=new NyARMat(a.row, b.clm);\r
 807         NyARException.trap("未チェックのパス");\r
 808         matrixMul(dest, a, b);\r
 809         return dest;\r
 810     }\r
 811     /*static double mdet(double *ap, int dimen, int rowa)*/\r
 812     private static double Det_mdet(double[][] ap, int dimen, int rowa) throws NyARException\r
 813     {\r
 814         NyARException.trap("動作未チェック/配列化未チェック");\r
 815         double det = 1.0;\r
 816         double work;\r
 817         int    is = 0;\r
 818         int    mmax;\r
 819     \r
 820         for(int k = 0; k < dimen - 1; k++) {\r
 821             mmax = k;\r
 822             for(int i = k + 1; i < dimen; i++){\r
 823 //              if (Math.abs(arMatrixDet_MATRIX_get(ap, i, k, rowa)) > Math.abs(arMatrixDet_MATRIX_get(ap, mmax, k, rowa))){\r
 824                 if (Math.abs(ap[i][k]) > Math.abs(ap[mmax][k])){\r
 825                     mmax = i;\r
 826                 }\r
 827             }\r
 828             if(mmax != k) {\r
 829                 for (int j = k; j < dimen; j++) {\r
 830                     work = ap[k][j];//work = MATRIX(ap, k, j, rowa);\r
 831                     ap[k][j]=ap[mmax][j];//MATRIX(ap, k, j, rowa) = MATRIX(ap, mmax, j, rowa);\r
 832                     ap[mmax][j]=work;//MATRIX(ap, mmax, j, rowa) = work;\r
 833                 }\r
 834                 is++;\r
 835             }\r
 836             for(int i = k + 1; i < dimen; i++) {\r
 837                 work = ap[i][k]/ ap[k][k];//work = arMatrixDet_MATRIX_get(ap, i, k, rowa) / arMatrixDet_MATRIX_get(ap, k, k, rowa);\r
 838                 for (int j = k + 1; j < dimen; j++){\r
 839                         //MATRIX(ap, i, j, rowa) -= work * MATRIX(ap, k, j, rowa);\r
 840                         ap[i][j]-=work * ap[k][j];\r
 841                 }\r
 842             }\r
 843         }\r
 844         for(int i = 0; i < dimen; i++){\r
 845             det=ap[i][i];//det *= MATRIX(ap, i, i, rowa);\r
 846         }\r
 847         for(int i = 0; i < is; i++){ \r
 848             det *= -1.0;\r
 849         }\r
 850         return det;\r
 851     }\r
 852     /*double arMatrixDet(ARMat *m);*/\r
 853     public static double arMatrixDet(NyARMat m) throws NyARException\r
 854     {\r
 855         NyARException.trap("動作未チェック/配列化未チェック");\r
 856         if(m.row != m.clm){\r
 857             return 0.0;\r
 858         }\r
 859         return Det_mdet(m.getArray(), m.row, m.clm);//return mdet(m->m, m->row, m->row);\r
 860     }\r
 861 }