void Rossler::EventsConstraints (realtype t, N_Vector x, int * constraints, void * data) {
realtype a, b, c;
realtype x1, x2, x3;
realtype ris[3], xdot[3];
Parameters * parameters;
x1 = Ith (x, 0);
x2 = Ith (x, 1);
x3 = Ith (x, 2);
parameters = (Parameters *) data;
a = parameters->At(0);
b = parameters->At(1);
c = parameters->At(2);
RHS(t,x,xderiv,data);
for(int i=0; i<GetDimension(); i++)
xdot[i] = Ith(xderiv,i);
ris[0] = - xdot[1] - xdot[2];
ris[1] = xdot[0] + a*xdot[1];
ris[2] = xdot[0]*x3 + xdot[2]*(x1-c);
for(int i=0; i<GetNumberOfEvents(); i++)
constraints[i] = (ris[i] < 0 ? 1 : 0);
}
static int JacB(int NB, realtype t,
N_Vector y, N_Vector yB, N_Vector fyB,
DlsMat JB, void *user_dataB,
N_Vector tmp1B, N_Vector tmp2B, N_Vector tmp3B)
{
UserData data;
realtype y1, y2, y3;
realtype p1, p2, p3;
data = (UserData) user_dataB;
/* The p vector */
p1 = data->p[0]; p2 = data->p[1]; p3 = data->p[2];
/* The y vector */
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
/* Load JB */
IJth(JB,1,1) = p1; IJth(JB,1,2) = -p1;
IJth(JB,2,1) = -p2*y3; IJth(JB,2,2) = p2*y3+2.0*p3*y2;
IJth(JB,2,3) = RCONST(-2.0)*p3*y2;
IJth(JB,3,1) = -p2*y2; IJth(JB,3,2) = p2*y2;
return(0);
}
int setup_initial_states(N_Vector y)
{
/* Initialize y */
Ith(y,1) = Y1;
Ith(y,2) = Y2;
Ith(y,3) = Y3;
}
int Rossler::Jacobian (long int N, DenseMat J, realtype t, N_Vector x, N_Vector fy,
void *jac_data, N_Vector tmp1, N_Vector tmp2, N_Vector tmp3) {
#endif
#ifdef CVODE26
int Rossler::Jacobian (int N, realtype t, N_Vector x, N_Vector fy, DlsMat J,
void *jac_data, N_Vector tmp1, N_Vector tmp2, N_Vector tmp3) {
#endif
realtype a, b, c;
realtype x1, x2, x3;
Parameters * parameters;
x1 = Ith (x, 0);
x2 = Ith (x, 1);
x3 = Ith (x, 2);
parameters = (Parameters *) jac_data;
a = parameters->At(0);
b = parameters->At(1);
c = parameters->At(2);
IJth (J, 0, 0) = 0.0;
IJth (J, 0, 1) = -1.0;
IJth (J, 0, 2) = -1.0;
IJth (J, 1, 0) = 1.0;
IJth (J, 1, 1) = a;
IJth (J, 1, 2) = 0.0;
IJth (J, 2, 0) = x3;
IJth (J, 2, 1) = 0.0;
IJth (J, 2, 2) = x1-c;
return CV_SUCCESS;
}
static void g(realtype t, N_Vector y, realtype *gout, void *g_data)
{
realtype y1, y3;
y1 = Ith(y,1); y3 = Ith(y,3);
gout[0] = y1 - RCONST(0.0001);
gout[1] = y3 - RCONST(0.01);
}
void GetSol(void *cpode_mem, N_Vector yy0, realtype tol,
realtype tout, booleantype proj, N_Vector yref)
{
N_Vector yy, yp;
realtype t, x, y, xd, yd, g;
int flag;
long int nst, nfe, nsetups, nje, nfeLS, ncfn, netf;
if (proj) {
printf(" YES ");
CPodeSetProjFrequency(cpode_mem, 1);
} else {
CPodeSetProjFrequency(cpode_mem, 0);
printf(" NO ");
}
yy = N_VNew_Serial(4);
yp = N_VNew_Serial(4);
flag = CPodeReInit(cpode_mem, (void *)f, NULL, 0.0, yy0, NULL, CP_SS, tol, &tol);
flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
x = Ith(yy,1);
y = Ith(yy,2);
g = ABS(x*x + y*y - 1.0);
N_VLinearSum(1.0, yy, -1.0, yref, yy);
N_VAbs(yy, yy);
x = Ith(yy,1);
y = Ith(yy,2);
xd = Ith(yy,3);
yd = Ith(yy,4);
printf("%9.2e %9.2e %9.2e %9.2e | %9.2e |",
Ith(yy,1),Ith(yy,2),Ith(yy,3),Ith(yy,4),g);
CPodeGetNumSteps(cpode_mem, &nst);
CPodeGetNumFctEvals(cpode_mem, &nfe);
CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
CPodeGetNumErrTestFails(cpode_mem, &netf);
CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
CPDlsGetNumJacEvals(cpode_mem, &nje);
CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
printf(" %6ld %6ld+%-4ld %4ld (%3ld) | %3ld %3ld\n",
nst, nfe, nfeLS, nsetups, nje, ncfn, netf);
N_VDestroy_Serial(yy);
N_VDestroy_Serial(yp);
return;
}
static int g(realtype t, N_Vector y, realtype *gout, void *user_data)
{
realtype y1, y3;
y1 = Ith(y,1); y3 = Ith(y,3);
gout[0] = y1 - RCONST(0.0001);
gout[1] = y3 - RCONST(0.01);
return(0);
}
int setup_tolerances(N_Vector abstol)
{
/* Set the scalar relative tolerance */
//reltol = RTOL;
/* Set the vector absolute tolerance */
Ith(abstol,1) = ATOL1;
Ith(abstol,2) = ATOL2;
Ith(abstol,3) = ATOL3;
}
static int fQB(realtype t, N_Vector y, N_Vector yB,
N_Vector qBdot, void *user_dataB)
{
UserData data;
realtype y1, y2, y3;
realtype p1, p2, p3;
realtype l1, l2, l3;
realtype l21, l32, y23;
data = (UserData) user_dataB;
/* The p vector */
p1 = data->p[0]; p2 = data->p[1]; p3 = data->p[2];
/* The y vector */
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
/* The lambda vector */
l1 = Ith(yB,1); l2 = Ith(yB,2); l3 = Ith(yB,3);
/* Temporary variables */
l21 = l2-l1;
l32 = l3-l2;
y23 = y2*y3;
Ith(qBdot,1) = y1*l21;
Ith(qBdot,2) = - y23*l21;
Ith(qBdot,3) = y2*y2*l32;
return(0);
}
void operAccum(real pl[], real q[], integer N, real t, integer indx)
{
integer i;
real xnorm, ipr, ipi;
xnorm = norm2(pl, N);
for (i = 1; i <= nopers; i++) {
FSmul(&opers[i], t, pl, q);
inprod(pl, q, N, &ipr, &ipi);
Ith(opr[i], indx) += ipr / xnorm;
Ith(opi[i], indx) += ipi / xnorm;
}
}
void RefSol(realtype tout, N_Vector yref)
{
void *cpode_mem;
N_Vector yy, yp;
realtype tol, t, th, thd;
int flag;
yy = N_VNew_Serial(2);
yp = N_VNew_Serial(2);
Ith(yy,1) = 0.0; /* theta */
Ith(yy,2) = 0.0; /* thetad */
tol = TOL_REF;
cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
flag = CPodeSetMaxNumSteps(cpode_mem, 100000);
flag = CPodeInit(cpode_mem, (void *)fref, NULL, 0.0, yy, yp, CP_SS, tol, &tol);
flag = CPDense(cpode_mem, 2);
flag = CPodeSetStopTime(cpode_mem, tout);
flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
th = Ith(yy,1);
thd = Ith(yy,2);
Ith(yref,1) = cos(th);
Ith(yref,2) = sin(th);
Ith(yref,3) = -thd*sin(th);
Ith(yref,4) = thd*cos(th);
N_VDestroy_Serial(yy);
N_VDestroy_Serial(yp);
CPodeFree(&cpode_mem);
return;
}
static int fref(realtype t, N_Vector yy, N_Vector fy, void *f_data)
{
realtype th, thd, g;
g = 13.7503716373294544;
th = Ith(yy,1);
thd = Ith(yy,2);
Ith(fy,1) = thd;
Ith(fy,2) = -g*cos(th);
return(0);
}
static int fB(realtype t, N_Vector y, N_Vector yB, N_Vector yBdot, void *user_dataB)
{
UserData data;
realtype y2, y3;
realtype p1, p2, p3;
realtype l1, l2, l3;
realtype l21, l32;
data = (UserData) user_dataB;
/* The p vector */
p1 = data->p[0]; p2 = data->p[1]; p3 = data->p[2];
/* The y vector */
y2 = Ith(y,2); y3 = Ith(y,3);
/* The lambda vector */
l1 = Ith(yB,1); l2 = Ith(yB,2); l3 = Ith(yB,3);
/* Temporary variables */
l21 = l2-l1;
l32 = l3-l2;
/* Load yBdot */
Ith(yBdot,1) = - p1*l21;
Ith(yBdot,2) = p2*y3*l21 - RCONST(2.0)*p3*y2*l32;
Ith(yBdot,3) = p2*y2*l21 - RCONST(1.0);
return(0);
}
static void Jac(long int N, DenseMat J, realtype t,
N_Vector y, N_Vector fy, void *jac_data,
N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
{
realtype y1, y2, y3;
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
IJth(J,1,1) = RCONST(-0.04);
IJth(J,1,2) = RCONST(1.0e4)*y3;
IJth(J,1,3) = RCONST(1.0e4)*y2;
IJth(J,2,1) = RCONST(0.04);
IJth(J,2,2) = RCONST(-1.0e4)*y3-RCONST(6.0e7)*y2;
IJth(J,2,3) = RCONST(-1.0e4)*y2;
IJth(J,3,2) = RCONST(6.0e7)*y2;
}
static void Jac(long int N, DenseMat J, realtype t,
N_Vector y, N_Vector fy, void *jac_data,
N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
{
realtype y1, y2, y3;
UserData data;
realtype p1, p2, p3;
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
data = (UserData) jac_data;
p1 = data->p[0]; p2 = data->p[1]; p3 = data->p[2];
IJth(J,1,1) = -p1; IJth(J,1,2) = p2*y3; IJth(J,1,3) = p2*y2;
IJth(J,2,1) = p1; IJth(J,2,2) = -p2*y3-2*p3*y2; IJth(J,2,3) = -p2*y2;
IJth(J,3,2) = 2*p3*y2;
}
static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
{
realtype y1, y2, y3, yp1, yp2, yp3;
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
yp1 = Ith(yp,1); yp2 = Ith(yp,2); yp3 = Ith(yp,3);
Ith(res,1) = yp1 - (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3);
Ith(res,2) = yp2 + (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3 + RCONST(3.0e7)*y2*y2);
Ith(res,3) = yp3 - RCONST(3.0e7)*y2*y2;
return(0);
}
void PLL::EventsConstraints (realtype t, N_Vector X, int * constraints, void * data) {
#ifndef WITHPHIERR
constraints[0] = 1;
// minimum of x [we know that x is a sinusoid with mean 0]
constraints[1] = (Ith(X,0) < 0 ? 1 : 0);
constraints[2] = 1;
#endif
}
static int Jac(realtype t, N_Vector y, N_Vector fy, SUNMatrix J,
void *user_data, N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
{
realtype y2, y3;
UserData data;
realtype p1, p2, p3;
y2 = Ith(y,2); y3 = Ith(y,3);
data = (UserData) user_data;
p1 = data->p[0]; p2 = data->p[1]; p3 = data->p[2];
IJth(J,1,1) = -p1; IJth(J,1,2) = p2*y3; IJth(J,1,3) = p2*y2;
IJth(J,2,1) = p1; IJth(J,2,2) = -p2*y3-2*p3*y2; IJth(J,2,3) = -p2*y2;
IJth(J,3,1) = ZERO; IJth(J,3,2) = 2*p3*y2; IJth(J,3,3) = ZERO;
return(0);
}
int Jac(long int N, realtype t,
N_Vector y, N_Vector fy, DlsMat J, void *user_data,
N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
{
realtype y1, y2, y3;
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
IJth(J,1,1) = RCONST(-0.04);
IJth(J,1,2) = RCONST(1.0e4)*y3;
IJth(J,1,3) = RCONST(1.0e4)*y2;
IJth(J,2,1) = RCONST(0.04);
IJth(J,2,2) = RCONST(-1.0e4)*y3-RCONST(6.0e7)*y2;
IJth(J,2,3) = RCONST(-1.0e4)*y2;
IJth(J,3,2) = RCONST(6.0e7)*y2;
return(0);
}
static int ewt(N_Vector y, N_Vector w, void *user_data)
{
int i;
realtype yy, ww, rtol, atol[3];
rtol = RTOL;
atol[0] = ATOL1;
atol[1] = ATOL2;
atol[2] = ATOL3;
for (i=1; i<=3; i++) {
yy = Ith(y,i);
ww = rtol * SUNRabs(yy) + atol[i-1];
if (ww <= 0.0) return (-1);
Ith(w,i) = 1.0/ww;
}
return(0);
}
int main()
{
void *cpode_mem;
N_Vector yref, yy0;
realtype tol, tout;
int i, flag;
tout = 30.0;
/* Get reference solution */
yref = N_VNew_Serial(4);
RefSol(tout, yref);
/* Initialize solver */
tol = TOL;
cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
yy0 = N_VNew_Serial(4);
Ith(yy0,1) = 1.0; /* x */
Ith(yy0,2) = 0.0; /* y */
Ith(yy0,3) = 0.0; /* xd */
Ith(yy0,4) = 0.0; /* yd */
flag = CPodeInit(cpode_mem, (void *)f, NULL, 0.0, yy0, NULL, CP_SS, tol, &tol);
flag = CPodeSetMaxNumSteps(cpode_mem, 50000);
flag = CPodeSetStopTime(cpode_mem, tout);
flag = CPodeProjDefine(cpode_mem, proj, NULL);
flag = CPDense(cpode_mem, 4);
for (i=0;i<5;i++) {
printf("\n\n%.2e\n", tol);
GetSol(cpode_mem, yy0, tol, tout, TRUE, yref);
GetSol(cpode_mem, yy0, tol, tout, FALSE, yref);
tol /= 10.0;
}
N_VDestroy_Serial(yref);
CPodeFree(&cpode_mem);
return(0);
}
static int rhsQ(realtype t, N_Vector yy, N_Vector yp, N_Vector qdot, void *user_data)
{
realtype v1, v2, v3;
realtype m1, J1, m2, J2, a;
UserData data;
data = (UserData) user_data;
J1 = data->J1;
m1 = data->m1;
m2 = data->m2;
J2 = data->J2;
a = data->a;
v1 = Ith(yy,4);
v2 = Ith(yy,5);
v3 = Ith(yy,6);
Ith(qdot,1) = HALF*(J1*v1*v1 + m2*v2*v2 + J2*v3*v3);
return(0);
}
static int jacE(int N, realtype t,
N_Vector y, N_Vector fy,
DlsMat J, void *jac_data,
N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
{
realtype y1, y2, y3;
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
IJth(J,1,1) = RCONST(-0.04);
IJth(J,1,2) = RCONST(1.0e4)*y3;
IJth(J,1,3) = RCONST(1.0e4)*y2;
IJth(J,2,1) = RCONST(0.04);
IJth(J,2,2) = RCONST(-1.0e4)*y3-RCONST(6.0e7)*y2;
IJth(J,2,3) = RCONST(-1.0e4)*y2;
IJth(J,3,1) = ZERO;
IJth(J,3,2) = RCONST(6.0e7)*y2;
IJth(J,3,3) = ZERO;
return(0);
}
void PLL::ManageEvents(realtype t, N_Vector X, int * events, int * constraints) {
#ifndef WITHPHIERR
// everything is off for t < T
if(t < T) {
return;
}
int c[3] = {1,1,1};
if(constraints != NULL) {
for(int i=0; i<3; i++) c[i] = constraints[i];
}
if(events[0]) {
n++;
zu = true;
fprintf(stderr, "%c%s", ESC, CYAN);
fprintf(stderr, "zu <= 1 @ %e\n", t);
fprintf(stderr, "%c%s", ESC, NORMAL);
}
if(events[1] && c[1]) {
cnt++;
#ifndef FRACTIONAL
if((divout && cnt == floor((double) N/2)) || (!divout && cnt == ceil((double) N/2))) {
#else
if((divout && cnt == floor((double) N[idx]/2)) || (!divout && cnt == ceil((double) N[idx]/2))) {
#endif
cnt = 0;
divout = !divout;
if(divout) { // rising edge of the output of the frequency divider
zd = true;
#ifdef FRACTIONAL
idx = (idx+1) % ndiv;
#endif
fprintf(stderr, "%c%s", ESC, YELLOW);
fprintf(stderr, "zd <= 1 @ %e\n", t);
fprintf(stderr, "%c%s", ESC, NORMAL);
}
}
}
if(events[2]) {
zu = zd = false;
fprintf(stderr, "%c%s", ESC, MAGENTA);
fprintf(stderr, "zu <= zd <= 0 @ %e\tw = %e\n", t, Ith(X,3));
fprintf(stderr, "%c%s", ESC, NORMAL);
wait_reset = false;
}
if(zu && zd && ! wait_reset) {
treset = t;
wait_reset = true;
}
}
} // namespace bal
void integrate()
{
integer i, N, flag;
real tr, t;
N = RHS.N;
for (i = 1; i <= 2 * N; i++)
Ith(y, i) = Ith(ystart, i);
/* Initialize ODE solver */
cvode_mem = CVodeMalloc(2 * N, derivs, Ith(tlist, 1), y,
(method == 0) ? ADAMS : BDF,
(itertype == 0) ? FUNCTIONAL : NEWTON,
(nabstol == 1) ? SS : SV,
&reltol, abstolp, NULL, NULL, TRUE, iopt, ropt, NULL);
if (cvode_mem == NULL) {
fatal_error("CVodeMalloc failed.\n");
}
/* Call CVDiag */
CVDiag(cvode_mem);
fwrite(N_VDATA(y), sizeof(real), 2 * N, op);
for (i = 2; i <= ntimes; i++) {
tr = Ith(tlist, i);
flag = CVode1(cvode_mem, tr, y, &t, NORMAL);
if (flag != SUCCESS) {
sprintf(errmsg, "CVode failed, flag=%d.\n", flag);
fatal_error(errmsg);
}
fwrite(N_VDATA(y), sizeof(real), 2 * N, op);
progress += NHASH;
while (progress >= ntimes - 1) {
fprintf(stderr, "#");
progress -= ntimes - 1;
}
}
fprintf(stderr, "\n");
CVodeFree(cvode_mem);
}
int PLL::Events (realtype t, N_Vector X, realtype * event, void * data) {
#ifndef WITHPHIERR
realtype x, y, r, w;
Parameters * parameters = (Parameters *) data;
x = Ith (X, 0);
y = Ith (X, 1);
r = Ith (X, 2);
w = Ith (X, 3);
fREF = parameters->At(0);
T = 1.0/fREF;
R1 = parameters->At(1);
omega0 = 2*pi*parameters->At(2);
Vdd = parameters->At(3);
rho0 = parameters->At(4);
rhoap = parameters->At(5);
k0 = parameters->At(6);
Krho = parameters->At(7);
Kap = parameters->At(8);
alpha = parameters->At(9);
KVCOa = parameters->At(10);
KVCOb = parameters->At(11);
KVCOc = parameters->At(12);
tuning_coeff = parameters->At(13);
// rising edge of the clock
event[0] = t - n*T;
// extrema of x
realtype gamma = sqrt(x*x+y*y);
realtype vtune = w/tuning_coeff;
event[1] = ((rho0+Krho*vtune)/gamma - 1)*k0*x - ((1-alpha)*Kap*(gamma-rhoap) + 1 + alpha*vtune*(KVCOa + KVCOb*vtune + KVCOc*vtune*vtune))*omega0*y;
// reset
event[2] = t - (treset+tau_d+dt);
#endif
return CV_SUCCESS;
}
static int fS(int Ns, realtype t, N_Vector y, N_Vector ydot,
int iS, N_Vector yS, N_Vector ySdot,
void *user_data, N_Vector tmp1, N_Vector tmp2)
{
UserData data;
realtype p1, p2, p3;
realtype y1, y2, y3;
realtype s1, s2, s3;
realtype sd1, sd2, sd3;
data = (UserData) user_data;
p1 = data->p[0]; p2 = data->p[1]; p3 = data->p[2];
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
s1 = Ith(yS,1); s2 = Ith(yS,2); s3 = Ith(yS,3);
sd1 = -p1*s1 + p2*y3*s2 + p2*y2*s3;
sd3 = 2*p3*y2*s2;
sd2 = -sd1-sd3;
switch (iS) {
case 0:
sd1 += -y1;
sd2 += y1;
break;
case 1:
sd1 += y2*y3;
sd2 += -y2*y3;
break;
case 2:
sd2 += -y2*y2;
sd3 += y2*y2;
break;
}
Ith(ySdot,1) = sd1;
Ith(ySdot,2) = sd2;
Ith(ySdot,3) = sd3;
return(0);
}
static void f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
{
realtype y1, y2, y3, yd1, yd3;
y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
yd1 = Ith(ydot,1) = RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3;
yd3 = Ith(ydot,3) = RCONST(3.0e7)*y2*y2;
Ith(ydot,2) = -yd1 - yd3;
}
static void PrintOutput(N_Vector u)
{
#if defined(SUNDIALS_EXTENDED_PRECISION)
printf(" %8.6Lg %8.6Lg\n", Ith(u,1), Ith(u,2));
#elif defined(SUNDIALS_DOUBLE_PRECISION)
printf(" %8.6g %8.6g\n", Ith(u,1), Ith(u,2));
#else
printf(" %8.6g %8.6g\n", Ith(u,1), Ith(u,2));
#endif
}
static void PrintOutput(N_Vector y)
{
int i;
printf(" l=x+1 x u=1-x\n");
printf(" ----------------------------------\n");
for(i=1; i<=NVAR; i++) {
#if defined(SUNDIALS_EXTENDED_PRECISION)
printf(" %10.6Lg %10.6Lg %10.6Lg\n",
Ith(y,i+NVAR), Ith(y,i), Ith(y,i+2*NVAR));
#elif defined(SUNDIALS_DOUBLE_PRECISION)
printf(" %10.6g %10.6g %10.6g\n",
Ith(y,i+NVAR), Ith(y,i), Ith(y,i+2*NVAR));
#else
printf(" %10.6g %10.6g %10.6g\n",
Ith(y,i+NVAR), Ith(y,i), Ith(y,i+2*NVAR));
#endif
}
}
