88 lines
2.3 KiB
Text
88 lines
2.3 KiB
Text
|
/*
|
||
|
|
* This file is part of libsidplayfp, a SID player engine.
|
||
|
|
*
|
||
|
|
* Copyright 2011-2015 Leandro Nini <drfiemost@users.sourceforge.net>
|
||
|
|
* Copyright 2007-2010 Antti Lankila
|
||
|
|
*
|
||
|
|
* This program is free software; you can redistribute it and/or modify
|
||
|
|
* it under the terms of the GNU General Public License as published by
|
||
|
|
* the Free Software Foundation; either version 2 of the License, or
|
||
|
|
* (at your option) any later version.
|
||
|
|
*
|
||
|
|
* This program is distributed in the hope that it will be useful,
|
||
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
|
|
* GNU General Public License for more details.
|
||
|
|
*
|
||
|
|
* You should have received a copy of the GNU General Public License
|
||
|
|
* along with this program; if not, write to the Free Software
|
||
|
|
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "OpAmp.h"
|
||
|
|
|
||
|
|
#include <cmath>
|
||
|
|
|
||
|
|
#include "siddefs-fp.h"
|
||
|
|
|
||
|
|
namespace reSIDfp
|
||
|
|
{
|
||
|
|
|
||
|
|
const double EPSILON = 1e-8;
|
||
|
|
|
||
|
|
double OpAmp::solve(double n, double vi) const
|
||
|
|
{
|
||
|
|
// Start off with an estimate of x and a root bracket [ak, bk].
|
||
|
|
// f is decreasing, so that f(ak) > 0 and f(bk) < 0.
|
||
|
|
double ak = vmin;
|
||
|
|
double bk = vmax;
|
||
|
|
|
||
|
|
const double a = n + 1.;
|
||
|
|
const double b = kVddt;
|
||
|
|
double b_vi = (b - vi);
|
||
|
|
if (b_vi < 0.) b_vi = 0.;
|
||
|
|
const double c = n * (b_vi * b_vi);
|
||
|
|
|
||
|
|
for (;;)
|
||
|
|
{
|
||
|
|
const double xk = x;
|
||
|
|
|
||
|
|
// Calculate f and df.
|
||
|
|
|
||
|
|
Spline::Point out = opamp->evaluate(x);
|
||
|
|
const double vo = out.x;
|
||
|
|
const double dvo = out.y;
|
||
|
|
|
||
|
|
double b_vx = b - x;
|
||
|
|
if (b_vx < 0.) b_vx = 0.;
|
||
|
|
double b_vo = b - vo;
|
||
|
|
if (b_vo < 0.) b_vo = 0.;
|
||
|
|
|
||
|
|
// f = a*(b - vx)^2 - c - (b - vo)^2
|
||
|
|
const double f = a * (b_vx * b_vx) - c - (b_vo * b_vo);
|
||
|
|
|
||
|
|
// df = 2*((b - vo)*dvo - a*(b - vx))
|
||
|
|
const double df = 2. * (b_vo * dvo - a * b_vx);
|
||
|
|
|
||
|
|
// Newton-Raphson step: xk1 = xk - f(xk)/f'(xk)
|
||
|
|
x -= f / df;
|
||
|
|
|
||
|
|
if (unlikely(fabs(x - xk) < EPSILON))
|
||
|
|
{
|
||
|
|
out = opamp->evaluate(x);
|
||
|
|
return out.x;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Narrow down root bracket.
|
||
|
|
(f < 0. ? bk : ak) = xk;
|
||
|
|
|
||
|
|
if (unlikely(x <= ak) || unlikely(x >= bk))
|
||
|
|
{
|
||
|
|
// Bisection step (ala Dekker's method).
|
||
|
|
x = (ak + bk) * 0.5;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace reSIDfp
|