trunk/src/jp/nyatla/nyartoolkit/core/utils/NyAREquationSolver.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\r
   8  * modify it under the terms of the GNU Lesser General Public License\r
   9  * as published by the Free Software Foundation; either version 3\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 Lesser General Public License for more details\r
  16  * \r
  17  * You should have received a copy of the GNU Lesser General Public\r
  18  * License 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 import jp.nyatla.nyartoolkit.*;\r
  28 /**\r
  29  * 方程式を解く関数を定義します。\r
  30  *\r
  31  */\r
  32 public class NyAREquationSolver\r
  33 {\r
  34         public static int solve2Equation(double i_a, double i_b, double i_c,double[] o_result)\r
  35         {\r
  36                 assert i_a!=0;\r
  37                 return solve2Equation(i_b/i_a,i_c/i_a,o_result,0);\r
  38         }\r
  39         \r
  40         public static int solve2Equation(double i_b, double i_c,double[] o_result)\r
  41         {\r
  42                 return solve2Equation(i_b,i_c,o_result,0);\r
  43         }\r
  44         \r
  45         public static int solve2Equation(double i_b, double i_c,double[] o_result,int i_result_st)\r
  46         {\r
  47                 double t=i_b*i_b-4*i_c;\r
  48                 if(t<0){\r
  49                         //虚数根\r
  50                         return 0;\r
  51                 }\r
  52                 if(t==0){\r
  53                         //重根\r
  54                         o_result[i_result_st+0]=-i_b/(2);\r
  55                         return 1;\r
  56                 }\r
  57                 //実根２個\r
  58                 t=Math.sqrt(t);\r
  59                 o_result[i_result_st+0]=(-i_b+t)/(2);\r
  60                 o_result[i_result_st+1]=(-i_b-t)/(2);\r
  61                 return 2;\r
  62         }\r
  63 \r
  64         /**\r
  65          * ３次方程式 a*x^3+b*x^2+c*x+d=0の実根を求める。   \r
  66          * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html\r
  67          * のコードを基にしてます。\r
  68          * @param i_a\r
  69          * X^3の係数\r
  70          * @param i_b\r
  71          * X^2の係数\r
  72          * @param i_c\r
  73          * X^1の係数\r
  74          * @param i_d\r
  75          * X^0の係数\r
  76          * @param o_result\r
  77          * 実根。double[3]を指定すること。\r
  78          * @return\r
  79          */\r
  80         public static int solve3Equation(double i_a, double i_b, double i_c, double i_d,double[] o_result)\r
  81         {\r
  82                 assert (i_a != 0);\r
  83                 return solve3Equation(i_b/i_a,i_c/i_a,i_d/i_a,o_result);\r
  84         }\r
  85         \r
  86         /**\r
  87          * ３次方程式 x^3+b*x^2+c*x+d=0の実根を求める。\r
  88          * だけを求める。\r
  89          * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html\r
  90          * のコードを基にしてます。\r
  91          * @param i_b\r
  92          * X^2の係数\r
  93          * @param i_c\r
  94          * X^1の係数\r
  95          * @param i_d\r
  96          * X^0の係数\r
  97          * @param o_result\r
  98          * 実根。double[1]以上を指定すること。\r
  99          * @return\r
 100          */\r
 101         public static int solve3Equation(double i_b, double i_c, double i_d,double[] o_result)\r
 102         {\r
 103                 double tmp,b,   p, q;\r
 104                 b = i_b/(3);\r
 105                 p = b * b - i_c / 3;\r
 106                 q = (b * (i_c - 2 * b * b) - i_d) / 2;\r
 107                 if ((tmp = q * q - p * p * p) == 0) {\r
 108                         // 重根\r
 109                         q = Math.cbrt(q);\r
 110                         o_result[0] = 2 * q - b;\r
 111                         o_result[1] = -q - b;\r
 112                         return 2;\r
 113                 } else if (tmp > 0) {\r
 114                         // 実根1,虚根2\r
 115                         double a3 = Math.cbrt(q + ((q > 0) ? 1 : -1) * Math.sqrt(tmp));\r
 116                         double b3 = p / a3;\r
 117                         o_result[0] = a3 + b3 - b;\r
 118                         // 虚根:-0.5*(a3+b3)-b,Math.abs(a3-b3)*Math.sqrt(3.0)/2\r
 119                         return 1;\r
 120                 } else {\r
 121                         // 実根3\r
 122                         tmp = 2 * Math.sqrt(p);\r
 123                         double t = Math.acos(q / (p * tmp / 2));\r
 124                         o_result[0] = tmp * Math.cos(t / 3) - b;\r
 125                         o_result[1] = tmp * Math.cos((t + 2 * Math.PI) / 3) - b;\r
 126                         o_result[2] = tmp * Math.cos((t + 4 * Math.PI) / 3) - b;\r
 127                         return 3;\r
 128                 }\r
 129         }\r
 130 \r
 131         \r
 132         \r
 133         /**\r
 134          * ４次方程式の実根だけを求める。\r
 135          * @param i_a\r
 136          * X^3の係数\r
 137          * @param i_b\r
 138          * X^2の係数\r
 139          * @param i_c\r
 140          * X^1の係数\r
 141          * @param i_d\r
 142          * X^0の係数\r
 143          * @param o_result\r
 144          * 実根。double[3]を指定すること。\r
 145          * @return\r
 146          */\r
 147         public static int solve4Equation(double i_a, double i_b, double i_c, double i_d,double i_e,double[] o_result) throws NyARException\r
 148         {\r
 149                 assert (i_a != 0);\r
 150                 double A3,A2,A1,A0,B3;\r
 151                 A3=i_b/i_a;\r
 152                 A2=i_c/i_a;\r
 153                 A1=i_d/i_a;\r
 154                 A0=i_e/i_a;\r
 155                 B3=A3/4;\r
 156                 double p,q,r;\r
 157                 double B3_2=B3*B3;\r
 158                 p=A2-6*B3_2;//A2-6*B3*B3;\r
 159                 q=A1+B3*(-2*A2+8*B3_2);//A1-2*A2*B3+8*B3*B3*B3;\r
 160                 r=A0+B3*(-A1+A2*B3)-3*B3_2*B3_2;//A0-A1*B3+A2*B3*B3-3*B3*B3*B3*B3;\r
 161                 if(q==0){\r
 162                         double result_0,result_1;\r
 163                         //複二次式\r
 164                         int res=solve2Equation(p,r,o_result,0);\r
 165                         switch(res){\r
 166                         case 0:\r
 167                                 //全て虚数解\r
 168                                 return 0;\r
 169                         case 1:\r
 170                                 //重根\r
 171                                 //解は0,1,2の何れか。\r
 172                                 result_0=o_result[0];\r
 173                                 if(result_0<0){\r
 174                                         //全て虚数解\r
 175                                         return 0;\r
 176                                 }\r
 177                                 //実根1個\r
 178                                 if(result_0==0){\r
 179                                         //NC\r
 180                                         o_result[0]=0-B3;\r
 181                                         return 1;\r
 182                                 }\r
 183                                 //実根2個\r
 184                                 result_0=Math.sqrt(result_0);\r
 185                                 o_result[0]=result_0-B3;\r
 186                                 o_result[1]=-result_0-B3;\r
 187                                 return 2;\r
 188                         case 2:\r
 189                                 //実根２個だからt==t2==0はありえない。(case1)\r
 190                                 //解は、0,2,4の何れか。\r
 191                                 result_0=o_result[0];\r
 192                                 result_1=o_result[1];\r
 193                                 int number_of_result=0;\r
 194                                 if(result_0>0){\r
 195                                         //NC\r
 196                                         result_0=Math.sqrt(result_0);\r
 197                                         o_result[0]= result_0-B3;\r
 198                                         o_result[1]=-result_0-B3;\r
 199                                         number_of_result+=2;\r
 200                                 }\r
 201                                 if(result_1>0)\r
 202                                 {\r
 203                                         //NC\r
 204                                         result_1=Math.sqrt(result_1);\r
 205                                         o_result[number_of_result+0]= result_1-B3;\r
 206                                         o_result[number_of_result+1]=-result_1-B3;\r
 207                                         number_of_result+=2;\r
 208                                 }\r
 209                                 return number_of_result;\r
 210                         default:\r
 211                                 throw new NyARException();\r
 212                         }\r
 213                 }else{\r
 214                         //それ以外\r
 215                         //最適化ポイント:\r
 216                         //u^3  + (2*p)*u^2  +((- 4*r)+(p^2))*u -q^2= 0\r
 217                         double u=solve3Equation_1((2*p),(- 4*r)+(p*p),-q*q);\r
 218                         if(u<0){\r
 219                                 //全て虚数解\r
 220                                 return 0;\r
 221                         }\r
 222                         double ru=Math.sqrt(u);\r
 223                         //2次方程式を解いてyを計算(最適化ポイント)\r
 224                         int result_1st,result_2nd;\r
 225                         result_1st=solve2Equation(-ru,(p+u)/2+ru*q/(2*u),o_result,0);\r
 226                         //配列使い回しのために、変数に退避\r
 227                         switch(result_1st){\r
 228                         case 0:\r
 229                                 break;\r
 230                         case 1:\r
 231                                 o_result[0]=o_result[0]-B3;\r
 232                                 break;\r
 233                         case 2:\r
 234                                 o_result[0]=o_result[0]-B3;\r
 235                                 o_result[1]=o_result[1]-B3;\r
 236                                 break;\r
 237                         default:\r
 238                                 throw new NyARException();\r
 239                         }\r
 240                         result_2nd=solve2Equation(ru,(p+u)/2-ru*q/(2*u),o_result,result_1st);\r
 241                         //0,1番目に格納\r
 242                         switch(result_2nd){\r
 243                         case 0:\r
 244                                 break;\r
 245                         case 1:\r
 246                                 o_result[result_1st+0]=o_result[result_1st+0]-B3;\r
 247                                 break;\r
 248                         case 2:\r
 249                                 o_result[result_1st+0]=o_result[result_1st+0]-B3;\r
 250                                 o_result[result_1st+1]=o_result[result_1st+1]-B3;\r
 251                                 break;\r
 252                         default:\r
 253                                 throw new NyARException();\r
 254                         }\r
 255                         return result_1st+result_2nd;\r
 256                 }\r
 257         }\r
 258         /**\r
 259          * 3乗根を求められないシステムで、３乗根を求めます。\r
 260          * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html\r
 261          * @param i_in\r
 262          * @return\r
 263          */\r
 264         private double cuberoot(double i_in) {\r
 265                 double res = Math.pow(Math.abs(i_in), 1.0 / 3.0);\r
 266                 return (i_in >= 0) ? res : -res;\r
 267         }\r
 268         /**\r
 269          * 3次方程式の実根を１個だけ求める。\r
 270          * 4字方程式で使う。\r
 271          * @param i_b\r
 272          * @param i_c\r
 273          * @param i_d\r
 274          * @param o_result\r
 275          * @return\r
 276          */\r
 277         private static double solve3Equation_1(double i_b, double i_c, double i_d)\r
 278         {\r
 279                 double tmp,b,   p, q;\r
 280                 b = i_b/(3);\r
 281                 p = b * b - i_c / 3;\r
 282                 q = (b * (i_c - 2 * b * b) - i_d) / 2;\r
 283                 if ((tmp = q * q - p * p * p) == 0) {\r
 284                         // 重根\r
 285                         q = Math.cbrt(q);\r
 286                         return 2 * q - b;\r
 287                 } else if (tmp > 0) {\r
 288                         // 実根1,虚根2\r
 289                         double a3 = Math.cbrt(q + ((q > 0) ? 1 : -1) * Math.sqrt(tmp));\r
 290                         double b3 = p / a3;\r
 291                         return a3 + b3 - b;\r
 292                 } else {\r
 293                         // 実根3\r
 294                         tmp = 2 * Math.sqrt(p);\r
 295                         double t = Math.acos(q / (p * tmp / 2));\r
 296                         return tmp * Math.cos(t / 3) - b;\r
 297                 }\r
 298         }               \r
 299 \r
 300         public static void main(String[] args)\r
 301         {\r
 302                 NyAREquationSolver n = new NyAREquationSolver();\r
 303                 int l=0;\r
 304                 double[] r = new double[10];\r
 305                 try{\r
 306                         l=n.solve4Equation(1, 9, -18, -68, 120, r);\r
 307                 }catch(Exception e){\r
 308                         e.printStackTrace();\r
 309                 }\r
 310                 System.out.println(l);\r
 311         }\r
 312 }\r