tags/2.4.1/src/jp/nyatla/nyartoolkit/core/utils/NyARSystemOfLinearEquationsProcessor.java

   1 /* \r
   2  * PROJECT: NyARToolkit(Extension)\r
   3  * --------------------------------------------------------------------------------\r
   4  * The NyARToolkit is Java edition ARToolKit class library.\r
   5  * Copyright (C)2008-2009 Ryo Iizuka\r
   6  *\r
   7  * This program is free software: you can redistribute it and/or modify\r
   8  * it under the terms of the GNU General Public License as published by\r
   9  * the Free Software Foundation, either version 3 of the License, or\r
  10  * (at your option) any later version.\r
  11  * \r
  12  * This program is distributed in the hope that it will be useful,\r
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  15  * GNU General Public License for more details.\r
  16  *\r
  17  * You should have received a copy of the GNU General Public License\r
  18  * along with this program.  If not, see <http://www.gnu.org/licenses/>.\r
  19  * \r
  20  * For further information please contact.\r
  21  *      http://nyatla.jp/nyatoolkit/\r
  22  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  23  * \r
  24  */\r
  25 package jp.nyatla.nyartoolkit.core.utils;\r
  26 \r
  27 /**\r
  28  * 連立方程式を解くためのプロセッサクラスです。\r
  29  *\r
  30  */\r
  31 public class NyARSystemOfLinearEquationsProcessor\r
  32 {\r
  33         /**\r
  34          * i_reftとi_rightの整合性を確認します。\r
  35          * @param i_left\r
  36          * @param i_right\r
  37          * @return\r
  38          */\r
  39         private static boolean isValid2dArray(double[][] i_left,double[] i_right)\r
  40         {               \r
  41                 final int sm=i_left.length;\r
  42                 final int sn=i_left[0].length;\r
  43                 if(i_left.length!=sm){\r
  44                         return false;\r
  45                 }\r
  46                 if(i_right.length!=sm){\r
  47                         return false;\r
  48                 }\r
  49                 for(int i=1;i<sm;i++){\r
  50                         if(i_left[i].length!=sn){\r
  51                                 return false;\r
  52                         }\r
  53                 }\r
  54                 return true;\r
  55         }\r
  56         /**\r
  57          * [i_left_src]=[i_right_src]の式にガウスの消去法を実行して、[x][x]の要素が1になるように基本変形します。\r
  58          * i_mとi_nが等しくない時は、最終行までの[x][x]要素までを1になるように変形します。\r
  59          * @param i_left\r
  60          * 連立方程式の左辺値を指定します。[i_m][i_n]の配列を指定してください。\r
  61          * @param i_right\r
  62          * 連立方程式の右辺値を指定します。[i_m][i_n]の配列を指定してください。\r
  63          * @param i_n\r
  64          * 連立方程式の係数の数を指定します。\r
  65          * @param i_m\r
  66          * 連立方程式の数を指定します。\r
  67          * @return\r
  68          * 最終行まで基本変形ができてばtrueを返します。\r
  69          */\r
  70         public static boolean doGaussianElimination(double[][] i_left,double[] i_right,int i_n,int i_m)\r
  71         {\r
  72                 //整合性を確認する.\r
  73                 assert isValid2dArray(i_left,i_right);\r
  74                 \r
  75 \r
  76                 //1行目以降\r
  77                 for(int solve_row=0;solve_row<i_m;solve_row++)\r
  78                 {\r
  79                         {//ピボット操作\r
  80                                 int pivod=solve_row;\r
  81                                 double pivod_value=Math.abs(i_left[pivod][pivod]);\r
  82                                 for(int i=solve_row+1;i<i_m;i++){\r
  83                                         final double pivod_2=Math.abs(i_left[i][pivod]);\r
  84                                         if(pivod_value<Math.abs(pivod_2)){\r
  85                                                 pivod=i;\r
  86                                                 pivod_value=pivod_2;\r
  87                                         }\r
  88                                 }\r
  89                                 if(solve_row!=pivod){\r
  90                                         //行の入れ替え(Cの時はポインタテーブル使って！)\r
  91                                         final double[] t=i_left[solve_row];\r
  92                                         i_left[solve_row]=i_left[pivod];\r
  93                                         i_left[pivod]=t;\r
  94                                         final double t2=i_right[solve_row];\r
  95                                         i_right[solve_row]=i_right[pivod];\r
  96                                         i_right[pivod]=t2;\r
  97                                 }\r
  98                         }\r
  99                         final double[] dest_l_n=i_left[solve_row];\r
 100                         final double dest_l_nn=i_left[solve_row][solve_row];\r
 101                         if(dest_l_nn==0.0){\r
 102                                 //選択後の対角要素が0になってしまったら失敗する。\r
 103                                 return false;\r
 104                         }                       \r
 105 \r
 106                         //消去計算(0 - solve_row-1項までの消去)\r
 107                         for(int i=0;i<solve_row;i++){\r
 108                                 double s=dest_l_n[i];\r
 109                                 for(int i2=0;i2<i_n;i2++)\r
 110                                 {\r
 111                                         final double p=i_left[i][i2]*s;\r
 112                                         dest_l_n[i2]=dest_l_n[i2]-p;\r
 113                                 }\r
 114                                 final double k=i_right[i]*s;\r
 115                                 i_right[solve_row]=i_right[solve_row]-k;\r
 116                                 \r
 117                         }\r
 118                         //消去法の実行(割り算)\r
 119                         final double d=dest_l_n[solve_row];\r
 120                         for(int i2=0;i2<solve_row;i2++){\r
 121                                 dest_l_n[i2]=0;\r
 122                         }\r
 123                         if(d!=1.0){\r
 124                                 dest_l_n[solve_row]=1.0;\r
 125                                 for(int i=solve_row+1;i<i_n;i++){\r
 126                                         dest_l_n[i]/=d;\r
 127                                 }\r
 128                                 i_right[solve_row]/=d;\r
 129                         }\r
 130                 }\r
 131                 return true;    \r
 132         }\r
 133         /**\r
 134          * i_leftとi_rightの連立方程式を解いて、i_left,i_right内容を更新します。\r
 135          * i_right[n]の内容が、i_left[x][n]番目の係数の解になります。\r
 136          * @return\r
 137          * 方程式が解ければtrueを返します。\r
 138          */\r
 139         public static boolean solve(double[][] i_left,double[] i_right,int i_number_of_system)\r
 140         {\r
 141                 return doGaussianElimination(i_left,i_right,i_number_of_system,i_number_of_system);\r
 142         }\r
 143 }\r