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

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