1 /*
2 * This file is part of libsidplayfp, a SID player engine.
3 *
4 * Copyright 2011-2015 Leandro Nini <drfiemost@users.sourceforge.net>
5 * Copyright 2007-2010 Antti Lankila
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20 */
21
22 #include "OpAmp.h"
23
24 #include <cmath>
25
26 #include "siddefs-fp.h"
27
28 namespace reSIDfp
29 {
30
31 const double EPSILON = 1e-8;
32
solve(double n,double vi) const33 double OpAmp::solve(double n, double vi) const
34 {
35 // Start off with an estimate of x and a root bracket [ak, bk].
36 // f is decreasing, so that f(ak) > 0 and f(bk) < 0.
37 double ak = vmin;
38 double bk = vmax;
39
40 const double a = n + 1.;
41 const double b = Vddt;
42 const double b_vi = (b > vi) ? (b - vi) : 0.;
43 const double c = n * (b_vi * b_vi);
44
45 for (;;)
46 {
47 const double xk = x;
48
49 // Calculate f and df.
50
51 Spline::Point out = opamp->evaluate(x);
52 const double vo = out.x;
53 const double dvo = out.y;
54
55 const double b_vx = (b > x) ? b - x : 0.;
56 const double b_vo = (b > vo) ? b - vo : 0.;
57
58 // f = a*(b - vx)^2 - c - (b - vo)^2
59 const double f = a * (b_vx * b_vx) - c - (b_vo * b_vo);
60
61 // df = 2*((b - vo)*dvo - a*(b - vx))
62 const double df = 2. * (b_vo * dvo - a * b_vx);
63
64 // Newton-Raphson step: xk1 = xk - f(xk)/f'(xk)
65 x -= f / df;
66
67 if (unlikely(fabs(x - xk) < EPSILON))
68 {
69 out = opamp->evaluate(x);
70 return out.x;
71 }
72
73 // Narrow down root bracket.
74 (f < 0. ? bk : ak) = xk;
75
76 if (unlikely(x <= ak) || unlikely(x >= bk))
77 {
78 // Bisection step (ala Dekker's method).
79 x = (ak + bk) * 0.5;
80 }
81 }
82 }
83
84 } // namespace reSIDfp
85