/*Translated by FOR_C, v3.4.2 (-), on 07/09/115 at 08:33:15 */ /*FOR_C Options SET: ftn=u io=c no=p op=aimnv pf=,p_sdasl5 s=dbov str=l x=f - prototypes */ #include #include "fcrt.h" #include #include #include #include "p_sdasl5.h" #include "drsdasl5.h" /* PARAMETER translations */ #define LDC (2*NEQ) #define LIW (30 + NEQ) #define LRW (45 + (5 + 2*MAXCON + 4)*NEQ + 2*SQ(NEQ)) #define MAXCON 4 #define NDIG 4 #define NEQ 5 #define TOL (powif(10.e0,-NDIG)) /* end of PARAMETER translations */ /* COMMON translations */ struct t_counts { long int kr, kf, kc; } counts; /* end of COMMON translations */ int main( ) { LOGICAL32 constraint; long int _l0, _n, i, idid, info[16], iwork[LIW], kk; float atol[NEQ], c[NEQ][LDC], drl, drv, ftol, length, rnktol, rtol[NEQ], rwork[LRW], t, tout, y[NEQ], yprime[NEQ]; /* OFFSET Vectors w/subscript range: 1 to dimension */ float *const Atol = &atol[0] - 1; long *const Info = &info[0] - 1; long *const Iwork = &iwork[0] - 1; float *const Rtol = &rtol[0] - 1; float *const Rwork = &rwork[0] - 1; float *const Y = &y[0] - 1; float *const Yprime = &yprime[0] - 1; /* end of OFFSET VECTORS */ /*>> 2008-10-26 DRSDASL5 Krogh Moved Format statements up for C conv. *>> 2008-10-24 DRSDASL5 Krogh Removed in INCLUDE statement & cDEC$... *>> 2008-08-27 DRSDASL5 solve for initial y' value, change user codes *>> 2006-12-18 DRSDASL5 add total energy as a constraint. *>> 2001-10-11 DRSDASL5 R. J. Hanson Example 5 Code, with Download */ /* Solve a planar pendulum problem in rectangular coordinates. * The equation is transformed from "Index 3" to "Index 1" * by differentiating the length constraint twice. The system * is integrated (using SDASSFA) using these three constraints, * including total energy. * In the second integration the problem is transformed from * "Index 3" to "Index 0" by differentiating the length constraint * three times. The routine SDASSFB uses four constraints, * including total energy. * This example shows that the constraints remain satisfied, * and the integration can succeed with either approach. * It is more efficient to use the "Index 0" problem than * the "Index 1" problem. *--S replaces "?": DR?DASL5, ?DASLS, ?DASLX, ?DASSFA, ?DASSFB */ /* Set number of equations: */ /* Set maximum number of constraints. */ /* Work space sizes: */ /*++S Default NDIG = 4 *++ Default NDIG = 11 *++ Substitute for NDIG below */ printf(" Example Results for a Constrained Pendulum Problem, Index 1\n"); /* Tolerances: */ for (i = 1; i <= NEQ; i++) { Atol[i] = TOL; Rtol[i] = TOL; } /* Setup options: */ for (i = 1; i <= 16; i++) { Info[i] = 0; } /* Use partial derivatives provided in user code: */ Info[5] = 2; /* Constrain solution, forward with 3 constraints: */ Info[10] = 3; /* Compute the initial value of YPRIME(*). * Base computation of the initial y' on the * report by Krogh, Hanson, (2008), "Solving * Constrained Differential-Algebraic Systems * Using Projections," (8/2008). */ t = 0; /* Tolerances for a small F and a rank tolerance: */ ftol = powf(FLT_EPSILON,2./3.); rnktol = ftol; /* Give starting values to y and y' before Newton method: */ for (i = 1; i <= NEQ; i++) { Y[i] = 0.e0; Yprime[i] = 0.e0; } /* (Initial value for y(1) reset when ires is 0) */ sdasls( sdassfa, NEQ, &t, y, yprime, info, ftol, rnktol, (float*)c, ADR(_l0,LDC), NEQ, &idid, rwork, LRW, iwork, LIW ); printf(" Initial Position and Derivative Values using SDASLS\n"); /* Write the computed values for initial position and derivatives. */ printf("\n T y_1 y_2 y_3 y_4 y_5\n%12.4e", t); for(_n=0L; _n < sizeof(y)/sizeof(float); _n++) printf("%12.4e", y[_n]); printf("\n"); printf("\n T y'_1 y'_2 y'_3 y'_4 y'_5\n%12.4e", t); for(_n=0L; _n < sizeof(yprime)/sizeof(float); _n++) printf("%12.4e", yprime[_n]); printf("\n"); /* Now have initial values of y' so no need to compute them. * Allow more than nominal number of steps. */ Info[12] = 50000; /* This is the pendulum length */ length = 1.1e0; /* This is how long we will integrate. */ kk = 1000; for (i = 1; i <= kk; i += 10) { /* Integrate from T=I-1 to TOUT=T+10. * If the solution drifts away from the constraints, stop. */ t = i - 1; tout = t + 10; sdaslx( sdassfa, NEQ, &t, y, yprime, tout, info, rtol, atol, &idid, rwork, LRW, iwork, LIW ); /* Compute residuals on length and its variation. They * should be smaller than the tolerances used. */ drl = (SQ(Y[1]) + SQ(Y[2]) - SQ(length))/(Rtol[1]*SQ(length) + Atol[1]); drv = (Y[1]*Y[3] + Y[2]*Y[4])/(Rtol[1]*(fabsf( Y[1]*Y[3] ) + fabsf( Y[1]*Y[4] )) + Atol[1]); constraint = fabsf( drl ) <= 1.e0 && fabsf( drv ) <= 1.e0; if (!constraint) goto L_40; } L_40: ; printf("\n T y_1 y_2 y_3 y_4 y_5\n%12.4e", tout); for(_n=0L; _n < sizeof(y)/sizeof(float); _n++) printf("%12.4e", y[_n]); printf("\n"); printf(" No. of Residual Evaluations Factorizations Projection Solves\nConstraint Partials- %8ld %8ld %8ld\n\n", counts.kr, counts.kf, counts.kc); printf(" At the time value%10.2f the integration was stopped.\n", tout); if (!constraint) { printf("The pendulum length or variation has large weighted errors.\nThese should remain less than 1 is magnitude: %8.2f%8.2f\n\n", drl, drv); } /* Start the integration over for the index 0 problem. */ Info[1] = 0; /* Set the number of constraints to 4. */ Info[10] = 4; /* Starting values to y and y' before Newton method: */ for (i = 1; i <= NEQ; i++) { Y[i] = 0.e0; Yprime[i] = 0.e0; } /* (Initial value for y(1) and yprime(4) reset when ires is 0) */ for (i = 1; i <= kk; i += 10) { /* Integrate from T=I-1 to TOUT=T+10. */ t = i - 1; tout = t + 10; sdaslx( sdassfb, NEQ, &t, y, yprime, tout, info, rtol, atol, &idid, rwork, LRW, iwork, LIW ); /* Compute residuals on length and its variation. They * should be smaller than the tolerances used. */ drl = (SQ(Y[1]) + SQ(Y[2]) - SQ(length))/(Rtol[1]*SQ(length) + Atol[1]); drv = (Y[1]*Y[3] + Y[2]*Y[4])/(Rtol[1]*(fabsf( Y[1]*Y[3] ) + fabsf( Y[1]*Y[4] )) + Atol[1]); constraint = fabsf( drl ) <= 1.e0 && fabsf( drv ) <= 1.e0; if (!constraint) goto L_60; } L_60: ; printf(" Example Results for a Constrained Pendulum Problem, Index 0\n"); printf("\n T y_1 y_2 y_3 y_4 y_5\n%12.4e", tout); for(_n=0L; _n < sizeof(y)/sizeof(float); _n++) printf("%12.4e", y[_n]); printf("\n"); printf(" No. of Residual Evaluations Factorizations Projection Solves\nConstraint Partials- %8ld %8ld %8ld\n\n", counts.kr, counts.kf, counts.kc); printf(" At the time value%10.2f the integration was stopped.\n", tout); if (!constraint) { printf("The pendulum length or variation has large weighted errors.\nThese should remain less than 1 is magnitude: %8.2f%8.2f\n\n", drl, drv); } exit(0); } /* end of function */ /* PARAMETER translations */ #define GRAVITY 9.806650e0 #define LENGTH 1.1e0 #define MASS 1.e0 #define ONE 1.e0 #define TWO 2.e0 #define ZERO 0.e0 /* end of PARAMETER translations */ void /*FUNCTION*/ sdassfa( float *t, float y[], float yprime[], float delta[], float *d, long *ldd, float *cj, long *ires, float rwork[], long iwork[]) { #define D(I_,J_) (*(d+(I_)*(*ldd)+(J_))) static float lsq, mg; /* OFFSET Vectors w/subscript range: 1 to dimension */ float *const Delta = &delta[0] - 1; long *const Iwork = &iwork[0] - 1; float *const Rwork = &rwork[0] - 1; float *const Y = &y[0] - 1; float *const Yprime = &yprime[0] - 1; /* end of OFFSET VECTORS */ /* Routine for the swinging simple pendulum problem, * with constraints on the index 2 and 3 equations. */ /* This is the setup call. */ if (*ires == 0) { mg = MASS*GRAVITY; lsq = SQ(LENGTH); Y[1] = LENGTH; /* This is the only nonzero value for y'_4(0) but the startup * procedure will compute it. It is written here for * reference. * YPRIME(4)=-Gravity */ counts.kr = 0; counts.kf = 0; counts.kc = 0; } mg = MASS*GRAVITY; lsq = SQ(LENGTH); /* The system residual value. */ if (*ires == 1) { Delta[1] = Y[3] - Yprime[1]; Delta[2] = Y[4] - Yprime[2]; Delta[3] = -Y[1]*Y[5] - MASS*Yprime[3]; Delta[4] = -Y[2]*Y[5] - MASS*Yprime[4] - mg; Delta[5] = MASS*(SQ(Y[3]) + SQ(Y[4])) - mg*Y[2] - lsq*Y[5]; /* Count residual evaluations. */ counts.kr += 1; } /* The mixed partial derivative matrix DF/Dy + c * DF/Dy' */ if (*ires == 2) { D(0,0) = -*cj; D(0,2) = -Y[5]; D(1,1) = -*cj; D(1,3) = -Y[5]; D(1,4) = -mg; D(2,0) = ONE; D(2,2) = -MASS**cj; D(2,4) = TWO*MASS*Y[3]; D(3,1) = ONE; D(3,3) = D(2,2); D(3,4) = TWO*MASS*Y[4]; D(4,2) = -Y[1]; D(4,3) = -Y[2]; D(4,4) = -lsq; counts.kf += 1; } /* The constraining equations after the corrector has converged. * Both partials and residuals are required. */ if (*ires == 5) { D(0,5) = Y[1]*TWO; D(1,5) = Y[2]*TWO; D(2,5) = ZERO; D(3,5) = ZERO; D(4,5) = ZERO; D(0,6) = Y[3]; D(1,6) = Y[4]; D(2,6) = Y[1]; D(3,6) = Y[2]; D(4,6) = ZERO; /* Constraining total energy - */ D(0,7) = ZERO; D(1,7) = mg; D(2,7) = MASS*Y[3]; D(3,7) = MASS*Y[4]; D(4,7) = ZERO; Delta[6] = SQ(Y[1]) + SQ(Y[2]) - lsq; Delta[7] = Y[1]*Y[3] + Y[2]*Y[4]; Delta[8] = 0.5e0*MASS*(SQ(Y[3]) + SQ(Y[4])) + mg*Y[2]; counts.kc += 1; } /* What follows are Partials w.r.t. T, Y' and Y, needed for * computing the starting values of y' based on the * Krogh/Hanson starting algorithm. * These values (6,7,8) of IRES occur only for use in * the computation of initial values for y'. */ if (*ires == 6) { /* This is the partial of F, the residual function, w.r.t. T. * There is no explicit time dependence so this is zero. * The contents of DELTA(:) are set zero by the calling program * so there is nothing to do. * DELTA(1:6)=ZERO */ } /* This is the partial of F w.r.t. y'. * Values not assigned are set zero by the calling program. */ if (*ires == 7) { D(0,0) = -ONE; D(1,1) = -ONE; D(2,2) = -MASS; D(3,3) = -MASS; counts.kf += 1; } /* This is the partial of F w.r.t. y. * Values not assigned are set zero by the calling program. */ if (*ires == 8) { D(0,2) = -Y[5]; D(1,3) = -Y[5]; D(1,4) = -mg; D(2,0) = ONE; D(2,4) = TWO*MASS*Y[3]; D(3,1) = ONE; D(3,4) = TWO*MASS*Y[4]; D(4,2) = -Y[1]; D(4,3) = -Y[2]; D(4,4) = -lsq; counts.kf += 1; } return; #undef D } /* end of function */ /* PARAMETER translations */ #define THREE 3.e0 /* end of PARAMETER translations */ void /*FUNCTION*/ sdassfb( float *t, float y[], float yprime[], float delta[], float *p, long *ldp, float *cj, long *ires, float rwork[], long iwork[]) { #define P(I_,J_) (*(p+(I_)*(*ldp)+(J_))) static float lsq, mg; /* OFFSET Vectors w/subscript range: 1 to dimension */ float *const Delta = &delta[0] - 1; long *const Iwork = &iwork[0] - 1; float *const Rwork = &rwork[0] - 1; float *const Y = &y[0] - 1; float *const Yprime = &yprime[0] - 1; /* end of OFFSET VECTORS */ /* Routine for the swinging simple pendulum problem, * with constraints on the index 1,2 amd 3 equations. * Use P(:,:) to distinguish from D(:,:) in routine SDASSFA. */ /* This is the setup call. */ if (*ires == 0) { mg = MASS*GRAVITY; lsq = SQ(LENGTH); Y[1] = LENGTH; Yprime[4] = -GRAVITY; counts.kr = 0; counts.kf = 0; counts.kc = 0; } /* The sytem residual value. */ if (*ires == 1) { Delta[1] = Y[3] - Yprime[1]; Delta[2] = Y[4] - Yprime[2]; Delta[3] = -Y[1]*Y[5] - MASS*Yprime[3]; Delta[4] = -Y[2]*Y[5] - MASS*Yprime[4] - mg; Delta[5] = -THREE*mg*Y[4] - lsq*Yprime[5]; counts.kr += 1; } /* The partial derivative matrix. Entries are all zero * so only non-zero values are needed. */ if (*ires == 2) { P(0,0) = -*cj; P(0,2) = -Y[5]; P(1,1) = -*cj; P(1,3) = -Y[5]; P(2,0) = ONE; P(2,2) = -MASS**cj; P(3,1) = ONE; P(3,3) = P(2,2); P(3,4) = -THREE*mg; P(4,2) = -Y[1]; P(4,3) = -Y[2]; P(4,4) = -lsq**cj; counts.kf += 1; } /* The constraining equations after the corrector has converged. * Both partials and residuals are required. */ if (*ires == 5) { P(0,5) = Y[1]*TWO; P(1,5) = Y[2]*TWO; P(2,5) = ZERO; P(3,5) = ZERO; P(4,5) = ZERO; P(0,6) = Y[3]; P(1,6) = Y[4]; P(2,6) = Y[1]; P(3,6) = Y[2]; P(4,6) = ZERO; P(0,7) = ZERO; P(1,7) = -mg; P(2,7) = TWO*MASS*Y[3]; P(3,7) = TWO*MASS*Y[4]; P(4,7) = -lsq; /* Constraining total energy - */ P(0,8) = ZERO; P(1,8) = mg; P(2,8) = MASS*Y[3]; P(3,8) = MASS*Y[4]; P(4,8) = ZERO; Delta[6] = SQ(Y[1]) + SQ(Y[2]) - lsq; Delta[7] = Y[1]*Y[3] + Y[2]*Y[4]; Delta[8] = MASS*(SQ(Y[3]) + SQ(Y[4])) - mg*Y[2] - lsq*Y[5]; Delta[9] = 0.5e0*MASS*(SQ(Y[3]) + SQ(Y[4])) + mg*Y[2]; counts.kc += 1; } return; #undef P } /* end of function */