/* roots.c,v 1.1.1.1 1995/02/27 07:38:35 explorer Exp */

/*
 *  Utility functions to find cubic and quartic roots,
 *  coefficients are passed like this:
 *
 *      c[0] + c[1]*x + c[2]*x^2 + c[3]*x^3 + c[4]*x^4 = 0
 *
 *  The functions return the number of non-complex roots and
 *  put the values into the s array.
 *
 *  Author:         Jochen Schwarze (schwarze@isa.de)
 *
 */

#include "libcommon/common.h"
#include "roots.h"

/*
 * In case M_PI isn't defined in math.h
 */
#ifndef M_PI
#define M_PI PI
#endif

/* epsilon surrounding for near zero values */
#define     EQN_EPS     1e-9
#define	    IsZero(x)	((x) > -EQN_EPS && (x) < EQN_EPS)
#define	    IsZero2(x)	((x) > -EQN_EPS*EQN_EPS && (x) < EQN_EPS*EQN_EPS)

#ifndef HAVE_CBRT
#define     cbrt(x)     ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
			  ((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))
#endif


int SolveQuadric(c, s)
     double c[3];
     double s[2];
{
  double p, q, D, sqrt_D;
  
  /* normal form: x^2 + px + q = 0 */
  
  p = c[ 1 ] / (2 * c[ 2 ]);
  q = c[ 0 ] / c[ 2 ];
  
  D = p * p - q;
  
  if (IsZero(D)) {
    s[ 0 ] = - p;
    return 1;
  }
  else if (D > 0) {
    sqrt_D = sqrt(D);
    s[ 0 ] =   sqrt_D - p;
    s[ 1 ] = - sqrt_D - p;
    return 2;
  }
  else /* if (D < 0) */
    return 0;
}


int SolveCubic(c, s)
     double c[ 4 ];
     double s[ 3 ];
{
  int     i, num;
  double  sub;
  double  A, B, C;
  double  sq_A, p, q;
  double  cb_p, D;
  double  u, v, t, sqrt_D, phi;
  
  /* normal form: x^3 + Ax^2 + Bx + C = 0 */
  
  A = c[ 2 ] / c[ 3 ];
  B = c[ 1 ] / c[ 3 ];
  C = c[ 0 ] / c[ 3 ];
  
  /*  substitute x = y - A/3 to eliminate quadric term:
      x^3 +px + q = 0 */
  
  sq_A = A * A;
  p = 1.0/3 * (- 1.0/3 * sq_A + B);
  q = 1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C);
  
  /* use Cardano's formula */
  
  cb_p = p * p * p;
  D = q * q + cb_p;
  
  if (IsZero2(D)) {
    if (IsZero(q)) { /* one triple solution */
      s[ 0 ] = 0;
      num = 1;
    } else { /* one single and one double solution */
      u = cbrt(-q);
      s[ 0 ] = 2 * u;
      s[ 1 ] = - u;
      num = 2;
    }
  } else if (D < 0) { /* Casus irreducibilis: three real solutions */
    phi = 1.0/3 * acos(-q / sqrt(-cb_p));
    t = 2 * sqrt(-p);
    
    s[ 0 ] =   t * cos(phi);
    s[ 1 ] = - t * cos(phi + M_PI / 3);
    s[ 2 ] = - t * cos(phi - M_PI / 3);
    num = 3;
  } else { /* one real solution */
    sqrt_D = sqrt(D);
    u = cbrt(sqrt_D - q);
    v = - cbrt(sqrt_D + q);
    
    s[ 0 ] = u + v;
    num = 1;
  }
  
  /* resubstitute */
  
  sub = 1.0/3 * A;
  
  for (i = 0; i < num; ++i)
    s[ i ] -= sub;
  
  return num;
}


int SolveQuartic(c, s)
     double c[ 5 ]; 
     double s[ 4 ];
{
  double  coeffs[ 4 ];
  double  z, u, v, sub;
  double  A, B, C, D;
  double  sq_A, p, q, r;
  int     i, num;
  
  /* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */
  
  A = c[ 3 ] / c[ 4 ];
  B = c[ 2 ] / c[ 4 ];
  C = c[ 1 ] / c[ 4 ];
  D = c[ 0 ] / c[ 4 ];
  
  /*  substitute x = y - A/4 to eliminate cubic term:
      x^4 + px^2 + qx + r = 0 */
  
  sq_A = A * A;
  p = - 3.0/8 * sq_A + B;
  q = 1.0/8 * sq_A * A - 1.0/2 * A * B + C;
  r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D;
  
  if (IsZero(r)) {
    /* no absolute term: y(y^3 + py + q) = 0 */
    
    coeffs[ 0 ] = q;
    coeffs[ 1 ] = p;
    coeffs[ 2 ] = 0;
    coeffs[ 3 ] = 1;
    
    num = SolveCubic(coeffs, s);
    
    s[ num++ ] = 0;
  } else {
    /* solve the resolvent cubic ... */
    
    coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q;
    coeffs[ 1 ] = - r;
    coeffs[ 2 ] = - 1.0/2 * p;
    coeffs[ 3 ] = 1;
    
    (void) SolveCubic(coeffs, s);
    
    /* ... and take the one real solution ... */
    
    z = s[ 0 ];
    
    /* ... to build two quadric equations */
    
    u = z * z - r;
    v = 2 * z - p;
    
    if (IsZero(u))
      u = 0;
    else if (u > 0)
      u = sqrt(u);
    else
      return 0;
    
    if (IsZero(v))
      v = 0;
    else if (v > 0)
      v = sqrt(v);
    else
      return 0;
    
    coeffs[ 0 ] = z - u;
    coeffs[ 1 ] = q < 0 ? -v : v;
    coeffs[ 2 ] = 1;
    
    num = SolveQuadric(coeffs, s);
    
    coeffs[ 0 ]= z + u;
    coeffs[ 1 ] = q < 0 ? v : -v;
    coeffs[ 2 ] = 1;
    
    num += SolveQuadric(coeffs, s + num);
  }
  
  /* resubstitute */
  
  sub = 1.0/4 * A;
  
  for (i = 0; i < num; ++i)
    s[ i ] -= sub;
  
  return num;
}

