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;
}
}
}
