1 /*---------------------------------------------------------------------------*
2 Project: Horizon
3 File: math_Equation.cpp
4
5 Copyright (C)2009 Nintendo Co., Ltd. All rights reserved.
6
7 These coded instructions, statements, and computer programs contain
8 proprietary information of Nintendo of America Inc. and/or Nintendo
9 Company Ltd., and are protected by Federal copyright law. They may
10 not be disclosed to third parties or copied or duplicated in any form,
11 in whole or in part, without the prior written consent of Nintendo.
12
13 $Rev: 12449 $
14 *---------------------------------------------------------------------------*/
15
16 #include <nn/math.h>
17
18 #include <nn/math/math_Equation.h>
19 #include <nn/math/math_Arithmetic.h>
20 #include <nn/math/math_Triangular.h>
21 #include <nn/types.h>
22
23 namespace nn { namespace math {
24
25
26 namespace
27 {
28 // 等しいと判定する最大誤差
29 const f32 EPS = 2e-4f;
30
31
32 /*!--------------------------------------------------------------------------*
33 Name: spow<int>
34
35 @brief 対象の数の自然数乗を求めます。
36 最適化によって静的なコードが生成される事を期待します。
37
38 @param[in] idx 指数です。定数で指定しなければなりません。
39 @param[in] x 底です。
40
41 @return x の idx 乗を返します。
42 *---------------------------------------------------------------------------*/
43 template <int idx>
44 NN_MATH_INLINE f32
spow(f32 x)45 spow(f32 x)
46 {
47 return spow<idx-1>(x) * x;
48 }
49
50 template <>
51 NN_MATH_INLINE f32
spow(f32 x)52 spow<1>(f32 x)
53 {
54 return x;
55 }
56
57 /*!--------------------------------------------------------------------------*
58 Name: SolveEquation2
59
60 @brief 二次方程式 x^2 + bx + c = 0 の実数解を求めます。
61 SolveEquation4 用です。
62
63 @param[out] root 実数解を格納する配列へのポインタ。
64 二次方程式には最大で2つの実数解が存在しますので、
65 配列の大きさは2以上でなければなりません。
66 @param[in] b,c 対象の二次方程式の係数。
67
68 @return 実数解の個数。
69 root が指す配列の先頭から順に個数分の解が格納されます。
70 *---------------------------------------------------------------------------*/
71 int
SolveEquation2(f32 * root,f32 b,f32 c)72 SolveEquation2(f32* root, /*f32 a==1,*/ f32 b, f32 c)
73 {
74 if( b == 0 )
75 {
76 if( c < -EPS ) // c < 0
77 {
78 f32 r_c = FSqrt(-c);
79 root[0] = r_c;
80 root[1] = - r_c;
81 return 2;
82 }
83 else if( c <= EPS ) // c == 0
84 {
85 root[0] = 0;
86 return 1;
87 }
88 else // c > 0
89 {
90 return 0;
91 }
92 }
93 else
94 {
95 f32 A = b / 2;
96 f32 B = c / spow<2>(A);
97 f32 D = 1 - B;
98
99 if( D > 0 )
100 {
101 f32 C = -1 - FSqrt(D);
102
103 root[0] = A * C;
104 root[1] = A * B / C;
105
106 return 2;
107 }
108 else if( FAbs(D) < EPS )
109 {
110 root[0] = -A;
111
112 return 1;
113 }
114 else
115 {
116 return 0;
117 }
118 }
119 }
120
121
122
123 /*!--------------------------------------------------------------------------*
124 Name: SolveEquation3
125
126 @brief 三次方程式 x^3 + bx^2 + cx + d = 0 の実数解を1つ求めます。
127 SolveEquation4 用です。
128
129 @param[in] b,c,d 対象の三次方程式の係数。
130
131 @return 実数解のうちのどれか1つの値。
132 *---------------------------------------------------------------------------*/
133 f32
SolveEquation3(f32 b,f32 c,f32 d)134 SolveEquation3(/*f32* root, f32 a==1,*/ f32 b, f32 c, f32 d)
135 {
136 f32 q = (spow<2>(b) - 3 * c) / 9;
137 f32 r = (2 * spow<3>(b) - 9 * b * c + 27 * d) / 54;
138 f32 D = spow<3>(q) - spow<2>(r);
139 f32 b_3 = b / 3;
140
141 if( D > EPS )
142 {
143 f32 theta = AcosRad( r / FSqrt(spow<3>(q)) );
144 f32 theta_3 = theta/3;
145 f32 r_q2 = -2 * FSqrt(q);
146
147 return r_q2 * CosRad(theta_3) - b_3;
148 }
149 else if( D < - EPS )
150 {
151 f32 r3_Dr = FCbrt( FSqrt(- D) + FAbs(r) );
152 f32 xp = r3_Dr + q / r3_Dr;
153
154 return - FCopySign(xp, r) - b_3;
155 }
156 else
157 {
158 f32 xp = FSqrt(q);
159
160 return FCopySign(xp, r) - b_3;
161 }
162 }
163 }
164
165
166
167
168
169
170
171 /*!--------------------------------------------------------------------------*
172 Name: SolveEquation2
173
174 @brief 二次方程式 ax^2 + bx + c = 0 の実数解を求めます。
175
176 @param[out] root 実数解を格納する配列へのポインタ。
177 二次方程式には最大で2つの実数解が存在しますので、
178 配列の大きさは2以上でなければなりません。
179 @param[in] a,b,c 対象の二次方程式の係数。
180 a が 0 であってはいけません。
181
182 @return 実数解の個数。
183 root が指す配列の先頭から順に個数分の解が格納されます。
184 *---------------------------------------------------------------------------*/
185 int
SolveEquation2(f32 * root,f32 a,f32 b,f32 c)186 SolveEquation2(f32* root, f32 a, f32 b, f32 c)
187 {
188 NN_POINTER_TASSERT_(root);
189 NN_TASSERT_( a != 0 );
190
191 if( b != 0 )
192 {
193 // 一般的な解の公式
194 // x = {-b ± √(b^2 - 4ac)} / 2a
195 // では -b ± √(略) の部分で -b の正負と ± の正負が異なる方の解を
196 // 求める時に、近しい値の減算が発生し精度が大きく落ちます。
197 // これを回避するため、解の一方を
198 // x = b{-1 - √(1 - 4ac/b^2)} / 2a
199 // と b を括りだして求めます。
200 // 他方の解には、上記式に解と係数の関係
201 // αβ = a / c
202 // を適用して
203 // x = 2c / b{-1 - √(1 - 4ac/b^2)}
204 // から求めます。
205
206 f32 A = b / (2 * a);
207 f32 B = c / (a * A * A);
208 f32 D = 1 - B;
209
210 if( D > EPS )
211 {
212 f32 C = -1 - FSqrt(D);
213
214 root[0] = A * C;
215 root[1] = A * B / C;
216
217 return 2;
218 }
219 else if( D >= - EPS )
220 {
221 root[0] = -A;
222
223 return 1;
224 }
225 else
226 {
227 return 0;
228 }
229 }
230 else
231 {
232 // 上の方法では b = 0 の場合に 0 除算が発生するため場合分けします。
233
234 f32 c_a = - c / a;
235
236 if( c_a > EPS )
237 {
238 f32 r_c_a = FSqrt(c_a);
239 root[0] = + r_c_a;
240 root[1] = - r_c_a;
241 return 2;
242 }
243 else if( c_a >= - EPS )
244 {
245 root[0] = 0;
246 return 1;
247 }
248 else
249 {
250 return 0;
251 }
252 }
253 }
254
255
256
257 /*!--------------------------------------------------------------------------*
258 Name: SolveEquation3
259
260 @brief 三次方程式 ax^3 + bx^2 + cx + d = 0 の実数解を求めます。
261
262 @param[out] root 実数解を格納する配列へのポインタ。
263 三次方程式には最大で3つの実数解が存在しますので、
264 配列の大きさは3以上でなければなりません。
265 @param[in] a,b,c,d 対象の三次方程式の係数。
266 a が 0 であってはいけません。
267
268 @return 実数解の個数。
269 root が指す配列の先頭から順に個数分の解が格納されます。
270 *---------------------------------------------------------------------------*/
271 int
SolveEquation3(f32 * root,f32 a,f32 b,f32 c,f32 d)272 SolveEquation3(f32* root, f32 a, f32 b, f32 c, f32 d)
273 {
274 NN_POINTER_TASSERT_(root);
275 NN_TASSERT_( a != 0 );
276
277 //---- 両辺を最高次の係数で割って最高次の係数を 1 にします
278 b /= a;
279 c /= a;
280 d /= a;
281
282 //---- 判別式 D の値を求めます
283 f32 q = (spow<2>(b) - 3 * c) / 9;
284 f32 r = (2 * spow<3>(b) - 9 * b * c + 27 * d) / 54;
285 f32 D = spow<3>(q) - spow<2>(r);
286
287 f32 b_3 = b / 3;
288
289 if( D > EPS )
290 {
291 f32 theta = AcosRad( r / FSqrt(spow<3>(q)) );
292 f32 theta_3 = theta/3;
293 f32 r_q2 = -2 * FSqrt(q);
294
295 root[0] = r_q2 * CosRad(theta_3 + 0 * F_PI / 3) - b_3;
296 root[1] = r_q2 * CosRad(theta_3 + 2 * F_PI / 3) - b_3;
297 root[2] = r_q2 * CosRad(theta_3 + 4 * F_PI / 3) - b_3;
298
299 return 3;
300 }
301 else if( D < - EPS )
302 {
303 f32 r3_Dr = FCbrt( FSqrt(- D) + FAbs(r) );
304 f32 xp = r3_Dr + q / r3_Dr;
305
306 root[0] = - FCopySign(xp, r) - b_3;
307
308 return 1;
309 }
310 else
311 {
312 f32 xp = FSqrt(q);
313 f32 sxp = FCopySign(xp, r);
314
315 root[0] = 1 * sxp - b_3;
316 root[1] = -2 * sxp - b_3;
317
318 return 2;
319 }
320 }
321
322
323
324 /*!--------------------------------------------------------------------------*
325 Name: SolveEquation4
326
327 @brief 四次方程式 ax^4 + bx^3 + cx^2 + dx + e = 0 の実数解を
328 求めます。
329
330 @param[out] root 実数解を格納する配列へのポインタ。
331 四次方程式には最大で4つの実数解が存在しますので、
332 配列の大きさは4以上でなければなりません。
333 @param[in] a,b,c,d,e 対象の四次方程式の係数。
334 a が 0 であってはいけません。
335
336 @return 実数解の個数。
337 root が指す配列の先頭から順に個数分の解が格納されます。
338 *---------------------------------------------------------------------------*/
339 int
SolveEquation4(f32 * root,f32 a,f32 b,f32 c,f32 d,f32 e)340 SolveEquation4(f32* root, f32 a, f32 b, f32 c, f32 d, f32 e)
341 {
342 NN_POINTER_TASSERT_(root);
343 NN_TASSERT_( a != 0 );
344
345 f32 m, n, y;
346
347 //---- 両辺を最高次の係数で割って最高次の係数を 1 にします
348 b /= a;
349 c /= a;
350 d /= a;
351 e /= a;
352
353 //---- 平方完成
354 {
355 // t = x + b/4 と置くと x = t - b/4
356 // これを x に代入して整理すると3次の項が消えて、
357 // t^4 + pt^2 + qt + r = 0
358 // となる。ここで p, q, r はそれぞれ以下のとおり。
359
360 f32 p = - 3.f * spow<2>(b) / 8 + c;
361 f32 q = spow<3>(b) / 8 - b * c / 2 + d;
362 f32 r = - 3.f * spow<4>(b) / 256 + spow<2>(b) * c / 16 - b * d / 4 + e;
363
364
365 // t に関する式を以下の形に平方完成させる事を考える
366 // (t^2 + y)^2 = (mt + n)^2 ------- (1)
367
368 if( q != 0 )
369 {
370 // まず t^2 に関して任意の値 y について
371 // (t^2 + y)^2 = 2yt^2 + y^2 - (pt^2 + qt + r)
372 // = (2y - p)t^2 + qt + (y^2 - r) ------- (2)
373 // と平方完成させる事ができる
374 // さらに右辺も平方完成させるには右辺の二次式の判別式 D = 0 であればよいので
375 // q^2 - 4(2y - p)(y^2 - r) = 0
376 // を満たす y を求めればよい。 y に関して整理すると
377 // y^3 - (p/2)y^2 - ry + (pr/2 - (q^2)/8) = 0
378 // この三次式を解いて解の一つを y として採用する。
379
380 y = SolveEquation3(-p / 2, -r, p * r / 2 - spow<2>(q) / 8);
381
382
383 // (1)式の右辺を展開すると
384 // (mt + n)^2 = m^2t^2 + 2mnt + n^2
385 // これを(2)式と比較して以下を得る
386
387 n = FSqrt(spow<2>(y) - r);
388 m = -q / (2 * n);
389 }
390 else
391 {
392 // q = 0 のとき上記三次方程式を解くと y = p/2, ±sqrt(r)
393 // SolveEquation3 によって ±sqrt(r) が選ばれると n = 0 となり、
394 // 0 除算が発生する。
395 // さらに p = q = r = 0 の場合必ず n = 0 となる。
396 // これらを回避する。
397
398 y = p / 2;
399 n = FSqrt(spow<2>(y) - r);
400 m = 0;
401 }
402 }
403
404 // 与式を(1)式の形に変形できたので、これを解く。
405 // (1)式の両辺の平方根を取ると
406 // t^2 + y = ±(mt + n)
407 // これは 2 つの二次式
408 // t^2 + mt + (y + n) = 0
409 // t^2 - mt + (y - n) = 0
410 // を表すので、これらを解けば t の値が求まる。
411 // さらに x = t - b/4 より x が求まる。
412
413 int nRoot = 0;
414 f32 b4 = b / 4;
415
416 //---- 二次方程式１
417 {
418 f32 root01[2];
419
420 int nRoot01 = SolveEquation2(root01, m, y + n);
421
422 root[nRoot] = root01[0] - b4;
423 root[nRoot+1] = root01[1] - b4;
424 nRoot += nRoot01;
425 }
426
427 //---- 二次方程式２
428 {
429 f32 root23[2];
430
431 int nRoot23 = SolveEquation2(root23, -m, y - n);
432
433 root[nRoot] = root23[0] - b4;
434 root[nRoot+1] = root23[1] - b4;
435 nRoot += nRoot23;
436 }
437
438 return nRoot;
439 }
440
441
442 }} // nw::math
443