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 edition ARToolKit class library.\r
  11  * Copyright (C)2008-2009 Ryo Iizuka\r
  12  *\r
  13  * This program is free software: you can redistribute it and/or modify\r
  14  * it under the terms of the GNU General Public License as published by\r
  15  * the Free Software Foundation, either version 3 of the License, or\r
  16  * (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 program.  If not, see <http://www.gnu.org/licenses/>.\r
  25  * \r
  26  * For further information please contact.\r
  27  *      http://nyatla.jp/nyatoolkit/\r
  28  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  29  * \r
  30  */\r
  31 package jp.nyatla.nyartoolkit.core;\r
  32 \r
  33 import jp.nyatla.nyartoolkit.NyARException;\r
  34 \r
  35 \r
  36 \r
  37 /**\r
  38  * ARMat構造体に対応するクラス typedef struct { double *m; int row; int clm; }ARMat;\r
  39  * \r
  40  */\r
  41 public class NyARMat\r
  42 {\r
  43         /**\r
  44          * 配列サイズと行列サイズは必ずしも一致しないことに注意 返された配列のサイズを行列の大きさとして使わないこと！\r
  45          * \r
  46          */\r
  47         protected double[][] _m;\r
  48         private int[] __matrixSelfInv_nos;\r
  49 \r
  50         protected int clm;\r
  51         protected int row;\r
  52 \r
  53         /**\r
  54          * デフォルトコンストラクタは機能しません。\r
  55          * \r
  56          * @throws NyARException\r
  57          */\r
  58         protected NyARMat() throws NyARException\r
  59         {\r
  60                 throw new NyARException();\r
  61         }\r
  62 \r
  63         public NyARMat(int i_row, int i_clm)\r
  64         {\r
  65                 this._m = new double[i_row][i_clm];\r
  66                 this.clm = i_clm;\r
  67                 this.row = i_row;\r
  68                 this.__matrixSelfInv_nos=new int[i_row];\r
  69                 return;\r
  70         }\r
  71         \r
  72         /**\r
  73          * 行列の列数を返します。\r
  74          * @return\r
  75          */\r
  76         public int getClm()\r
  77         {\r
  78                 return clm;\r
  79         }\r
  80         /**\r
  81          * 行列の行数を返します。\r
  82          * @return\r
  83          */\r
  84         public int getRow()\r
  85         {\r
  86                 return row;\r
  87         }       \r
  88         \r
  89         /**\r
  90          * 行列のサイズを変更します。\r
  91          * 実行後、行列の各値は不定になります。\r
  92          * @param i_row\r
  93          * @param i_clm\r
  94          */\r
  95         public void realloc(int i_row, int i_clm)\r
  96         {\r
  97                 if (i_row <= this._m.length && i_clm <= this._m[0].length) {\r
  98                         // 十分な配列があれば何もしない。\r
  99                 } else {\r
 100                         // 不十分なら取り直す。\r
 101                         this._m = new double[i_row][i_clm];\r
 102                         this.__matrixSelfInv_nos=new int[i_row];\r
 103                 }\r
 104                 this.clm = i_clm;\r
 105                 this.row = i_row;\r
 106                 return;\r
 107         }\r
 108         \r
 109         \r
 110         \r
 111         /**\r
 112          * i_mat_aとi_mat_bの積を計算して、thisへ格納します。\r
 113          * @param i_mat_a\r
 114          * @param i_mat_b\r
 115          * @throws NyARException\r
 116          */\r
 117         public void matrixMul(NyARMat i_mat_a, NyARMat i_mat_b) throws NyARException\r
 118         {\r
 119                 assert i_mat_a.clm == i_mat_b.row && this.row==i_mat_a.row && this.clm==i_mat_b.clm;\r
 120 \r
 121                 double w;\r
 122                 int r, c, i;\r
 123                 double[][] am = i_mat_a._m, bm = i_mat_b._m, dm = this._m;\r
 124                 // For順変更禁止\r
 125                 for (r = 0; r < this.row; r++) {\r
 126                         for (c = 0; c < this.clm; c++) {\r
 127                                 w = 0.0;\r
 128                                 for (i = 0; i < i_mat_a.clm; i++) {\r
 129                                         w += am[r][i] * bm[i][c];\r
 130                                 }\r
 131                                 dm[r][c] = w;\r
 132                         }\r
 133                 }\r
 134                 return;\r
 135         }       \r
 136         \r
 137         /**\r
 138          * 逆行列を計算して、thisへ格納します。\r
 139          * @throws NyARException\r
 140          */\r
 141         public boolean matrixSelfInv() throws NyARException\r
 142         {\r
 143                 double[][] ap = this._m;\r
 144                 int dimen = this.row;\r
 145                 int dimen_1 = dimen - 1;\r
 146                 double[] ap_n, ap_ip, ap_i;// wap;\r
 147                 int j, ip, nwork;\r
 148                 int[] nos = __matrixSelfInv_nos;//ワーク変数\r
 149                 // double epsl;\r
 150                 double p, pbuf, work;\r
 151 \r
 152                 /* check size */\r
 153                 switch (dimen) {\r
 154                 case 0:\r
 155                         throw new NyARException();\r
 156                 case 1:\r
 157                         ap[0][0] = 1.0 / ap[0][0];// *ap = 1.0 / (*ap);\r
 158                         return true;/* 1 dimension */\r
 159                 }\r
 160 \r
 161                 for (int n = 0; n < dimen; n++) {\r
 162                         nos[n] = n;\r
 163                 }\r
 164 \r
 165                 /*\r
 166                  * nyatla memo ipが定まらないで計算が行われる場合があるので挿入。 ループ内で0初期化していいかが判らない。\r
 167                  */\r
 168                 ip = 0;\r
 169                 // For順変更禁止\r
 170                 for (int n = 0; n < dimen; n++) {\r
 171                         ap_n = ap[n];// wcp = ap + n * rowa;\r
 172                         p = 0.0;\r
 173                         for (int i = n; i < dimen; i++) {\r
 174                                 if (p < (pbuf = Math.abs(ap[i][0]))) {\r
 175                                         p = pbuf;\r
 176                                         ip = i;\r
 177                                 }\r
 178                         }\r
 179                         // if (p <= matrixSelfInv_epsl){\r
 180                         if (p == 0.0) {\r
 181                                 return false;\r
 182                                 // throw new NyARException();\r
 183                         }\r
 184 \r
 185                         nwork = nos[ip];\r
 186                         nos[ip] = nos[n];\r
 187                         nos[n] = nwork;\r
 188 \r
 189                         ap_ip = ap[ip];\r
 190                         for (j = 0; j < dimen; j++) {// for(j = 0, wap = ap + ip * rowa,\r
 191                                                                                         // wbp = wcp; j < dimen ; j++) {\r
 192                                 work = ap_ip[j]; // work = *wap;\r
 193                                 ap_ip[j] = ap_n[j];\r
 194                                 ap_n[j] = work;\r
 195                         }\r
 196 \r
 197                         work = ap_n[0];\r
 198                         for (j = 0; j < dimen_1; j++) {\r
 199                                 ap_n[j] = ap_n[j + 1] / work;// *wap = *(wap + 1) / work;\r
 200                         }\r
 201                         ap_n[j] = 1.0 / work;// *wap = 1.0 / work;\r
 202                         for (int i = 0; i < dimen; i++) {\r
 203                                 if (i != n) {\r
 204                                         ap_i = ap[i];// wap = ap + i * rowa;\r
 205                                         work = ap_i[0];\r
 206                                         for (j = 0; j < dimen_1; j++) {// for(j = 1, wbp = wcp,work = *wap;j < dimen ;j++, wap++, wbp++)\r
 207                                                 ap_i[j] = ap_i[j + 1] - work * ap_n[j];// wap = *(wap +1) - work *(*wbp);\r
 208                                         }\r
 209                                         ap_i[j] = -work * ap_n[j];// *wap = -work * (*wbp);\r
 210                                 }\r
 211                         }\r
 212                 }\r
 213 \r
 214                 for (int n = 0; n < dimen; n++) {\r
 215                         for (j = n; j < dimen; j++) {\r
 216                                 if (nos[j] == n) {\r
 217                                         break;\r
 218                                 }\r
 219                         }\r
 220                         nos[j] = nos[n];\r
 221                         for (int i = 0; i < dimen; i++) {\r
 222                                 ap_i = ap[i];\r
 223                                 work = ap_i[j];// work = *wap;\r
 224                                 ap_i[j] = ap_i[n];// *wap = *wbp;\r
 225                                 ap_i[n] = work;// *wbp = work;\r
 226                         }\r
 227                 }\r
 228                 return true;\r
 229         }\r
 230 \r
 231         /**\r
 232          * 行列をゼロクリアする。\r
 233          */\r
 234         public void zeroClear()\r
 235         {\r
 236                 int i, i2;\r
 237                 // For順変更OK\r
 238                 for (i = row - 1; i >= 0; i--) {\r
 239                         for (i2 = clm - 1; i2 >= 0; i2--) {\r
 240                                 _m[i][i2] = 0.0;\r
 241                         }\r
 242                 }\r
 243         }\r
 244 \r
 245         /**\r
 246          * i_copy_fromの内容を自分自身にコピーします。 高さ・幅は同一で無いと失敗します。\r
 247          * \r
 248          * @param i_copy_from\r
 249          */\r
 250         public void copyFrom(NyARMat i_copy_from) throws NyARException\r
 251         {\r
 252                 // サイズ確認\r
 253                 if (this.row != i_copy_from.row || this.clm != i_copy_from.clm) {\r
 254                         throw new NyARException();\r
 255                 }\r
 256                 // 値コピー\r
 257                 for (int r = this.row - 1; r >= 0; r--) {\r
 258                         for (int c = this.clm - 1; c >= 0; c--) {\r
 259                                 this._m[r][c] = i_copy_from._m[r][c];\r
 260                         }\r
 261                 }\r
 262         }\r
 263 \r
 264         public double[][] getArray()\r
 265         {\r
 266                 return _m;\r
 267         }\r
 268 \r
 269 \r
 270         /**\r
 271          * sourceの転置行列をdestに得る。arMatrixTrans()の代替品\r
 272          * \r
 273          * @param dest\r
 274          * @param source\r
 275          * @return\r
 276          */\r
 277         public static void matrixTrans(NyARMat dest, NyARMat source) throws NyARException\r
 278         {\r
 279                 if (dest.row != source.clm || dest.clm != source.row) {\r
 280                         throw new NyARException();\r
 281                 }\r
 282                 NyARException.trap("未チェックのパス");\r
 283                 // For順変更禁止\r
 284                 for (int r = 0; r < dest.row; r++) {\r
 285                         for (int c = 0; c < dest.clm; c++) {\r
 286                                 dest._m[r][c] = source._m[c][r];\r
 287                         }\r
 288                 }\r
 289         }\r
 290 \r
 291         /**\r
 292          * unitを単位行列に初期化する。arMatrixUnitの代替品\r
 293          * \r
 294          * @param unit\r
 295          */\r
 296         public static void matrixUnit(NyARMat unit) throws NyARException\r
 297         {\r
 298                 if (unit.row != unit.clm) {\r
 299                         throw new NyARException();\r
 300                 }\r
 301                 NyARException.trap("未チェックのパス");\r
 302                 // For順変更禁止\r
 303                 for (int r = 0; r < unit.getRow(); r++) {\r
 304                         for (int c = 0; c < unit.getClm(); c++) {\r
 305                                 if (r == c) {\r
 306                                         unit._m[r][c] = 1.0;\r
 307                                 } else {\r
 308                                         unit._m[r][c] = 0.0;\r
 309                                 }\r
 310                         }\r
 311                 }\r
 312         }\r
 313 \r
 314         /**\r
 315          * sourceの内容を自身に複製する。 Optimized 2008.04.19\r
 316          * \r
 317          * @param i_source\r
 318          * @return\r
 319          */\r
 320         public void matrixDup(NyARMat i_source) throws NyARException\r
 321         {\r
 322                 // 自身の配列サイズを相手のそれより大きいことを保障する。\r
 323                 this.realloc(i_source.row, i_source.clm);\r
 324                 // 内容を転写\r
 325                 int r, c;\r
 326                 double[][] src_m, dest_m;\r
 327                 src_m = i_source._m;\r
 328                 dest_m = this._m;\r
 329                 // コピーはFor順を変えてもOK\r
 330                 for (r = this.row - 1; r >= 0; r--) {\r
 331                         for (c = this.clm - 1; c >= 0; c--) {\r
 332                                 dest_m[r][c] = src_m[r][c];\r
 333                         }\r
 334                 }\r
 335         }\r
 336 \r
 337         public NyARMat matrixAllocDup() throws NyARException\r
 338         {\r
 339                 NyARMat result = new NyARMat(this.row, this.clm);\r
 340                 // コピー\r
 341                 int r, c;\r
 342                 double[][] dest_m, src_m;\r
 343                 dest_m = result._m;\r
 344                 src_m = this._m;\r
 345                 // コピーはFor順を変えてもOK\r
 346                 for (r = this.row - 1; r >= 0; r--) {\r
 347                         for (c = this.clm - 1; c >= 0; c--) {\r
 348                                 dest_m[r][c] = src_m[r][c];\r
 349                         }\r
 350                 }\r
 351                 return result;\r
 352         }\r
 353 \r
 354 \r
 355         private static final double PCA_EPS = 1e-6; // #define EPS 1e-6\r
 356 \r
 357         private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100\r
 358 \r
 359         private static final double PCA_VZERO = 1e-16; // #define VZERO 1e-16\r
 360 \r
 361         /**\r
 362          * static int EX( ARMat *input, ARVec *mean )の代替関数 Optimize:STEP:[144->110]\r
 363          * \r
 364          * @param input\r
 365          * @param mean\r
 366          * @return\r
 367          * @throws NyARException\r
 368          */\r
 369         private void PCA_EX(NyARVec mean) throws NyARException\r
 370         {\r
 371                 int lrow, lclm;\r
 372                 int i, i2;\r
 373                 lrow = this.row;\r
 374                 lclm = this.clm;\r
 375                 double[][] lm = this._m;\r
 376 \r
 377                 if (lrow <= 0 || lclm <= 0) {\r
 378                         throw new NyARException();\r
 379                 }\r
 380                 if (mean.getClm() != lclm) {\r
 381                         throw new NyARException();\r
 382                 }\r
 383                 // double[] mean_array=mean.getArray();\r
 384                 // mean.zeroClear();\r
 385                 final double[] mean_array = mean.getArray();\r
 386                 double w;\r
 387                 // For順変更禁止\r
 388                 for (i2 = 0; i2 < lclm; i2++) {\r
 389                         w = 0.0;\r
 390                         for (i = 0; i < lrow; i++) {\r
 391                                 // *(v++) += *(m++);\r
 392                                 w += lm[i][i2];\r
 393                         }\r
 394                         mean_array[i2] = w / lrow;// mean->v[i] /= row;\r
 395                 }\r
 396         }\r
 397 \r
 398         /**\r
 399          * static int CENTER( ARMat *inout, ARVec *mean )の代替関数\r
 400          * \r
 401          * @param inout\r
 402          * @param mean\r
 403          * @return\r
 404          */\r
 405         private static void PCA_CENTER(NyARMat inout, NyARVec mean)throws NyARException\r
 406         {\r
 407                 double[] v;\r
 408                 int row, clm;\r
 409 \r
 410                 row = inout.row;\r
 411                 clm = inout.clm;\r
 412                 if (mean.getClm() != clm) {\r
 413                         throw new NyARException();\r
 414                 }\r
 415                 double[][] im = inout._m;\r
 416                 double[] im_i;\r
 417                 double w0, w1;\r
 418                 v = mean.getArray();\r
 419                 // 特にパフォーマンスが劣化するclm=1と2ときだけ、別パスで処理します。\r
 420                 switch (clm) {\r
 421                 case 1:\r
 422                         w0 = v[0];\r
 423                         for (int i = 0; i < row; i++) {\r
 424                                 im[i][0] -= w0;\r
 425                         }\r
 426                         break;\r
 427                 case 2:\r
 428                         w0 = v[0];\r
 429                         w1 = v[1];\r
 430                         for (int i = 0; i < row; i++) {\r
 431                                 im_i = im[i];\r
 432                                 im_i[0] -= w0;\r
 433                                 im_i[1] -= w1;\r
 434                         }\r
 435                         break;\r
 436                 default:\r
 437                         for (int i = 0; i < row; i++) {\r
 438                                 im_i = im[i];\r
 439                                 for (int j = 0; j < clm; j++) {\r
 440                                         // *(m++) -= *(v++);\r
 441                                         im_i[j] -= v[j];\r
 442                                 }\r
 443                         }\r
 444                         break;\r
 445                 }\r
 446                 return;\r
 447         }\r
 448 \r
 449         /**\r
 450          * int x_by_xt( ARMat *input, ARMat *output )の代替関数\r
 451          * \r
 452          * @param input\r
 453          * @param output\r
 454          * @throws NyARException\r
 455          */\r
 456         private static void PCA_x_by_xt(NyARMat input, NyARMat output) throws NyARException\r
 457         {\r
 458                 NyARException.trap("動作未チェック/配列化未チェック");\r
 459                 int row, clm;\r
 460                 // double[][] out;\r
 461                 double[] in1, in2;\r
 462 \r
 463                 NyARException.trap("未チェックのパス");\r
 464                 row = input.row;\r
 465                 clm = input.clm;\r
 466                 NyARException.trap("未チェックのパス");\r
 467                 if (output.row != row || output.clm != row) {\r
 468                         throw new NyARException();\r
 469                 }\r
 470 \r
 471                 // out = output.getArray();\r
 472                 for (int i = 0; i < row; i++) {\r
 473                         for (int j = 0; j < row; j++) {\r
 474                                 if (j < i) {\r
 475                                         NyARException.trap("未チェックのパス");\r
 476                                         output._m[i][j] = output._m[j][i];// *out =\r
 477                                                                                                         // output->m[j*row+i];\r
 478                                 } else {\r
 479                                         NyARException.trap("未チェックのパス");\r
 480                                         in1 = input._m[i];// input.getRowArray(i);//in1 = &(input->m[clm*i]);\r
 481                                         in2 = input._m[j];// input.getRowArray(j);//in2 = &(input->m[clm*j]);\r
 482                                         output._m[i][j] = 0;// *out = 0.0;\r
 483                                         for (int k = 0; k < clm; k++) {\r
 484                                                 output._m[i][j] += (in1[k] * in2[k]);// *out += *(in1++)\r
 485                                                                                                                         // * *(in2++);\r
 486                                         }\r
 487                                 }\r
 488                                 // out.incPtr();\r
 489                         }\r
 490                 }\r
 491         }\r
 492 \r
 493         /**\r
 494          * static int xt_by_x( ARMat *input, ARMat *output )の代替関数\r
 495          * Optimize:2008.04.19\r
 496          * \r
 497          * @param input\r
 498          * @param i_output\r
 499          * @throws NyARException\r
 500          */\r
 501         private static void PCA_xt_by_x(NyARMat input, NyARMat i_output) throws NyARException\r
 502         {\r
 503                 double[] in_;\r
 504                 int row, clm;\r
 505 \r
 506                 row = input.row;\r
 507                 clm = input.clm;\r
 508                 if (i_output.row != clm || i_output.clm != clm) {\r
 509                         throw new NyARException();\r
 510                 }\r
 511 \r
 512                 int k, j;\r
 513                 double[][] out_m = i_output._m;\r
 514                 double w;\r
 515                 for (int i = 0; i < clm; i++) {\r
 516                         for (j = 0; j < clm; j++) {\r
 517                                 if (j < i) {\r
 518                                         out_m[i][j] = out_m[j][i];// *out = output->m[j*clm+i];\r
 519                                 } else {\r
 520                                         w = 0.0;// *out = 0.0;\r
 521                                         for (k = 0; k < row; k++) {\r
 522                                                 in_ = input._m[k];// in=input.getRowArray(k);\r
 523                                                 w += (in_[i] * in_[j]);// *out += *in1 * *in2;\r
 524                                         }\r
 525                                         out_m[i][j] = w;\r
 526                                 }\r
 527                         }\r
 528                 }\r
 529         }\r
 530 \r
 531         private final NyARVec wk_PCA_QRM_ev = new NyARVec(1);\r
 532 \r
 533         /**\r
 534          * static int QRM( ARMat *a, ARVec *dv )の代替関数\r
 535          * \r
 536          * @param a\r
 537          * @param dv\r
 538          * @throws NyARException\r
 539          */\r
 540         private void PCA_QRM(NyARVec dv) throws NyARException\r
 541         {\r
 542                 double w, t, s, x, y, c;\r
 543                 int dim, iter;\r
 544                 double[] dv_array = dv.getArray();\r
 545 \r
 546                 dim = this.row;\r
 547                 if (dim != this.clm || dim < 2) {\r
 548                         throw new NyARException();\r
 549                 }\r
 550                 if (dv.getClm() != dim) {\r
 551                         throw new NyARException();\r
 552                 }\r
 553 \r
 554                 NyARVec ev = this.wk_PCA_QRM_ev;\r
 555                 ev.realloc(dim);\r
 556                 double[] ev_array = ev.getArray();\r
 557                 if (ev == null) {\r
 558                         throw new NyARException();\r
 559                 }\r
 560                 final double[][] L_m = this._m;\r
 561                 this.vecTridiagonalize(dv, ev, 1);\r
 562 \r
 563                 ev_array[0] = 0.0;// ev->v[0] = 0.0;\r
 564                 for (int h = dim - 1; h > 0; h--) {\r
 565                         int j = h;\r
 566                         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])))\r
 567                                 // j--;\r
 568                                 j--;\r
 569                         }\r
 570                         if (j == h) {\r
 571                                 continue;\r
 572                         }\r
 573                         iter = 0;\r
 574                         do {\r
 575                                 iter++;\r
 576                                 if (iter > PCA_MAX_ITER) {\r
 577                                         break;\r
 578                                 }\r
 579                                 w = (dv_array[h - 1] - dv_array[h]) / 2;// w = (dv->v[h-1] -dv->v[h]) / 2;//ここ？\r
 580                                 t = ev_array[h] * ev_array[h];// t = ev->v[h] * ev->v[h];\r
 581                                 s = Math.sqrt(w * w + t);\r
 582                                 if (w < 0) {\r
 583                                         s = -s;\r
 584                                 }\r
 585                                 x = dv_array[j] - dv_array[h] + t / (w + s);// x = dv->v[j] -dv->v[h] +t/(w+s);\r
 586                                 y = ev_array[j + 1];// y = ev->v[j+1];\r
 587                                 for (int k = j; k < h; k++) {\r
 588                                         if (Math.abs(x) >= Math.abs(y)) {\r
 589                                                 if (Math.abs(x) > PCA_VZERO) {\r
 590                                                         t = -y / x;\r
 591                                                         c = 1 / Math.sqrt(t * t + 1);\r
 592                                                         s = t * c;\r
 593                                                 } else {\r
 594                                                         c = 1.0;\r
 595                                                         s = 0.0;\r
 596                                                 }\r
 597                                         } else {\r
 598                                                 t = -x / y;\r
 599                                                 s = 1.0 / Math.sqrt(t * t + 1);\r
 600                                                 c = t * s;\r
 601                                         }\r
 602                                         w = dv_array[k] - dv_array[k + 1];// w = dv->v[k] -dv->v[k+1];\r
 603                                         t = (w * s + 2 * c * ev_array[k + 1]) * s;// t = (w * s +2 * c *ev->v[k+1]) *s;\r
 604                                         dv_array[k] -= t;// dv->v[k] -= t;\r
 605                                         dv_array[k + 1] += t;// dv->v[k+1] += t;\r
 606                                         if (k > j) {\r
 607                                                 NyARException.trap("未チェックパス");\r
 608                                                 {\r
 609                                                         ev_array[k] = c * ev_array[k] - s * y;// ev->v[k]= c *ev->v[k]- s * y;\r
 610                                                 }\r
 611                                         }\r
 612                                         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
 613 \r
 614                                         for (int i = 0; i < dim; i++) {\r
 615                                                 x = L_m[k][i];// x = a->m[k*dim+i];\r
 616                                                 y = L_m[k + 1][i];// y = a->m[(k+1)*dim+i];\r
 617                                                 L_m[k][i] = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
 618                                                 L_m[k + 1][i] = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
 619                                         }\r
 620                                         if (k < h - 1) {\r
 621                                                 NyARException.trap("未チェックパス");\r
 622                                                 {\r
 623                                                         x = ev_array[k + 1];// x = ev->v[k+1];\r
 624                                                         y = -s * ev_array[k + 2];// y = -s * ev->v[k+2];\r
 625                                                         ev_array[k + 2] *= c;// ev->v[k+2] *= c;\r
 626                                                 }\r
 627                                         }\r
 628                                 }\r
 629                         } while (Math.abs(ev_array[h]) > PCA_EPS\r
 630                                         * (Math.abs(dv_array[h - 1]) + Math.abs(dv_array[h])));\r
 631                 }\r
 632                 for (int k = 0; k < dim - 1; k++) {\r
 633                         int h = k;\r
 634                         t = dv_array[h];// t = dv->v[h];\r
 635                         for (int i = k + 1; i < dim; i++) {\r
 636                                 if (dv_array[i] > t) {// if( dv->v[i] > t ) {\r
 637                                         h = i;\r
 638                                         t = dv_array[h];// t = dv->v[h];\r
 639                                 }\r
 640                         }\r
 641                         dv_array[h] = dv_array[k];// dv->v[h] = dv->v[k];\r
 642                         dv_array[k] = t;// dv->v[k] = t;\r
 643                         this.flipRow(h, k);\r
 644                 }\r
 645         }\r
 646 \r
 647         /**\r
 648          * i_row_1番目の行と、i_row_2番目の行を入れ替える。\r
 649          * \r
 650          * @param i_row_1\r
 651          * @param i_row_2\r
 652          */\r
 653         private void flipRow(int i_row_1, int i_row_2)\r
 654         {\r
 655                 int i;\r
 656                 double w;\r
 657                 double[] r1 = this._m[i_row_1], r2 = this._m[i_row_2];\r
 658                 // For順変更OK\r
 659                 for (i = clm - 1; i >= 0; i--) {\r
 660                         w = r1[i];\r
 661                         r1[i] = r2[i];\r
 662                         r2[i] = w;\r
 663                 }\r
 664         }\r
 665 \r
 666         /**\r
 667          * static int EV_create( ARMat *input, ARMat *u, ARMat *output, ARVec *ev\r
 668          * )の代替関数\r
 669          * \r
 670          * @param input\r
 671          * @param u\r
 672          * @param output\r
 673          * @param ev\r
 674          * @throws NyARException\r
 675          */\r
 676         private static void PCA_EV_create(NyARMat input, NyARMat u, NyARMat output,NyARVec ev) throws NyARException\r
 677         {\r
 678                 NyARException.trap("未チェックのパス");\r
 679                 int row, clm;\r
 680                 row = input.row;// row = input->row;\r
 681                 clm = input.clm;// clm = input->clm;\r
 682                 if (row <= 0 || clm <= 0) {\r
 683                         throw new NyARException();\r
 684                 }\r
 685                 if (u.row != row || u.clm != row) {// if( u->row != row || u->clm !=\r
 686                                                                                         // row ){\r
 687                         throw new NyARException();\r
 688                 }\r
 689                 if (output.row != row || output.clm != clm) {// if( output->row !=\r
 690                                                                                                                 // row || output->clm !=\r
 691                                                                                                                 // clm ){\r
 692                         throw new NyARException();\r
 693                 }\r
 694                 if (ev.getClm() != row) {// if( ev->clm != row ){\r
 695                         throw new NyARException();\r
 696                 }\r
 697                 double[][] m, in_;\r
 698                 double[] m1, ev_array;\r
 699                 double sum, work;\r
 700 \r
 701                 NyARException.trap("未チェックのパス");\r
 702                 m = output._m;// m = output->m;\r
 703                 in_ = input._m;\r
 704                 int i;\r
 705                 ev_array = ev.getArray();\r
 706                 for (i = 0; i < row; i++) {\r
 707                         NyARException.trap("未チェックのパス");\r
 708                         if (ev_array[i] < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
 709                                 break;\r
 710                         }\r
 711                         NyARException.trap("未チェックのパス");\r
 712                         work = 1 / Math.sqrt(Math.abs(ev_array[i]));// work = 1 /\r
 713                                                                                                                 // sqrt(fabs(ev->v[i]));\r
 714                         for (int j = 0; j < clm; j++) {\r
 715                                 sum = 0.0;\r
 716                                 m1 = u._m[i];// m1 = &(u->m[i*row]);\r
 717                                 // m2=input.getPointer(j);//m2 = &(input->m[j]);\r
 718                                 for (int k = 0; k < row; k++) {\r
 719                                         sum += m1[k] + in_[k][j];// sum += *m1 * *m2;\r
 720                                         // m1.incPtr(); //m1++;\r
 721                                         // m2.addPtr(clm);//m2 += clm;\r
 722                                 }\r
 723                                 m1[j] = sum * work;// *(m++) = sum * work;\r
 724                                 // {//*(m++) = sum * work;\r
 725                                 // m.set(sum * work);\r
 726                                 // m.incPtr();}\r
 727                         }\r
 728                 }\r
 729                 for (; i < row; i++) {\r
 730                         NyARException.trap("未チェックのパス");\r
 731                         ev_array[i] = 0.0;// ev->v[i] = 0.0;\r
 732                         for (int j = 0; j < clm; j++) {\r
 733                                 m[i][j] = 0.0;\r
 734                                 // m.set(0.0);//*(m++) = 0.0;\r
 735                                 // m.incPtr();\r
 736                         }\r
 737                 }\r
 738         }\r
 739 \r
 740         private NyARMat wk_PCA_PCA_u = null;\r
 741 \r
 742         /**\r
 743          * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
 744          * \r
 745          * @param output\r
 746          * @param o_ev\r
 747          * @throws NyARException\r
 748          */\r
 749         private void PCA_PCA(NyARMat o_output, NyARVec o_ev) throws NyARException\r
 750         {\r
 751 \r
 752                 int l_row, l_clm, min;\r
 753                 double[] ev_array = o_ev.getArray();\r
 754 \r
 755                 l_row = this.row;// row = input->row;\r
 756                 l_clm = this.clm;// clm = input->clm;\r
 757                 min = (l_clm < l_row) ? l_clm : l_row;\r
 758                 if (l_row < 2 || l_clm < 2) {\r
 759                         throw new NyARException();\r
 760                 }\r
 761                 if (o_output.clm != this.clm) {// if( output->clm != input->clm ){\r
 762                         throw new NyARException();\r
 763                 }\r
 764                 if (o_output.row != min) {// if( output->row != min ){\r
 765                         throw new NyARException();\r
 766                 }\r
 767                 if (o_ev.getClm() != min) {// if( ev->clm != min ){\r
 768                         throw new NyARException();\r
 769                 }\r
 770 \r
 771                 NyARMat u;// u =new NyARMat( min, min );\r
 772                 if (this.wk_PCA_PCA_u == null) {\r
 773                         u = new NyARMat(min, min);\r
 774                         this.wk_PCA_PCA_u = u;\r
 775                 } else {\r
 776                         u = this.wk_PCA_PCA_u;\r
 777                         u.realloc(min, min);\r
 778                 }\r
 779 \r
 780                 if (l_row < l_clm) {\r
 781                         NyARException.trap("未チェックのパス");\r
 782                         PCA_x_by_xt(this, u);// if(x_by_xt( input, u ) < 0 ) {\r
 783                 } else {\r
 784                         PCA_xt_by_x(this, u);// if(xt_by_x( input, u ) < 0 ) {\r
 785                 }\r
 786                 u.PCA_QRM(o_ev);\r
 787 \r
 788                 double[][] m1, m2;\r
 789                 if (l_row < l_clm) {\r
 790                         NyARException.trap("未チェックのパス");\r
 791                         PCA_EV_create(this, u, o_output, o_ev);\r
 792                 } else {\r
 793                         m1 = u._m;// m1 = u->m;\r
 794                         m2 = o_output._m;// m2 = output->m;\r
 795                         int i;\r
 796                         for (i = 0; i < min; i++) {\r
 797                                 if (ev_array[i] < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
 798                                         break;\r
 799                                 }\r
 800                                 for (int j = 0; j < min; j++) {\r
 801                                         m2[i][j] = m1[i][j];// *(m2++) = *(m1++);\r
 802                                 }\r
 803                         }\r
 804                         for (; i < min; i++) {// for( ; i < min; i++){\r
 805                                 // コードを見た限りあってそうだからコメントアウト(2008/03/26)NyARException.trap("未チェックのパス");\r
 806                                 ev_array[i] = 0.0;// ev->v[i] = 0.0;\r
 807                                 for (int j = 0; j < min; j++) {\r
 808                                         m2[i][j] = 0.0;// *(m2++) = 0.0;\r
 809                                 }\r
 810                         }\r
 811                 }\r
 812         }\r
 813         /**\r
 814          * 主成分分析を実行して、結果をthisと引数へ格納します。\r
 815          * @param o_evec\r
 816          * @param o_ev\r
 817          * @param o_mean\r
 818          * @throws NyARException\r
 819          */\r
 820         public void pca(NyARMat o_evec, NyARVec o_ev, NyARVec o_mean)throws NyARException\r
 821         {\r
 822                 final double l_row = this.row;// row = input->row;\r
 823                 final double l_clm = this.clm;// clm = input->clm;\r
 824                 final double check=(l_row < l_clm) ? l_row : l_clm;\r
 825                 \r
 826                 assert l_row >= 2 || l_clm >= 2;\r
 827                 assert o_evec.clm == l_clm && o_evec.row == check;\r
 828                 assert o_ev.getClm() == check;\r
 829                 assert o_mean.getClm() == l_clm;\r
 830                 \r
 831                 final double srow = Math.sqrt((double) l_row);\r
 832                 PCA_EX(o_mean);\r
 833 \r
 834                 PCA_CENTER(this, o_mean);\r
 835 \r
 836                 int i, j;\r
 837                 // For順変更OK\r
 838                 for (i = 0; i < l_row; i++) {\r
 839                         for (j = 0; j < l_clm; j++) {\r
 840                                 this._m[i][j] /= srow;// work->m[i] /= srow;\r
 841                         }\r
 842                 }\r
 843 \r
 844                 PCA_PCA(o_evec, o_ev);\r
 845 \r
 846                 double sum = 0.0;\r
 847                 double[] ev_array = o_ev.getArray();\r
 848                 int ev_clm = o_ev.getClm();\r
 849                 // For順変更禁止\r
 850                 for (i = 0; i < ev_clm; i++) {// for(int i = 0; i < ev->clm; i++ ){\r
 851                         sum += ev_array[i];// sum += ev->v[i];\r
 852                 }\r
 853                 // For順変更禁止\r
 854                 for (i = 0; i < ev_clm; i++) {// for(int i = 0; i < ev->clm; i++ ){\r
 855                         ev_array[i] /= sum;// ev->v[i] /= sum;\r
 856                 }\r
 857                 return;\r
 858         }\r
 859         \r
 860 \r
 861 \r
 862         private final NyARVec wk_vecTridiagonalize_vec = new NyARVec(0);\r
 863 \r
 864         private final NyARVec wk_vecTridiagonalize_vec2 = new NyARVec(0);\r
 865 \r
 866         /**\r
 867          * arVecTridiagonalize関数の代替品 a,d,e間で演算をしてる。何をどうしているかはさっぱりさっぱり\r
 868          * \r
 869          * @param a\r
 870          * @param d\r
 871          * @param e\r
 872          * @param i_e_start\r
 873          *            演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる)\r
 874          * @return\r
 875          * @throws NyARException\r
 876          */\r
 877         private void vecTridiagonalize(NyARVec d, NyARVec e, int i_e_start)throws NyARException\r
 878         {\r
 879                 NyARVec vec = wk_vecTridiagonalize_vec;\r
 880                 // double[][] a_array=a.getArray();\r
 881                 double s, t, p, q;\r
 882                 int dim;\r
 883 \r
 884                 if (this.clm != this.row) {// if(a.getClm()!=a.getRow()){\r
 885                         throw new NyARException();\r
 886                 }\r
 887                 if (this.clm != d.getClm()) {// if(a.getClm() != d.clm){\r
 888                         throw new NyARException();\r
 889                 }\r
 890                 if (this.clm != e.getClm()) {// if(a.getClm() != e.clm){\r
 891                         throw new NyARException();\r
 892                 }\r
 893                 dim = this.getClm();\r
 894 \r
 895                 double[] d_vec, e_vec;\r
 896                 d_vec = d.getArray();\r
 897                 e_vec = e.getArray();\r
 898                 double[] a_vec_k;\r
 899 \r
 900                 for (int k = 0; k < dim - 2; k++) {\r
 901 \r
 902                         a_vec_k = this._m[k];\r
 903                         vec.setNewArray(a_vec_k, clm);// vec=this.getRowVec(k);//double[]\r
 904                                                                                         // vec_array=vec.getArray();\r
 905                         NyARException.trap("未チェックパス");\r
 906                         d_vec[k] = a_vec_k[k];// d.v[k]=vec.v[k];//d.set(k,v.get(k));\r
 907                                                                         // //d->v[k] = v[k];\r
 908 \r
 909                         // wv1.clm = dim-k-1;\r
 910                         // wv1.v = &(v[k+1]);\r
 911                         NyARException.trap("未チェックパス");\r
 912                         e_vec[k + i_e_start] = vec.vecHousehold(k + 1);// e.v[k+i_e_start]=vec.vecHousehold(k+1);//e->v[k]= arVecHousehold(&wv1);\r
 913                         if (e_vec[k + i_e_start] == 0.0) {// if(e.v[k+i_e_start]== 0.0){//if(e.v[k+i_e_start]== 0.0){\r
 914                                 continue;\r
 915                         }\r
 916 \r
 917                         for (int i = k + 1; i < dim; i++) {\r
 918                                 s = 0.0;\r
 919                                 for (int j = k + 1; j < i; j++) {\r
 920                                         NyARException.trap("未チェックのパス");\r
 921                                         s += this._m[j][i] * a_vec_k[j];// s += a_array[j][i] *vec.v[j];//s +=a.get(j*dim+i) *v.get(j);//s +=a->m[j*dim+i] * v[j];\r
 922                                 }\r
 923                                 for (int j = i; j < dim; j++) {\r
 924                                         NyARException.trap("未チェックのパス");\r
 925                                         s += this._m[i][j] * a_vec_k[j];// s += a_array[i][j] *vec.v[j];//s +=a.get(i*dim+j) *v.get(j);//s +=a->m[i*dim+j] * v[j];\r
 926                                 }\r
 927                                 NyARException.trap("未チェックのパス");\r
 928                                 d_vec[i] = s;// d.v[i]=s;//d->v[i] = s;\r
 929                         }\r
 930 \r
 931                         // wv1.clm = wv2.clm = dim-k-1;\r
 932                         // wv1.v = &(v[k+1]);\r
 933                         // wv2.v = &(d->v[k+1]);\r
 934                         a_vec_k = this._m[k];\r
 935                         vec.setNewArray(a_vec_k, clm);// vec=this.getRowVec(k);\r
 936                         // vec_array=vec.getArray();\r
 937                         NyARException.trap("未チェックパス");\r
 938                         t = vec.vecInnerproduct(d, k + 1) / 2;\r
 939                         for (int i = dim - 1; i > k; i--) {\r
 940                                 NyARException.trap("未チェックパス");\r
 941                                 p = a_vec_k[i];// p = v.get(i);//p = v[i];\r
 942                                 d_vec[i] -= t * p;\r
 943                                 q = d_vec[i];// d.v[i]-=t*p;q=d.v[i];//q = d->v[i] -= t*p；\r
 944                                 for (int j = i; j < dim; j++) {\r
 945                                         NyARException.trap("未チェックパス");\r
 946                                         this._m[i][j] -= p * (d_vec[j] + q * a_vec_k[j]);// a.m[i][j]-=p*(d.v[j] +q*vec.v[j]);//a->m[i*dim+j] -=p*(d->v[j]) + q*v[j];\r
 947                                 }\r
 948                         }\r
 949                 }\r
 950 \r
 951                 if (dim >= 2) {\r
 952                         d_vec[dim - 2] = this._m[dim - 2][dim - 2];// d.v[dim-2]=a.m[dim-2][dim-2];//d->v[dim-2]=a->m[(dim-2)*dim+(dim-2)];\r
 953                         e_vec[dim - 2 + i_e_start] = this._m[dim - 2][dim - 1];// e.v[dim-2+i_e_start]=a.m[dim-2][dim-1];//e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)];\r
 954                 }\r
 955 \r
 956                 if (dim >= 1) {\r
 957                         d_vec[dim - 1] = this._m[dim - 1][dim - 1];// d.v[dim-1]=a_array[dim-1][dim-1];//d->v[dim-1] =a->m[(dim-1)*dim+(dim-1)];\r
 958                 }\r
 959                 NyARVec vec2 = this.wk_vecTridiagonalize_vec2;\r
 960                 for (int k = dim - 1; k >= 0; k--) {\r
 961                         a_vec_k = this._m[k];\r
 962                         vec.setNewArray(a_vec_k, clm);// vec=this.getRowVec(k);//v =a.getPointer(k*dim);//v = &(a->m[k*dim]);\r
 963                         if (k < dim - 2) {\r
 964                                 for (int i = k + 1; i < dim; i++) {\r
 965                                         // wv1.clm = wv2.clm = dim-k-1;\r
 966                                         // wv1.v = &(v[k+1]);\r
 967                                         // wv2.v = &(a->m[i*dim+k+1]);\r
 968                                         vec2.setNewArray(this._m[i], clm);// vec2=this.getRowVec(i);\r
 969 \r
 970                                         t = vec.vecInnerproduct(vec2, k + 1);\r
 971                                         for (int j = k + 1; j < dim; j++) {\r
 972                                                 NyARException.trap("未チェックパス");\r
 973                                                 this._m[i][j] -= t * a_vec_k[j];// a_array[i][j]-=t*vec.v[j];//a.subValue(i*dim+j,t*v.get(j));//a->m[i*dim+j]-= t * v[j];\r
 974                                         }\r
 975                                 }\r
 976                         }\r
 977                         for (int i = 0; i < dim; i++) {\r
 978                                 a_vec_k[i] = 0.0;// v.set(i,0.0);//v[i] = 0.0;\r
 979                         }\r
 980                         a_vec_k[k] = 1;// v.set(k,1);//v[k] = 1;\r
 981                 }\r
 982                 return;\r
 983         }\r
 984 }