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
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
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
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
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
25 package jp.nyatla.nyartoolkit.core.utils;
\r
27 import jp.nyatla.nyartoolkit.*;
\r
32 public class NyAREquationSolver
\r
34 public static int solve2Equation(double i_a, double i_b, double i_c,double[] o_result)
\r
37 return solve2Equation(i_b/i_a,i_c/i_a,o_result,0);
\r
40 public static int solve2Equation(double i_b, double i_c,double[] o_result)
\r
42 return solve2Equation(i_b,i_c,o_result,0);
\r
45 public static int solve2Equation(double i_b, double i_c,double[] o_result,int i_result_st)
\r
47 double t=i_b*i_b-4*i_c;
\r
54 o_result[i_result_st+0]=-i_b/(2);
\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
65 * 3次方程式 a*x^3+b*x^2+c*x+d=0の実根を求める。
\r
66 * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html
\r
77 * 実根。double[3]を指定すること。
\r
80 public static int solve3Equation(double i_a, double i_b, double i_c, double i_d,double[] o_result)
\r
83 return solve3Equation(i_b/i_a,i_c/i_a,i_d/i_a,o_result);
\r
87 * 3次方程式 x^3+b*x^2+c*x+d=0の実根を求める。
\r
89 * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html
\r
98 * 実根。double[1]以上を指定すること。
\r
101 public static int solve3Equation(double i_b, double i_c, double i_d,double[] o_result)
\r
103 double tmp,b, p, q;
\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
110 o_result[0] = 2 * q - b;
\r
111 o_result[1] = -q - b;
\r
113 } else if (tmp > 0) {
\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
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
144 * 実根。double[3]を指定すること。
\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
150 double A3,A2,A1,A0,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
162 double result_0,result_1;
\r
164 int res=solve2Equation(p,r,o_result,0);
\r
172 result_0=o_result[0];
\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
189 //実根2個だからt==t2==0はありえない。(case1)
\r
191 result_0=o_result[0];
\r
192 result_1=o_result[1];
\r
193 int number_of_result=0;
\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
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
209 return number_of_result;
\r
211 throw new NyARException();
\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
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
227 switch(result_1st){
\r
231 o_result[0]=o_result[0]-B3;
\r
234 o_result[0]=o_result[0]-B3;
\r
235 o_result[1]=o_result[1]-B3;
\r
238 throw new NyARException();
\r
240 result_2nd=solve2Equation(ru,(p+u)/2-ru*q/(2*u),o_result,result_1st);
\r
242 switch(result_2nd){
\r
246 o_result[result_1st+0]=o_result[result_1st+0]-B3;
\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
253 throw new NyARException();
\r
255 return result_1st+result_2nd;
\r
259 * 3乗根を求められないシステムで、3乗根を求めます。
\r
260 * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html
\r
264 private static 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
269 * 3次方程式の実根を1個だけ求める。
\r
277 private static double solve3Equation_1(double i_b, double i_c, double i_d)
\r
279 double tmp,b, p, q;
\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
287 } else if (tmp > 0) {
\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
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
300 public static void main(String[] args)
\r
302 NyAREquationSolver n = new NyAREquationSolver();
\r
304 double[] r = new double[10];
\r
306 l=n.solve4Equation(1, 9, -18, -68, 120, r);
\r
307 }catch(Exception e){
\r
308 e.printStackTrace();
\r
310 System.out.println(l);
\r