forked from lijiext/lammps
140 lines
3.7 KiB
C++
140 lines
3.7 KiB
C++
#include "PolynomialSolver.h"
|
|
#include <limits>
|
|
#include <cmath>
|
|
#include <iostream>
|
|
#include "ATC_Error.h"
|
|
|
|
namespace ATC {
|
|
// Utility functions used by solvers, but not globally accessible.
|
|
static const double PI_OVER_3 = acos(-1.0)*(1.0/3.0);
|
|
static bool is_zero(double x)
|
|
{
|
|
static double GT_ZERO = 1.0e2*std::numeric_limits<double>::epsilon();
|
|
static double LT_ZERO = -GT_ZERO;
|
|
return x>LT_ZERO && x<GT_ZERO;
|
|
}
|
|
static double sign(double x)
|
|
{
|
|
static double s[] = {-1.0,1.0};
|
|
return s[x>0];
|
|
}
|
|
|
|
// Linear solver
|
|
int solve_linear(double c[2], double x0[1])
|
|
{
|
|
if (c[1] == 0) return 0; // constant function
|
|
*x0 = -c[0] / c[1];
|
|
return 1;
|
|
}
|
|
|
|
// Quadratic solver
|
|
int solve_quadratic(double c[3], double x0[2])
|
|
{
|
|
if (is_zero(c[2])) return solve_linear(c, x0);
|
|
const double ainv = 1.0/c[2]; // ax^2 + bx + c = 0
|
|
const double p = 0.5 * c[1] * ainv; // -b/2a
|
|
const double q = c[0] * ainv; // c/a
|
|
double D = p*p-q;
|
|
|
|
if (is_zero(D)) { // quadratic has one repeated root
|
|
x0[0] = -p;
|
|
return 1;
|
|
}
|
|
if (D > 0) { // quadratic has two real roots
|
|
D = sqrt(D);
|
|
x0[0] = D - p;
|
|
x0[1] = -D - p;
|
|
return 2;
|
|
}
|
|
return 0; // quadratic has no real roots
|
|
}
|
|
|
|
// Cubic solver
|
|
int solve_cubic(double c[4], double x0[3])
|
|
{
|
|
int num_roots;
|
|
if (is_zero(c[3])) return solve_quadratic(c, x0);
|
|
// normalize to x^3 + Ax^2 + Bx + C = 0
|
|
const double c3inv = 1.0/c[3];
|
|
const double A = c[2] * c3inv;
|
|
const double B = c[1] * c3inv;
|
|
const double C = c[0] * c3inv;
|
|
|
|
// substitute x = t - A/3 so t^3 + pt + q = 0
|
|
const double A2 = A*A;
|
|
const double p = (1.0/3.0)*((-1.0/3.0)*A2 + B);
|
|
const double q = 0.5*((2.0/27.0)*A*A2 - (1.0/3.0)*A*B + C);
|
|
|
|
// Cardano's fomula
|
|
const double p3 = p*p*p;
|
|
const double D = q*q + p3;
|
|
if (is_zero(D)) {
|
|
if (is_zero(q)) { // one triple soln
|
|
x0[0] = 0.0;
|
|
num_roots = 1;
|
|
}
|
|
else { // one single and one double soln
|
|
const double u = pow(fabs(q), 1.0/3.0)*sign(q);
|
|
x0[0] = -2.0*u;
|
|
x0[1] = u;
|
|
num_roots = 2;
|
|
}
|
|
}
|
|
else {
|
|
if (D < 0.0) { // three real roots
|
|
const double phi = 1.0/3.0 * acos(-q/sqrt(-p3));
|
|
const double t = 2.0 * sqrt(-p);
|
|
x0[0] = t * cos(phi);
|
|
x0[1] = -t * cos(phi + PI_OVER_3);
|
|
x0[2] = -t * cos(phi - PI_OVER_3);
|
|
num_roots = 3;
|
|
}
|
|
else { // one real root
|
|
const double sqrt_D = sqrt(D);
|
|
const double u = pow(sqrt_D + fabs(q), 1.0/3.0);
|
|
if (q > 0) x0[0] = -u + p / u;
|
|
else x0[0] = u - p / u;
|
|
num_roots = 1;
|
|
}
|
|
}
|
|
double sub = (1.0/3.0)*A;
|
|
for (int i=0; i<num_roots; i++) x0[i] -= sub;
|
|
return num_roots;
|
|
}
|
|
|
|
// solve ode with polynomial source : y'n + a_n-1 y'n-1 + ... = b_n x^n +...
|
|
void integrate_ode(double x,
|
|
int na, double * a, double * y0, double * y, int nb, double *b )
|
|
{
|
|
if (na == 2) {
|
|
// particular
|
|
if ( a[1] == 0) {
|
|
if ( a[0] == 0) {
|
|
y[0] = y0[0]+y0[1]*x;
|
|
y[1] = y0[1];
|
|
}
|
|
else {
|
|
double c = sqrt(a[0]);
|
|
y[0] = y0[0]*cos(c*x)+y0[1]/c*sin(c*x);
|
|
y[1] = -c*y0[0]*cos(c*x)+y0[1] *sin(c*x);
|
|
}
|
|
}
|
|
else {
|
|
// use solve_quadratic
|
|
throw ATC_Error("not yet supported");
|
|
}
|
|
// homogenous
|
|
double c = 1.;
|
|
double z = x;
|
|
int j = 2;
|
|
for (int i = 0; i < nb; i++,j++) {
|
|
y[1] += j*c*z;
|
|
c /= j;
|
|
z *= x;
|
|
y[0] += c*z;
|
|
}
|
|
}
|
|
else throw ATC_Error("can only integrate 2nd order ODEs currently");
|
|
}
|
|
}
|