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