/*
Generated 13-Jan-2003 14:06:16 by SD/FAST, Order(N) formulation
(sdfast B.2.6 #70405) on machine ID 690c54f5
Copyright (c) 1990-1996 Symbolic Dynamics, Inc.
Copyright (c) 1990-1996 Parametric Technology Corp.
RESTRICTED RIGHTS LEGEND: Use, duplication, or disclosure by the U.S.
Government is subject to restrictions as set forth in subparagraph
(c)(1)(ii) of the Rights in Technical Data and Computer Software
clause at DFARS 52.227-7013 and similar clauses in the FAR and NASA
FAR Supplement.  Symbolic Dynamics, Inc., Mountain View, CA 94041
*/
#include <math.h>

/* These routines are passed to sdroot. */

sdposfunc(vars,param,resid)
    double vars[4],param[1],resid[5];
{
    int i;
    double pos[4],vel[4];

    for (i = 0; i < 4; i++) {
        vel[i] = 0.;
    }
    sdang2st(vars,pos);
    sdstate(param[0],pos,vel);
    sdperr(resid);
}

sdvelfunc(vars,param,resid)
    double vars[4],param[5],resid[5];
{

    sdstate(param[4],param,vars);
    sdverr(resid);
}

sdstatfunc(vars,param,resid)
    double vars[4],param[5],resid[9];
{
    double pos[4],qdotdum[4];

    sdang2st(vars,pos);
    sdstate(param[4],pos,param);
    sduforce(param[4],pos,param);
    sdperr(resid);
    sdderiv(qdotdum,&resid[5]);
}

sdstdyfunc(vars,param,resid)
    double vars[8],param[1],resid[14];
{
    double pos[4],qdotdum[4];

    sdang2st(vars,pos);
    sdstate(param[0],pos,&vars[4]);
    sduforce(param[0],pos,&vars[4]);
    sdperr(resid);
    sdverr(&resid[5]);
    sdderiv(qdotdum,&resid[10]);
}

/* This routine is passed to the integrator. */

sdmotfunc(time,state,dstate,param,status)
    double time,state[8],dstate[8],param[1];
    int *status;
{
    double err[5];
    int i;

    sdstate(time,state,&state[4]);
    sduforce(time,state,&state[4]);
    sdderiv(dstate,&dstate[4]);
    *status = 1;
    sdverr(err);
    for (i = 0; i < 5; i++) {
        if (fabs(err[i]) > param[0]) {
            return;
        }
    }
    sdperr(err);
    for (i = 0; i < 5; i++) {
        if (fabs(err[i]) > param[0]) {
            return;
        }
    }
    *status = 0;
}

/* This routine performs assembly analysis. */

sdassemble(time,state,lock,tol,maxevals,fcnt,err)
    double time,state[8];
    int lock[4];
    double tol;
    int maxevals,*fcnt,*err;
{
    double perrs[5],param[1];
    int i;
    double jw[20],dw[162],rw[71];
    int iw[36],rooterr;

    sdgentime(&i);
    if (i != 140615) {
        sdseterr(50,42);
    }
    param[0] = time;
    sdroot(sdposfunc,state,param,5,4,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;
        }
    }
}

/* This routine performs initial velocity analysis. */

sdinitvel(time,state,lock,tol,maxevals,fcnt,err)
    double time,state[8];
    int lock[4];
    double tol;
    int maxevals,*fcnt,*err;
{
    double verrs[5],param[5];
    int i;
    double jw[20],dw[162],rw[71];
    int iw[36],rooterr;

    sdgentime(&i);
    if (i != 140615) {
        sdseterr(51,42);
    }
    for (i = 0; i < 4; i++) {
        param[i] = state[i];
    }
    param[4] = time;
    sdroot(sdvelfunc,&state[4],param,5,4,0,lock,tol,tol,maxevals,
      jw,dw,rw,iw,verrs,fcnt,&rooterr);
    sdvelfunc(&state[4],param,verrs);
    *fcnt = *fcnt+1;
    if (rooterr == 0) {
        *err = 0;
    } else {
        if (*fcnt >= maxevals) {
            *err = 2;
        } else {
            *err = 1;
        }
    }
}

/* This routine performs static analysis. */

sdstatic(time,state,lock,ctol,tol,maxevals,fcnt,err)
    double time,state[8];
    int lock[4];
    double ctol,tol;
    int maxevals,*fcnt,*err;
{
    double resid[9],param[5],jw[36],dw[338],rw[99];
    int iw[52],rooterr,i;

    sdgentime(&i);
    if (i != 140615) {
        sdseterr(52,42);
    }
    for (i = 0; i < 4; i++) {
        param[i] = state[4+i];
    }
    param[4] = time;
    sdroot(sdstatfunc,state,param,9,4,4,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;
        }
    }
}

/* This routine performs steady motion analysis. */

sdsteady(time,state,lock,ctol,tol,maxevals,fcnt,err)
    double time,state[8];
    int lock[8];
    double ctol,tol;
    int maxevals,*fcnt,*err;
{
    double resid[14],param[1];
    double jw[112],dw[968],rw[170];
    int iw[88],rooterr,i;

    sdgentime(&i);
    if (i != 140615) {
        sdseterr(53,42);
    }
    param[0] = time;
    sdroot(sdstdyfunc,state,param,14,8,4,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;
        }
    }
}

/* This routine performs state integration. */

sdmotion(time,state,dstate,dt,ctol,tol,flag,err)
    double *time,state[8],dstate[8],dt,ctol,tol;
    int *flag,*err;
{
    static double step;
    double work[48],ttime,param[1];
    int vintgerr,which,ferr,i;

    sdgentime(&i);
    if (i != 140615) {
        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,8,tol,work,&vintgerr,&
      which);
    *time = ttime;
    *err = vintgerr;
}

/* This routine performs state integration with a fixed-step integrator. */

sdfmotion(time,state,dstate,dt,ctol,flag,errest,err)
    double *time,state[8],dstate[8],dt,ctol;
    int *flag;
    double *errest;
    int *err;
{
    double work[32],ttime,param[1];
    int ferr,i;

    sdgentime(&i);
    if (i != 140615) {
        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,8,work,errest,&ferr);
    if (ferr != 0) {
        *err = 1;
    }
    *time = ttime;
}