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