void sdfmotion(double *time, double state[50], double dstate[50], double dt, double ctol, int *flag, double *errest, int *err) { double work[200],ttime,param[1]; int ferr,i; sdgentime(&i); if (i != 93540) { sdseterr(55,42); } param[0] = ctol; *err = 0; ttime = *time; if (*flag != 0) { sdmotfunc(ttime,state,dstate,param,&ferr); *flag = 0; } sdfinteg(sdmotfunc,&ttime,state,dstate,param,dt,50,work,errest,&ferr); if (ferr != 0) { *err = 1; } *time = ttime; }
void sdvrot(double ivec[3], double rvec[3], double theta, double ovec[3]) { double norm,idotu,ctheta,stheta,dotcos,uvec[3],icosvec[3],isinvec[3],uxisin[ 3]; /* Rotate a vector ivec around vector rvec by angle theta */ norm = sdvnorm(rvec); if (norm == 0.) { ovec[0] = ivec[0]; ovec[1] = ivec[1]; ovec[2] = ivec[2]; sdseterr(62,46); return; } stheta = sin(theta); ctheta = cos(theta); sdvmul(stheta,ivec,isinvec); sdvmul(ctheta,ivec,icosvec); norm = 1./norm; sdvmul(norm,rvec,uvec); idotu = sdvdot(ivec,uvec); sdvcross(uvec,isinvec,uxisin); dotcos = idotu*(1.-ctheta); sdvaxpy(dotcos,uvec,icosvec,ovec); sdvadd(ovec,uxisin,ovec); }
void sdmotion(double *time, double state[50], double dstate[50], double dt, double ctol, double tol, int *flag, int *err) { static double step; double work[300],ttime,param[1]; int vintgerr,which,ferr,i; sdgentime(&i); if (i != 93540) { sdseterr(54,42); } param[0] = ctol; ttime = *time; if (*flag != 0) { sdmotfunc(ttime,state,dstate,param,&ferr); step = dt; *flag = 0; } if (step <= 0.) { step = dt; } sdvinteg(sdmotfunc,&ttime,state,dstate,param,dt,&step,50,tol,work,&vintgerr, &which); *time = ttime; *err = vintgerr; }
void sdsteady(double time, double state[50], int lock[50], double ctol, double tol, int maxevals, int *fcnt, int *err) { double resid[49],param[1]; double jw[2450],dw[19602],rw[793]; int iw[396],rooterr,i; sdgentime(&i); if (i != 93540) { sdseterr(53,42); } param[0] = time; sdroot(sdstdyfunc,state,param,49,50,25,lock, ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr); sdstdyfunc(state,param,resid); *fcnt = *fcnt+1; if (rooterr == 0) { *err = 0; } else { if (*fcnt >= maxevals) { *err = 2; } else { *err = 1; } } }
void sdstatic(double time, double state[50], int lock[25], double ctol, double tol, int maxevals, int *fcnt, int *err) { double resid[37],param[26],jw[925],dw[7688],rw[484]; int iw[248],rooterr,i; sdgentime(&i); if (i != 93540) { sdseterr(52,42); } for (i = 0; i < 25; i++) { param[i] = state[25+i]; } param[25] = time; sdroot(sdstatfunc,state,param,37,25,25,lock, ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr); sdstatfunc(state,param,resid); *fcnt = *fcnt+1; if (rooterr == 0) { *err = 0; } else { if (*fcnt >= maxevals) { *err = 2; } else { *err = 1; } } }
void sdinitvel(double time, double state[50], int lock[25], double tol, int maxevals, int *fcnt, int *err) { double verrs[12],param[26]; int i; double jw[300],dw[2738],rw[309]; int iw[148],rooterr; sdgentime(&i); if (i != 93540) { sdseterr(51,42); } for (i = 0; i < 25; i++) { param[i] = state[i]; } param[25] = time; sdroot(sdvelfunc,&state[25],param,12,25,0,lock,tol,tol,maxevals, jw,dw,rw,iw,verrs,fcnt,&rooterr); sdvelfunc(&state[25],param,verrs); *fcnt = *fcnt+1; if (rooterr == 0) { *err = 0; } else { if (*fcnt >= maxevals) { *err = 2; } else { *err = 1; } } }
void sdassemble(double time, double state[50], int lock[25], double tol, int maxevals, int *fcnt, int *err) { double perrs[12],param[1]; int i; double jw[300],dw[2738],rw[309]; int iw[148],rooterr; sdgentime(&i); if (i != 93540) { sdseterr(50,42); } param[0] = time; sdroot(sdposfunc,state,param,12,25,0,lock,tol,tol,maxevals, jw,dw,rw,iw,perrs,fcnt,&rooterr); sdposfunc(state,param,perrs); *fcnt = *fcnt+1; if (rooterr == 0) { *err = 0; } else { if (*fcnt >= maxevals) { *err = 2; } else { *err = 1; } } }