dali-physics/third-party/bullet3/src/BulletSoftBody/poly34.cpp

   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 //-----------------------------------------------------------------------------