trunk/src/jp/nyatla/nyartoolkit/core/utils/NyARSystemOfLinearEquationsProcessor.java

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