bool pos_on_bezier(const Vector2D& pos, double range, const ControlPoint& p1, const ControlPoint& p2, Vector2D& pOut, double& tOut) { assert(p1.segment_after == SEGMENT_CURVE); // Find intersections with the horizontal and vertical lines through p0 // theoretically we would need to check in all directions, but this covers enough BezierCurve curve(p1, p2); double roots[6]; UInt count; count = solve_cubic(curve.a.y, curve.b.y, curve.c.y, curve.d.y - pos.y, roots); count += solve_cubic(curve.a.x, curve.b.x, curve.c.x, curve.d.x - pos.x, roots + count); // append intersections // take the best intersection point double bestDistSqr = std::numeric_limits<double>::max(); //infinity for(UInt i = 0 ; i < count ; ++i) { double t = roots[i]; if (t >= 0 && t < 1) { Vector2D pnt = curve.pointAt(t); double distSqr = (pnt - pos).lengthSqr(); if (distSqr < bestDistSqr) { bestDistSqr = distSqr; pOut = pnt; tOut = t; } } } return bestDistSqr <= range * range; }
UInt solve_cubic(double a, double b, double c, double d, double* roots) { if (a == 0) { return solve_quadratic(b, c, d, roots); } else { return solve_cubic(b/a, c/a, d/a, roots); } }
/// Use the single_phase table to invert for x given a y void CoolProp::BicubicBackend::invert_single_phase_x(const SinglePhaseGriddedTableData &table, const std::vector<std::vector<CellCoeffs> > &coeffs, parameters other_key, double other, double y, std::size_t i, std::size_t j) { // Get the cell const CellCoeffs &cell = coeffs[i][j]; // Get the alpha coefficients const std::vector<double> &alpha = cell.get(other_key); std::size_t NNN = alpha.size(); // Normalized value in the range (0, 1) double yhat = (y - table.yvec[j])/(table.yvec[j+1] - table.yvec[j]); double y_0 = 1, y_1 = yhat, y_2 = yhat*yhat, y_3 = yhat*yhat*yhat; double a = alpha[3+0*4]*y_0+alpha[3+1*4]*y_1+alpha[3+2*4]*y_2+alpha[3+3*4]*y_3; // factors of xhat^3 double b = alpha[2+0*4]*y_0+alpha[2+1*4]*y_1+alpha[2+2*4]*y_2+alpha[2+3*4]*y_3; // factors of xhar^2 double c = alpha[1+0*4]*y_0+alpha[1+1*4]*y_1+alpha[1+2*4]*y_2+alpha[1+3*4]*y_3; // factors of xhat double d = alpha[0+0*4]*y_0+alpha[0+1*4]*y_1+alpha[0+2*4]*y_2+alpha[0+3*4]*y_3 - other; // constant factors int N = 0; double xhat0, xhat1, xhat2, val, xhat; solve_cubic(a, b, c, d, N, xhat0, xhat1, xhat2); if (N == 1){ xhat = xhat0; } else if (N == 2){ xhat = std::abs(xhat0) < std::abs(xhat1) ? xhat0 : xhat1; } else if (N == 3){ if (std::abs(xhat0) < std::abs(xhat1) && std::abs(xhat0) < std::abs(xhat2)){ xhat = xhat0; } // Already know that xhat1 < xhat0 (xhat0 is not the minimum) else if (std::abs(xhat1) < std::abs(xhat2)){ xhat = xhat1; } else{ xhat = xhat2; } } else if (N == 0){ throw ValueError("Could not find a solution in invert_single_phase_x"); } // Unpack xhat into actual value // xhat = (x-x_{i})/(x_{i+1}-x_{i}) val = xhat*(table.xvec[i+1] - table.xvec[i]) + table.xvec[i]; // Cache the output value calculated switch(table.xkey){ case iHmolar: _hmolar = val; break; case iT: _T = val; break; default: throw ValueError("Invalid output variable in invert_single_phase_x"); } }
/// Use the single_phase table to solve for y given an x void CoolProp::BicubicBackend::invert_single_phase_y(const SinglePhaseGriddedTableData &table, const std::vector<std::vector<CellCoeffs> > &coeffs, parameters other_key, double other, double x, std::size_t i, std::size_t j) { // Get the cell const CellCoeffs &cell = coeffs[i][j]; // Get the alpha coefficients const std::vector<double> &alpha = cell.get(other_key); // Normalized value in the range (0, 1) double xhat = (x - table.xvec[i])/(table.xvec[i+1] - table.xvec[i]); double x_0 = 1, x_1 = xhat, x_2 = xhat*xhat, x_3 = xhat*xhat*xhat; double a = alpha[0+3*4]*x_0 + alpha[1+3*4]*x_1 + alpha[2+3*4]*x_2 + alpha[3+3*4]*x_3; // factors of yhat^3 (m= 3) double b = alpha[0+2*4]*x_0 + alpha[1+2*4]*x_1 + alpha[2+2*4]*x_2 + alpha[3+2*4]*x_3; // factors of yhat^2 double c = alpha[0+1*4]*x_0 + alpha[1+1*4]*x_1 + alpha[2+1*4]*x_2 + alpha[3+1*4]*x_3; // factors of yhat double d = alpha[0+0*4]*x_0 + alpha[1+0*4]*x_1 + alpha[2+0*4]*x_2 + alpha[3+0*4]*x_3 - other; // constant factors int N = 0; double yhat0, yhat1, yhat2, val, yhat = _HUGE; solve_cubic(a, b, c, d, N, yhat0, yhat1, yhat2); if (N == 1){ yhat = yhat0; } else if (N == 2){ yhat = std::abs(yhat0) < std::abs(yhat1) ? yhat0 : yhat1; } else if (N == 3){ if (std::abs(yhat0) < std::abs(yhat1) && std::abs(yhat0) < std::abs(yhat2)){ yhat = yhat0; } // Already know that yhat1 < yhat0 (yhat0 is not the minimum) else if (std::abs(yhat1) < std::abs(yhat2)){ yhat = yhat1; } else{ yhat = yhat2; } } else if (N == 0){ throw ValueError("Could not find a solution in invert_single_phase_x"); } // Unpack xhat into actual value // yhat = (y-y_{j})/(y_{j+1}-y_{j}) val = yhat*(table.yvec[j+1] - table.yvec[j]) + table.yvec[j]; // Cache the output value calculated switch(table.ykey){ case iP: _p = val; break; default: throw ValueError("Invalid output variable in invert_single_phase_x"); } }
UInt intersect_bezier_ray(const ControlPoint& p1, const ControlPoint& p2, const Vector2D& pos) { // Looking only at the y coordinate // we can use the cubic formula to find roots, points where the horizontal line // through pos intersects the (extended) curve BezierCurve curve(p1,p2); double roots[3]; UInt count = solve_cubic(curve.a.y, curve.b.y, curve.c.y, curve.d.y - pos.y, roots); // now check if the solutions are left of pos.x UInt solsInRange = 0; for(UInt i = 0 ; i < count ; ++i) { double t = roots[i]; if (t >= 0 && t < 1 && curve.pointAt(t).x < pos.x) { solsInRange += 1; } } return solsInRange; }
int Solve_Polynomial(int n, DBL *c0, DBL *r, int sturm, DBL epsilon) { int roots, i; DBL *c; Increase_Counter(stats[Polynomials_Tested]); roots = 0; /* * Determine the "real" order of the polynomial, i.e. * eliminate small leading coefficients. */ i = 0; while ((fabs(c0[i]) < SMALL_ENOUGH) && (i < n)) { i++; } n -= i; c = &c0[i]; switch (n) { case 0: break; case 1: /* Solve linear polynomial. */ if (c[0] != 0.0) { r[roots++] = -c[1] / c[0]; } break; case 2: /* Solve quadratic polynomial. */ roots = solve_quadratic(c, r); break; case 3: /* Root elimination? */ if (epsilon > 0.0) { if ((c[2] != 0.0) && (fabs(c[3]/c[2]) < epsilon)) { Increase_Counter(stats[Roots_Eliminated]); roots = solve_quadratic(c, r); break; } } /* Solve cubic polynomial. */ if (sturm) { roots = polysolve(3, c, r); } else { roots = solve_cubic(c, r); } break; case 4: /* Root elimination? */ if (epsilon > 0.0) { if ((c[3] != 0.0) && (fabs(c[4]/c[3]) < epsilon)) { Increase_Counter(stats[Roots_Eliminated]); if (sturm) { roots = polysolve(3, c, r); } else { roots = solve_cubic(c, r); } break; } } /* Test for difficult coeffs. */ if (difficult_coeffs(4, c)) { sturm = true; } /* Solve quartic polynomial. */ if (sturm) { roots = polysolve(4, c, r); } else { roots = solve_quartic(c, r); } break; default: if (epsilon > 0.0) { if ((c[n-1] != 0.0) && (fabs(c[n]/c[n-1]) < epsilon)) { Increase_Counter(stats[Roots_Eliminated]); roots = polysolve(n-1, c, r); } } /* Solve n-th order polynomial. */ roots = polysolve(n, c, r); break; } return(roots); }
static int solve_quartic(DBL *x, DBL *results) { DBL cubic[4]; DBL roots[3]; DBL c12, z, p, q, q1, q2, r, d1, d2; DBL c0, c1, c2, c3, c4; int i; /* Make sure the quartic has a leading coefficient of 1.0 */ c0 = x[0]; if (c0 != 1.0) { c1 = x[1] / c0; c2 = x[2] / c0; c3 = x[3] / c0; c4 = x[4] / c0; } else { c1 = x[1]; c2 = x[2]; c3 = x[3]; c4 = x[4]; } /* Compute the cubic resolvant */ c12 = c1 * c1; p = -0.375 * c12 + c2; q = 0.125 * c12 * c1 - 0.5 * c1 * c2 + c3; r = -0.01171875 * c12 * c12 + 0.0625 * c12 * c2 - 0.25 * c1 * c3 + c4; cubic[0] = 1.0; cubic[1] = -0.5 * p; cubic[2] = -r; cubic[3] = 0.5 * r * p - 0.125 * q * q; i = solve_cubic(cubic, roots); if (i > 0) { z = roots[0]; } else { return(0); } d1 = 2.0 * z - p; if (d1 < 0.0) { if (d1 > -SMALL_ENOUGH) { d1 = 0.0; } else { return(0); } } if (d1 < SMALL_ENOUGH) { d2 = z * z - r; if (d2 < 0.0) { return(0); } d2 = sqrt(d2); } else { d1 = sqrt(d1); d2 = 0.5 * q / d1; } /* Set up useful values for the quadratic factors */ q1 = d1 * d1; q2 = -0.25 * c1; i = 0; /* Solve the first quadratic */ p = q1 - 4.0 * (z - d2); if (p == 0) { results[i++] = -0.5 * d1 - q2; } else { if (p > 0) { p = sqrt(p); results[i++] = -0.5 * (d1 + p) + q2; results[i++] = -0.5 * (d1 - p) + q2; } } /* Solve the second quadratic */ p = q1 - 4.0 * (z + d2); if (p == 0) { results[i++] = 0.5 * d1 - q2; } else { if (p > 0) { p = sqrt(p); results[i++] = 0.5 * (d1 + p) + q2; results[i++] = 0.5 * (d1 - p) + q2; } } return(i); }
static int solve_quartic(DBL *x, DBL *results) { DBL cubic[4], roots[3]; DBL a0, a1, y, d1, x1, t1, t2; DBL c0, c1, c2, c3, c4, d2, q1, q2; int i; c0 = x[0]; if (c0 != 1.0) { c1 = x[1] / c0; c2 = x[2] / c0; c3 = x[3] / c0; c4 = x[4] / c0; } else { c1 = x[1]; c2 = x[2]; c3 = x[3]; c4 = x[4]; } /* The first step is to take the original equation: x^4 + b*x^3 + c*x^2 + d*x + e = 0 and rewrite it as: x^4 + b*x^3 = -c*x^2 - d*x - e, adding (b*x/2)^2 + (x^2 + b*x/2)y + y^2/4 to each side gives a perfect square on the lhs: (x^2 + b*x/2 + y/2)^2 = (b^2/4 - c + y)x^2 + (b*y/2 - d)x + y^2/4 - e By choosing the appropriate value for y, the rhs can be made a perfect square also. This value is found when the rhs is treated as a quadratic in x with the discriminant equal to 0. This will be true when: (b*y/2 - d)^2 - 4.0 * (b^2/4 - c*y)*(y^2/4 - e) = 0, or y^3 - c*y^2 + (b*d - 4*e)*y - b^2*e + 4*c*e - d^2 = 0. This is called the resolvent of the quartic equation. */ a0 = 4.0 * c4; cubic[0] = 1.0; cubic[1] = -1.0 * c2; cubic[2] = c1 * c3 - a0; cubic[3] = a0 * c2 - c1 * c1 * c4 - c3 * c3; i = solve_cubic(&cubic[0], &roots[0]); if (i > 0) { y = roots[0]; } else { return(0); } /* What we are left with is a quadratic squared on the lhs and a linear term on the right. The linear term has one of two signs, take each and add it to the lhs. The form of the quartic is now: a' = b^2/4 - c + y, b' = b*y/2 - d, (from rhs quadritic above) (x^2 + b*x/2 + y/2) = +sqrt(a'*(x + 1/2 * b'/a')^2), and (x^2 + b*x/2 + y/2) = -sqrt(a'*(x + 1/2 * b'/a')^2). By taking the linear term from each of the right hand sides and adding to the appropriate part of the left hand side, two quadratic formulas are created. By solving each of these the four roots of the quartic are determined. */ i = 0; a0 = c1 / 2.0; a1 = y / 2.0; t1 = a0 * a0 - c2 + y; if (t1 < 0.0) { if (t1 > FUDGE_FACTOR2) { t1 = 0.0; } else { /* First Special case, a' < 0 means all roots are complex. */ return(0); } } if (t1 < FUDGE_FACTOR3) { /* Second special case, the "x" term on the right hand side above has vanished. In this case: (x^2 + b*x/2 + y/2) = +sqrt(y^2/4 - e), and (x^2 + b*x/2 + y/2) = -sqrt(y^2/4 - e). */ t2 = a1 * a1 - c4; if (t2 < 0.0) { return(0); } x1 = 0.0; d1 = sqrt(t2); } else { x1 = sqrt(t1); d1 = 0.5 * (a0 * y - c3) / x1; } /* Solve the first quadratic */ q1 = -a0 - x1; q2 = a1 + d1; d2 = q1 * q1 - 4.0 * q2; if (d2 >= 0.0) { d2 = sqrt(d2); results[0] = 0.5 * (q1 + d2); results[1] = 0.5 * (q1 - d2); i = 2; } /* Solve the second quadratic */ q1 = q1 + x1 + x1; q2 = a1 - d1; d2 = q1 * q1 - 4.0 * q2; if (d2 >= 0.0) { d2 = sqrt(d2); results[i++] = 0.5 * (q1 + d2); results[i++] = 0.5 * (q1 - d2); } return(i); }
static double approx_s(int order, double qq) { double approx; double c0, c1, c2; if (order < 1) { GSL_ERROR_VAL("Undefined order for Mathieu function", GSL_EINVAL, 0.0); } switch (order) { case 1: if (qq <= 4) return (5 - 0.5*(qq + sqrt(5*qq*qq + 16*qq + 64))); /* Eqn. 35 */ else return asymptotic(order-1, qq); break; case 2: if (qq <= 5) return (10 - sqrt(36 + qq*qq)); /* Eqn. 36 */ else return asymptotic(order-1, qq); break; case 3: if (qq <= 6.25) { c2 = qq - 8; /* Eqn. 37 */ c1 = -128 - 16*qq - 2*qq*qq; c0 = qq*qq*(8 - qq); } else return asymptotic(order-1, qq); break; default: if (order < 70) { if (1.7*order > 2*sqrt(qq)) { /* Eqn. 30 */ double n2 = (double)(order*order); double n22 = (double)((n2 - 1)*(n2 - 1)); double q2 = qq*qq; double q4 = q2*q2; approx = n2 + 0.5*q2/(n2 - 1); approx += (5*n2 + 7)*q4/(32*n22*(n2 - 1)*(n2 - 4)); approx += (9*n2*n2 + 58*n2 + 29)*q4*q2/ (64*n22*n22*(n2 - 1)*(n2 - 4)*(n2 - 9)); if (1.4*order < 2*sqrt(qq)) { approx += asymptotic(order-1, qq); approx *= 0.5; } } else approx = asymptotic(order-1, qq); return approx; } else return order*order; } /* Solve the cubic x^3 + c2*x^2 + c1*x + c0 = 0 */ approx = solve_cubic(c2, c1, c0); if ( approx < 0 && sqrt(qq) > 0.1*order ) return asymptotic(order-1, qq); else return (order*order + fabs(approx)); }
int TrajectoryRep1D::solve( double K[3], double x, int extrapolate ) { #ifdef DEBUG_TRAJECTORY std::cout << "solve( x = " << x << " ):\n"; switch( _rep ) { case TRAJ_EMPTY: std::cout << " rep = TRAJ_EMPTY\n"; break; case TRAJ_LINEAR: std::cout << " rep = TRAJ_LINEAR\n"; break; case TRAJ_QUADRATIC: std::cout << " rep = TRAJ_QUADRATIC\n"; break; case TRAJ_CUBIC: std::cout << " rep = TRAJ_CUBIC\n"; break; }; #endif switch( _rep ) { case TRAJ_EMPTY: throw( Error( ERROR_LOCATION, "empty representation" ) ); break; case TRAJ_LINEAR: { if( _A == 0.0 ) return( 0 ); // Some tests fail here of spotting K equal to one // Rounding to 64-bit from 80-bit help // This is really not the way to go... temporary volatile double KT = (x-_B) / _A; K[0] = KT; if( in( K[0], extrapolate ) ) return( 1 ); break; } case TRAJ_QUADRATIC: { int nroots = solve_quadratic( _A, _B, _C-x, &K[0], &K[1] ); #ifdef DEBUG_TRAJECTORY std::cout << " nroots = " << nroots << "\n"; for( int a = 0; a < nroots; a++ ) std::cout << " K[" << a << "] = " << K[a] <<"\n"; #endif if( nroots == 0 ) { return( 0 ); } else if( nroots == 1 ) { if( in( K[0], extrapolate ) ) { return( 1 ); } else { return( 0 ); } } else /* nroots = 2 */ { if( in( K[0], extrapolate ) ) { if( in( K[1], extrapolate ) ) { return( 2 ); } else { return( 1 ); } } else { if( in( K[1], extrapolate ) ) { K[0] = K[1]; return( 1 ); } else { return( 0 ); } } } break; } case TRAJ_CUBIC: { int nroots = solve_cubic( _A, _B, _C, _D-x, &K[0], &K[1], &K[2] ); #ifdef DEBUG_TRAJECTORY std::cout << " nroots = " << nroots << "\n"; for( int a = 0; a < nroots; a++ ) std::cout << " K[" << a << "] = " << K[a] <<"\n"; #endif if( nroots == 0 ) { return( 0 ); } else if( nroots == 1 ) { if( in( K[0], extrapolate ) ) { return( 1 ); } } else if( nroots == 2 ) { if( in( K[0], extrapolate ) ) { if( in( K[1], extrapolate ) ) { return( 2 ); } else { return( 1 ); } } else { if( in( K[1], extrapolate ) ) { K[0] = K[1]; return( 1 ); } else { return( 0 ); } } } else /* nroots = 3 */ { if( in( K[0], extrapolate ) ) { if( in( K[1], extrapolate ) ) { if( in( K[2], extrapolate ) ) { return( 3 ); } else { return( 2 ); } } else { if( in( K[2], extrapolate ) ) { K[1] = K[2]; return( 2 ); } else { return( 1 ); } } } else { if( in( K[1], extrapolate ) ) { if( in( K[2], extrapolate ) ) { K[0] = K[1]; K[1] = K[2]; return( 2 ); } else { K[0] = K[1]; return( 1 ); } } else { if( in( K[2], extrapolate ) ) { K[0] = K[2]; return( 1 ); } else { return( 0 ); } } } } break; } }; return( 0 ); }