Bullet Collision Detection & Physics Library: poly34.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

poly34.cpp
Go to the documentation of this file.
1// poly34.cpp : solution of cubic and quartic equation
2// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
3// khash2 (at) gmail.com
4// Thanks to Alexandr Rakhmanin <rakhmanin (at) gmail.com>
5// public domain
6//
7#include <math.h>
8
9#include "poly34.h" // solution of cubic and quartic equation
10#define TwoPi 6.28318530717958648
11 const btScalar eps = SIMD_EPSILON;
12
13//=============================================================================
14// _root3, root3 from http://prografix.narod.ru
15//=============================================================================
16 static SIMD_FORCE_INLINE btScalar _root3(btScalar x)
17{
18 btScalar s = 1.;
19 while (x < 1.)
20 {
21 x *= 8.;
22 s *= 0.5;
23 }
24 while (x > 8.)
25 {
26 x *= 0.125;
27 s *= 2.;
28 }
29 btScalar r = 1.5;
30 r -= 1. / 3. * (r - x / (r * r));
31 r -= 1. / 3. * (r - x / (r * r));
32 r -= 1. / 3. * (r - x / (r * r));
33 r -= 1. / 3. * (r - x / (r * r));
34 r -= 1. / 3. * (r - x / (r * r));
35 r -= 1. / 3. * (r - x / (r * r));
36 return r * s;
37}
38
39 btScalar SIMD_FORCE_INLINE root3(btScalar x)
40{
41 if (x > 0)
42 return _root3(x);
43 else if (x < 0)
44 return -_root3(-x);
45 else
46 return 0.;
47}
48
49// x - array of size 2
50// return 2: 2 real roots x[0], x[1]
51// return 0: pair of complex roots: x[0]i*x[1]
52 int SolveP2(btScalar* x, btScalar a, btScalar b)
53{ // solve equation x^2 + a*x + b = 0
54 btScalar D = 0.25 * a * a - b;
55 if (D >= 0)
56 {
57 D = sqrt(D);
58 x[0] = -0.5 * a + D;
59 x[1] = -0.5 * a - D;
60 return 2;
61 }
62 x[0] = -0.5 * a;
63 x[1] = sqrt(-D);
64 return 0;
65}
66//---------------------------------------------------------------------------
67// x - array of size 3
68// In case 3 real roots: => x[0], x[1], x[2], return 3
69// 2 real roots: x[0], x[1], return 2
70// 1 real root : x[0], x[1] i*x[2], return 1
71 int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c)
72{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0
73 btScalar a2 = a * a;
74 btScalar q = (a2 - 3 * b) / 9;
75 if (q < 0)
76 q = eps;
77 btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
78 // equation x^3 + q*x + r = 0
79 btScalar r2 = r * r;
80 btScalar q3 = q * q * q;
81 btScalar A, B;
82 if (r2 <= (q3 + eps))
83 { //<<-- FIXED!
84 btScalar t = r / sqrt(q3);
85 if (t < -1)
86 t = -1;
87 if (t > 1)
88 t = 1;
89 t = acos(t);
90 a /= 3;
91 q = -2 * sqrt(q);
92 x[0] = q * cos(t / 3) - a;
93 x[1] = q * cos((t + TwoPi) / 3) - a;
94 x[2] = q * cos((t - TwoPi) / 3) - a;
95 return (3);
96 }
97 else
98 {
99 //A =-pow(fabs(r)+sqrt(r2-q3),1./3);
100 A = -root3(fabs(r) + sqrt(r2 - q3));
101 if (r < 0)
102 A = -A;
103 B = (A == 0 ? 0 : q / A);
104
105 a /= 3;
106 x[0] = (A + B) - a;
107 x[1] = -0.5 * (A + B) - a;
108 x[2] = 0.5 * sqrt(3.) * (A - B);
109 if (fabs(x[2]) < eps)
110 {
111 x[2] = x[1];
112 return (2);
113 }
114 return (1);
115 }
116} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) {
117//---------------------------------------------------------------------------
118// a>=0!
119 void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y)
120{
121 btScalar r = sqrt(x * x + y * y);
122 if (y == 0)
123 {
124 r = sqrt(r);
125 if (x >= 0)
126 {
127 a = r;
128 b = 0;
129 }
130 else
131 {
132 a = 0;
133 b = r;
134 }
135 }
136 else
137 { // y != 0
138 a = sqrt(0.5 * (x + r));
139 b = 0.5 * y / a;
140 }
141}
142//---------------------------------------------------------------------------
143 int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0
144{
145 btScalar D = b * b - 4 * d;
146 if (D >= 0)
147 {
148 btScalar sD = sqrt(D);
149 btScalar x1 = (-b + sD) / 2;
150 btScalar x2 = (-b - sD) / 2; // x2 <= x1
151 if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots
152 {
153 btScalar sx1 = sqrt(x1);
154 btScalar sx2 = sqrt(x2);
155 x[0] = -sx1;
156 x[1] = sx1;
157 x[2] = -sx2;
158 x[3] = sx2;
159 return 4;
160 }
161 if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots
162 {
163 btScalar sx1 = sqrt(-x1);
164 btScalar sx2 = sqrt(-x2);
165 x[0] = 0;
166 x[1] = sx1;
167 x[2] = 0;
168 x[3] = sx2;
169 return 0;
170 }
171 // now x2 < 0 <= x1 , two real roots and one pair of imginary root
172 btScalar sx1 = sqrt(x1);
173 btScalar sx2 = sqrt(-x2);
174 x[0] = -sx1;
175 x[1] = sx1;
176 x[2] = 0;
177 x[3] = sx2;
178 return 2;
179 }
180 else
181 { // if( D < 0 ), two pair of compex roots
182 btScalar sD2 = 0.5 * sqrt(-D);
183 CSqrt(-0.5 * b, sD2, x[0], x[1]);
184 CSqrt(-0.5 * b, -sD2, x[2], x[3]);
185 return 0;
186 } // if( D>=0 )
187} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d
188//---------------------------------------------------------------------------
189#define SWAP(a, b) \
190 { \
191 t = b; \
192 b = a; \
193 a = t; \
194 }
195 static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c
196{
197 btScalar t;
198 if (a > b)
199 SWAP(a, b); // now a<=b
200 if (c < b)
201 {
202 SWAP(b, c); // now a<=b, b<=c
203 if (a > b)
204 SWAP(a, b); // now a<=b
205 }
206}
207//---------------------------------------------------------------------------
208 int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
209{
210 //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
211 if (fabs(c) < 1e-14 * (fabs(b) + fabs(d)))
212 return SolveP4Bi(x, b, d); // After that, c!=0
213
214 int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent
215 // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
216 if (res3 > 1) // 3 real roots,
217 {
218 dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
219 // Note: x[0]*x[1]*x[2]= c*c > 0
220 if (x[0] > 0) // all roots are positive
221 {
222 btScalar sz1 = sqrt(x[0]);
223 btScalar sz2 = sqrt(x[1]);
224 btScalar sz3 = sqrt(x[2]);
225 // Note: sz1*sz2*sz3= -c (and not equal to 0)
226 if (c > 0)
227 {
228 x[0] = (-sz1 - sz2 - sz3) / 2;
229 x[1] = (-sz1 + sz2 + sz3) / 2;
230 x[2] = (+sz1 - sz2 + sz3) / 2;
231 x[3] = (+sz1 + sz2 - sz3) / 2;
232 return 4;
233 }
234 // now: c<0
235 x[0] = (-sz1 - sz2 + sz3) / 2;
236 x[1] = (-sz1 + sz2 - sz3) / 2;
237 x[2] = (+sz1 - sz2 - sz3) / 2;
238 x[3] = (+sz1 + sz2 + sz3) / 2;
239 return 4;
240 } // if( x[0] > 0) // all roots are positive
241 // now x[0] <= x[1] < 0, x[2] > 0
242 // two pair of comlex roots
243 btScalar sz1 = sqrt(-x[0]);
244 btScalar sz2 = sqrt(-x[1]);
245 btScalar sz3 = sqrt(x[2]);
246
247 if (c > 0) // sign = -1
248 {
249 x[0] = -sz3 / 2;
250 x[1] = (sz1 - sz2) / 2; // x[0]i*x[1]
251 x[2] = sz3 / 2;
252 x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3]
253 return 0;
254 }
255 // now: c<0 , sign = +1
256 x[0] = sz3 / 2;
257 x[1] = (-sz1 + sz2) / 2;
258 x[2] = -sz3 / 2;
259 x[3] = (sz1 + sz2) / 2;
260 return 0;
261 } // if( res3>1 ) // 3 real roots,
262 // now resoventa have 1 real and pair of compex roots
263 // x[0] - real root, and x[0]>0,
264 // x[1]i*x[2] - complex roots,
265 // x[0] must be >=0. But one times x[0]=~ 1e-17, so:
266 if (x[0] < 0)
267 x[0] = 0;
268 btScalar sz1 = sqrt(x[0]);
269 btScalar szr, szi;
270 CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
271 if (c > 0) // sign = -1
272 {
273 x[0] = -sz1 / 2 - szr; // 1st real root
274 x[1] = -sz1 / 2 + szr; // 2nd real root
275 x[2] = sz1 / 2;
276 x[3] = szi;
277 return 2;
278 }
279 // now: c<0 , sign = +1
280 x[0] = sz1 / 2 - szr; // 1st real root
281 x[1] = sz1 / 2 + szr; // 2nd real root
282 x[2] = -sz1 / 2;
283 x[3] = szi;
284 return 2;
285} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d
286//-----------------------------------------------------------------------------
287 btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
288{
289 btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x)
290 if (fxs == 0)
291 return x; //return 1e99; <<-- FIXED!
292 btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x)
293 return x - fx / fxs;
294}
295//-----------------------------------------------------------------------------
296// x - array of size 4
297// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
298// return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3],
299// return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3],
300 int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d)
301{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
302 // move to a=0:
303 btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
304 btScalar c1 = c + 0.5 * a * (0.25 * a * a - b);
305 btScalar b1 = b - 0.375 * a * a;
306 int res = SolveP4De(x, b1, c1, d1);
307 if (res == 4)
308 {
309 x[0] -= a / 4;
310 x[1] -= a / 4;
311 x[2] -= a / 4;
312 x[3] -= a / 4;
313 }
314 else if (res == 2)
315 {
316 x[0] -= a / 4;
317 x[1] -= a / 4;
318 x[2] -= a / 4;
319 }
320 else
321 {
322 x[0] -= a / 4;
323 x[2] -= a / 4;
324 }
325 // one Newton step for each real root:
326 if (res > 0)
327 {
328 x[0] = N4Step(x[0], a, b, c, d);
329 x[1] = N4Step(x[1], a, b, c, d);
330 }
331 if (res > 2)
332 {
333 x[2] = N4Step(x[2], a, b, c, d);
334 x[3] = N4Step(x[3], a, b, c, d);
335 }
336 return res;
337}
338//-----------------------------------------------------------------------------
339#define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e)
340//-----------------------------------------------------------------------------
341 btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
342{
343 int cnt;
344 if (fabs(e) < eps)
345 return 0;
346
347 btScalar brd = fabs(a); // brd - border of real roots
348 if (fabs(b) > brd)
349 brd = fabs(b);
350 if (fabs(c) > brd)
351 brd = fabs(c);
352 if (fabs(d) > brd)
353 brd = fabs(d);
354 if (fabs(e) > brd)
355 brd = fabs(e);
356 brd++; // brd - border of real roots
357
358 btScalar x0, f0; // less than root
359 btScalar x1, f1; // greater than root
360 btScalar x2, f2, f2s; // next values, f(x2), f'(x2)
361 btScalar dx = 0;
362
363 if (e < 0)
364 {
365 x0 = 0;
366 x1 = brd;
367 f0 = e;
368 f1 = F5(x1);
369 x2 = 0.01 * brd;
370 } // positive root
371 else
372 {
373 x0 = -brd;
374 x1 = 0;
375 f0 = F5(x0);
376 f1 = e;
377 x2 = -0.01 * brd;
378 } // negative root
379
380 if (fabs(f0) < eps)
381 return x0;
382 if (fabs(f1) < eps)
383 return x1;
384
385 // now x0<x1, f(x0)<0, f(x1)>0
386 // Firstly 10 bisections
387 for (cnt = 0; cnt < 10; cnt++)
388 {
389 x2 = (x0 + x1) / 2; // next point
390 //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point
391 f2 = F5(x2); // f(x2)
392 if (fabs(f2) < eps)
393 return x2;
394 if (f2 > 0)
395 {
396 x1 = x2;
397 f1 = f2;
398 }
399 else
400 {
401 x0 = x2;
402 f0 = f2;
403 }
404 }
405
406 // At each step:
407 // x0<x1, f(x0)<0, f(x1)>0.
408 // x2 - next value
409 // we hope that x0 < x2 < x1, but not necessarily
410 do
411 {
412 if (cnt++ > 50)
413 break;
414 if (x2 <= x0 || x2 >= x1)
415 x2 = (x0 + x1) / 2; // now x0 < x2 < x1
416 f2 = F5(x2); // f(x2)
417 if (fabs(f2) < eps)
418 return x2;
419 if (f2 > 0)
420 {
421 x1 = x2;
422 f1 = f2;
423 }
424 else
425 {
426 x0 = x2;
427 f0 = f2;
428 }
429 f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2)
430 if (fabs(f2s) < eps)
431 {
432 x2 = 1e99;
433 continue;
434 }
435 dx = f2 / f2s;
436 x2 -= dx;
437 } while (fabs(dx) > eps);
438 return x2;
439} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
440//-----------------------------------------------------------------------------
441 int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
442{
443 btScalar r = x[0] = SolveP5_1(a, b, c, d, e);
444 btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
445 return 1 + SolveP4(x + 1, a1, b1, c1, d1);
446} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
447//-----------------------------------------------------------------------------
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition btScalar.h:98
SIMD_EPSILON
#define SIMD_EPSILON
Definition btScalar.h:543
root3
btScalar root3(btScalar x)
Definition poly34.cpp:39
SolveP5
int SolveP5(btScalar *x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e)
Definition poly34.cpp:441
CSqrt
void CSqrt(btScalar x, btScalar y, btScalar &a, btScalar &b)
Definition poly34.cpp:119
SolveP5_1
btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e)
Definition poly34.cpp:341
eps
const btScalar eps
Definition poly34.cpp:11
SolveP4
int SolveP4(btScalar *x, btScalar a, btScalar b, btScalar c, btScalar d)
Definition poly34.cpp:300
_root3
static btScalar _root3(btScalar x)
Definition poly34.cpp:16
dblSort3
static void dblSort3(btScalar &a, btScalar &b, btScalar &c)
Definition poly34.cpp:195
SolveP3
int SolveP3(btScalar *x, btScalar a, btScalar b, btScalar c)
Definition poly34.cpp:71
SolveP4Bi
int SolveP4Bi(btScalar *x, btScalar b, btScalar d)
Definition poly34.cpp:143
SWAP
#define SWAP(a, b)
Definition poly34.cpp:189
N4Step
btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d)
Definition poly34.cpp:287
F5
#define F5(t)
Definition poly34.cpp:339
SolveP2
int SolveP2(btScalar *x, btScalar a, btScalar b)
Definition poly34.cpp:52
SolveP4De
int SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d)
Definition poly34.cpp:208
TwoPi
#define TwoPi
Definition poly34.cpp:10
poly34.h