1 /*---------------------------------------------------------------------------*
2 Project: Horizon
3 File: math_Equation.cpp
4 Copyright (C)2009 Nintendo Co., Ltd. All rights reserved.
5 These coded instructions, statements, and computer programs contain
6 proprietary information of Nintendo of America Inc. and/or Nintendo
7 Company Ltd., and are protected by Federal copyright law. They may
8 not be disclosed to third parties or copied or duplicated in any form,
9 in whole or in part, without the prior written consent of Nintendo.
10 $Rev: 12449 $
11 *---------------------------------------------------------------------------
12
13
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 // Maximum error for determining equality
29 const f32 EPS = 2e-4f;
30
31
32 /*!--------------------------------------------------------------------------*
33 Name: spow<int>
34
35
36
37
38
39
40
41
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
61
62
63
64
65
66
67
68
69
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
127
128
129
130
131
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
175
176
177
178
179
180
181
182
183
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 // Standard solution formula
194 // With the standard solution equation, x = {-b ± √(b^2 - 4ac)} / 2a, when finding a solution when the sign of -b and the ± of the -b ± √(omitted) portion, subtraction of close value occurs and precision drops a lot.
195 //
196 //
197 // To avoid this, set one side of the solution to x = b{-1 - √(1 - 4ac/b^2)} / 2a, factor out b, and solve.
198 //
199 //
200 // For the other side, apply the relationship between the solution and the coefficients (αβ = a / c) to the above formula, and solve using x = 2c / b{-1 - √(1 - 4ac/b^2)}.
201 //
202 //
203 //
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 // When using this method, division by 0 will occur if b = 0, so handle as appropriate.
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
261
262
263
264
265
266
267
268
269
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 //---- Divide both sides of the equation by the coefficient of the term of highest degree so that the coefficient for that term is 1.
278 b /= a;
279 c /= a;
280 d /= a;
281
282 //---- Find the value of the fundamental descriminant 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
328
329
330
331
332
333
334
335
336
337
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 //---- Divide both sides of the equation by the coefficient of the term of highest degree so that the coefficient for that term is 1.
348 b /= a;
349 c /= a;
350 d /= a;
351 e /= a;
352
353 //---- Complete the square
354 {
355 // Positing t = x + b/4 leads to x = t - b/4
356 // The third degree term disappears if the above is used to substitute for x and then simplification is applied.
357 // t^4 + pt^2 + qt + r = 0
358 // results. Here, p, q, r are each as given below.
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 // Consider completing the square in a form such as the following for the formula for t.
366 // (t^2 + y)^2 = (mt + n)^2 ------- (1)
367
368 if( q != 0 )
369 {
370 // First, for any value y for t^2
371 // (t^2 + y)^2 = 2yt^2 + y^2 - (pt^2 + qt + r)
372 // = (2y - p)t^2 + qt + (y^2 - r) ------- (2)
373 // can be used to complete the square.
374 // To finally complete the square on the right side as well, it needs to meet the fundamental descriminant of the quadratic on the right side D = 0, and use:
375 // q^2 - 4(2y - p)(y^2 - r) = 0
376 // and find y that satisfies the above formula. Organizing for y gives
377 // y^3 - (p/2)y^2 - ry + (pr/2 - (q^2)/8) = 0
378 // Solving this cubic equation, one solution can be used as y.
379
380 y = SolveEquation3(-p / 2, -r, p * r / 2 - spow<2>(q) / 8);
381
382
383 // (1) Expanding the right side gives
384 // (mt + n)^2 = m^2t^2 + 2mnt + n^2
385 // Get the following by comparing this with Equation (2).
386
387 n = FSqrt(spow<2>(y) - r);
388 m = -q / (2 * n);
389 }
390 else
391 {
392 // When q = 0, y = p/2, ±sqrt(r) results when solving the above cubic equation
393 // Given SolveEquation3, if ±sqrt(r) is selected, n = 0 results,
394 // and division by 0 occurs.
395 // Finally, if p = q = r = 0, then n must equal 0.
396 // Avoid all this.
397
398 y = p / 2;
399 n = FSqrt(spow<2>(y) - r);
400 m = 0;
401 }
402 }
403
404 // Since the given equation has been converted to the form of Equation 1, solve it.
405 // Taking the square root of both sides of Equation 1 results in,
406 // t^2 + y = ±(mt + n)
407 // This can be represented using two quadratic formulas:
408 // t^2 + mt + (y + n) = 0
409 // t^2 - mt + (y - n) = 0
410 // The value of t is found by solving these.
411 // x is then found using x = t - b/4.
412
413 int nRoot = 0;
414 f32 b4 = b / 4;
415
416 //---- Quadratic equation 1
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 //---- Quadratic equation 2
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