//============================================== // // DO NOT EDIT THIS FILE! // //============================================== //============================================== // // This file automaticly generated by: // BOC from SDFast Output (or LIB). // // BOC Version: // /uf6/dbrogan/Sim/UI/bin/boc // ($Revision: 1.11 $) // // Input Prefix: // fly // // Output File: // boc_fly.C // // Send comments, bug reports, etc. .. to: // James F. O'Brien, obrienj@cc.gatech.edu // //============================================== //============================================== // // DO NOT EDIT THIS FILE! // you have been warned.... // //============================================== // File includes from: fly_dyn.c : #include #include // File includes from: fly_sar.c : #include // File includes from: flylib.c : #include #include //============================================== #include "boc_lib.H" #include "boc_fly.H" //============================================== //============================================== //============================================== // BEGIN source: fly_sar.c : /* Generated 27-Mar-2000 00:59:06 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 69086664 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 */ /* These routines are passed to sdroot. */ void BOCfly :: sdposfunc ( double vars[23], double param[1], double resid[1] ) { int i; double pos[24],vel[23]; for (i = 0; i < 23; i++) { vel[i] = 0.; } sdang2st(vars,pos); sdstate(param[0],pos,vel); sdperr(resid); } void BOCfly :: sdvelfunc ( double vars[23], double param[25], double resid[1] ) { sdstate(param[24],param,vars); sdverr(resid); } void BOCfly :: sdstatfunc ( double vars[23], double param[24], double resid[23] ) { double pos[24],qdotdum[24]; sdang2st(vars,pos); sdstate(param[23],pos,param); sduforce(param[23],pos,param); sdperr(resid); sdderiv(qdotdum,&resid[0]); } void BOCfly :: sdstdyfunc ( double vars[46], double param[1], double resid[23] ) { double pos[24],qdotdum[24]; sdang2st(vars,pos); sdstate(param[0],pos,&vars[23]); sduforce(param[0],pos,&vars[23]); sdperr(resid); sdverr(&resid[0]); sdderiv(qdotdum,&resid[0]); } /* This routine is passed to the integrator. */ void BOCfly :: sdmotfunc ( double time, double state[47], double dstate[47], double param[1], int *status ) { sdstate(time,state,&state[24]); sduforce(time,state,&state[24]); sdderiv(dstate,&dstate[24]); *status = 0; } /* This routine performs assembly analysis. */ void BOCfly :: sdassemble ( double time, double state[47], int lock[23], double tol, int maxevals, int *fcnt, int *err ) { double perrs[1],param[1]; int i; sdgentime(&i); if (i != 5853) { sdseterr(50,42); } param[0] = time; sdst2ang(state,state); *err = 0; *fcnt = 0; sdposfunc(state,param,perrs); *fcnt = *fcnt+1; sdang2st(state,state); } /* This routine performs initial velocity analysis. */ void BOCfly :: sdinitvel ( double time, double state[47], int lock[23], double tol, int maxevals, int *fcnt, int *err ) { double verrs[1],param[25]; int i; sdgentime(&i); if (i != 5853) { sdseterr(51,42); } for (i = 0; i < 24; i++) { param[i] = state[i]; } param[24] = time; *err = 0; *fcnt = 0; sdvelfunc(&state[24],param,verrs); *fcnt = *fcnt+1; } /* This routine performs static analysis. */ void BOCfly :: sdstatic ( double time, double state[47], int lock[23], double ctol, double tol, int maxevals, int *fcnt, int *err ) { double resid[23],param[24],jw[529],dw[4232],rw[368]; int iw[184],rooterr,i; sdgentime(&i); if (i != 5853) { sdseterr(52,42); } for (i = 0; i < 23; i++) { param[i] = state[24+i]; } param[23] = time; sdst2ang(state,state); sdroot(&BOCfly::sdstatfunc,state,param,23,23,23,lock,ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr); sdstatfunc(state,param,resid); *fcnt = *fcnt+1; sdang2st(state,state); if (rooterr == 0) { *err = 0; } else { if (*fcnt >= maxevals) { *err = 2; } else { *err = 1; } } } /* This routine performs steady motion analysis. */ void BOCfly :: sdsteady ( double time, double state[47], int lock[46], double ctol, double tol, int maxevals, int *fcnt, int *err ) { double resid[23],param[1],vars[46]; double jw[1058],dw[9522],rw[575]; int iw[276],rooterr,i; sdgentime(&i); if (i != 5853) { sdseterr(53,42); } param[0] = time; sdst2ang(state,vars); for (i = 0; i < 23; i++) { vars[23+i] = state[24+i]; } sdroot(&BOCfly::sdstdyfunc,vars,param,23,46,23,lock,ctol,tol,maxevals,jw,dw,rw,iw,resid,fcnt,&rooterr); sdstdyfunc(vars,param,resid); *fcnt = *fcnt+1; sdang2st(vars,state); for (i = 0; i < 23; i++) { state[24+i] = vars[23+i]; } if (rooterr == 0) { *err = 0; } else { if (*fcnt >= maxevals) { *err = 2; } else { *err = 1; } } } /* This routine performs state integration. */ void BOCfly :: sdmotion ( double *time, double state[47], double dstate[47], double dt, double ctol, double tol, int *flag, int *err ) { static double step; double work[282],ttime,param[1]; int vintgerr,which,ferr,i; sdgentime(&i); if (i != 5853) { 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(&BOCfly::sdmotfunc,&ttime,state,dstate,param,dt,&step,47,tol,work,&vintgerr,&which); *time = ttime; *err = vintgerr; } /* This routine performs state integration with a fixed-step integrator. */ void BOCfly :: sdfmotion ( double *time, double state[47], double dstate[47], double dt, double ctol, int *flag, double *errest, int *err ) { double work[188],ttime,param[1]; int ferr,i; sdgentime(&i); if (i != 5853) { sdseterr(55,42); } param[0] = ctol; *err = 0; ttime = *time; if (*flag != 0) { sdmotfunc(ttime,state,dstate,param,&ferr); *flag = 0; } sdfinteg(&BOCfly::sdmotfunc,&ttime,state,dstate,param,dt,47,work,errest,&ferr); if (ferr != 0) { *err = 1; } *time = ttime; } // END source: fly_sar.c : //============================================== //============================================== //============================================== //============================================== // BEGIN source: flylib.c : /* Generated 27-Mar-2000 00:59:06 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 69086664 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 */ #define lasterr (error_.lasterr_) #define lastrou (error_.lastrou_) static const BOCfly::error_t BOCflyerrorCONST = { /* Error parameters */ /* lasterr */ 0, /* lastrou */ 0, }; void BOCfly :: sderror ( int *routine, int *errnum ) { *errnum = lasterr; *routine = lastrou; } void BOCfly :: sdprinterr ( FILE *fnum ) { sdprerrmsg(fnum,lastrou,lasterr); } void BOCfly :: sdclearerr ( ) { lasterr = 0; lastrou = 0; } void BOCfly :: sdseterr ( int routine, int errnum ) { if (lasterr == 0) { lasterr = errnum; lastrou = routine; } } /* =========================================================== LDU decomposition scheme for solving Mx=b M: na X na symmetric, pos. definite, upper rt triangle filled b: na X 1 x: na X 1 (returned) Actual dimensions (n) may be larger, map says where to find the na interesting elements. =========================================================== */ void BOCfly :: sdldudcomp ( int n, int na, int map[], double tol, double ld[], double sum[], double m[], double l[], double d[] ) { int r,c,row,col,cm1,j,mj; double dtemp,dtempi,ldtemp,tsum; for (c = 0; c <= na-1; c++) { col = map[c]; cm1 = c-1; for (r = c; r <= na-1; r++) { row = map[r]; tsum = m[col*n+row]; for (j = 0; j <= cm1; j++) { mj = map[j]; tsum = tsum-ld[r*na+j]*l[col*n+mj]; } sum[r] = tsum; } dtemp = sum[c]; dtempi = 0.; if (fabs(dtemp) > tol) dtempi = 1./dtemp; d[col] = dtempi; for (r = c; r <= na-1; r++) { row = map[r]; ldtemp = sum[r]; ld[r*na+c] = ldtemp; l[row*n+col] = ldtemp*dtempi; } } } void BOCfly :: sdldubsl ( int n, int na, int map[], double l[], double b[], double x[] ) { int i,j,im1,mi,mj; double sum; mi = map[0]; x[mi] = b[mi]; for (i = 1; i <= na-1; i++) { mi = map[i]; im1 = i-1; sum = b[mi]; for (j = 0; j <= im1; j++) { mj = map[j]; sum = sum-l[mi*n+mj]*x[mj]; } x[mi] = sum; } } void BOCfly :: sdldubsd ( int n, int na, int map[], double d[], double b[], double x[] ) { int i,mi; for (i = 0; i <= na-1; i++) { mi = map[i]; x[mi] = b[mi]*d[mi]; } } void BOCfly :: sdldubsu ( int n, int na, int map[], double l[], double b[], double x[] ) { int i,j,ip1,mi,mj; double sum; mi = map[na-1]; x[mi] = b[mi]; i = na-2; while (i >= 0) { mi = map[i]; ip1 = i+1; sum = b[mi]; for (j = ip1; j <= na-1; j++) { mj = map[j]; sum = sum-l[mj*n+mi]*x[mj]; } x[mi] = sum; i = i-1; } } void BOCfly :: sdldubslv ( int n, int na, int map[], double work[], double l[], double d[], double b[], double x[] ) { sdldubsl(n,na,map,l,b,work); sdldubsd(n,na,map,d,work,work); sdldubsu(n,na,map,l,work,x); } void BOCfly :: sdlduslv ( int n, int na, int map[], double tol, double work1[], double work2[], double m[], double b[], double l[], double d[], double x[] ) { sdldudcomp(n,na,map,tol,work1,work2,m,l,d); sdldubslv(n,na,map,work2,l,d,b,x); } /* =========================================================== QR decomposition scheme for solving Wx=b W is nra by nca b is nra by 1 x (returned) is nca by 1 Actual dimensions (nr,nc) may be larger, mapr (mapc) says where to find the nra (nca) interesting rows (columns). On return, W is overwritten by the Q and R matrices in compact form. Solution yields least squares residual for overdetermined systems. Underdetermined systems give a solution in which elements corresponding to redundant or missing equations are set to 0. (Not necessarily the LS solution.) =========================================================== */ void BOCfly :: sdqrdcomp ( int nr, int nc, int nra, int nca, int mapr[], int mapc[], double w[], double qraux[], int jpvt[] ) { int i,j,l,lp1,maxj,mi,mj,mlr,mlc,mmaxj,minrca; double maxnrm,nrmxl,t; for (j = 0; j <= nca-1; j++) { mj = mapc[j]; jpvt[j] = j; t = 0.; for (l = 0; l <= nra-1; l++) { mlr = mapr[l]; t = t+w[mlr*nc+mj]*w[mlr*nc+mj]; } qraux[j] = sqrt(t); } if (nca < nra) { minrca = nca; } else { minrca = nra; } for (l = 0; l <= minrca-1; l++) { mlr = mapr[l]; mlc = mapc[l]; maxnrm = 0.; maxj = l; for (j = l; j <= nca-1; j++) { if (qraux[j] > maxnrm) { maxnrm = qraux[j]; maxj = j; } } if (maxj != l) { mmaxj = mapc[maxj]; for (i = 0; i <= nra-1; i++) { mi = mapr[i]; t = w[mi*nc+mlc]; w[mi*nc+mlc] = w[mi*nc+mmaxj]; w[mi*nc+mmaxj] = t; } qraux[maxj] = qraux[l]; i = jpvt[maxj]; jpvt[maxj] = jpvt[l]; jpvt[l] = i; } qraux[l] = 0.; if (l != nra-1) { t = 0.; for (i = l; i <= nra-1; i++) { mi = mapr[i]; t = t+w[mi*nc+mlc]*w[mi*nc+mlc]; } nrmxl = sqrt(t); if (nrmxl != 0.) { if (w[mlr*nc+mlc] < 0.) { nrmxl = -nrmxl; } t = 1./nrmxl; for (i = l; i <= nra-1; i++) { mi = mapr[i]; w[mi*nc+mlc] = t*w[mi*nc+mlc]; } w[mlr*nc+mlc] = 1.+w[mlr*nc+mlc]; lp1 = l+1; for (j = lp1; j <= nca-1; j++) { mj = mapc[j]; t = 0.; for (i = l; i <= nra-1; i++) { mi = mapr[i]; t = t-w[mi*nc+mlc]*w[mi*nc+mj]; } t = t/w[mlr*nc+mlc]; for (i = l; i <= nra-1; i++) { mi = mapr[i]; w[mi*nc+mj] = w[mi*nc+mj]+t*w[mi*nc+mlc]; } t = 0.; for (i = lp1; i <= nra-1; i++) { mi = mapr[i]; t = t+w[mi*nc+mj]*w[mi*nc+mj]; } qraux[j] = sqrt(t); } qraux[l] = w[mlr*nc+mlc]; w[mlr*nc+mlc] = -nrmxl; } } } } void BOCfly :: sdqrsl ( int nr, int nc, int nra, int nca, int mapr[], int mapc[], int k, double work[], double w[], double qraux[], double b[], double x[] ) { int i,j,jj,jm1,mir,mic,mjr,mjc,minrk; double t,tt; for (i = 0; i <= nra-1; i++) { mir = mapr[i]; work[i] = b[mir]; } minrk = nra-1; if (k < minrk) { minrk = k; } for (j = 0; j <= minrk-1; j++) { mjr = mapr[j]; mjc = mapc[j]; if (qraux[j] != 0.) { tt = w[mjr*nc+mjc]; w[mjr*nc+mjc] = qraux[j]; t = 0.; for (i = j; i <= nra-1; i++) { mir = mapr[i]; t = t-w[mir*nc+mjc]*work[i]; } t = t/w[mjr*nc+mjc]; for (i = j; i <= nra-1; i++) { mir = mapr[i]; work[i] = work[i]+t*w[mir*nc+mjc]; } w[mjr*nc+mjc] = tt; } } for (j = 0; j <= k-1; j++) { mjc = mapc[j]; x[mjc] = work[j]; } for (jj = 0; jj <= k-1; jj++) { j = k-jj-1; jm1 = j-1; mjr = mapr[j]; mjc = mapc[j]; x[mjc] = x[mjc]/w[mjr*nc+mjc]; t = -x[mjc]; for (i = 0; i <= jm1; i++) { mir = mapr[i]; mic = mapc[i]; x[mic] = x[mic]+t*w[mir*nc+mjc]; } } } void BOCfly :: sdqrbslv ( int nr, int nc, int nra, int nca, int mapr[], int mapc[], double tol, double work[], int iwork[], double w[], double qraux[], int jpvt[], double b[], double x[], int *rank ) { int j,k,kk,mjc,mkkr,mkkc,minrca; double t; for (j = 0; j < nca; j++) { iwork[j] = jpvt[j]; } if (nca < nra) { minrca = nca; } else { minrca = nra; } k = 0; mkkr = mapr[0]; mkkc = mapc[0]; t = tol*fabs(w[mkkr*nc+mkkc]); if (t < .1*tol) { t = .1*tol; } for (kk = 0; kk <= minrca-1; kk++) { mkkr = mapr[kk]; mkkc = mapc[kk]; if (fabs(w[mkkr*nc+mkkc]) <= t) { break; } else { k = k+1; } } if (k != 0) { sdqrsl(nr,nc,nra,nca,mapr,mapc,k,work,w,qraux,b,x); } for (j = k; j <= nca-1; j++) { x[mapc[j]] = 0.; } for (j = 0; j <= nca-1; j++) { kk = iwork[j]; while (kk != j) { mjc = mapc[j]; mkkc = mapc[kk]; t = x[mjc]; x[mjc] = x[mkkc]; x[mkkc] = t; iwork[j] = iwork[kk]; iwork[kk] = kk; kk = iwork[j]; } } *rank = k; } void BOCfly :: sdqrslv ( int nr, int nc, int nra, int nca, int mapr[], int mapc[], double tol, int jpvt[], double qraux[], double work[], int iwork[], double w[], double b[], double x[], int *rank ) { sdqrdcomp(nr,nc,nra,nca,mapr,mapc,(double*)w,qraux,jpvt); sdqrbslv(nr,nc,nra,nca,mapr,mapc,tol,work,iwork,(double*)w,qraux,jpvt,b,x,rank); } /* =========================================================== Linear equation solver for Wx=b W is nra by nca b is nra by 1 x (returned) is nca by 1 Actual dimensions (nr,nc) may be larger, mapr (mapc) says where to find the nra (nca) interesting rows (columns). On return, W and b are unchanged. Solution yields least squares residual for overdetermined systems and a least squares solution vector for underdetermined systems. If ndes > 0, the first nra-ndes rows are `required' while the remaining ndes rows are `desired'. The returned solution minimizes the residual for the required rows, then does the best it can on the desired rows. The performance and numerical properties of this routine are considerably worse than those of the qrslv routine. However, it works very well as a guide to the root finding routine. Work arrays should be dimensioned as follows: dw is 2*(nra+nca)**2 rw is 4*(nra+nca) iw is 3*(nra+nca) =========================================================== */ void BOCfly :: sdlsslv ( int nr, int nc, int nra, int nca, int ndes, int mapr[], int mapc[], double tol, double dw[], double rw[], int iw[], double w[], double b[], double x[] ) { int i,j,k,nreq,dsiz,rank,mapi,mapj,mapk; int ix,wwt,dloc,ddt,qraux,work,rhs,soln,map,jpvt,iwork; nreq = nra-ndes; dsiz = nca+nreq; wwt = 0; dloc = 0; ddt = dloc+dsiz*dsiz; qraux = 0; work = qraux+nca+nra; rhs = work+nca+nra; soln = rhs+nca+nra; map = 0; jpvt = map+nca+nra; iwork = jpvt+nca+nra; if (ndes == 0) { for (i = 0; i <= nra-1; i++) { mapi = mapr[i]; rw[rhs+i] = b[mapi]; for (j = i; j <= nra-1; j++) { mapj = mapr[j]; ix = wwt+i*nra+j; dw[ix] = 0.; for (k = 0; k <= nca-1; k++) { mapk = mapc[k]; dw[ix] = dw[ix]+w[mapi*nc+mapk]*w[mapj*nc+mapk]; } dw[wwt+j*nra+i] = dw[ix]; } iw[map+i] = i; } sdqrslv(nra,nra,nra,nra,&iw[map],&iw[map],tol,&iw[jpvt],&rw[qraux],&rw[work],&iw[iwork],&dw[wwt],&rw[rhs],&rw[soln],&rank); for (i = 0; i <= nca-1; i++) { mapi = mapc[i]; x[mapi] = 0.; for (j = 0; j <= nra-1; j++) { mapj = mapr[j]; x[mapi] = x[mapi]+w[mapj*nc+mapi]*rw[soln+j]; } } } else { for (i = 0; i <= nca-1; i++) { mapi = mapc[i]; for (j = i; j <= nca-1; j++) { mapj = mapc[j]; ix = dloc+i*dsiz+j; dw[ix] = 0.; for (k = 0; k <= ndes-1; k++) { mapk = mapr[nreq+k]; dw[ix] = dw[ix]+w[mapk*nc+mapi]*w[mapk*nc+mapj]; } dw[dloc+j*dsiz+i] = dw[ix]; } for (j = 0; j <= nreq-1; j++) { mapj = mapr[j]; ix = dloc+i*dsiz+(j+nca); dw[ix] = w[mapj*nc+mapi]; dw[dloc+(j+nca)*dsiz+i] = dw[ix]; } rw[rhs+i] = 0.; for (j = 0; j <= ndes-1; j++) { mapj = mapr[nreq+j]; rw[rhs+i] = rw[rhs+i]+w[mapj*nc+mapi]*b[mapj]; } } for (i = 0; i <= nreq-1; i++) { for (j = 0; j <= nreq-1; j++) { dw[dloc+(i+nca)*dsiz+(j+nca)] = 0.; } rw[rhs+(i+nca)] = b[mapr[i]]; } for (i = 0; i <= dsiz-1; i++) { iw[map+i] = i; for (j = i; j <= dsiz-1; j++) { ix = ddt+i*dsiz+j; dw[ix] = 0.; for (k = 0; k <= dsiz-1; k++) { dw[ix] = dw[ix]+dw[dloc+i*dsiz+k]*dw[dloc+j*dsiz+k]; } dw[ddt+j*dsiz+i] = dw[ix]; } } sdqrslv(dsiz,dsiz,dsiz,dsiz,&iw[map],&iw[map],tol,&iw[jpvt],&rw[qraux],&rw[work],&iw[iwork],&dw[ddt],&rw[rhs],&rw[soln],&rank); for (i = 0; i <= nca-1; i++) { mapi = mapc[i]; x[mapi] = 0.; for (j = 0; j <= dsiz-1; j++) { x[mapi] = x[mapi]+dw[dloc+j*dsiz+i]*rw[soln+j]; } } } } /* Utility routines for use with sdroot. */ void BOCfly :: sdcalcerrs ( double fval[], int nfunc, int ndes, int dnorm, double *maxderr, double *maxrerr, double *derrnorm ) { int i,nreq; double tmp; nreq = nfunc-ndes; *maxderr = 0.; *maxrerr = 0.; *derrnorm = 0.; for (i = 0; i < nreq; i++) { tmp = fabs(fval[i]); if (tmp > *maxrerr) { *maxrerr = tmp; } } for (i = 0; i < ndes; i++) { tmp = fabs(fval[nreq+i]); if (tmp > *maxderr) { *maxderr = tmp; } *derrnorm = *derrnorm+tmp; } if (dnorm == 1) { *derrnorm = *maxderr; } } void BOCfly :: sdadjvars ( void (BOCfly::*func)( double *, double *, double * ) , double vars[], double param[], int nfunc, int ndes, int dnorm, int nvar, double deltas[], double step, double rerr, double tderr, double rtol, int *fcnt, double newvars[], double newerrs[] ) { int i,cnt,alldone; double impr,maxchg,pmaxfact,maxfact,pmaxrerr,pderrnorm,maxderr,maxrerr, derrnorm,mag; maxfact = 1.; for (i = 0; i < nvar; i++) { impr = fabs(deltas[i]*step); maxchg = .1*fabs(vars[i])+1.; if (impr/maxchg > maxfact) { maxfact = impr/maxchg; } } maxfact = 1./maxfact; pmaxrerr = rerr; pderrnorm = tderr; pmaxfact = maxfact; cnt = 0; alldone = 0; retry: ; cnt = cnt+1; for (i = 0; i < nvar; i++) { impr = deltas[i]*step; newvars[i] = vars[i] - impr*maxfact; } (this->*func)(newvars,param,newerrs); *fcnt = *fcnt+1; if (alldone != 0) { return; } sdcalcerrs(newerrs,nfunc,ndes,dnorm,&maxderr,&maxrerr,&derrnorm); if ((pmaxrerr <= rtol) && (maxrerr <= rtol)) { impr = pderrnorm-derrnorm; } else { impr = pmaxrerr-maxrerr; } pmaxrerr = maxrerr; pderrnorm = derrnorm; if (impr <= 0.) { if (maxfact != pmaxfact) { maxfact = pmaxfact; alldone = 1; goto retry; } } else { if (maxfact < .5) { if (cnt < 5) { mag = 1.25; } else { if (cnt < 10) { mag = 2.; } else { mag = 10.; } } pmaxfact = maxfact; maxfact = mag*maxfact; if (maxfact > 1.) { maxfact = 1.; } goto retry; } } } void BOCfly :: sdcalcjac ( void (BOCfly::*func)( double *, double *, double * ) , double vars[], double param[], int nfunc, int nvar, int lock[], double delta, double fval[], double ftmp[], double jw[], int *fcnt, double scale[] ) { int i,j; double save,chg,vchg,maxelt; for (j = 0; j < nvar; j++) { if (lock[j] != 0) { for (i = 0; i < nfunc; i++) { jw[i*nvar+j] = 0.; } } else { save = vars[j]; vchg = delta*(fabs(save)+1.); vars[j] = vars[j]+vchg; (this->*func)(vars,param,ftmp); *fcnt = *fcnt+1; vars[j] = save; for (i = 0; i < nfunc; i++) { chg = ftmp[i]-fval[i]; if (fabs(chg) < 1e-13) { jw[i*nvar+j] = 0.; } else { jw[i*nvar+j] = chg/vchg; } } } } for (i = 0; i < nfunc; i++) { maxelt = 0.; for (j = 0; j < nvar; j++) { if (fabs(jw[i*nvar+j]) > maxelt) { maxelt = fabs(jw[i*nvar+j]); } } if (maxelt > 0.) { scale[i] = 1./maxelt; for (j = 0; j < nvar; j++) { jw[i*nvar+j] = jw[i*nvar+j]*scale[i]; } } else { scale[i] = 1.; } } } /* ==================================================================== Root-finding scheme for solving a set of nfunc=nreq+ndes nonlinear equations in nvar unknowns: r (v) = r (v) = ... = r (v) = 0 (actually |r | <= rtol) 1 2 nreq i d (v) = d (v) = ... = d (v) = 0 (actually |d | <= dtol) 1 2 ndes i The r's are "required" functions while the d's are only "desired". That is, we adjust the nvar variables v to find a solution which keeps each of the r's below rtol, and keeps the d's as small as possible without violating any of the r's. Least squares residuals are attempted if the equations cannot be solved. No guarantees can be made that this routine will find a solution even if there is one. The better the initial values for the v's, the more likely it is to succeed and the more quickly it will converge. A single function func is provided which calculates all of the r's and d's for the current v and returns the residuals. A parameter param is passed through unchanged to the function. The array lock has an element corresponding to each variable. Any variable which may be modified should have its corresponding lock element set to 0. If non-zero, the variable will not be altered here. Maxeval sets an upper limit on the number of calls to func which may be made. The root finder only returns after complete iterations, so it may make somewhat more than maxeval calls. On return, the actual number of calls made is returned in fcnt. Err is returned 0 if we successfully reduce all the r's to below rtol and all the d's to below dtol. If the r's are met but not the d's, we return with err=1, and if the r's are not met we return err=2. Work arrays should be dimensioned as follows: jw is nf*nv dw is 2*(nf+nv)**2 rw is 7*nf+9*nv iw is 4*(nf+nv) ==================================================================== */ void BOCfly :: sdroot ( void (BOCfly::*func)( double *, double *, double * ) , double vars[], double param[], int nfunc, int nvar, int ndesin, int lock[], double rtol, double dtol, int maxeval, double jw[], double dw[], double rw[], int iw[], double fret[], int *fcnt, int *err ) { int i,slowcnt,tooslow,ntrys,nreq,fixits,ndes,dnorm; int f1,f2,scale,deltav,guess,rvars,rdeltav,rguess,morerw,mapf,mapv,moreiw; double qrtol,maxderr,maxrerr,derrnorm,pmaxderr,pmaxrerr,pderrnorm,step,crude ,impr,rstep,preverr; if (ndesin < 0) { ndes = -ndesin; dnorm = 1; } else { ndes = ndesin; dnorm = 0; } nreq = nfunc-ndes; qrtol = 1e-13; *fcnt = 0; *err = 0; f1 = 0; f2 = f1+nfunc; scale = f2+nfunc; deltav = scale+nfunc; guess = deltav+nvar; rvars = guess+nvar; rdeltav = rvars+nvar; rguess = rdeltav+nvar; morerw = rguess+nvar; mapf = 0; mapv = mapf+nfunc; moreiw = mapv+nvar; for (i = 0; i < nfunc; i++) { iw[mapf+i] = i; } for (i = 0; i < nvar; i++) { iw[mapv+i] = i; } (this->*func)(vars,param,fret); *fcnt = *fcnt+1; sdcalcerrs(fret,nfunc,ndes,dnorm,&pmaxderr,&pmaxrerr,&pderrnorm); crude = 1e-6; for (ntrys = 0; ntrys < 2; ntrys++) { slowcnt = 0; step = 1.; for (;;) { if ((pmaxrerr <= rtol) && (pmaxderr <= dtol)) { return; } if (*fcnt >= maxeval) { goto givingUp; } sdcalcjac(func,vars,param,nfunc,nvar,lock,crude,fret,&rw[f1],jw,fcnt,&rw[scale]); for (i = 0; i < nfunc; i++) { rw[f1+i] = fret[i]*rw[scale+i]; } sdlsslv(nfunc,nvar,nfunc,nvar,ndes,&iw[mapf],&iw[mapv],qrtol,dw,&rw[morerw],&iw[moreiw],jw,&rw[f1],&rw[deltav]); for (;;) { sdadjvars(func,vars,param,nfunc,ndes,dnorm,nvar,&rw[deltav],step,pmaxrerr,pderrnorm,rtol,fcnt,&rw[guess],&rw[f1]); sdcalcerrs(&rw[f1],nfunc,ndes,dnorm,&maxderr,&maxrerr,&derrnorm); if ((pderrnorm > derrnorm) && (pmaxrerr <= rtol) && (maxrerr > rtol) && (maxrerr <= .05)) { for (i = 0; i < nvar; i++) { rw[rvars+i] = rw[guess+i]; } rstep = 1.; preverr = maxrerr; for (fixits = 0; fixits < 5; fixits++) { sdcalcjac(func,&rw[rvars],param,nreq,nvar,lock,crude,&rw[f1],&rw[f2],jw,fcnt,&rw[scale]); for (i = 0; i < nreq; i++) { rw[f2+i] = rw[f1+i]*rw[scale+i]; } sdlsslv(nreq,nvar,nreq,nvar,0,&iw[mapf],&iw[mapv],qrtol,dw,&rw[morerw],&iw[moreiw],jw,&rw[f2],&rw[rdeltav]); for (;;) { sdadjvars(func,&rw[rvars],param,nfunc,ndes,dnorm,nvar,&rw[rdeltav],rstep,preverr,pderrnorm,rtol,fcnt,&rw[rguess],&rw[f2]); sdcalcerrs(&rw[f2],nfunc,ndes,dnorm,&maxderr,&maxrerr,&derrnorm); if (preverr-maxrerr >= rtol) { break; } else { rstep = rstep*.5; if (rstep < .01) { goto couldntFixReqs; } } } if (derrnorm > pderrnorm) { goto couldntFixReqs; } for (i = 0; i < nfunc; i++) { rw[f1+i] = rw[f2+i]; } for (i = 0; i < nvar; i++) { rw[rvars+i] = rw[rguess+i]; } preverr = maxrerr; if (maxrerr <= rtol) { goto fixUpSucceeded; } rstep = rstep*2.; if (rstep > 1.) { rstep = 1.; } } goto couldntFixReqs; fixUpSucceeded: ; for (i = 0; i < nvar; i++) { rw[guess+i] = rw[rvars+i]; } } if ((pmaxrerr <= rtol) && (maxrerr <= rtol)) { impr = pderrnorm-derrnorm; if (impr < .1*dtol+.01*derrnorm) { tooslow = 1; } else { tooslow = 0; } } else { impr = pmaxrerr-maxrerr; if (impr < .01*rtol+.01*maxrerr) { tooslow = 1; } else { tooslow = 0; } } if (impr >= 0.) { break; } couldntFixReqs: ; step = step*.5; if (ntrys == 1) { if (step < 1e-5) { goto nextcrude; } } else { if (step < .01) { goto nextcrude; } } } for (i = 0; i < nfunc; i++) { fret[i] = rw[f1+i]; } for (i = 0; i < nvar; i++) { vars[i] = rw[guess+i]; } pmaxderr = maxderr; pmaxrerr = maxrerr; pderrnorm = derrnorm; if (tooslow != 0) { slowcnt = slowcnt+1; if (slowcnt >= 3) { goto nextcrude; } } else { slowcnt = 0; } step = step*2.; if (step > 1.) { step = 1.; } } nextcrude: ; crude = crude*.001; } givingUp: ; if (pmaxrerr > rtol) { *err = 2; } else { if (pmaxderr > dtol) { *err = 1; } } } /* Utility routine for use with sdfinteg and sdvinteg. Work is 2*neq. */ void BOCfly :: sdrk4m ( void (BOCfly::*func)( double, double *, double *, double *, int * ) , double time, double st[], double dst0[], double param[], double step, double nst[], int neq, double work[], double errs[], double *maxerr, int *which ) { int i,dst1,dst2,errf; double step2,step3,step6,step8,err,old,ast; dst1 = 0; dst2 = dst1+neq; step2 = step/2.; step3 = step/3.; step6 = step/6.; step8 = step/8.; for (i = 0; i < neq; i++) { nst[i] = st[i]+dst0[i]*step3; } (this->*func)(time+step3,nst,&work[dst1],param,&errf); for (i = 0; i < neq; i++) { nst[i] = st[i]+(dst0[i]+work[dst1+i])*step6; } (this->*func)(time+step3,nst,&work[dst1],param,&errf); for (i = 0; i < neq; i++) { nst[i] = st[i]+(dst0[i]+3.*work[dst1+i])*step8; } (this->*func)(time+step2,nst,&work[dst2],param,&errf); for (i = 0; i < neq; i++) { nst[i] = st[i]+(dst0[i]-3.*work[dst1+i]+4.*work[dst2+i])*step2; } (this->*func)(time+step,nst,&work[dst1],param,&errf); *maxerr = 0.; *which = 0; for (i = 0; i < neq; i++) { old = nst[i]; nst[i] = st[i]+(dst0[i]+4.*work[dst2+i]+work[dst1+i])*step6; err = fabs(.2*(old-nst[i])); ast = fabs(nst[i]); if (ast > .1) { errs[i] = err/(ast+1e-20); } else { errs[i] = 10.*err; } if (errs[i] > *maxerr) { *maxerr = errs[i]; *which = i; } } } /* A fixed-step integrator. Work should be dimensioned 4*neq. */ void BOCfly :: sdfinteg ( void (BOCfly::*func)( double, double *, double *, double *, int * ) , double *time, double st[], double dst[], double param[], double step, int neq, double work[], double *errest, int *status ) { int i,which,nst,errs,morework; double ttime; nst = 0; errs = nst+neq; morework = errs+neq; ttime = *time; if (step > 0.) { sdrk4m(func,ttime,st,dst,param,step,&work[nst],neq,&work[morework],&work[errs],errest,&which); for (i = 0; i < neq; i++) { st[i] = work[nst+i]; } ttime = ttime+step; } else { *errest = 0.; } *status = 0; (this->*func)(ttime,st,dst,param,status); *time = ttime; } /* A variable-step integrator. Work should be dimensioned 6*neq. */ void BOCfly :: sdvinteg ( void (BOCfly::*func)( double, double *, double *, double *, int * ) , double *time, double st[], double dst[], double param[], double dt, double *step, int neqin, double tol, double work[], int *err, int *which ) { int i,wh,wh2,xst,xst2,xdst,errs,morework,errf,neq,earlyret; double ttime,ntime,xtime,tfin,stp,stp2,nstp,maxerr,minstep; if (neqin < 0) { neq = -neqin; earlyret = 1; } else { neq = neqin; earlyret = 0; } xst = 0; xst2 = xst+neq; xdst = xst2+neq; errs = xdst+neq; morework = errs+neq; minstep = 1e-10; ttime = *time; tfin = ttime+dt; stp = *step; if (stp < minstep) { stp = minstep; } *err = 0; if (dt <= 0.) { errf = 0; (this->*func)(ttime,st,dst,param,&errf); if (errf != 0) { *err = 3; *which = errf; } return; } while (ttime < tfin) { if (ttime+1.25*stp >= tfin) { stp = tfin-ttime; } nstp = stp; for (;;) { sdrk4m(func,ttime,st,dst,param,nstp,&work[xst],neq,&work[morework],&work[errs],&maxerr,&wh); xtime = ttime+nstp; if (maxerr <= tol) { errf = 0; (this->*func)(xtime,&work[xst],&work[xdst],param,&errf); if (errf == 0) { stp2 = 2.*nstp; if ((maxerr*64. <= tol) && (stp2 <= dt)) { stp = stp2; } else { stp = nstp; } break; } } stp2 = nstp/2.; if (stp2 < minstep) { errf = 0; (this->*func)(xtime,&work[xst],&work[xdst],param,&errf); if (errf != 0) { *err = 3; *which = errf; *time = ttime; *step = nstp; (this->*func)(ttime,st,dst,param,&errf); return; } if (*err == 0) { *err = 1; *which = wh; } if (xtime+nstp > tfin) { stp2 = tfin-xtime; } else { stp2 = nstp; } ntime = xtime+stp2; if (ntime == xtime) { stp = nstp; break; } sdrk4m(func,xtime,&work[xst],&work[xdst],param,stp2,&work[xst2],neq,&work[morework],&work[errs],&maxerr,&wh2); if (work[errs+wh] <= tol) { (this->*func)(xtime,&work[xst],&work[xdst],param,&errf); stp = nstp; break; } *err = 2; *which = wh; *time = ttime; *step = nstp; (this->*func)(ttime,st,dst,param,&errf); return; } else { nstp = stp2; } } ttime = xtime; for (i = 0; i < neq; i++) { st[i] = work[xst+i]; dst[i] = work[xdst+i]; } if (stp >= minstep) { *step = stp; } ntime = ttime+stp; if (ntime == ttime) { ttime = tfin; } if (earlyret != 0) { *time = ttime; return; } } *time = tfin; } void BOCfly :: sddc2ang ( double dircos[3][3], double *a1, double *a2, double *a3 ) { double quot,angle,th1,th2,th3,costh2,temp[10]; if ((fabs((fabs(dircos[0][2])-1.)) <= 1e-10) ) { if ((dircos[0][2] > 0.) ) { temp[0] = 1.5707963267949; } else { temp[0] = -1.5707963267949; } th2 = temp[0]; if ((dircos[2][1] > 1.) ) { temp[0] = 1.; } else { if ((dircos[2][1] < -1.) ) { temp[1] = -1.; } else { temp[1] = dircos[2][1]; } temp[0] = temp[1]; } angle = asin(temp[0]); if ((dircos[1][1] >= 0.) ) { temp[0] = angle; } else { temp[0] = (3.14159265358979-angle); } th1 = temp[0]; th3 = 0.; } else { if ((dircos[0][2] > 1.) ) { temp[0] = 1.; } else { if ((dircos[0][2] < -1.) ) { temp[1] = -1.; } else { temp[1] = dircos[0][2]; } temp[0] = temp[1]; } th2 = asin(temp[0]); costh2 = cos(th2); quot = ((-dircos[1][2])/costh2); if ((quot > 1.) ) { temp[0] = 1.; } else { if ((quot < -1.) ) { temp[1] = -1.; } else { temp[1] = quot; } temp[0] = temp[1]; } angle = asin(temp[0]); if ((dircos[2][2] >= 0.) ) { temp[0] = angle; } else { temp[0] = (3.14159265358979-angle); } th1 = temp[0]; quot = ((-dircos[0][1])/costh2); if ((quot > 1.) ) { temp[0] = 1.; } else { if ((quot < -1.) ) { temp[1] = -1.; } else { temp[1] = quot; } temp[0] = temp[1]; } angle = asin(temp[0]); if ((dircos[0][0] >= 0.) ) { temp[0] = angle; } else { temp[0] = (3.14159265358979-angle); } th3 = temp[0]; } if ((th1 > 3.14159265358979) ) { temp[0] = (th1-6.28318530717959); } else { temp[0] = th1; } *a1 = temp[0]; *a2 = th2; if ((th3 > 3.14159265358979) ) { temp[0] = (th3-6.28318530717959); } else { temp[0] = th3; } *a3 = temp[0]; } void BOCfly :: sddc2quat ( double dircos[3][3], double *e1, double *e2, double *e3, double *e4 ) { double tmp,tmp1,tmp2,tmp3,tmp4,temp[10]; tmp = (.25*(1.-(dircos[0][0]+(dircos[1][1]+dircos[2][2])))); tmp4 = (.5-tmp); if ((tmp4 <= 0.) ) { temp[0] = 0.; } else { temp[0] = sqrt(tmp4); } tmp4 = temp[0]; tmp1 = (tmp+(.5*dircos[0][0])); if ((tmp1 <= 0.) ) { temp[0] = 0.; } else { temp[0] = sqrt(tmp1); } tmp1 = temp[0]; tmp2 = (tmp+(.5*dircos[1][1])); if ((tmp2 <= 0.) ) { temp[0] = 0.; } else { temp[0] = sqrt(tmp2); } tmp2 = temp[0]; tmp3 = (tmp+(.5*dircos[2][2])); if ((tmp3 <= 0.) ) { temp[0] = 0.; } else { temp[0] = sqrt(tmp3); } tmp3 = temp[0]; if (((tmp1 >= tmp2) && (tmp1 >= tmp3)) ) { if ((dircos[2][1] < dircos[1][2]) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp1 = (tmp1*temp[0]); if (((tmp1*(dircos[0][1]+dircos[1][0])) < 0.) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp2 = (tmp2*temp[0]); if (((tmp1*(dircos[0][2]+dircos[2][0])) < 0.) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp3 = (tmp3*temp[0]); } else { if (((tmp2 >= tmp1) && (tmp2 >= tmp3)) ) { if ((dircos[0][2] < dircos[2][0]) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp2 = (tmp2*temp[0]); if (((tmp2*(dircos[1][2]+dircos[2][1])) < 0.) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp3 = (tmp3*temp[0]); if (((tmp2*(dircos[0][1]+dircos[1][0])) < 0.) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp1 = (tmp1*temp[0]); } else { if ((dircos[1][0] < dircos[0][1]) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp3 = (tmp3*temp[0]); if (((tmp3*(dircos[0][2]+dircos[2][0])) < 0.) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp1 = (tmp1*temp[0]); if (((tmp3*(dircos[1][2]+dircos[2][1])) < 0.) ) { temp[0] = -1.; } else { temp[0] = 1.; } tmp2 = (tmp2*temp[0]); } } tmp = (1./sqrt(((tmp1*tmp1)+((tmp2*tmp2)+((tmp3*tmp3)+(tmp4*tmp4)))))); *e1 = (tmp*tmp1); *e2 = (tmp*tmp2); *e3 = (tmp*tmp3); *e4 = (tmp*tmp4); } void BOCfly :: sdang2dc ( double a1, double a2, double a3, double dircos[3][3] ) { double cos1,cos2,cos3,sin1,sin2,sin3; cos1 = cos(a1); cos2 = cos(a2); cos3 = cos(a3); sin1 = sin(a1); sin2 = sin(a2); sin3 = sin(a3); dircos[0][0] = (cos2*cos3); dircos[0][1] = -(cos2*sin3); dircos[0][2] = sin2; dircos[1][0] = ((cos1*sin3)+(sin1*(cos3*sin2))); dircos[1][1] = ((cos1*cos3)-(sin1*(sin2*sin3))); dircos[1][2] = -(cos2*sin1); dircos[2][0] = ((sin1*sin3)-(cos1*(cos3*sin2))); dircos[2][1] = ((cos1*(sin2*sin3))+(cos3*sin1)); dircos[2][2] = (cos1*cos2); } void BOCfly :: sdquat2dc ( double ie1, double ie2, double ie3, double ie4, double dircos[3][3] ) { double e1,e2,e3,e4,e11,e22,e33,e44,norm; e11 = ie1*ie1; e22 = ie2*ie2; e33 = ie3*ie3; e44 = ie4*ie4; norm = sqrt(e11+e22+e33+e44); if (norm == 0.) { e4 = 1.; norm = 1.; } else { e4 = ie4; } norm = 1./norm; e1 = ie1*norm; e2 = ie2*norm; e3 = ie3*norm; e4 = e4*norm; e11 = e1*e1; e22 = e2*e2; e33 = e3*e3; dircos[0][0] = 1.-(2.*(e22+e33)); dircos[0][1] = 2.*(e1*e2-e3*e4); dircos[0][2] = 2.*(e1*e3+e2*e4); dircos[1][0] = 2.*(e1*e2+e3*e4); dircos[1][1] = 1.-(2.*(e11+e33)); dircos[1][2] = 2.*(e2*e3-e1*e4); dircos[2][0] = 2.*(e1*e3-e2*e4); dircos[2][1] = 2.*(e2*e3+e1*e4); dircos[2][2] = 1.-(2.*(e11+e22)); } double BOCfly :: sdvdot ( double ivec1[3], double ivec2[3] ) { double dprod; /* Compute the dot product of two vectors */ dprod = ivec1[0]*ivec2[0]+ivec1[1]*ivec2[1]+ivec1[2]*ivec2[2]; return dprod; } double BOCfly :: sdvnorm ( double ivec[3] ) { double norm; /* Compute the length (norm) of a vector */ norm = sqrt(sdvdot(ivec,ivec)); return norm; } void BOCfly :: sdvcopy ( double ivec[3], double ovec[3] ) { /* Copy vector ivec to vector ovec */ ovec[0] = ivec[0]; ovec[1] = ivec[1]; ovec[2] = ivec[2]; } void BOCfly :: sdvset ( double sclr1, double sclr2, double sclr3, double ovec[3] ) { /* Set ovec to [sclr1,sclr2,sclr3] */ ovec[0] = sclr1; ovec[1] = sclr2; ovec[2] = sclr3; } void BOCfly :: sdvadd ( double ivec1[3], double ivec2[3], double ovec[3] ) { /* Add two vectors */ ovec[0] = ivec1[0]+ivec2[0]; ovec[1] = ivec1[1]+ivec2[1]; ovec[2] = ivec1[2]+ivec2[2]; } void BOCfly :: sdvsub ( double ivec1[3], double ivec2[3], double ovec[3] ) { /* Subtract two vectors */ ovec[0] = ivec1[0]-ivec2[0]; ovec[1] = ivec1[1]-ivec2[1]; ovec[2] = ivec1[2]-ivec2[2]; } void BOCfly :: sdvmul ( double sclr, double ivec[3], double ovec[3] ) { /* Multiply a vector by a scalar */ ovec[0] = sclr*ivec[0]; ovec[1] = sclr*ivec[1]; ovec[2] = sclr*ivec[2]; } void BOCfly :: sdvaxpy ( double sclr, double ivec1[3], double ivec2[3], double ovec[3] ) { /* Multiply a vector by a scalar and add in another vector */ ovec[0] = sclr*ivec1[0]+ivec2[0]; ovec[1] = sclr*ivec1[1]+ivec2[1]; ovec[2] = sclr*ivec1[2]+ivec2[2]; } void BOCfly :: sdvcross ( double ivec1[3], double ivec2[3], double ovec[3] ) { double tempout[3]; /* Compute the cross product of two vectors */ tempout[0] = ivec1[1]*ivec2[2]-ivec1[2]*ivec2[1]; tempout[1] = ivec1[2]*ivec2[0]-ivec1[0]*ivec2[2]; tempout[2] = ivec1[0]*ivec2[1]-ivec1[1]*ivec2[0]; ovec[0] = tempout[0]; ovec[1] = tempout[1]; ovec[2] = tempout[2]; } void BOCfly :: 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 BOCfly :: sdprerrmsg ( FILE *fnum, int routine, int errnum ) { if (errnum == 0) { return; } fprintf(fnum,"\n"); if (routine == 1) { fprintf(fnum,"sdgrav:\n"); } if (routine == 2) { fprintf(fnum,"sdmass:\n"); } if (routine == 3) { fprintf(fnum,"sdiner:\n"); } if (routine == 4) { fprintf(fnum,"sdbtj:\n"); } if (routine == 5) { fprintf(fnum,"sditj:\n"); } if (routine == 6) { fprintf(fnum,"sdpin:\n"); } if (routine == 7) { fprintf(fnum,"sdinit:\n"); } if (routine == 8) { fprintf(fnum,"sdstate:\n"); } if (routine == 9) { fprintf(fnum,"sdpsstate:\n"); } if (routine == 13) { fprintf(fnum,"sdpresacc:\n"); } if (routine == 14) { fprintf(fnum,"sdpresvel:\n"); } if (routine == 15) { fprintf(fnum,"sdprespos:\n"); } if (routine == 10) { fprintf(fnum,"sdhinget:\n"); } if (routine == 11) { fprintf(fnum,"sdpointf:\n"); } if (routine == 12) { fprintf(fnum,"sdbodyt:\n"); } if (routine == 17) { fprintf(fnum,"sdderiv:\n"); } if (routine == 16) { fprintf(fnum,"sdresid:\n"); } if (routine == 18) { fprintf(fnum,"sdpseudo:\n"); } if (routine == 19) { fprintf(fnum,"sdmom:\n"); } if (routine == 20) { fprintf(fnum,"sdsys:\n"); } if (routine == 21) { fprintf(fnum,"sdpos:\n"); } if (routine == 22) { fprintf(fnum,"sdvel:\n"); } if (routine == 23) { fprintf(fnum,"sdorient:\n"); } if (routine == 24) { fprintf(fnum,"sdangvel:\n"); } if (routine == 25) { fprintf(fnum,"sdtrans:\n"); } if (routine == 26) { fprintf(fnum,"sdperr:\n"); } if (routine == 27) { fprintf(fnum,"sdverr:\n"); } if (routine == 28) { fprintf(fnum,"sdpsqdot:\n"); } if (routine == 29) { fprintf(fnum,"sdpsudot:\n"); } if (routine == 30) { fprintf(fnum,"sdgetht:\n"); } if (routine == 31) { fprintf(fnum,"sdreac:\n"); } if (routine == 32) { fprintf(fnum,"sdacc:\n"); } if (routine == 33) { fprintf(fnum,"sdangacc:\n"); } if (routine == 34) { fprintf(fnum,"sdmult:\n"); } if (routine == 35) { fprintf(fnum,"sdaerr:\n"); } if (routine == 36) { fprintf(fnum,"sdindx:\n"); } if (routine == 37) { fprintf(fnum,"sdpres:\n"); } if (routine == 38) { fprintf(fnum,"sdstab:\n"); } if (routine == 39) { fprintf(fnum,"sdgetgrav:\n"); } if (routine == 40) { fprintf(fnum,"sdgetmass:\n"); } if (routine == 41) { fprintf(fnum,"sdgetiner:\n"); } if (routine == 42) { fprintf(fnum,"sdgetbtj:\n"); } if (routine == 43) { fprintf(fnum,"sdgetitj:\n"); } if (routine == 44) { fprintf(fnum,"sdgetpin:\n"); } if (routine == 45) { fprintf(fnum,"sdgetpres:\n"); } if (routine == 46) { fprintf(fnum,"sdgetstab:\n"); } if (routine == 47) { fprintf(fnum,"sdinfo:\n"); } if (routine == 48) { fprintf(fnum,"sdjnt:\n"); } if (routine == 49) { fprintf(fnum,"sdcons:\n"); } if (routine == 50) { fprintf(fnum,"sdassemble:\n"); } if (routine == 51) { fprintf(fnum,"sdinitvel:\n"); } if (routine == 52) { fprintf(fnum,"sdstatic:\n"); } if (routine == 53) { fprintf(fnum,"sdsteady:\n"); } if (routine == 54) { fprintf(fnum,"sdmotion:\n"); } if (routine == 55) { fprintf(fnum,"sdfmotion:\n"); } if (routine == 56) { fprintf(fnum,"sdequivht:\n"); } if (routine == 57) { fprintf(fnum,"sdmassmat:\n"); } if (routine == 58) { fprintf(fnum,"sdfrcmat:\n"); } if (routine == 59) { fprintf(fnum,"sdrel2cart:\n"); } if (routine == 60) { fprintf(fnum,"sdcomptrq:\n"); } if (routine == 61) { fprintf(fnum,"sdfulltrq:\n"); } if (routine == 62) { fprintf(fnum,"sdvrot:\n"); } if (errnum == 1) { fprintf(fnum,"a tree joint pin axis was zero\n"); } if (errnum == 2) { fprintf(fnum,"the 1st inboard pin for a loop joint was zero\n"); } if (errnum == 3) { fprintf(fnum,"the 2nd inboard pin for a loop joint was zero\n"); } if (errnum == 4) { fprintf(fnum,"the 3rd inboard pin for a loop joint was zero\n"); } if (errnum == 5) { fprintf(fnum,"an inboard reference line was zero\n"); } if (errnum == 6) { fprintf(fnum,"a set of loop joint axes was not right handed\n"); } if (errnum == 7) { fprintf(fnum,"a loop joint bodypin was zero\n"); } if (errnum == 8) { fprintf(fnum,"a loop joint body reference line was zero\n"); } if (errnum == 9) { fprintf(fnum,"1st/2nd pins in a loop joint not perpendicular\n"); } if (errnum == 10) { fprintf(fnum,"2nd/3rd pins in a loop joint not perpendicular\n"); } if (errnum == 11) { fprintf(fnum,"1st/3rd pins in a loop joint not perpendicular\n"); } if (errnum == 12) { fprintf(fnum,"a loop jt pin and inbref were not perpendicular\n"); } if (errnum == 13) { fprintf(fnum,"a bodypin and bodyref were not perpendicular\n"); } if (errnum == 14) { fprintf(fnum,"Euler parameters were far from normalized\n"); } if (errnum == 15) { fprintf(fnum,"illegal body number\n"); } if (errnum == 16) { fprintf(fnum,"illegal joint number\n"); } if (errnum == 17) { fprintf(fnum,"illegal axis number\n"); } if (errnum == 18) { fprintf(fnum,"illegal axis number for this joint\n"); } if (errnum == 19) { fprintf(fnum,"tried to set non-variable (i.e., non-?) parameter\n"); } if (errnum == 20) { fprintf(fnum,"prescribed motion was neither 0 (off) or 1 (on)\n"); } if (errnum == 21) { fprintf(fnum,"illegal user constraint number\n"); } if (errnum == 22) { fprintf(fnum,"sdinit must be called first\n"); } if (errnum == 23) { fprintf(fnum,"sdstate must be called first\n"); } if (errnum == 24) { fprintf(fnum,"sdderiv must be called first\n"); } if (errnum == 25) { fprintf(fnum,"a gravity ? parameter is unspecified\n"); } if (errnum == 26) { fprintf(fnum,"a ? mass is unspecified\n"); } if (errnum == 27) { fprintf(fnum,"a ? inertia is unspecified\n"); } if (errnum == 28) { fprintf(fnum,"a ? tree jt pin is unspecified\n"); } if (errnum == 29) { fprintf(fnum,"a ? tree bodyToJoint vector is unspecified\n"); } if (errnum == 30) { fprintf(fnum,"a ? tree inbToJoint vector is unspecified\n"); } if (errnum == 31) { fprintf(fnum,"a ? prescribed tree jt axis is unspecified\n"); } if (errnum == 32) { fprintf(fnum,"the stabvel ? parameter is unspecified\n"); } if (errnum == 33) { fprintf(fnum,"the stabpos ? parameter is unspecified\n"); } if (errnum == 34) { fprintf(fnum,"a ? loop jt inboard pin is unspecified\n"); } if (errnum == 35) { fprintf(fnum,"a ? loop jt inbref is unspecified\n"); } if (errnum == 36) { fprintf(fnum,"a ? loop jt bodypin is unspecified\n"); } if (errnum == 37) { fprintf(fnum,"a ? loop jt bodyref is unspecified\n"); } if (errnum == 38) { fprintf(fnum,"a ? loop jt bodyToJoint vector is unspecified\n"); } if (errnum == 39) { fprintf(fnum,"a ? loop jt inbToJoint vector is unspecified\n"); } if (errnum == 40) { fprintf(fnum,"a ? prescribed loop jt axis is unspecified\n"); } if (errnum == 41) { fprintf(fnum,"Dynamics & Library File serial nos. differ\n"); } if (errnum == 42) { fprintf(fnum,"Dynamics & Analysis File gen. times differ\n"); } if (errnum == 43) { fprintf(fnum,"A tree gimbal joint is in gimbal lock\n"); } if (errnum == 44) { fprintf(fnum,"A loop gimbal joint is in gimbal lock\n"); } if (errnum == 45) { fprintf(fnum,"Bad relative coordinate number\n"); } if (errnum == 46) { fprintf(fnum,"The vector about which to rotate was zero\n"); } if (errnum == 47) { fprintf(fnum,"Singular mass matrix - bad inertialess body?\n"); } } void BOCfly :: sdserialno ( int *serno ) { *serno = 70405; } // END source: flylib.c : //============================================== //============================================== //============================================== //============================================== // BEGIN source: fly_dyn.c : /* Generated 27-Mar-2000 00:58:53 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 69086664 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 ROADMAP (sd.dat) Bodies Inb No Name body Joint type Coords q --- --------- ---- ----------- ---------------- -1 $ground 0 TORSO -1 Sixdof 0 1 2 ... 3 4 5 23 1 ROPEL_U -1 Pin 6 2 ROPER_U -1 Pin 7 3 HEAD 0 Pin 8 4 ULEGR 0 Pin 9 5 ULEGL 0 Pin 10 6 LLEGR 4 Pin 11 7 LLEGL 5 Pin 12 8 FOOTR 6 Pin 13 9 FOOTL 7 Pin 14 10 UARMR 0 Pin 15 11 UARML 0 Pin 16 12 LARMR 10 Pin 17 13 LARML 11 Pin 18 14 HANDR 12 Pin 19 15 HANDL 13 Pin 20 16 ROPEL_L 1 Pin 21 17 ROPER_L 2 Pin 22 */ #define ground (topo_.ground_) #define nbod (topo_.nbod_) #define ndof (topo_.ndof_) #define ncons (topo_.ncons_) #define nloop (topo_.nloop_) #define nldof (topo_.nldof_) #define nloopc (topo_.nloopc_) #define nball (topo_.nball_) #define nlball (topo_.nlball_) #define npres (topo_.npres_) #define nuser (topo_.nuser_) #define jtype (topo_.jtype_) #define inb (topo_.inb_) #define outb (topo_.outb_) #define njntdof (topo_.njntdof_) #define njntc (topo_.njntc_) #define njntp (topo_.njntp_) #define firstq (topo_.firstq_) #define ballq (topo_.ballq_) #define firstm (topo_.firstm_) #define firstp (topo_.firstp_) #define trans (topo_.trans_) #define grav (input_.grav_) #define mk (input_.mk_) #define ik (input_.ik_) #define pin (input_.pin_) #define rk (input_.rk_) #define ri (input_.ri_) #define pres (input_.pres_) #define stabvel (input_.stabvel_) #define stabpos (input_.stabpos_) #define rhead (input_.rhead_) #define rcom (input_.rcom_) #define psrcomg (input_.psrcomg_) #define psrcom (input_.psrcom_) #define mkrcomt (input_.mkrcomt_) #define psmk (input_.psmk_) #define psik (input_.psik_) #define psrk (input_.psrk_) #define psri (input_.psri_) #define psmkg (input_.psmkg_) #define psikg (input_.psikg_) #define psrkg (input_.psrkg_) #define psrig (input_.psrig_) #define mtot (input_.mtot_) #define mfrcflg (input_.mfrcflg_) #define roustate (input_.roustate_) #define vpkflg (input_.vpkflg_) #define inerflg (input_.inerflg_) #define mmflg (input_.mmflg_) #define mmlduflg (input_.mmlduflg_) #define wwflg (input_.wwflg_) #define ltauflg (input_.ltauflg_) #define fs0flg (input_.fs0flg_) #define ii (input_.ii_) #define mmap (input_.mmap_) #define gravq (input_.gravq_) #define mkq (input_.mkq_) #define ikq (input_.ikq_) #define pinq (input_.pinq_) #define rkq (input_.rkq_) #define riq (input_.riq_) #define presq (input_.presq_) #define stabvelq (input_.stabvelq_) #define stabposq (input_.stabposq_) #define curtim (state_.curtim_) #define q (state_.q_) #define qn (state_.qn_) #define u (state_.u_) #define cnk (state_.cnk_) #define cnb (state_.cnb_) #define rnkg (state_.rnkg_) #define rnk (state_.rnk_) #define rnb (state_.rnb_) #define vnk (state_.vnk_) #define vnb (state_.vnb_) #define wk (state_.wk_) #define wb (state_.wb_) #define com (state_.com_) #define Cik (state_.Cik_) #define Cib (state_.Cib_) #define rikt (state_.rikt_) #define Iko (state_.Iko_) #define mkrk (state_.mkrk_) #define Wkk (state_.Wkk_) #define Vkk (state_.Vkk_) #define dik (state_.dik_) #define rpp (state_.rpp_) #define rpk (state_.rpk_) #define rik (state_.rik_) #define rik2 (state_.rik2_) #define rpri (state_.rpri_) #define Wik (state_.Wik_) #define Vik (state_.Vik_) #define Wirk (state_.Wirk_) #define rkWkk (state_.rkWkk_) #define Wkrpk (state_.Wkrpk_) #define VikWkr (state_.VikWkr_) #define wbrcom (state_.wbrcom_) #define perr (state_.perr_) #define verr (state_.verr_) #define aerr (state_.aerr_) #define mult (state_.mult_) #define ufk (state_.ufk_) #define utk (state_.utk_) #define utau (state_.utau_) #define mfk (state_.mfk_) #define mtk (state_.mtk_) #define mtau (state_.mtau_) #define uacc (state_.uacc_) #define uvel (state_.uvel_) #define upos (state_.upos_) #define s6 (state_.s6_) #define c6 (state_.c6_) #define s7 (state_.s7_) #define c7 (state_.c7_) #define s8 (state_.s8_) #define c8 (state_.c8_) #define s9 (state_.s9_) #define c9 (state_.c9_) #define s10 (state_.s10_) #define c10 (state_.c10_) #define s11 (state_.s11_) #define c11 (state_.c11_) #define s12 (state_.s12_) #define c12 (state_.c12_) #define s13 (state_.s13_) #define c13 (state_.c13_) #define s14 (state_.s14_) #define c14 (state_.c14_) #define s15 (state_.s15_) #define c15 (state_.c15_) #define s16 (state_.s16_) #define c16 (state_.c16_) #define s17 (state_.s17_) #define c17 (state_.c17_) #define s18 (state_.s18_) #define c18 (state_.c18_) #define s19 (state_.s19_) #define c19 (state_.c19_) #define s20 (state_.s20_) #define c20 (state_.c20_) #define s21 (state_.s21_) #define c21 (state_.c21_) #define s22 (state_.s22_) #define c22 (state_.c22_) #define qdot (lhs_.qdot_) #define Otk (lhs_.Otk_) #define Atk (lhs_.Atk_) #define AiOiWi (lhs_.AiOiWi_) #define Fstar (lhs_.Fstar_) #define Tstar (lhs_.Tstar_) #define fs0 (lhs_.fs0_) #define Fstark (lhs_.Fstark_) #define Tstark (lhs_.Tstark_) #define IkWk (lhs_.IkWk_) #define IkbWk (lhs_.IkbWk_) #define WkIkWk (lhs_.WkIkWk_) #define WkIkbWk (lhs_.WkIkbWk_) #define gk (lhs_.gk_) #define w0w0 (lhs_.w0w0_) #define w1w1 (lhs_.w1w1_) #define w2w2 (lhs_.w2w2_) #define w0w1 (lhs_.w0w1_) #define w0w2 (lhs_.w0w2_) #define w1w2 (lhs_.w1w2_) #define w00w11 (lhs_.w00w11_) #define w00w22 (lhs_.w00w22_) #define w11w22 (lhs_.w11w22_) #define ww (lhs_.ww_) #define qraux (lhs_.qraux_) #define PH1 (lhs_.PH1_) #define PH2 (lhs_.PH2_) #define P11 (lhs_.P11_) #define Pd (lhs_.Pd_) #define P22 (lhs_.P22_) #define L11 (lhs_.L11_) #define L21 (lhs_.L21_) #define L22 (lhs_.L22_) #define D11 (lhs_.D11_) #define D22 (lhs_.D22_) #define N11 (lhs_.N11_) #define N21 (lhs_.N21_) #define N22 (lhs_.N22_) #define HL1 (lhs_.HL1_) #define HL2 (lhs_.HL2_) #define psi11 (lhs_.psi11_) #define psi12 (lhs_.psi12_) #define psi21 (lhs_.psi21_) #define psi22 (lhs_.psi22_) #define psiD11 (lhs_.psiD11_) #define psiD12 (lhs_.psiD12_) #define psiD21 (lhs_.psiD21_) #define psiD22 (lhs_.psiD22_) #define sL11 (lhs_.sL11_) #define sL21 (lhs_.sL21_) #define sL22 (lhs_.sL22_) #define sD1 (lhs_.sD1_) #define sD2 (lhs_.sD2_) #define sD1INV (lhs_.sD1INV_) #define sD2INV (lhs_.sD2INV_) #define sL11D1 (lhs_.sL11D1_) #define sL22D2 (lhs_.sL22D2_) #define sD1L21 (lhs_.sD1L21_) #define DD (lhs_.DD_) #define G1 (lhs_.G1_) #define G2 (lhs_.G2_) #define ping (lhs_.ping_) #define hngpt (lhs_.hngpt_) #define wmap (lhs_.wmap_) #define multmap (lhs_.multmap_) #define jpvt (lhs_.jpvt_) #define wsiz (lhs_.wsiz_) #define wrank (lhs_.wrank_) #define fs (rhs_.fs_) #define udot (rhs_.udot_) #define ank (rhs_.ank_) #define anb (rhs_.anb_) #define onk (rhs_.onk_) #define onb (rhs_.onb_) #define Onkb (rhs_.Onkb_) #define AOnkri (rhs_.AOnkri_) #define Ankb (rhs_.Ankb_) #define AnkAtk (rhs_.AnkAtk_) #define dyrcom (rhs_.dyrcom_) #define ffk (rhs_.ffk_) #define ttk (rhs_.ttk_) #define fccikt (rhs_.fccikt_) #define ffkb (rhs_.ffkb_) #define ttkb (rhs_.ttkb_) #define dyad (rhs_.dyad_) #define fc (rhs_.fc_) #define tc (rhs_.tc_) #define tauc (rhs_.tauc_) #define temp (temp_.temp_) #define tmat1 (temp_.tmat1_) #define tmat2 (temp_.tmat2_) #define tvec1 (temp_.tvec1_) #define tvec2 (temp_.tvec2_) #define tvec3 (temp_.tvec3_) #define tvec4 (temp_.tvec4_) #define tvec5 (temp_.tvec5_) #define tsc1 (temp_.tsc1_) #define tsc2 (temp_.tsc2_) #define tsc3 (temp_.tsc3_) static const BOCfly::topo_t BOCflytopoCONST = { /* Topological information */ /* ground */ 0, /* nbod */ 18, /* ndof */ 23, /* ncons */ 0, /* nloop */ 0, /* nldof */ 0, /* nloopc */ 0, /* nball */ 1, /* nlball */ 0, /* npres */ 0, /* nuser */ 0, /* jtype[0] */ 6, /* jtype[1] */ 1, /* jtype[2] */ 1, /* jtype[3] */ 1, /* jtype[4] */ 1, /* jtype[5] */ 1, /* jtype[6] */ 1, /* jtype[7] */ 1, /* jtype[8] */ 1, /* jtype[9] */ 1, /* jtype[10] */ 1, /* jtype[11] */ 1, /* jtype[12] */ 1, /* jtype[13] */ 1, /* jtype[14] */ 1, /* jtype[15] */ 1, /* jtype[16] */ 1, /* jtype[17] */ 1, /* inb[0] */ -1, /* inb[1] */ -1, /* inb[2] */ -1, /* inb[3] */ 0, /* inb[4] */ 0, /* inb[5] */ 0, /* inb[6] */ 4, /* inb[7] */ 5, /* inb[8] */ 6, /* inb[9] */ 7, /* inb[10] */ 0, /* inb[11] */ 0, /* inb[12] */ 10, /* inb[13] */ 11, /* inb[14] */ 12, /* inb[15] */ 13, /* inb[16] */ 1, /* inb[17] */ 2, /* outb[0] */ 0, /* outb[1] */ 1, /* outb[2] */ 2, /* outb[3] */ 3, /* outb[4] */ 4, /* outb[5] */ 5, /* outb[6] */ 6, /* outb[7] */ 7, /* outb[8] */ 8, /* outb[9] */ 9, /* outb[10] */ 10, /* outb[11] */ 11, /* outb[12] */ 12, /* outb[13] */ 13, /* outb[14] */ 14, /* outb[15] */ 15, /* outb[16] */ 16, /* outb[17] */ 17, /* njntdof[0] */ 6, /* njntdof[1] */ 1, /* njntdof[2] */ 1, /* njntdof[3] */ 1, /* njntdof[4] */ 1, /* njntdof[5] */ 1, /* njntdof[6] */ 1, /* njntdof[7] */ 1, /* njntdof[8] */ 1, /* njntdof[9] */ 1, /* njntdof[10] */ 1, /* njntdof[11] */ 1, /* njntdof[12] */ 1, /* njntdof[13] */ 1, /* njntdof[14] */ 1, /* njntdof[15] */ 1, /* njntdof[16] */ 1, /* njntdof[17] */ 1, /* njntc[0] */ 0, /* njntc[1] */ 0, /* njntc[2] */ 0, /* njntc[3] */ 0, /* njntc[4] */ 0, /* njntc[5] */ 0, /* njntc[6] */ 0, /* njntc[7] */ 0, /* njntc[8] */ 0, /* njntc[9] */ 0, /* njntc[10] */ 0, /* njntc[11] */ 0, /* njntc[12] */ 0, /* njntc[13] */ 0, /* njntc[14] */ 0, /* njntc[15] */ 0, /* njntc[16] */ 0, /* njntc[17] */ 0, /* njntp[0] */ 0, /* njntp[1] */ 0, /* njntp[2] */ 0, /* njntp[3] */ 0, /* njntp[4] */ 0, /* njntp[5] */ 0, /* njntp[6] */ 0, /* njntp[7] */ 0, /* njntp[8] */ 0, /* njntp[9] */ 0, /* njntp[10] */ 0, /* njntp[11] */ 0, /* njntp[12] */ 0, /* njntp[13] */ 0, /* njntp[14] */ 0, /* njntp[15] */ 0, /* njntp[16] */ 0, /* njntp[17] */ 0, /* firstq[0] */ 0, /* firstq[1] */ 6, /* firstq[2] */ 7, /* firstq[3] */ 8, /* firstq[4] */ 9, /* firstq[5] */ 10, /* firstq[6] */ 11, /* firstq[7] */ 12, /* firstq[8] */ 13, /* firstq[9] */ 14, /* firstq[10] */ 15, /* firstq[11] */ 16, /* firstq[12] */ 17, /* firstq[13] */ 18, /* firstq[14] */ 19, /* firstq[15] */ 20, /* firstq[16] */ 21, /* firstq[17] */ 22, /* ballq[0] */ 23, /* ballq[1] */ -104, /* ballq[2] */ -104, /* ballq[3] */ -104, /* ballq[4] */ -104, /* ballq[5] */ -104, /* ballq[6] */ -104, /* ballq[7] */ -104, /* ballq[8] */ -104, /* ballq[9] */ -104, /* ballq[10] */ -104, /* ballq[11] */ -104, /* ballq[12] */ -104, /* ballq[13] */ -104, /* ballq[14] */ -104, /* ballq[15] */ -104, /* ballq[16] */ -104, /* ballq[17] */ -104, /* firstm[0] */ -1, /* firstm[1] */ -1, /* firstm[2] */ -1, /* firstm[3] */ -1, /* firstm[4] */ -1, /* firstm[5] */ -1, /* firstm[6] */ -1, /* firstm[7] */ -1, /* firstm[8] */ -1, /* firstm[9] */ -1, /* firstm[10] */ -1, /* firstm[11] */ -1, /* firstm[12] */ -1, /* firstm[13] */ -1, /* firstm[14] */ -1, /* firstm[15] */ -1, /* firstm[16] */ -1, /* firstm[17] */ -1, /* firstp[0] */ -1, /* firstp[1] */ -1, /* firstp[2] */ -1, /* firstp[3] */ -1, /* firstp[4] */ -1, /* firstp[5] */ -1, /* firstp[6] */ -1, /* firstp[7] */ -1, /* firstp[8] */ -1, /* firstp[9] */ -1, /* firstp[10] */ -1, /* firstp[11] */ -1, /* firstp[12] */ -1, /* firstp[13] */ -1, /* firstp[14] */ -1, /* firstp[15] */ -1, /* firstp[16] */ -1, /* firstp[17] */ -1, /* trans[0] */ 1, /* trans[1] */ 1, /* trans[2] */ 1, /* trans[3] */ 0, /* trans[4] */ 0, /* trans[5] */ 0, /* trans[6] */ 0, /* trans[7] */ 0, /* trans[8] */ 0, /* trans[9] */ 0, /* trans[10] */ 0, /* trans[11] */ 0, /* trans[12] */ 0, /* trans[13] */ 0, /* trans[14] */ 0, /* trans[15] */ 0, /* trans[16] */ 0, /* trans[17] */ 0, /* trans[18] */ 0, /* trans[19] */ 0, /* trans[20] */ 0, /* trans[21] */ 0, /* trans[22] */ 0, }; static const BOCfly::input_t BOCflyinputCONST = { /* Model parameters from the input file */ /* gravity */ /* grav[0] */ 0., /* grav[1] */ 0., /* grav[2] */ -9.80665, /* mass */ /* mk[0] */ 11.02968359, /* mk[1] */ .18166784, /* mk[2] */ .1816698, /* mk[3] */ 9.84983599, /* mk[4] */ 2.89586298, /* mk[5] */ 2.8984419, /* mk[6] */ 1.02133193, /* mk[7] */ 1.02007998, /* mk[8] */ .53380008, /* mk[9] */ .5325902, /* mk[10] */ 1.00985702, /* mk[11] */ 1.01701701, /* mk[12] */ .43362906, /* mk[13] */ .43358398, /* mk[14] */ .05946345, /* mk[15] */ .05944511, /* mk[16] */ .0260106, /* mk[17] */ .02601109, /* inertia */ /* ik[0][0][0] */ .48506516, /* ik[0][0][1] */ -5.312e-5, /* ik[0][0][2] */ -.01450199, /* ik[0][1][0] */ -5.312e-5, /* ik[0][1][1] */ .47268161, /* ik[0][1][2] */ -.00031516, /* ik[0][2][0] */ -.01450199, /* ik[0][2][1] */ -.00031516, /* ik[0][2][2] */ .03863225, /* ik[1][0][0] */ .04182592, /* ik[1][0][1] */ 8e-8, /* ik[1][0][2] */ -7.5e-7, /* ik[1][1][0] */ 8e-8, /* ik[1][1][1] */ .04205639, /* ik[1][1][2] */ -.00226939, /* ik[1][2][0] */ -7.5e-7, /* ik[1][2][1] */ -.00226939, /* ik[1][2][2] */ .00022475, /* ik[2][0][0] */ .04183501, /* ik[2][0][1] */ -8e-8, /* ik[2][0][2] */ -7.5e-7, /* ik[2][1][0] */ -8e-8, /* ik[2][1][1] */ .04207299, /* ik[2][1][2] */ .00230647, /* ik[2][2][0] */ -7.5e-7, /* ik[2][2][1] */ .00230647, /* ik[2][2][2] */ .00023224, /* ik[3][0][0] */ .52257967, /* ik[3][0][1] */ 1.657e-5, /* ik[3][0][2] */ .00823665, /* ik[3][1][0] */ 1.657e-5, /* ik[3][1][1] */ .52474672, /* ik[3][1][2] */ -.00036016, /* ik[3][2][0] */ .00823665, /* ik[3][2][1] */ -.00036016, /* ik[3][2][2] */ .01799022, /* ik[4][0][0] */ .05459333, /* ik[4][0][1] */ -5.918e-5, /* ik[4][0][2] */ .00223918, /* ik[4][1][0] */ -5.918e-5, /* ik[4][1][1] */ .05477009, /* ik[4][1][2] */ .00147758, /* ik[4][2][0] */ .00223918, /* ik[4][2][1] */ .00147758, /* ik[4][2][2] */ .00343412, /* ik[5][0][0] */ .05476817, /* ik[5][0][1] */ 6.279e-5, /* ik[5][0][2] */ .00224427, /* ik[5][1][0] */ 6.279e-5, /* ik[5][1][1] */ .05493593, /* ik[5][1][2] */ -.00156061, /* ik[5][2][0] */ .00224427, /* ik[5][2][1] */ -.00156061, /* ik[5][2][2] */ .00344556, /* ik[6][0][0] */ .00826684, /* ik[6][0][1] */ 1.273e-5, /* ik[6][0][2] */ -.00026397, /* ik[6][1][0] */ 1.273e-5, /* ik[6][1][1] */ .00843352, /* ik[6][1][2] */ .00020104, /* ik[6][2][0] */ -.00026397, /* ik[6][2][1] */ .00020104, /* ik[6][2][2] */ .00057599, /* ik[7][0][0] */ .00821833, /* ik[7][0][1] */ -1.283e-5, /* ik[7][0][2] */ -.00026918, /* ik[7][1][0] */ -1.283e-5, /* ik[7][1][1] */ .00838445, /* ik[7][1][2] */ -.00019836, /* ik[7][2][0] */ -.00026918, /* ik[7][2][1] */ -.00019836, /* ik[7][2][2] */ .00057568, /* ik[8][0][0] */ .00031494, /* ik[8][0][1] */ -1.886e-5, /* ik[8][0][2] */ .00016083, /* ik[8][1][0] */ -1.886e-5, /* ik[8][1][1] */ .00112921, /* ik[8][1][2] */ 4.29e-6, /* ik[8][2][0] */ .00016083, /* ik[8][2][1] */ 4.29e-6, /* ik[8][2][2] */ .00103272, /* ik[9][0][0] */ .00031249, /* ik[9][0][1] */ 1.71e-5, /* ik[9][0][2] */ .00016063, /* ik[9][1][0] */ 1.71e-5, /* ik[9][1][1] */ .00112646, /* ik[9][1][2] */ -4.13e-6, /* ik[9][2][0] */ .00016063, /* ik[9][2][1] */ -4.13e-6, /* ik[9][2][2] */ .00102891, /* ik[10][0][0] */ .03667794, /* ik[10][0][1] */ -9.282e-5, /* ik[10][0][2] */ .00098554, /* ik[10][1][0] */ -9.282e-5, /* ik[10][1][1] */ .03636003, /* ik[10][1][2] */ .00236551, /* ik[10][2][0] */ .00098554, /* ik[10][2][1] */ .00236551, /* ik[10][2][2] */ .00072894, /* ik[11][0][0] */ .03712787, /* ik[11][0][1] */ 9.583e-5, /* ik[11][0][2] */ .00099076, /* ik[11][1][0] */ 9.583e-5, /* ik[11][1][1] */ .03679631, /* ik[11][1][2] */ -.00244367, /* ik[11][2][0] */ .00099076, /* ik[11][2][1] */ -.00244367, /* ik[11][2][2] */ .00074417, /* ik[12][0][0] */ .00933952, /* ik[12][0][1] */ -2.97e-5, /* ik[12][0][2] */ .00047536, /* ik[12][1][0] */ -2.97e-5, /* ik[12][1][1] */ .00915719, /* ik[12][1][2] */ .00105064, /* ik[12][2][0] */ .00047536, /* ik[12][2][1] */ .00105064, /* ik[12][2][2] */ .00032059, /* ik[13][0][0] */ .00933861, /* ik[13][0][1] */ 3.024e-5, /* ik[13][0][2] */ .00047517, /* ik[13][1][0] */ 3.024e-5, /* ik[13][1][1] */ .00915219, /* ik[13][1][2] */ -.00106364, /* ik[13][2][0] */ .00047517, /* ik[13][2][1] */ -.00106364, /* ik[13][2][2] */ .00032466, /* ik[14][0][0] */ .00086458, /* ik[14][0][1] */ -2.761e-5, /* ik[14][0][2] */ 7.331e-5, /* ik[14][1][0] */ -2.761e-5, /* ik[14][1][1] */ .00082117, /* ik[14][1][2] */ -.00020674, /* ik[14][2][0] */ 7.331e-5, /* ik[14][2][1] */ -.00020674, /* ik[14][2][2] */ 3.059e-5, /* ik[15][0][0] */ .00086575, /* ik[15][0][1] */ 2.77e-5, /* ik[15][0][2] */ 7.338e-5, /* ik[15][1][0] */ 2.77e-5, /* ik[15][1][1] */ .00082196, /* ik[15][1][2] */ .00020754, /* ik[15][2][0] */ 7.338e-5, /* ik[15][2][1] */ .00020754, /* ik[15][2][2] */ 3.093e-5, /* ik[16][0][0] */ .00046692, /* ik[16][0][1] */ 1e-8, /* ik[16][0][2] */ -4e-8, /* ik[16][1][0] */ 1e-8, /* ik[16][1][1] */ .0004999, /* ik[16][1][2] */ -.00013462, /* ik[16][2][0] */ -4e-8, /* ik[16][2][1] */ -.00013462, /* ik[16][2][2] */ 3.217e-5, /* ik[17][0][0] */ .00046679, /* ik[17][0][1] */ -1e-8, /* ik[17][0][2] */ -4e-8, /* ik[17][1][0] */ -1e-8, /* ik[17][1][1] */ .00050087, /* ik[17][1][2] */ .00013693, /* ik[17][2][0] */ -4e-8, /* ik[17][2][1] */ .00013693, /* ik[17][2][2] */ 3.326e-5, /* tree hinge axis vectors */ /* pin[0][0] */ 1., /* pin[0][1] */ 0., /* pin[0][2] */ 0., /* pin[1][0] */ 0., /* pin[1][1] */ 1., /* pin[1][2] */ 0., /* pin[2][0] */ 0., /* pin[2][1] */ 0., /* pin[2][2] */ 1., /* pin[3][0] */ 0., /* pin[3][1] */ 0., /* pin[3][2] */ 0., /* pin[4][0] */ 0., /* pin[4][1] */ 0., /* pin[4][2] */ 0., /* pin[5][0] */ 0., /* pin[5][1] */ 0., /* pin[5][2] */ 0., /* pin[6][0] */ 0., /* pin[6][1] */ 1., /* pin[6][2] */ 0., /* pin[7][0] */ 0., /* pin[7][1] */ 1., /* pin[7][2] */ 0., /* pin[8][0] */ 0., /* pin[8][1] */ 1., /* pin[8][2] */ 0., /* pin[9][0] */ 0., /* pin[9][1] */ 1., /* pin[9][2] */ 0., /* pin[10][0] */ 0., /* pin[10][1] */ 1., /* pin[10][2] */ 0., /* pin[11][0] */ 0., /* pin[11][1] */ 1., /* pin[11][2] */ 0., /* pin[12][0] */ 0., /* pin[12][1] */ 1., /* pin[12][2] */ 0., /* pin[13][0] */ 0., /* pin[13][1] */ 1., /* pin[13][2] */ 0., /* pin[14][0] */ 0., /* pin[14][1] */ 1., /* pin[14][2] */ 0., /* pin[15][0] */ 0., /* pin[15][1] */ 1., /* pin[15][2] */ 0., /* pin[16][0] */ 0., /* pin[16][1] */ 1., /* pin[16][2] */ 0., /* pin[17][0] */ 0., /* pin[17][1] */ 1., /* pin[17][2] */ 0., /* pin[18][0] */ 0., /* pin[18][1] */ 1., /* pin[18][2] */ 0., /* pin[19][0] */ 0., /* pin[19][1] */ 1., /* pin[19][2] */ 0., /* pin[20][0] */ 0., /* pin[20][1] */ 1., /* pin[20][2] */ 0., /* pin[21][0] */ 0., /* pin[21][1] */ 1., /* pin[21][2] */ 0., /* pin[22][0] */ 0., /* pin[22][1] */ 1., /* pin[22][2] */ 0., /* tree bodytojoint vectors */ /* rk[0][0] */ 0., /* rk[0][1] */ 0., /* rk[0][2] */ 0., /* rk[1][0] */ -5.201e-5, /* rk[1][1] */ 3.3e-7, /* rk[1][2] */ 1.2242604, /* rk[2][0] */ -5.197e-5, /* rk[2][1] */ .00249756, /* rk[2][2] */ 1.22369868, /* rk[3][0] */ -.01691003, /* rk[3][1] */ -.0007183, /* rk[3][2] */ .12000071, /* rk[4][0] */ -.0094289, /* rk[4][1] */ .00015877, /* rk[4][2] */ .13168857, /* rk[5][0] */ -.00943639, /* rk[5][1] */ -.00235524, /* rk[5][2] */ .13190398, /* rk[6][0] */ .01672166, /* rk[6][1] */ -.00561135, /* rk[6][2] */ .12618299, /* rk[7][0] */ .01674373, /* rk[7][1] */ .00342896, /* rk[7][2] */ .12587504, /* rk[8][0] */ -.03543467, /* rk[8][1] */ -.00193095, /* rk[8][2] */ .03983908, /* rk[9][0] */ -.03539679, /* rk[9][1] */ -.00095499, /* rk[9][2] */ .03987912, /* rk[10][0] */ -.00307461, /* rk[10][1] */ .00885657, /* rk[10][2] */ .20905614, /* rk[11][0] */ -.00305957, /* rk[11][1] */ -.00995927, /* rk[11][2] */ .21008605, /* rk[12][0] */ -.01407264, /* rk[12][1] */ -.00055916, /* rk[12][2] */ .15249501, /* rk[13][0] */ -.01407902, /* rk[13][1] */ -.00074575, /* rk[13][2] */ .15260355, /* rk[14][0] */ -.01360932, /* rk[14][1] */ .00486049, /* rk[14][2] */ -.01387793, /* rk[15][0] */ -.01361601, /* rk[15][1] */ -.00560346, /* rk[15][2] */ -.01382852, /* rk[16][0] */ -5.2e-5, /* rk[16][1] */ 5.59e-6, /* rk[16][2] */ .28373605, /* rk[17][0] */ -5.194e-5, /* rk[17][1] */ .00250244, /* rk[17][2] */ .28317448, /* tree inbtojoint vectors */ /* ri[0][0] */ 0., /* ri[0][1] */ 0., /* ri[0][2] */ 0., /* ri[1][0] */ 0., /* ri[1][1] */ .156437, /* ri[1][2] */ 2.442, /* ri[2][0] */ 0., /* ri[2][1] */ -.156437, /* ri[2][2] */ 2.442, /* ri[3][0] */ -.00252618, /* ri[3][1] */ -.00120864, /* ri[3][2] */ .3829286, /* ri[4][0] */ -.01952618, /* ri[4][1] */ -.04820864, /* ri[4][2] */ -.0620714, /* ri[5][0] */ -.01952618, /* ri[5][1] */ .04579136, /* ri[5][2] */ -.0620714, /* ri[6][0] */ .0065711, /* ri[6][1] */ -.00284123, /* ri[6][2] */ -.15931143, /* ri[7][0] */ .00656361, /* ri[7][1] */ .00064476, /* ri[7][2] */ -.15909602, /* ri[8][0] */ .00172166, /* ri[8][1] */ -.00061135, /* ri[8][2] */ -.17081701, /* ri[9][0] */ .00174373, /* ri[9][1] */ -.00157104, /* ri[9][2] */ -.17112496, /* ri[10][0] */ -.01052618, /* ri[10][1] */ -.10120864, /* ri[10][2] */ .2639286, /* ri[11][0] */ -.01052618, /* ri[11][1] */ .09879136, /* ri[11][2] */ .2639286, /* ri[12][0] */ -.00307461, /* ri[12][1] */ -.02714343, /* ri[12][2] */ .01305614, /* ri[13][0] */ -.00305957, /* ri[13][1] */ .02604073, /* ri[13][2] */ .01408605, /* ri[14][0] */ .00792736, /* ri[14][1] */ -.02455916, /* ri[14][2] */ -.01750499, /* ri[15][0] */ .00792098, /* ri[15][1] */ .02325425, /* ri[15][2] */ -.01739645, /* ri[16][0] */ -5.201e-5, /* ri[16][1] */ 3.3e-7, /* ri[16][2] */ -.4177396, /* ri[17][0] */ -5.197e-5, /* ri[17][1] */ .00249756, /* ri[17][2] */ -.41830132, /* tree prescribed motion */ /* pres[0] */ 0., /* pres[1] */ 0., /* pres[2] */ 0., /* pres[3] */ 0., /* pres[4] */ 0., /* pres[5] */ 0., /* pres[6] */ 0., /* pres[7] */ 0., /* pres[8] */ 0., /* pres[9] */ 0., /* pres[10] */ 0., /* pres[11] */ 0., /* pres[12] */ 0., /* pres[13] */ 0., /* pres[14] */ 0., /* pres[15] */ 0., /* pres[16] */ 0., /* pres[17] */ 0., /* pres[18] */ 0., /* pres[19] */ 0., /* pres[20] */ 0., /* pres[21] */ 0., /* pres[22] */ 0., /* stabilization parameters */ /* stabvel */ 0., /* stabpos */ 0., /* miscellaneous */ /* mfrcflg */ 0, /* roustate */ 0, /* vpkflg */ 0, /* inerflg */ 0, /* mmflg */ 0, /* mmlduflg */ 0, /* wwflg */ 0, /* ltauflg */ 0, /* fs0flg */ 0, /* ii */ 0, /* mmap[0] */ 0, /* mmap[1] */ 1, /* mmap[2] */ 2, /* mmap[3] */ 3, /* mmap[4] */ 4, /* mmap[5] */ 5, /* mmap[6] */ 6, /* mmap[7] */ 7, /* mmap[8] */ 8, /* mmap[9] */ 9, /* mmap[10] */ 10, /* mmap[11] */ 11, /* mmap[12] */ 12, /* mmap[13] */ 13, /* mmap[14] */ 14, /* mmap[15] */ 15, /* mmap[16] */ 16, /* mmap[17] */ 17, /* mmap[18] */ 18, /* mmap[19] */ 19, /* mmap[20] */ 20, /* mmap[21] */ 21, /* mmap[22] */ 22, /* Which parameters were "?" (1) or "?" (3) */ /* gravq[0] */ 0, /* gravq[1] */ 0, /* gravq[2] */ 0, /* mkq[0] */ 0, /* mkq[1] */ 0, /* mkq[2] */ 0, /* mkq[3] */ 0, /* mkq[4] */ 0, /* mkq[5] */ 0, /* mkq[6] */ 0, /* mkq[7] */ 0, /* mkq[8] */ 0, /* mkq[9] */ 0, /* mkq[10] */ 0, /* mkq[11] */ 0, /* mkq[12] */ 0, /* mkq[13] */ 0, /* mkq[14] */ 0, /* mkq[15] */ 0, /* mkq[16] */ 0, /* mkq[17] */ 0, /* ikq[0][0][0] */ 0, /* ikq[0][0][1] */ 0, /* ikq[0][0][2] */ 0, /* ikq[0][1][0] */ 0, /* ikq[0][1][1] */ 0, /* ikq[0][1][2] */ 0, /* ikq[0][2][0] */ 0, /* ikq[0][2][1] */ 0, /* ikq[0][2][2] */ 0, /* ikq[1][0][0] */ 0, /* ikq[1][0][1] */ 0, /* ikq[1][0][2] */ 0, /* ikq[1][1][0] */ 0, /* ikq[1][1][1] */ 0, /* ikq[1][1][2] */ 0, /* ikq[1][2][0] */ 0, /* ikq[1][2][1] */ 0, /* ikq[1][2][2] */ 0, /* ikq[2][0][0] */ 0, /* ikq[2][0][1] */ 0, /* ikq[2][0][2] */ 0, /* ikq[2][1][0] */ 0, /* ikq[2][1][1] */ 0, /* ikq[2][1][2] */ 0, /* ikq[2][2][0] */ 0, /* ikq[2][2][1] */ 0, /* ikq[2][2][2] */ 0, /* ikq[3][0][0] */ 0, /* ikq[3][0][1] */ 0, /* ikq[3][0][2] */ 0, /* ikq[3][1][0] */ 0, /* ikq[3][1][1] */ 0, /* ikq[3][1][2] */ 0, /* ikq[3][2][0] */ 0, /* ikq[3][2][1] */ 0, /* ikq[3][2][2] */ 0, /* ikq[4][0][0] */ 0, /* ikq[4][0][1] */ 0, /* ikq[4][0][2] */ 0, /* ikq[4][1][0] */ 0, /* ikq[4][1][1] */ 0, /* ikq[4][1][2] */ 0, /* ikq[4][2][0] */ 0, /* ikq[4][2][1] */ 0, /* ikq[4][2][2] */ 0, /* ikq[5][0][0] */ 0, /* ikq[5][0][1] */ 0, /* ikq[5][0][2] */ 0, /* ikq[5][1][0] */ 0, /* ikq[5][1][1] */ 0, /* ikq[5][1][2] */ 0, /* ikq[5][2][0] */ 0, /* ikq[5][2][1] */ 0, /* ikq[5][2][2] */ 0, /* ikq[6][0][0] */ 0, /* ikq[6][0][1] */ 0, /* ikq[6][0][2] */ 0, /* ikq[6][1][0] */ 0, /* ikq[6][1][1] */ 0, /* ikq[6][1][2] */ 0, /* ikq[6][2][0] */ 0, /* ikq[6][2][1] */ 0, /* ikq[6][2][2] */ 0, /* ikq[7][0][0] */ 0, /* ikq[7][0][1] */ 0, /* ikq[7][0][2] */ 0, /* ikq[7][1][0] */ 0, /* ikq[7][1][1] */ 0, /* ikq[7][1][2] */ 0, /* ikq[7][2][0] */ 0, /* ikq[7][2][1] */ 0, /* ikq[7][2][2] */ 0, /* ikq[8][0][0] */ 0, /* ikq[8][0][1] */ 0, /* ikq[8][0][2] */ 0, /* ikq[8][1][0] */ 0, /* ikq[8][1][1] */ 0, /* ikq[8][1][2] */ 0, /* ikq[8][2][0] */ 0, /* ikq[8][2][1] */ 0, /* ikq[8][2][2] */ 0, /* ikq[9][0][0] */ 0, /* ikq[9][0][1] */ 0, /* ikq[9][0][2] */ 0, /* ikq[9][1][0] */ 0, /* ikq[9][1][1] */ 0, /* ikq[9][1][2] */ 0, /* ikq[9][2][0] */ 0, /* ikq[9][2][1] */ 0, /* ikq[9][2][2] */ 0, /* ikq[10][0][0] */ 0, /* ikq[10][0][1] */ 0, /* ikq[10][0][2] */ 0, /* ikq[10][1][0] */ 0, /* ikq[10][1][1] */ 0, /* ikq[10][1][2] */ 0, /* ikq[10][2][0] */ 0, /* ikq[10][2][1] */ 0, /* ikq[10][2][2] */ 0, /* ikq[11][0][0] */ 0, /* ikq[11][0][1] */ 0, /* ikq[11][0][2] */ 0, /* ikq[11][1][0] */ 0, /* ikq[11][1][1] */ 0, /* ikq[11][1][2] */ 0, /* ikq[11][2][0] */ 0, /* ikq[11][2][1] */ 0, /* ikq[11][2][2] */ 0, /* ikq[12][0][0] */ 0, /* ikq[12][0][1] */ 0, /* ikq[12][0][2] */ 0, /* ikq[12][1][0] */ 0, /* ikq[12][1][1] */ 0, /* ikq[12][1][2] */ 0, /* ikq[12][2][0] */ 0, /* ikq[12][2][1] */ 0, /* ikq[12][2][2] */ 0, /* ikq[13][0][0] */ 0, /* ikq[13][0][1] */ 0, /* ikq[13][0][2] */ 0, /* ikq[13][1][0] */ 0, /* ikq[13][1][1] */ 0, /* ikq[13][1][2] */ 0, /* ikq[13][2][0] */ 0, /* ikq[13][2][1] */ 0, /* ikq[13][2][2] */ 0, /* ikq[14][0][0] */ 0, /* ikq[14][0][1] */ 0, /* ikq[14][0][2] */ 0, /* ikq[14][1][0] */ 0, /* ikq[14][1][1] */ 0, /* ikq[14][1][2] */ 0, /* ikq[14][2][0] */ 0, /* ikq[14][2][1] */ 0, /* ikq[14][2][2] */ 0, /* ikq[15][0][0] */ 0, /* ikq[15][0][1] */ 0, /* ikq[15][0][2] */ 0, /* ikq[15][1][0] */ 0, /* ikq[15][1][1] */ 0, /* ikq[15][1][2] */ 0, /* ikq[15][2][0] */ 0, /* ikq[15][2][1] */ 0, /* ikq[15][2][2] */ 0, /* ikq[16][0][0] */ 0, /* ikq[16][0][1] */ 0, /* ikq[16][0][2] */ 0, /* ikq[16][1][0] */ 0, /* ikq[16][1][1] */ 0, /* ikq[16][1][2] */ 0, /* ikq[16][2][0] */ 0, /* ikq[16][2][1] */ 0, /* ikq[16][2][2] */ 0, /* ikq[17][0][0] */ 0, /* ikq[17][0][1] */ 0, /* ikq[17][0][2] */ 0, /* ikq[17][1][0] */ 0, /* ikq[17][1][1] */ 0, /* ikq[17][1][2] */ 0, /* ikq[17][2][0] */ 0, /* ikq[17][2][1] */ 0, /* ikq[17][2][2] */ 0, /* pinq[0][0] */ 0, /* pinq[0][1] */ 0, /* pinq[0][2] */ 0, /* pinq[1][0] */ 0, /* pinq[1][1] */ 0, /* pinq[1][2] */ 0, /* pinq[2][0] */ 0, /* pinq[2][1] */ 0, /* pinq[2][2] */ 0, /* pinq[3][0] */ 0, /* pinq[3][1] */ 0, /* pinq[3][2] */ 0, /* pinq[4][0] */ 0, /* pinq[4][1] */ 0, /* pinq[4][2] */ 0, /* pinq[5][0] */ 0, /* pinq[5][1] */ 0, /* pinq[5][2] */ 0, /* pinq[6][0] */ 0, /* pinq[6][1] */ 0, /* pinq[6][2] */ 0, /* pinq[7][0] */ 0, /* pinq[7][1] */ 0, /* pinq[7][2] */ 0, /* pinq[8][0] */ 0, /* pinq[8][1] */ 0, /* pinq[8][2] */ 0, /* pinq[9][0] */ 0, /* pinq[9][1] */ 0, /* pinq[9][2] */ 0, /* pinq[10][0] */ 0, /* pinq[10][1] */ 0, /* pinq[10][2] */ 0, /* pinq[11][0] */ 0, /* pinq[11][1] */ 0, /* pinq[11][2] */ 0, /* pinq[12][0] */ 0, /* pinq[12][1] */ 0, /* pinq[12][2] */ 0, /* pinq[13][0] */ 0, /* pinq[13][1] */ 0, /* pinq[13][2] */ 0, /* pinq[14][0] */ 0, /* pinq[14][1] */ 0, /* pinq[14][2] */ 0, /* pinq[15][0] */ 0, /* pinq[15][1] */ 0, /* pinq[15][2] */ 0, /* pinq[16][0] */ 0, /* pinq[16][1] */ 0, /* pinq[16][2] */ 0, /* pinq[17][0] */ 0, /* pinq[17][1] */ 0, /* pinq[17][2] */ 0, /* pinq[18][0] */ 0, /* pinq[18][1] */ 0, /* pinq[18][2] */ 0, /* pinq[19][0] */ 0, /* pinq[19][1] */ 0, /* pinq[19][2] */ 0, /* pinq[20][0] */ 0, /* pinq[20][1] */ 0, /* pinq[20][2] */ 0, /* pinq[21][0] */ 0, /* pinq[21][1] */ 0, /* pinq[21][2] */ 0, /* pinq[22][0] */ 0, /* pinq[22][1] */ 0, /* pinq[22][2] */ 0, /* rkq[0][0] */ 0, /* rkq[0][1] */ 0, /* rkq[0][2] */ 0, /* rkq[1][0] */ 0, /* rkq[1][1] */ 0, /* rkq[1][2] */ 0, /* rkq[2][0] */ 0, /* rkq[2][1] */ 0, /* rkq[2][2] */ 0, /* rkq[3][0] */ 0, /* rkq[3][1] */ 0, /* rkq[3][2] */ 0, /* rkq[4][0] */ 0, /* rkq[4][1] */ 0, /* rkq[4][2] */ 0, /* rkq[5][0] */ 0, /* rkq[5][1] */ 0, /* rkq[5][2] */ 0, /* rkq[6][0] */ 0, /* rkq[6][1] */ 0, /* rkq[6][2] */ 0, /* rkq[7][0] */ 0, /* rkq[7][1] */ 0, /* rkq[7][2] */ 0, /* rkq[8][0] */ 0, /* rkq[8][1] */ 0, /* rkq[8][2] */ 0, /* rkq[9][0] */ 0, /* rkq[9][1] */ 0, /* rkq[9][2] */ 0, /* rkq[10][0] */ 0, /* rkq[10][1] */ 0, /* rkq[10][2] */ 0, /* rkq[11][0] */ 0, /* rkq[11][1] */ 0, /* rkq[11][2] */ 0, /* rkq[12][0] */ 0, /* rkq[12][1] */ 0, /* rkq[12][2] */ 0, /* rkq[13][0] */ 0, /* rkq[13][1] */ 0, /* rkq[13][2] */ 0, /* rkq[14][0] */ 0, /* rkq[14][1] */ 0, /* rkq[14][2] */ 0, /* rkq[15][0] */ 0, /* rkq[15][1] */ 0, /* rkq[15][2] */ 0, /* rkq[16][0] */ 0, /* rkq[16][1] */ 0, /* rkq[16][2] */ 0, /* rkq[17][0] */ 0, /* rkq[17][1] */ 0, /* rkq[17][2] */ 0, /* riq[0][0] */ 0, /* riq[0][1] */ 0, /* riq[0][2] */ 0, /* riq[1][0] */ 0, /* riq[1][1] */ 0, /* riq[1][2] */ 0, /* riq[2][0] */ 0, /* riq[2][1] */ 0, /* riq[2][2] */ 0, /* riq[3][0] */ 0, /* riq[3][1] */ 0, /* riq[3][2] */ 0, /* riq[4][0] */ 0, /* riq[4][1] */ 0, /* riq[4][2] */ 0, /* riq[5][0] */ 0, /* riq[5][1] */ 0, /* riq[5][2] */ 0, /* riq[6][0] */ 0, /* riq[6][1] */ 0, /* riq[6][2] */ 0, /* riq[7][0] */ 0, /* riq[7][1] */ 0, /* riq[7][2] */ 0, /* riq[8][0] */ 0, /* riq[8][1] */ 0, /* riq[8][2] */ 0, /* riq[9][0] */ 0, /* riq[9][1] */ 0, /* riq[9][2] */ 0, /* riq[10][0] */ 0, /* riq[10][1] */ 0, /* riq[10][2] */ 0, /* riq[11][0] */ 0, /* riq[11][1] */ 0, /* riq[11][2] */ 0, /* riq[12][0] */ 0, /* riq[12][1] */ 0, /* riq[12][2] */ 0, /* riq[13][0] */ 0, /* riq[13][1] */ 0, /* riq[13][2] */ 0, /* riq[14][0] */ 0, /* riq[14][1] */ 0, /* riq[14][2] */ 0, /* riq[15][0] */ 0, /* riq[15][1] */ 0, /* riq[15][2] */ 0, /* riq[16][0] */ 0, /* riq[16][1] */ 0, /* riq[16][2] */ 0, /* riq[17][0] */ 0, /* riq[17][1] */ 0, /* riq[17][2] */ 0, /* presq[0] */ 0, /* presq[1] */ 0, /* presq[2] */ 0, /* presq[3] */ 0, /* presq[4] */ 0, /* presq[5] */ 0, /* presq[6] */ 0, /* presq[7] */ 0, /* presq[8] */ 0, /* presq[9] */ 0, /* presq[10] */ 0, /* presq[11] */ 0, /* presq[12] */ 0, /* presq[13] */ 0, /* presq[14] */ 0, /* presq[15] */ 0, /* presq[16] */ 0, /* presq[17] */ 0, /* presq[18] */ 0, /* presq[19] */ 0, /* presq[20] */ 0, /* presq[21] */ 0, /* presq[22] */ 0, /* stabvelq */ 3, /* stabposq */ 3, /* End of values from input file */ }; void BOCfly :: sdinit ( ) { /* Initialization routine This routine must be called before the first call to sdstate(), after supplying values for any `?' parameters in the input. */ double sumsq,norminv; int i,j,k; /* Check that all `?' parameters have been assigned values */ for (k = 0; k < 3; k++) { if (gravq[k] == 1) { sdseterr(7,25); } } for (k = 0; k < 18; k++) { if (mkq[k] == 1) { sdseterr(7,26); } for (i = 0; i < 3; i++) { if (rkq[k][i] == 1) { sdseterr(7,29); } if (riq[k][i] == 1) { sdseterr(7,30); } for (j = 0; j < 3; j++) { if (ikq[k][i][j] == 1) { sdseterr(7,27); } } } } for (k = 0; k < 23; k++) { for (i = 0; i < 3; i++) { if (pinq[k][i] == 1) { sdseterr(7,28); } } } /* Normalize pin vectors if necessary */ /* Zero out ping and hngpt */ for (i = 0; i < 23; i++) { for (j = 0; j < 3; j++) { ping[i][j] = 0.; hngpt[i][j] = 0.; } } /* Compute pseudobody-related constants */ rcom[0][0] = 0.; rcom[0][1] = 0.; rcom[0][2] = 0.; rcom[1][0] = 0.; rcom[1][1] = 0.; rcom[1][2] = 0.; rcom[2][0] = 0.; rcom[2][1] = 0.; rcom[2][2] = 0.; rcom[3][0] = 0.; rcom[3][1] = 0.; rcom[3][2] = 0.; rcom[4][0] = 0.; rcom[4][1] = 0.; rcom[4][2] = 0.; rcom[5][0] = 0.; rcom[5][1] = 0.; rcom[5][2] = 0.; rcom[6][0] = 0.; rcom[6][1] = 0.; rcom[6][2] = 0.; rcom[7][0] = 0.; rcom[7][1] = 0.; rcom[7][2] = 0.; rcom[8][0] = 0.; rcom[8][1] = 0.; rcom[8][2] = 0.; rcom[9][0] = 0.; rcom[9][1] = 0.; rcom[9][2] = 0.; rcom[10][0] = 0.; rcom[10][1] = 0.; rcom[10][2] = 0.; rcom[11][0] = 0.; rcom[11][1] = 0.; rcom[11][2] = 0.; rcom[12][0] = 0.; rcom[12][1] = 0.; rcom[12][2] = 0.; rcom[13][0] = 0.; rcom[13][1] = 0.; rcom[13][2] = 0.; rcom[14][0] = 0.; rcom[14][1] = 0.; rcom[14][2] = 0.; rcom[15][0] = 0.; rcom[15][1] = 0.; rcom[15][2] = 0.; rcom[16][0] = 0.; rcom[16][1] = 0.; rcom[16][2] = 0.; rcom[17][0] = 0.; rcom[17][1] = 0.; rcom[17][2] = 0.; /* Compute mass properties-related constants */ mtot = 33.20998161; sdserialno(&i); if (i != 70405) { sdseterr(7,41); } roustate = 1; } void BOCfly :: sdstate ( double timein, double qin[24], double uin[23] ) { /* Compute kinematic information and store it in sdgstate. Generated 27-Mar-2000 00:58:54 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 69086664 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 */ int i,j,qchg,uchg; double ee,stab; if ((roustate != 1) && (roustate != 2) && (roustate != 3)) { sdseterr(8,22); return; } if (roustate == 1) { for (i = 0; i < 23; i++) { if (presq[i] == 1) { sdseterr(8,31); } } } /* See if time or any qs have changed since last call */ if ((roustate != 1) && (timein == curtim)) { qchg = 0; for (i = 0; i < 24; i++) { if (qin[i] != q[i]) { qchg = 1; break; } } } else { qchg = 1; } /* If time and qs are unchanged, check us */ if (qchg == 0) { uchg = 0; for (i = 0; i < 23; i++) { if (uin[i] != u[i]) { uchg = 1; break; } } } else { uchg = 1; } curtim = timein; roustate = 2; if (qchg == 0) { goto skipqs; } /* Position-related variables need to be computed */ vpkflg = 0; mmflg = 0; mmlduflg = 0; wwflg = 0; for (i = 0; i < 24; i++) { q[i] = qin[i]; } /* Normalize Euler parameters in state */ sdnrmsterr(q,qn,8); /* Compute sines and cosines of q */ s6 = sin(q[6]); c6 = cos(q[6]); s7 = sin(q[7]); c7 = cos(q[7]); s8 = sin(q[8]); c8 = cos(q[8]); s9 = sin(q[9]); c9 = cos(q[9]); s10 = sin(q[10]); c10 = cos(q[10]); s11 = sin(q[11]); c11 = cos(q[11]); s12 = sin(q[12]); c12 = cos(q[12]); s13 = sin(q[13]); c13 = cos(q[13]); s14 = sin(q[14]); c14 = cos(q[14]); s15 = sin(q[15]); c15 = cos(q[15]); s16 = sin(q[16]); c16 = cos(q[16]); s17 = sin(q[17]); c17 = cos(q[17]); s18 = sin(q[18]); c18 = cos(q[18]); s19 = sin(q[19]); c19 = cos(q[19]); s20 = sin(q[20]); c20 = cos(q[20]); s21 = sin(q[21]); c21 = cos(q[21]); s22 = sin(q[22]); c22 = cos(q[22]); /* Compute across-axis direction cosines Cik */ Cik[3][0][0] = (1.-(2.*((qn[4]*qn[4])+(qn[5]*qn[5])))); Cik[3][0][1] = (2.*((qn[3]*qn[4])-(qn[5]*qn[23]))); Cik[3][0][2] = (2.*((qn[3]*qn[5])+(qn[4]*qn[23]))); Cik[3][1][0] = (2.*((qn[3]*qn[4])+(qn[5]*qn[23]))); Cik[3][1][1] = (1.-(2.*((qn[3]*qn[3])+(qn[5]*qn[5])))); Cik[3][1][2] = (2.*((qn[4]*qn[5])-(qn[3]*qn[23]))); Cik[3][2][0] = (2.*((qn[3]*qn[5])-(qn[4]*qn[23]))); Cik[3][2][1] = (2.*((qn[3]*qn[23])+(qn[4]*qn[5]))); Cik[3][2][2] = (1.-(2.*((qn[3]*qn[3])+(qn[4]*qn[4])))); /* Compute across-joint direction cosines Cib */ /* Compute gravity */ gk[8][0] = (9.80665*((Cik[3][2][2]*s8)-(Cik[3][2][0]*c8))); gk[8][2] = -(9.80665*((Cik[3][2][0]*s8)+(Cik[3][2][2]*c8))); gk[9][0] = (9.80665*((Cik[3][2][2]*s9)-(Cik[3][2][0]*c9))); gk[9][2] = -(9.80665*((Cik[3][2][0]*s9)+(Cik[3][2][2]*c9))); gk[10][0] = (9.80665*((Cik[3][2][2]*s10)-(Cik[3][2][0]*c10))); gk[10][2] = -(9.80665*((Cik[3][2][0]*s10)+(Cik[3][2][2]*c10))); gk[11][0] = ((gk[9][0]*c11)-(gk[9][2]*s11)); gk[11][2] = ((gk[9][0]*s11)+(gk[9][2]*c11)); gk[12][0] = ((gk[10][0]*c12)-(gk[10][2]*s12)); gk[12][2] = ((gk[10][0]*s12)+(gk[10][2]*c12)); gk[13][0] = ((gk[11][0]*c13)-(gk[11][2]*s13)); gk[13][2] = ((gk[11][0]*s13)+(gk[11][2]*c13)); gk[14][0] = ((gk[12][0]*c14)-(gk[12][2]*s14)); gk[14][2] = ((gk[12][0]*s14)+(gk[12][2]*c14)); gk[15][0] = (9.80665*((Cik[3][2][2]*s15)-(Cik[3][2][0]*c15))); gk[15][2] = -(9.80665*((Cik[3][2][0]*s15)+(Cik[3][2][2]*c15))); gk[16][0] = (9.80665*((Cik[3][2][2]*s16)-(Cik[3][2][0]*c16))); gk[16][2] = -(9.80665*((Cik[3][2][0]*s16)+(Cik[3][2][2]*c16))); gk[17][0] = ((gk[15][0]*c17)-(gk[15][2]*s17)); gk[17][2] = ((gk[15][0]*s17)+(gk[15][2]*c17)); gk[18][0] = ((gk[16][0]*c18)-(gk[16][2]*s18)); gk[18][2] = ((gk[16][0]*s18)+(gk[16][2]*c18)); gk[19][0] = ((gk[17][0]*c19)-(gk[17][2]*s19)); gk[19][2] = ((gk[17][0]*s19)+(gk[17][2]*c19)); gk[20][0] = ((gk[18][0]*c20)-(gk[18][2]*s20)); gk[20][2] = ((gk[18][0]*s20)+(gk[18][2]*c20)); gk[21][0] = (9.80665*((s6*c21)+(s21*c6))); gk[21][2] = (9.80665*((s6*s21)-(c6*c21))); gk[22][0] = (9.80665*((s7*c22)+(s22*c7))); gk[22][2] = (9.80665*((s7*s22)-(c7*c22))); /* Compute cnk & cnb (direction cosines in N) */ cnk[8][0][0] = ((Cik[3][0][0]*c8)-(Cik[3][0][2]*s8)); cnk[8][0][2] = ((Cik[3][0][0]*s8)+(Cik[3][0][2]*c8)); cnk[8][1][0] = ((Cik[3][1][0]*c8)-(Cik[3][1][2]*s8)); cnk[8][1][2] = ((Cik[3][1][0]*s8)+(Cik[3][1][2]*c8)); cnk[8][2][0] = ((Cik[3][2][0]*c8)-(Cik[3][2][2]*s8)); cnk[8][2][2] = ((Cik[3][2][0]*s8)+(Cik[3][2][2]*c8)); cnk[9][0][0] = ((Cik[3][0][0]*c9)-(Cik[3][0][2]*s9)); cnk[9][0][2] = ((Cik[3][0][0]*s9)+(Cik[3][0][2]*c9)); cnk[9][1][0] = ((Cik[3][1][0]*c9)-(Cik[3][1][2]*s9)); cnk[9][1][2] = ((Cik[3][1][0]*s9)+(Cik[3][1][2]*c9)); cnk[9][2][0] = ((Cik[3][2][0]*c9)-(Cik[3][2][2]*s9)); cnk[9][2][2] = ((Cik[3][2][0]*s9)+(Cik[3][2][2]*c9)); cnk[10][0][0] = ((Cik[3][0][0]*c10)-(Cik[3][0][2]*s10)); cnk[10][0][2] = ((Cik[3][0][0]*s10)+(Cik[3][0][2]*c10)); cnk[10][1][0] = ((Cik[3][1][0]*c10)-(Cik[3][1][2]*s10)); cnk[10][1][2] = ((Cik[3][1][0]*s10)+(Cik[3][1][2]*c10)); cnk[10][2][0] = ((Cik[3][2][0]*c10)-(Cik[3][2][2]*s10)); cnk[10][2][2] = ((Cik[3][2][0]*s10)+(Cik[3][2][2]*c10)); cnk[11][0][0] = ((cnk[9][0][0]*c11)-(cnk[9][0][2]*s11)); cnk[11][0][2] = ((cnk[9][0][0]*s11)+(cnk[9][0][2]*c11)); cnk[11][1][0] = ((cnk[9][1][0]*c11)-(cnk[9][1][2]*s11)); cnk[11][1][2] = ((cnk[9][1][0]*s11)+(cnk[9][1][2]*c11)); cnk[11][2][0] = ((cnk[9][2][0]*c11)-(cnk[9][2][2]*s11)); cnk[11][2][2] = ((cnk[9][2][0]*s11)+(cnk[9][2][2]*c11)); cnk[12][0][0] = ((cnk[10][0][0]*c12)-(cnk[10][0][2]*s12)); cnk[12][0][2] = ((cnk[10][0][0]*s12)+(cnk[10][0][2]*c12)); cnk[12][1][0] = ((cnk[10][1][0]*c12)-(cnk[10][1][2]*s12)); cnk[12][1][2] = ((cnk[10][1][0]*s12)+(cnk[10][1][2]*c12)); cnk[12][2][0] = ((cnk[10][2][0]*c12)-(cnk[10][2][2]*s12)); cnk[12][2][2] = ((cnk[10][2][0]*s12)+(cnk[10][2][2]*c12)); cnk[13][0][0] = ((cnk[11][0][0]*c13)-(cnk[11][0][2]*s13)); cnk[13][0][2] = ((cnk[11][0][0]*s13)+(cnk[11][0][2]*c13)); cnk[13][1][0] = ((cnk[11][1][0]*c13)-(cnk[11][1][2]*s13)); cnk[13][1][2] = ((cnk[11][1][0]*s13)+(cnk[11][1][2]*c13)); cnk[13][2][0] = ((cnk[11][2][0]*c13)-(cnk[11][2][2]*s13)); cnk[13][2][2] = ((cnk[11][2][0]*s13)+(cnk[11][2][2]*c13)); cnk[14][0][0] = ((cnk[12][0][0]*c14)-(cnk[12][0][2]*s14)); cnk[14][0][2] = ((cnk[12][0][0]*s14)+(cnk[12][0][2]*c14)); cnk[14][1][0] = ((cnk[12][1][0]*c14)-(cnk[12][1][2]*s14)); cnk[14][1][2] = ((cnk[12][1][0]*s14)+(cnk[12][1][2]*c14)); cnk[14][2][0] = ((cnk[12][2][0]*c14)-(cnk[12][2][2]*s14)); cnk[14][2][2] = ((cnk[12][2][0]*s14)+(cnk[12][2][2]*c14)); cnk[15][0][0] = ((Cik[3][0][0]*c15)-(Cik[3][0][2]*s15)); cnk[15][0][2] = ((Cik[3][0][0]*s15)+(Cik[3][0][2]*c15)); cnk[15][1][0] = ((Cik[3][1][0]*c15)-(Cik[3][1][2]*s15)); cnk[15][1][2] = ((Cik[3][1][0]*s15)+(Cik[3][1][2]*c15)); cnk[15][2][0] = ((Cik[3][2][0]*c15)-(Cik[3][2][2]*s15)); cnk[15][2][2] = ((Cik[3][2][0]*s15)+(Cik[3][2][2]*c15)); cnk[16][0][0] = ((Cik[3][0][0]*c16)-(Cik[3][0][2]*s16)); cnk[16][0][2] = ((Cik[3][0][0]*s16)+(Cik[3][0][2]*c16)); cnk[16][1][0] = ((Cik[3][1][0]*c16)-(Cik[3][1][2]*s16)); cnk[16][1][2] = ((Cik[3][1][0]*s16)+(Cik[3][1][2]*c16)); cnk[16][2][0] = ((Cik[3][2][0]*c16)-(Cik[3][2][2]*s16)); cnk[16][2][2] = ((Cik[3][2][0]*s16)+(Cik[3][2][2]*c16)); cnk[17][0][0] = ((cnk[15][0][0]*c17)-(cnk[15][0][2]*s17)); cnk[17][0][2] = ((cnk[15][0][0]*s17)+(cnk[15][0][2]*c17)); cnk[17][1][0] = ((cnk[15][1][0]*c17)-(cnk[15][1][2]*s17)); cnk[17][1][2] = ((cnk[15][1][0]*s17)+(cnk[15][1][2]*c17)); cnk[17][2][0] = ((cnk[15][2][0]*c17)-(cnk[15][2][2]*s17)); cnk[17][2][2] = ((cnk[15][2][0]*s17)+(cnk[15][2][2]*c17)); cnk[18][0][0] = ((cnk[16][0][0]*c18)-(cnk[16][0][2]*s18)); cnk[18][0][2] = ((cnk[16][0][0]*s18)+(cnk[16][0][2]*c18)); cnk[18][1][0] = ((cnk[16][1][0]*c18)-(cnk[16][1][2]*s18)); cnk[18][1][2] = ((cnk[16][1][0]*s18)+(cnk[16][1][2]*c18)); cnk[18][2][0] = ((cnk[16][2][0]*c18)-(cnk[16][2][2]*s18)); cnk[18][2][2] = ((cnk[16][2][0]*s18)+(cnk[16][2][2]*c18)); cnk[19][0][0] = ((cnk[17][0][0]*c19)-(cnk[17][0][2]*s19)); cnk[19][0][2] = ((cnk[17][0][0]*s19)+(cnk[17][0][2]*c19)); cnk[19][1][0] = ((cnk[17][1][0]*c19)-(cnk[17][1][2]*s19)); cnk[19][1][2] = ((cnk[17][1][0]*s19)+(cnk[17][1][2]*c19)); cnk[19][2][0] = ((cnk[17][2][0]*c19)-(cnk[17][2][2]*s19)); cnk[19][2][2] = ((cnk[17][2][0]*s19)+(cnk[17][2][2]*c19)); cnk[20][0][0] = ((cnk[18][0][0]*c20)-(cnk[18][0][2]*s20)); cnk[20][0][2] = ((cnk[18][0][0]*s20)+(cnk[18][0][2]*c20)); cnk[20][1][0] = ((cnk[18][1][0]*c20)-(cnk[18][1][2]*s20)); cnk[20][1][2] = ((cnk[18][1][0]*s20)+(cnk[18][1][2]*c20)); cnk[20][2][0] = ((cnk[18][2][0]*c20)-(cnk[18][2][2]*s20)); cnk[20][2][2] = ((cnk[18][2][0]*s20)+(cnk[18][2][2]*c20)); cnk[21][0][0] = ((c6*c21)-(s6*s21)); cnk[21][0][2] = ((s6*c21)+(s21*c6)); cnk[21][2][0] = -((s6*c21)+(s21*c6)); cnk[21][2][2] = ((c6*c21)-(s6*s21)); cnk[22][0][0] = ((c7*c22)-(s7*s22)); cnk[22][0][2] = ((s7*c22)+(s22*c7)); cnk[22][2][0] = -((s7*c22)+(s22*c7)); cnk[22][2][2] = ((c7*c22)-(s7*s22)); cnb[0][0][0] = Cik[3][0][0]; cnb[0][0][1] = Cik[3][0][1]; cnb[0][0][2] = Cik[3][0][2]; cnb[0][1][0] = Cik[3][1][0]; cnb[0][1][1] = Cik[3][1][1]; cnb[0][1][2] = Cik[3][1][2]; cnb[0][2][0] = Cik[3][2][0]; cnb[0][2][1] = Cik[3][2][1]; cnb[0][2][2] = Cik[3][2][2]; cnb[1][0][0] = c6; cnb[1][0][1] = 0.; cnb[1][0][2] = s6; cnb[1][1][0] = 0.; cnb[1][1][1] = 1.; cnb[1][1][2] = 0.; cnb[1][2][0] = -s6; cnb[1][2][1] = 0.; cnb[1][2][2] = c6; cnb[2][0][0] = c7; cnb[2][0][1] = 0.; cnb[2][0][2] = s7; cnb[2][1][0] = 0.; cnb[2][1][1] = 1.; cnb[2][1][2] = 0.; cnb[2][2][0] = -s7; cnb[2][2][1] = 0.; cnb[2][2][2] = c7; cnb[3][0][0] = cnk[8][0][0]; cnb[3][0][1] = Cik[3][0][1]; cnb[3][0][2] = cnk[8][0][2]; cnb[3][1][0] = cnk[8][1][0]; cnb[3][1][1] = Cik[3][1][1]; cnb[3][1][2] = cnk[8][1][2]; cnb[3][2][0] = cnk[8][2][0]; cnb[3][2][1] = Cik[3][2][1]; cnb[3][2][2] = cnk[8][2][2]; cnb[4][0][0] = cnk[9][0][0]; cnb[4][0][1] = Cik[3][0][1]; cnb[4][0][2] = cnk[9][0][2]; cnb[4][1][0] = cnk[9][1][0]; cnb[4][1][1] = Cik[3][1][1]; cnb[4][1][2] = cnk[9][1][2]; cnb[4][2][0] = cnk[9][2][0]; cnb[4][2][1] = Cik[3][2][1]; cnb[4][2][2] = cnk[9][2][2]; cnb[5][0][0] = cnk[10][0][0]; cnb[5][0][1] = Cik[3][0][1]; cnb[5][0][2] = cnk[10][0][2]; cnb[5][1][0] = cnk[10][1][0]; cnb[5][1][1] = Cik[3][1][1]; cnb[5][1][2] = cnk[10][1][2]; cnb[5][2][0] = cnk[10][2][0]; cnb[5][2][1] = Cik[3][2][1]; cnb[5][2][2] = cnk[10][2][2]; cnb[6][0][0] = cnk[11][0][0]; cnb[6][0][1] = Cik[3][0][1]; cnb[6][0][2] = cnk[11][0][2]; cnb[6][1][0] = cnk[11][1][0]; cnb[6][1][1] = Cik[3][1][1]; cnb[6][1][2] = cnk[11][1][2]; cnb[6][2][0] = cnk[11][2][0]; cnb[6][2][1] = Cik[3][2][1]; cnb[6][2][2] = cnk[11][2][2]; cnb[7][0][0] = cnk[12][0][0]; cnb[7][0][1] = Cik[3][0][1]; cnb[7][0][2] = cnk[12][0][2]; cnb[7][1][0] = cnk[12][1][0]; cnb[7][1][1] = Cik[3][1][1]; cnb[7][1][2] = cnk[12][1][2]; cnb[7][2][0] = cnk[12][2][0]; cnb[7][2][1] = Cik[3][2][1]; cnb[7][2][2] = cnk[12][2][2]; cnb[8][0][0] = cnk[13][0][0]; cnb[8][0][1] = Cik[3][0][1]; cnb[8][0][2] = cnk[13][0][2]; cnb[8][1][0] = cnk[13][1][0]; cnb[8][1][1] = Cik[3][1][1]; cnb[8][1][2] = cnk[13][1][2]; cnb[8][2][0] = cnk[13][2][0]; cnb[8][2][1] = Cik[3][2][1]; cnb[8][2][2] = cnk[13][2][2]; cnb[9][0][0] = cnk[14][0][0]; cnb[9][0][1] = Cik[3][0][1]; cnb[9][0][2] = cnk[14][0][2]; cnb[9][1][0] = cnk[14][1][0]; cnb[9][1][1] = Cik[3][1][1]; cnb[9][1][2] = cnk[14][1][2]; cnb[9][2][0] = cnk[14][2][0]; cnb[9][2][1] = Cik[3][2][1]; cnb[9][2][2] = cnk[14][2][2]; cnb[10][0][0] = cnk[15][0][0]; cnb[10][0][1] = Cik[3][0][1]; cnb[10][0][2] = cnk[15][0][2]; cnb[10][1][0] = cnk[15][1][0]; cnb[10][1][1] = Cik[3][1][1]; cnb[10][1][2] = cnk[15][1][2]; cnb[10][2][0] = cnk[15][2][0]; cnb[10][2][1] = Cik[3][2][1]; cnb[10][2][2] = cnk[15][2][2]; cnb[11][0][0] = cnk[16][0][0]; cnb[11][0][1] = Cik[3][0][1]; cnb[11][0][2] = cnk[16][0][2]; cnb[11][1][0] = cnk[16][1][0]; cnb[11][1][1] = Cik[3][1][1]; cnb[11][1][2] = cnk[16][1][2]; cnb[11][2][0] = cnk[16][2][0]; cnb[11][2][1] = Cik[3][2][1]; cnb[11][2][2] = cnk[16][2][2]; cnb[12][0][0] = cnk[17][0][0]; cnb[12][0][1] = Cik[3][0][1]; cnb[12][0][2] = cnk[17][0][2]; cnb[12][1][0] = cnk[17][1][0]; cnb[12][1][1] = Cik[3][1][1]; cnb[12][1][2] = cnk[17][1][2]; cnb[12][2][0] = cnk[17][2][0]; cnb[12][2][1] = Cik[3][2][1]; cnb[12][2][2] = cnk[17][2][2]; cnb[13][0][0] = cnk[18][0][0]; cnb[13][0][1] = Cik[3][0][1]; cnb[13][0][2] = cnk[18][0][2]; cnb[13][1][0] = cnk[18][1][0]; cnb[13][1][1] = Cik[3][1][1]; cnb[13][1][2] = cnk[18][1][2]; cnb[13][2][0] = cnk[18][2][0]; cnb[13][2][1] = Cik[3][2][1]; cnb[13][2][2] = cnk[18][2][2]; cnb[14][0][0] = cnk[19][0][0]; cnb[14][0][1] = Cik[3][0][1]; cnb[14][0][2] = cnk[19][0][2]; cnb[14][1][0] = cnk[19][1][0]; cnb[14][1][1] = Cik[3][1][1]; cnb[14][1][2] = cnk[19][1][2]; cnb[14][2][0] = cnk[19][2][0]; cnb[14][2][1] = Cik[3][2][1]; cnb[14][2][2] = cnk[19][2][2]; cnb[15][0][0] = cnk[20][0][0]; cnb[15][0][1] = Cik[3][0][1]; cnb[15][0][2] = cnk[20][0][2]; cnb[15][1][0] = cnk[20][1][0]; cnb[15][1][1] = Cik[3][1][1]; cnb[15][1][2] = cnk[20][1][2]; cnb[15][2][0] = cnk[20][2][0]; cnb[15][2][1] = Cik[3][2][1]; cnb[15][2][2] = cnk[20][2][2]; cnb[16][0][0] = cnk[21][0][0]; cnb[16][0][1] = 0.; cnb[16][0][2] = cnk[21][0][2]; cnb[16][1][0] = 0.; cnb[16][1][1] = 1.; cnb[16][1][2] = 0.; cnb[16][2][0] = cnk[21][2][0]; cnb[16][2][1] = 0.; cnb[16][2][2] = cnk[21][2][2]; cnb[17][0][0] = cnk[22][0][0]; cnb[17][0][1] = 0.; cnb[17][0][2] = cnk[22][0][2]; cnb[17][1][0] = 0.; cnb[17][1][1] = 1.; cnb[17][1][2] = 0.; cnb[17][2][0] = cnk[22][2][0]; cnb[17][2][1] = 0.; cnb[17][2][2] = cnk[22][2][2]; /* Compute q-related auxiliary variables */ rik[6][0] = (5.201e-5-(2.442*s6)); rik[6][2] = ((2.442*c6)-1.2242604); rik[7][0] = (5.197e-5-(2.442*s7)); rik[7][2] = ((2.442*c7)-1.22369868); rik[8][0] = (.01691003-((.00252618*c8)+(.3829286*s8))); rik[8][2] = (((.3829286*c8)-(.00252618*s8))-.12000071); rik[9][0] = (.0094289+((.0620714*s9)-(.01952618*c9))); rik[9][2] = -(.13168857+((.01952618*s9)+(.0620714*c9))); rik[10][0] = (.00943639+((.0620714*s10)-(.01952618*c10))); rik[10][2] = -(.13190398+((.01952618*s10)+(.0620714*c10))); rik[11][0] = (((.0065711*c11)+(.15931143*s11))-.01672166); rik[11][2] = (((.0065711*s11)-(.15931143*c11))-.12618299); rik[12][0] = (((.00656361*c12)+(.15909602*s12))-.01674373); rik[12][2] = (((.00656361*s12)-(.15909602*c12))-.12587504); rik[13][0] = (.03543467+((.00172166*c13)+(.17081701*s13))); rik[13][2] = (((.00172166*s13)-(.17081701*c13))-.03983908); rik[14][0] = (.03539679+((.00174373*c14)+(.17112496*s14))); rik[14][2] = (((.00174373*s14)-(.17112496*c14))-.03987912); rik[15][0] = (.00307461-((.01052618*c15)+(.2639286*s15))); rik[15][2] = (((.2639286*c15)-(.01052618*s15))-.20905614); rik[16][0] = (.00305957-((.01052618*c16)+(.2639286*s16))); rik[16][2] = (((.2639286*c16)-(.01052618*s16))-.21008605); rik[17][0] = (.01407264-((.00307461*c17)+(.01305614*s17))); rik[17][2] = (((.01305614*c17)-(.00307461*s17))-.15249501); rik[18][0] = (.01407902-((.00305957*c18)+(.01408605*s18))); rik[18][2] = (((.01408605*c18)-(.00305957*s18))-.15260355); rik[19][0] = (.01360932+((.00792736*c19)+(.01750499*s19))); rik[19][2] = (.01387793+((.00792736*s19)-(.01750499*c19))); rik[20][0] = (.01361601+((.00792098*c20)+(.01739645*s20))); rik[20][2] = (.01382852+((.00792098*s20)-(.01739645*c20))); rik[21][0] = (5.2e-5+((.4177396*s21)-(5.201e-5*c21))); rik[21][2] = -(.28373605+((5.201e-5*s21)+(.4177396*c21))); rik[22][0] = (5.194e-5+((.41830132*s22)-(5.197e-5*c22))); rik[22][2] = -(.28317448+((5.197e-5*s22)+(.41830132*c22))); rikt[8][1][0] = ((.3829286*c8)-(.00252618*s8)); rikt[8][1][2] = ((.00252618*c8)+(.3829286*s8)); rikt[9][1][0] = -((.01952618*s9)+(.0620714*c9)); rikt[9][1][2] = ((.01952618*c9)-(.0620714*s9)); rikt[10][1][0] = -((.01952618*s10)+(.0620714*c10)); rikt[10][1][2] = ((.01952618*c10)-(.0620714*s10)); rikt[11][1][0] = ((.016*s11)-(.291*c11)); rikt[11][1][2] = -((.016*c11)+(.291*s11)); rikt[12][1][0] = ((.016*s12)-(.291*c12)); rikt[12][1][2] = -((.016*c12)+(.291*s12)); rikt[13][1][0] = -((.015*s13)+(.297*c13)); rikt[13][1][2] = ((.015*c13)-(.297*s13)); rikt[14][1][0] = -((.015*s14)+(.297*c14)); rikt[14][1][2] = ((.015*c14)-(.297*s14)); rikt[15][1][0] = ((.2639286*c15)-(.01052618*s15)); rikt[15][1][2] = ((.01052618*c15)+(.2639286*s15)); rikt[16][1][0] = ((.2639286*c16)-(.01052618*s16)); rikt[16][1][2] = ((.01052618*c16)+(.2639286*s16)); rikt[19][1][0] = ((.022*s19)-(.17*c19)); rikt[19][1][2] = -((.022*c19)+(.17*s19)); rikt[20][1][0] = ((.022*s20)-(.17*c20)); rikt[20][1][2] = -((.022*c20)+(.17*s20)); /* Compute rnk & rnb (mass center locations in N) */ rnk[6][0] = ((5.201e-5*c6)-(1.2242604*s6)); rnk[6][2] = (2.442-((5.201e-5*s6)+(1.2242604*c6))); rnk[7][0] = ((5.197e-5*c7)-(1.22369868*s7)); rnk[7][2] = (2.442-((5.197e-5*s7)+(1.22369868*c7))); rnk[8][0] = ((q[0]+((.3829286*Cik[3][0][2])-((.00120864*Cik[3][0][1])+( .00252618*Cik[3][0][0]))))+(((.0007183*Cik[3][0][1])+(.01691003* cnk[8][0][0]))-(.12000071*cnk[8][0][2]))); rnk[8][1] = ((q[1]+((.3829286*Cik[3][1][2])-((.00120864*Cik[3][1][1])+( .00252618*Cik[3][1][0]))))+(((.0007183*Cik[3][1][1])+(.01691003* cnk[8][1][0]))-(.12000071*cnk[8][1][2]))); rnk[8][2] = ((q[2]+((.3829286*Cik[3][2][2])-((.00120864*Cik[3][2][1])+( .00252618*Cik[3][2][0]))))+(((.0007183*Cik[3][2][1])+(.01691003* cnk[8][2][0]))-(.12000071*cnk[8][2][2]))); rnk[9][0] = ((q[0]-((.0620714*Cik[3][0][2])+((.01952618*Cik[3][0][0])+( .04820864*Cik[3][0][1]))))+(((.0094289*cnk[9][0][0])-(.00015877* Cik[3][0][1]))-(.13168857*cnk[9][0][2]))); rnk[9][1] = ((q[1]-((.0620714*Cik[3][1][2])+((.01952618*Cik[3][1][0])+( .04820864*Cik[3][1][1]))))+(((.0094289*cnk[9][1][0])-(.00015877* Cik[3][1][1]))-(.13168857*cnk[9][1][2]))); rnk[9][2] = ((q[2]-((.0620714*Cik[3][2][2])+((.01952618*Cik[3][2][0])+( .04820864*Cik[3][2][1]))))+(((.0094289*cnk[9][2][0])-(.00015877* Cik[3][2][1]))-(.13168857*cnk[9][2][2]))); rnk[10][0] = ((q[0]+(((.04579136*Cik[3][0][1])-(.01952618*Cik[3][0][0]))-( .0620714*Cik[3][0][2])))+(((.00235524*Cik[3][0][1])+(.00943639* cnk[10][0][0]))-(.13190398*cnk[10][0][2]))); rnk[10][1] = ((q[1]+(((.04579136*Cik[3][1][1])-(.01952618*Cik[3][1][0]))-( .0620714*Cik[3][1][2])))+(((.00235524*Cik[3][1][1])+(.00943639* cnk[10][1][0]))-(.13190398*cnk[10][1][2]))); rnk[10][2] = ((q[2]+(((.04579136*Cik[3][2][1])-(.01952618*Cik[3][2][0]))-( .0620714*Cik[3][2][2])))+(((.00235524*Cik[3][2][1])+(.00943639* cnk[10][2][0]))-(.13190398*cnk[10][2][2]))); rnk[11][0] = ((rnk[9][0]+(((.0065711*cnk[9][0][0])-(.00284123*Cik[3][0][1])) -(.15931143*cnk[9][0][2])))+(((.00561135*Cik[3][0][1])-(.01672166* cnk[11][0][0]))-(.12618299*cnk[11][0][2]))); rnk[11][1] = ((rnk[9][1]+(((.0065711*cnk[9][1][0])-(.00284123*Cik[3][1][1])) -(.15931143*cnk[9][1][2])))+(((.00561135*Cik[3][1][1])-(.01672166* cnk[11][1][0]))-(.12618299*cnk[11][1][2]))); rnk[11][2] = ((rnk[9][2]+(((.0065711*cnk[9][2][0])-(.00284123*Cik[3][2][1])) -(.15931143*cnk[9][2][2])))+(((.00561135*Cik[3][2][1])-(.01672166* cnk[11][2][0]))-(.12618299*cnk[11][2][2]))); rnk[12][0] = ((rnk[10][0]+(((.00064476*Cik[3][0][1])+(.00656361* cnk[10][0][0]))-(.15909602*cnk[10][0][2])))-((.12587504*cnk[12][0][2])+(( .00342896*Cik[3][0][1])+(.01674373*cnk[12][0][0])))); rnk[12][1] = ((rnk[10][1]+(((.00064476*Cik[3][1][1])+(.00656361* cnk[10][1][0]))-(.15909602*cnk[10][1][2])))-((.12587504*cnk[12][1][2])+(( .00342896*Cik[3][1][1])+(.01674373*cnk[12][1][0])))); rnk[12][2] = ((rnk[10][2]+(((.00064476*Cik[3][2][1])+(.00656361* cnk[10][2][0]))-(.15909602*cnk[10][2][2])))-((.12587504*cnk[12][2][2])+(( .00342896*Cik[3][2][1])+(.01674373*cnk[12][2][0])))); rnk[13][0] = ((rnk[11][0]+(((.00172166*cnk[11][0][0])-(.00061135* Cik[3][0][1]))-(.17081701*cnk[11][0][2])))+(((.00193095*Cik[3][0][1])+( .03543467*cnk[13][0][0]))-(.03983908*cnk[13][0][2]))); rnk[13][1] = ((rnk[11][1]+(((.00172166*cnk[11][1][0])-(.00061135* Cik[3][1][1]))-(.17081701*cnk[11][1][2])))+(((.00193095*Cik[3][1][1])+( .03543467*cnk[13][1][0]))-(.03983908*cnk[13][1][2]))); rnk[13][2] = ((rnk[11][2]+(((.00172166*cnk[11][2][0])-(.00061135* Cik[3][2][1]))-(.17081701*cnk[11][2][2])))+(((.00193095*Cik[3][2][1])+( .03543467*cnk[13][2][0]))-(.03983908*cnk[13][2][2]))); rnk[14][0] = ((rnk[12][0]+(((.00174373*cnk[12][0][0])-(.00157104* Cik[3][0][1]))-(.17112496*cnk[12][0][2])))+(((.00095499*Cik[3][0][1])+( .03539679*cnk[14][0][0]))-(.03987912*cnk[14][0][2]))); rnk[14][1] = ((rnk[12][1]+(((.00174373*cnk[12][1][0])-(.00157104* Cik[3][1][1]))-(.17112496*cnk[12][1][2])))+(((.00095499*Cik[3][1][1])+( .03539679*cnk[14][1][0]))-(.03987912*cnk[14][1][2]))); rnk[14][2] = ((rnk[12][2]+(((.00174373*cnk[12][2][0])-(.00157104* Cik[3][2][1]))-(.17112496*cnk[12][2][2])))+(((.00095499*Cik[3][2][1])+( .03539679*cnk[14][2][0]))-(.03987912*cnk[14][2][2]))); rnk[15][0] = ((q[0]+((.2639286*Cik[3][0][2])-((.01052618*Cik[3][0][0])+( .10120864*Cik[3][0][1]))))+(((.00307461*cnk[15][0][0])-(.00885657* Cik[3][0][1]))-(.20905614*cnk[15][0][2]))); rnk[15][1] = ((q[1]+((.2639286*Cik[3][1][2])-((.01052618*Cik[3][1][0])+( .10120864*Cik[3][1][1]))))+(((.00307461*cnk[15][1][0])-(.00885657* Cik[3][1][1]))-(.20905614*cnk[15][1][2]))); rnk[15][2] = ((q[2]+((.2639286*Cik[3][2][2])-((.01052618*Cik[3][2][0])+( .10120864*Cik[3][2][1]))))+(((.00307461*cnk[15][2][0])-(.00885657* Cik[3][2][1]))-(.20905614*cnk[15][2][2]))); rnk[16][0] = ((q[0]+((.2639286*Cik[3][0][2])+((.09879136*Cik[3][0][1])-( .01052618*Cik[3][0][0]))))+(((.00305957*cnk[16][0][0])+(.00995927* Cik[3][0][1]))-(.21008605*cnk[16][0][2]))); rnk[16][1] = ((q[1]+((.2639286*Cik[3][1][2])+((.09879136*Cik[3][1][1])-( .01052618*Cik[3][1][0]))))+(((.00305957*cnk[16][1][0])+(.00995927* Cik[3][1][1]))-(.21008605*cnk[16][1][2]))); rnk[16][2] = ((q[2]+((.2639286*Cik[3][2][2])+((.09879136*Cik[3][2][1])-( .01052618*Cik[3][2][0]))))+(((.00305957*cnk[16][2][0])+(.00995927* Cik[3][2][1]))-(.21008605*cnk[16][2][2]))); rnk[17][0] = ((rnk[15][0]+((.01305614*cnk[15][0][2])-((.00307461* cnk[15][0][0])+(.02714343*Cik[3][0][1]))))+(((.00055916*Cik[3][0][1])+( .01407264*cnk[17][0][0]))-(.15249501*cnk[17][0][2]))); rnk[17][1] = ((rnk[15][1]+((.01305614*cnk[15][1][2])-((.00307461* cnk[15][1][0])+(.02714343*Cik[3][1][1]))))+(((.00055916*Cik[3][1][1])+( .01407264*cnk[17][1][0]))-(.15249501*cnk[17][1][2]))); rnk[17][2] = ((rnk[15][2]+((.01305614*cnk[15][2][2])-((.00307461* cnk[15][2][0])+(.02714343*Cik[3][2][1]))))+(((.00055916*Cik[3][2][1])+( .01407264*cnk[17][2][0]))-(.15249501*cnk[17][2][2]))); rnk[18][0] = ((rnk[16][0]+((.01408605*cnk[16][0][2])+((.02604073* Cik[3][0][1])-(.00305957*cnk[16][0][0]))))+(((.00074575*Cik[3][0][1])+( .01407902*cnk[18][0][0]))-(.15260355*cnk[18][0][2]))); rnk[18][1] = ((rnk[16][1]+((.01408605*cnk[16][1][2])+((.02604073* Cik[3][1][1])-(.00305957*cnk[16][1][0]))))+(((.00074575*Cik[3][1][1])+( .01407902*cnk[18][1][0]))-(.15260355*cnk[18][1][2]))); rnk[18][2] = ((rnk[16][2]+((.01408605*cnk[16][2][2])+((.02604073* Cik[3][2][1])-(.00305957*cnk[16][2][0]))))+(((.00074575*Cik[3][2][1])+( .01407902*cnk[18][2][0]))-(.15260355*cnk[18][2][2]))); rnk[19][0] = ((rnk[17][0]+(((.00792736*cnk[17][0][0])-(.02455916* Cik[3][0][1]))-(.01750499*cnk[17][0][2])))+((.01387793*cnk[19][0][2])+(( .01360932*cnk[19][0][0])-(.00486049*Cik[3][0][1])))); rnk[19][1] = ((rnk[17][1]+(((.00792736*cnk[17][1][0])-(.02455916* Cik[3][1][1]))-(.01750499*cnk[17][1][2])))+((.01387793*cnk[19][1][2])+(( .01360932*cnk[19][1][0])-(.00486049*Cik[3][1][1])))); rnk[19][2] = ((rnk[17][2]+(((.00792736*cnk[17][2][0])-(.02455916* Cik[3][2][1]))-(.01750499*cnk[17][2][2])))+((.01387793*cnk[19][2][2])+(( .01360932*cnk[19][2][0])-(.00486049*Cik[3][2][1])))); rnk[20][0] = ((rnk[18][0]+(((.00792098*cnk[18][0][0])+(.02325425* Cik[3][0][1]))-(.01739645*cnk[18][0][2])))+((.01382852*cnk[20][0][2])+(( .00560346*Cik[3][0][1])+(.01361601*cnk[20][0][0])))); rnk[20][1] = ((rnk[18][1]+(((.00792098*cnk[18][1][0])+(.02325425* Cik[3][1][1]))-(.01739645*cnk[18][1][2])))+((.01382852*cnk[20][1][2])+(( .00560346*Cik[3][1][1])+(.01361601*cnk[20][1][0])))); rnk[20][2] = ((rnk[18][2]+(((.00792098*cnk[18][2][0])+(.02325425* Cik[3][2][1]))-(.01739645*cnk[18][2][2])))+((.01382852*cnk[20][2][2])+(( .00560346*Cik[3][2][1])+(.01361601*cnk[20][2][0])))); rnk[21][0] = ((rnk[6][0]-((5.201e-5*c6)+(.4177396*s6)))+((5.2e-5* cnk[21][0][0])-(.28373605*cnk[21][0][2]))); rnk[21][2] = ((rnk[6][2]+((5.201e-5*s6)-(.4177396*c6)))+((5.2e-5* cnk[21][2][0])-(.28373605*cnk[21][2][2]))); rnk[22][0] = ((rnk[7][0]-((5.197e-5*c7)+(.41830132*s7)))+((5.194e-5* cnk[22][0][0])-(.28317448*cnk[22][0][2]))); rnk[22][2] = ((rnk[7][2]+((5.197e-5*s7)-(.41830132*c7)))+((5.194e-5* cnk[22][2][0])-(.28317448*cnk[22][2][2]))); rnb[0][0] = q[0]; rnb[0][1] = q[1]; rnb[0][2] = q[2]; rnb[1][0] = rnk[6][0]; rnb[1][1] = .15643667; rnb[1][2] = rnk[6][2]; rnb[2][0] = rnk[7][0]; rnb[2][1] = -.15893456; rnb[2][2] = rnk[7][2]; rnb[3][0] = rnk[8][0]; rnb[3][1] = rnk[8][1]; rnb[3][2] = rnk[8][2]; rnb[4][0] = rnk[9][0]; rnb[4][1] = rnk[9][1]; rnb[4][2] = rnk[9][2]; rnb[5][0] = rnk[10][0]; rnb[5][1] = rnk[10][1]; rnb[5][2] = rnk[10][2]; rnb[6][0] = rnk[11][0]; rnb[6][1] = rnk[11][1]; rnb[6][2] = rnk[11][2]; rnb[7][0] = rnk[12][0]; rnb[7][1] = rnk[12][1]; rnb[7][2] = rnk[12][2]; rnb[8][0] = rnk[13][0]; rnb[8][1] = rnk[13][1]; rnb[8][2] = rnk[13][2]; rnb[9][0] = rnk[14][0]; rnb[9][1] = rnk[14][1]; rnb[9][2] = rnk[14][2]; rnb[10][0] = rnk[15][0]; rnb[10][1] = rnk[15][1]; rnb[10][2] = rnk[15][2]; rnb[11][0] = rnk[16][0]; rnb[11][1] = rnk[16][1]; rnb[11][2] = rnk[16][2]; rnb[12][0] = rnk[17][0]; rnb[12][1] = rnk[17][1]; rnb[12][2] = rnk[17][2]; rnb[13][0] = rnk[18][0]; rnb[13][1] = rnk[18][1]; rnb[13][2] = rnk[18][2]; rnb[14][0] = rnk[19][0]; rnb[14][1] = rnk[19][1]; rnb[14][2] = rnk[19][2]; rnb[15][0] = rnk[20][0]; rnb[15][1] = rnk[20][1]; rnb[15][2] = rnk[20][2]; rnb[16][0] = rnk[21][0]; rnb[16][1] = .15643141; rnb[16][2] = rnk[21][2]; rnb[17][0] = rnk[22][0]; rnb[17][1] = -.15893944; rnb[17][2] = rnk[22][2]; /* Compute com (system mass center location in N) */ com[0] = (.0301114288994031*((.02601109*rnk[22][0])+((.0260106*rnk[21][0])+( (.05944511*rnk[20][0])+((.05946345*rnk[19][0])+((.43358398*rnk[18][0])+(( .43362906*rnk[17][0])+((1.01701701*rnk[16][0])+((1.00985702*rnk[15][0])+(( .5325902*rnk[14][0])+((.53380008*rnk[13][0])+((1.02007998*rnk[12][0])+(( 1.02133193*rnk[11][0])+((2.8984419*rnk[10][0])+((2.89586298*rnk[9][0])+(( 9.84983599*rnk[8][0])+((.1816698*rnk[7][0])+((.18166784*rnk[6][0])+( 11.02968359*q[0]))))))))))))))))))); com[1] = (.0301114288994031*(((.05944511*rnk[20][1])+((.05946345*rnk[19][1]) +((.43358398*rnk[18][1])+((.43362906*rnk[17][1])+((1.01701701*rnk[16][1])+ ((1.00985702*rnk[15][1])+((.5325902*rnk[14][1])+((.53380008*rnk[13][1])+(( 1.02007998*rnk[12][1])+((1.02133193*rnk[11][1])+((2.8984419*rnk[10][1])+(( 2.89586298*rnk[9][1])+((9.84983599*rnk[8][1])+((11.02968359*q[1])- .000454097792595202))))))))))))))-6.53132454436003e-5)); com[2] = (.0301114288994031*((.02601109*rnk[22][2])+((.0260106*rnk[21][2])+( (.05944511*rnk[20][2])+((.05946345*rnk[19][2])+((.43358398*rnk[18][2])+(( .43362906*rnk[17][2])+((1.01701701*rnk[16][2])+((1.00985702*rnk[15][2])+(( .5325902*rnk[14][2])+((.53380008*rnk[13][2])+((1.02007998*rnk[12][2])+(( 1.02133193*rnk[11][2])+((2.8984419*rnk[10][2])+((2.89586298*rnk[9][2])+(( 9.84983599*rnk[8][2])+((.1816698*rnk[7][2])+((.18166784*rnk[6][2])+( 11.02968359*q[2]))))))))))))))))))); skipqs: ; if (uchg == 0) { goto skipus; } /* Velocity-related variables need to be computed */ inerflg = 0; for (i = 0; i < 23; i++) { u[i] = uin[i]; } /* Compute u-related auxiliary variables */ /* Compute wk & wb (angular velocities) */ wk[8][0] = ((u[3]*c8)-(u[5]*s8)); wk[8][1] = (u[4]+u[8]); wk[8][2] = ((u[3]*s8)+(u[5]*c8)); wk[9][0] = ((u[3]*c9)-(u[5]*s9)); wk[9][1] = (u[4]+u[9]); wk[9][2] = ((u[3]*s9)+(u[5]*c9)); wk[10][0] = ((u[3]*c10)-(u[5]*s10)); wk[10][1] = (u[4]+u[10]); wk[10][2] = ((u[3]*s10)+(u[5]*c10)); wk[11][0] = ((wk[9][0]*c11)-(wk[9][2]*s11)); wk[11][1] = (u[11]+wk[9][1]); wk[11][2] = ((wk[9][0]*s11)+(wk[9][2]*c11)); wk[12][0] = ((wk[10][0]*c12)-(wk[10][2]*s12)); wk[12][1] = (u[12]+wk[10][1]); wk[12][2] = ((wk[10][0]*s12)+(wk[10][2]*c12)); wk[13][0] = ((wk[11][0]*c13)-(wk[11][2]*s13)); wk[13][1] = (u[13]+wk[11][1]); wk[13][2] = ((wk[11][0]*s13)+(wk[11][2]*c13)); wk[14][0] = ((wk[12][0]*c14)-(wk[12][2]*s14)); wk[14][1] = (u[14]+wk[12][1]); wk[14][2] = ((wk[12][0]*s14)+(wk[12][2]*c14)); wk[15][0] = ((u[3]*c15)-(u[5]*s15)); wk[15][1] = (u[4]+u[15]); wk[15][2] = ((u[3]*s15)+(u[5]*c15)); wk[16][0] = ((u[3]*c16)-(u[5]*s16)); wk[16][1] = (u[4]+u[16]); wk[16][2] = ((u[3]*s16)+(u[5]*c16)); wk[17][0] = ((wk[15][0]*c17)-(wk[15][2]*s17)); wk[17][1] = (u[17]+wk[15][1]); wk[17][2] = ((wk[15][0]*s17)+(wk[15][2]*c17)); wk[18][0] = ((wk[16][0]*c18)-(wk[16][2]*s18)); wk[18][1] = (u[18]+wk[16][1]); wk[18][2] = ((wk[16][0]*s18)+(wk[16][2]*c18)); wk[19][0] = ((wk[17][0]*c19)-(wk[17][2]*s19)); wk[19][1] = (u[19]+wk[17][1]); wk[19][2] = ((wk[17][0]*s19)+(wk[17][2]*c19)); wk[20][0] = ((wk[18][0]*c20)-(wk[18][2]*s20)); wk[20][1] = (u[20]+wk[18][1]); wk[20][2] = ((wk[18][0]*s20)+(wk[18][2]*c20)); wk[21][1] = (u[6]+u[21]); wk[22][1] = (u[7]+u[22]); wb[0][0] = u[3]; wb[0][1] = u[4]; wb[0][2] = u[5]; wb[1][0] = 0.; wb[1][1] = u[6]; wb[1][2] = 0.; wb[2][0] = 0.; wb[2][1] = u[7]; wb[2][2] = 0.; wb[3][0] = wk[8][0]; wb[3][1] = wk[8][1]; wb[3][2] = wk[8][2]; wb[4][0] = wk[9][0]; wb[4][1] = wk[9][1]; wb[4][2] = wk[9][2]; wb[5][0] = wk[10][0]; wb[5][1] = wk[10][1]; wb[5][2] = wk[10][2]; wb[6][0] = wk[11][0]; wb[6][1] = wk[11][1]; wb[6][2] = wk[11][2]; wb[7][0] = wk[12][0]; wb[7][1] = wk[12][1]; wb[7][2] = wk[12][2]; wb[8][0] = wk[13][0]; wb[8][1] = wk[13][1]; wb[8][2] = wk[13][2]; wb[9][0] = wk[14][0]; wb[9][1] = wk[14][1]; wb[9][2] = wk[14][2]; wb[10][0] = wk[15][0]; wb[10][1] = wk[15][1]; wb[10][2] = wk[15][2]; wb[11][0] = wk[16][0]; wb[11][1] = wk[16][1]; wb[11][2] = wk[16][2]; wb[12][0] = wk[17][0]; wb[12][1] = wk[17][1]; wb[12][2] = wk[17][2]; wb[13][0] = wk[18][0]; wb[13][1] = wk[18][1]; wb[13][2] = wk[18][2]; wb[14][0] = wk[19][0]; wb[14][1] = wk[19][1]; wb[14][2] = wk[19][2]; wb[15][0] = wk[20][0]; wb[15][1] = wk[20][1]; wb[15][2] = wk[20][2]; wb[16][0] = 0.; wb[16][1] = wk[21][1]; wb[16][2] = 0.; wb[17][0] = 0.; wb[17][1] = wk[22][1]; wb[17][2] = 0.; /* Compute auxiliary variables involving wk */ Wirk[8][0] = ((.00120864*u[5])+(.3829286*u[4])); Wirk[8][1] = -((.00252618*u[5])+(.3829286*u[3])); Wirk[8][2] = ((.00252618*u[4])-(.00120864*u[3])); Wirk[9][0] = ((.04820864*u[5])-(.0620714*u[4])); Wirk[9][1] = ((.0620714*u[3])-(.01952618*u[5])); Wirk[9][2] = ((.01952618*u[4])-(.04820864*u[3])); Wirk[10][0] = -((.04579136*u[5])+(.0620714*u[4])); Wirk[10][1] = ((.0620714*u[3])-(.01952618*u[5])); Wirk[10][2] = ((.01952618*u[4])+(.04579136*u[3])); Wirk[11][0] = ((.00284123*wk[9][2])-(.15931143*wk[9][1])); Wirk[11][1] = ((.0065711*wk[9][2])+(.15931143*wk[9][0])); Wirk[11][2] = -((.00284123*wk[9][0])+(.0065711*wk[9][1])); Wirk[12][0] = -((.00064476*wk[10][2])+(.15909602*wk[10][1])); Wirk[12][1] = ((.00656361*wk[10][2])+(.15909602*wk[10][0])); Wirk[12][2] = ((.00064476*wk[10][0])-(.00656361*wk[10][1])); Wirk[13][0] = ((.00061135*wk[11][2])-(.17081701*wk[11][1])); Wirk[13][1] = ((.00172166*wk[11][2])+(.17081701*wk[11][0])); Wirk[13][2] = -((.00061135*wk[11][0])+(.00172166*wk[11][1])); Wirk[14][0] = ((.00157104*wk[12][2])-(.17112496*wk[12][1])); Wirk[14][1] = ((.00174373*wk[12][2])+(.17112496*wk[12][0])); Wirk[14][2] = -((.00157104*wk[12][0])+(.00174373*wk[12][1])); Wirk[15][0] = ((.10120864*u[5])+(.2639286*u[4])); Wirk[15][1] = -((.01052618*u[5])+(.2639286*u[3])); Wirk[15][2] = ((.01052618*u[4])-(.10120864*u[3])); Wirk[16][0] = ((.2639286*u[4])-(.09879136*u[5])); Wirk[16][1] = -((.01052618*u[5])+(.2639286*u[3])); Wirk[16][2] = ((.01052618*u[4])+(.09879136*u[3])); Wirk[17][0] = ((.01305614*wk[15][1])+(.02714343*wk[15][2])); Wirk[17][1] = -((.00307461*wk[15][2])+(.01305614*wk[15][0])); Wirk[17][2] = ((.00307461*wk[15][1])-(.02714343*wk[15][0])); Wirk[18][0] = ((.01408605*wk[16][1])-(.02604073*wk[16][2])); Wirk[18][1] = -((.00305957*wk[16][2])+(.01408605*wk[16][0])); Wirk[18][2] = ((.00305957*wk[16][1])+(.02604073*wk[16][0])); Wirk[19][0] = ((.02455916*wk[17][2])-(.01750499*wk[17][1])); Wirk[19][1] = ((.00792736*wk[17][2])+(.01750499*wk[17][0])); Wirk[19][2] = -((.00792736*wk[17][1])+(.02455916*wk[17][0])); Wirk[20][0] = -((.01739645*wk[18][1])+(.02325425*wk[18][2])); Wirk[20][1] = ((.00792098*wk[18][2])+(.01739645*wk[18][0])); Wirk[20][2] = ((.02325425*wk[18][0])-(.00792098*wk[18][1])); Wkrpk[8][0] = -((.0007183*wk[8][2])+(.12000071*wk[8][1])); Wkrpk[8][1] = ((.01691003*wk[8][2])+(.12000071*wk[8][0])); Wkrpk[8][2] = ((.0007183*wk[8][0])-(.01691003*wk[8][1])); Wkrpk[9][0] = ((.00015877*wk[9][2])-(.13168857*wk[9][1])); Wkrpk[9][1] = ((.0094289*wk[9][2])+(.13168857*wk[9][0])); Wkrpk[9][2] = -((.00015877*wk[9][0])+(.0094289*wk[9][1])); Wkrpk[10][0] = -((.00235524*wk[10][2])+(.13190398*wk[10][1])); Wkrpk[10][1] = ((.00943639*wk[10][2])+(.13190398*wk[10][0])); Wkrpk[10][2] = ((.00235524*wk[10][0])-(.00943639*wk[10][1])); Wkrpk[11][0] = -((.00561135*wk[11][2])+(.12618299*wk[11][1])); Wkrpk[11][1] = ((.12618299*wk[11][0])-(.01672166*wk[11][2])); Wkrpk[11][2] = ((.00561135*wk[11][0])+(.01672166*wk[11][1])); Wkrpk[12][0] = ((.00342896*wk[12][2])-(.12587504*wk[12][1])); Wkrpk[12][1] = ((.12587504*wk[12][0])-(.01674373*wk[12][2])); Wkrpk[12][2] = ((.01674373*wk[12][1])-(.00342896*wk[12][0])); Wkrpk[13][0] = -((.00193095*wk[13][2])+(.03983908*wk[13][1])); Wkrpk[13][1] = ((.03543467*wk[13][2])+(.03983908*wk[13][0])); Wkrpk[13][2] = ((.00193095*wk[13][0])-(.03543467*wk[13][1])); Wkrpk[14][0] = -((.00095499*wk[14][2])+(.03987912*wk[14][1])); Wkrpk[14][1] = ((.03539679*wk[14][2])+(.03987912*wk[14][0])); Wkrpk[14][2] = ((.00095499*wk[14][0])-(.03539679*wk[14][1])); Wkrpk[15][0] = ((.00885657*wk[15][2])-(.20905614*wk[15][1])); Wkrpk[15][1] = ((.00307461*wk[15][2])+(.20905614*wk[15][0])); Wkrpk[15][2] = -((.00307461*wk[15][1])+(.00885657*wk[15][0])); Wkrpk[16][0] = -((.00995927*wk[16][2])+(.21008605*wk[16][1])); Wkrpk[16][1] = ((.00305957*wk[16][2])+(.21008605*wk[16][0])); Wkrpk[16][2] = ((.00995927*wk[16][0])-(.00305957*wk[16][1])); Wkrpk[17][0] = -((.00055916*wk[17][2])+(.15249501*wk[17][1])); Wkrpk[17][1] = ((.01407264*wk[17][2])+(.15249501*wk[17][0])); Wkrpk[17][2] = ((.00055916*wk[17][0])-(.01407264*wk[17][1])); Wkrpk[18][0] = -((.00074575*wk[18][2])+(.15260355*wk[18][1])); Wkrpk[18][1] = ((.01407902*wk[18][2])+(.15260355*wk[18][0])); Wkrpk[18][2] = ((.00074575*wk[18][0])-(.01407902*wk[18][1])); Wkrpk[19][0] = ((.00486049*wk[19][2])+(.01387793*wk[19][1])); Wkrpk[19][1] = ((.01360932*wk[19][2])-(.01387793*wk[19][0])); Wkrpk[19][2] = -((.00486049*wk[19][0])+(.01360932*wk[19][1])); Wkrpk[20][0] = ((.01382852*wk[20][1])-(.00560346*wk[20][2])); Wkrpk[20][1] = ((.01361601*wk[20][2])-(.01382852*wk[20][0])); Wkrpk[20][2] = ((.00560346*wk[20][0])-(.01361601*wk[20][1])); IkWk[5][0] = (((.48506516*u[3])-(5.312e-5*u[4]))-(.01450199*u[5])); IkWk[5][1] = (((.47268161*u[4])-(5.312e-5*u[3]))-(.00031516*u[5])); IkWk[5][2] = ((.03863225*u[5])-((.00031516*u[4])+(.01450199*u[3]))); WkIkWk[5][0] = ((IkWk[5][2]*u[4])-(IkWk[5][1]*u[5])); WkIkWk[5][1] = ((IkWk[5][0]*u[5])-(IkWk[5][2]*u[3])); WkIkWk[5][2] = ((IkWk[5][1]*u[3])-(IkWk[5][0]*u[4])); WkIkWk[6][0] = -(.00226939*(u[6]*u[6])); WkIkWk[6][2] = -(8e-8*(u[6]*u[6])); WkIkWk[7][0] = (.00230647*(u[7]*u[7])); WkIkWk[7][2] = (8e-8*(u[7]*u[7])); IkWk[8][0] = ((.00823665*wk[8][2])+((1.657e-5*wk[8][1])+(.52257967*wk[8][0]) )); IkWk[8][1] = (((1.657e-5*wk[8][0])+(.52474672*wk[8][1]))-(.00036016*wk[8][2] )); IkWk[8][2] = ((.01799022*wk[8][2])+((.00823665*wk[8][0])-(.00036016*wk[8][1] ))); WkIkWk[8][0] = ((IkWk[8][2]*wk[8][1])-(IkWk[8][1]*wk[8][2])); WkIkWk[8][1] = ((IkWk[8][0]*wk[8][2])-(IkWk[8][2]*wk[8][0])); WkIkWk[8][2] = ((IkWk[8][1]*wk[8][0])-(IkWk[8][0]*wk[8][1])); IkWk[9][0] = ((.00223918*wk[9][2])+((.05459333*wk[9][0])-(5.918e-5*wk[9][1]) )); IkWk[9][1] = ((.00147758*wk[9][2])+((.05477009*wk[9][1])-(5.918e-5*wk[9][0]) )); IkWk[9][2] = ((.00343412*wk[9][2])+((.00147758*wk[9][1])+(.00223918*wk[9][0] ))); WkIkWk[9][0] = ((IkWk[9][2]*wk[9][1])-(IkWk[9][1]*wk[9][2])); WkIkWk[9][1] = ((IkWk[9][0]*wk[9][2])-(IkWk[9][2]*wk[9][0])); WkIkWk[9][2] = ((IkWk[9][1]*wk[9][0])-(IkWk[9][0]*wk[9][1])); IkWk[10][0] = ((.00224427*wk[10][2])+((6.279e-5*wk[10][1])+(.05476817* wk[10][0]))); IkWk[10][1] = (((6.279e-5*wk[10][0])+(.05493593*wk[10][1]))-(.00156061* wk[10][2])); IkWk[10][2] = ((.00344556*wk[10][2])+((.00224427*wk[10][0])-(.00156061* wk[10][1]))); WkIkWk[10][0] = ((IkWk[10][2]*wk[10][1])-(IkWk[10][1]*wk[10][2])); WkIkWk[10][1] = ((IkWk[10][0]*wk[10][2])-(IkWk[10][2]*wk[10][0])); WkIkWk[10][2] = ((IkWk[10][1]*wk[10][0])-(IkWk[10][0]*wk[10][1])); IkWk[11][0] = (((1.273e-5*wk[11][1])+(.00826684*wk[11][0]))-(.00026397* wk[11][2])); IkWk[11][1] = ((.00020104*wk[11][2])+((1.273e-5*wk[11][0])+(.00843352* wk[11][1]))); IkWk[11][2] = ((.00057599*wk[11][2])+((.00020104*wk[11][1])-(.00026397* wk[11][0]))); WkIkWk[11][0] = ((IkWk[11][2]*wk[11][1])-(IkWk[11][1]*wk[11][2])); WkIkWk[11][1] = ((IkWk[11][0]*wk[11][2])-(IkWk[11][2]*wk[11][0])); WkIkWk[11][2] = ((IkWk[11][1]*wk[11][0])-(IkWk[11][0]*wk[11][1])); IkWk[12][0] = (((.00821833*wk[12][0])-(1.283e-5*wk[12][1]))-(.00026918* wk[12][2])); IkWk[12][1] = (((.00838445*wk[12][1])-(1.283e-5*wk[12][0]))-(.00019836* wk[12][2])); IkWk[12][2] = ((.00057568*wk[12][2])-((.00019836*wk[12][1])+(.00026918* wk[12][0]))); WkIkWk[12][0] = ((IkWk[12][2]*wk[12][1])-(IkWk[12][1]*wk[12][2])); WkIkWk[12][1] = ((IkWk[12][0]*wk[12][2])-(IkWk[12][2]*wk[12][0])); WkIkWk[12][2] = ((IkWk[12][1]*wk[12][0])-(IkWk[12][0]*wk[12][1])); IkWk[13][0] = ((.00016083*wk[13][2])+((.00031494*wk[13][0])-(1.886e-5* wk[13][1]))); IkWk[13][1] = ((4.29e-6*wk[13][2])+((.00112921*wk[13][1])-(1.886e-5* wk[13][0]))); IkWk[13][2] = ((.00103272*wk[13][2])+((4.29e-6*wk[13][1])+(.00016083* wk[13][0]))); WkIkWk[13][0] = ((IkWk[13][2]*wk[13][1])-(IkWk[13][1]*wk[13][2])); WkIkWk[13][1] = ((IkWk[13][0]*wk[13][2])-(IkWk[13][2]*wk[13][0])); WkIkWk[13][2] = ((IkWk[13][1]*wk[13][0])-(IkWk[13][0]*wk[13][1])); IkWk[14][0] = ((.00016063*wk[14][2])+((1.71e-5*wk[14][1])+(.00031249* wk[14][0]))); IkWk[14][1] = (((1.71e-5*wk[14][0])+(.00112646*wk[14][1]))-(4.13e-6* wk[14][2])); IkWk[14][2] = ((.00102891*wk[14][2])+((.00016063*wk[14][0])-(4.13e-6* wk[14][1]))); WkIkWk[14][0] = ((IkWk[14][2]*wk[14][1])-(IkWk[14][1]*wk[14][2])); WkIkWk[14][1] = ((IkWk[14][0]*wk[14][2])-(IkWk[14][2]*wk[14][0])); WkIkWk[14][2] = ((IkWk[14][1]*wk[14][0])-(IkWk[14][0]*wk[14][1])); IkWk[15][0] = ((.00098554*wk[15][2])+((.03667794*wk[15][0])-(9.282e-5* wk[15][1]))); IkWk[15][1] = ((.00236551*wk[15][2])+((.03636003*wk[15][1])-(9.282e-5* wk[15][0]))); IkWk[15][2] = ((.00072894*wk[15][2])+((.00098554*wk[15][0])+(.00236551* wk[15][1]))); WkIkWk[15][0] = ((IkWk[15][2]*wk[15][1])-(IkWk[15][1]*wk[15][2])); WkIkWk[15][1] = ((IkWk[15][0]*wk[15][2])-(IkWk[15][2]*wk[15][0])); WkIkWk[15][2] = ((IkWk[15][1]*wk[15][0])-(IkWk[15][0]*wk[15][1])); IkWk[16][0] = ((.00099076*wk[16][2])+((9.583e-5*wk[16][1])+(.03712787* wk[16][0]))); IkWk[16][1] = (((9.583e-5*wk[16][0])+(.03679631*wk[16][1]))-(.00244367* wk[16][2])); IkWk[16][2] = ((.00074417*wk[16][2])+((.00099076*wk[16][0])-(.00244367* wk[16][1]))); WkIkWk[16][0] = ((IkWk[16][2]*wk[16][1])-(IkWk[16][1]*wk[16][2])); WkIkWk[16][1] = ((IkWk[16][0]*wk[16][2])-(IkWk[16][2]*wk[16][0])); WkIkWk[16][2] = ((IkWk[16][1]*wk[16][0])-(IkWk[16][0]*wk[16][1])); IkWk[17][0] = ((.00047536*wk[17][2])+((.00933952*wk[17][0])-(2.97e-5* wk[17][1]))); IkWk[17][1] = ((.00105064*wk[17][2])+((.00915719*wk[17][1])-(2.97e-5* wk[17][0]))); IkWk[17][2] = ((.00032059*wk[17][2])+((.00047536*wk[17][0])+(.00105064* wk[17][1]))); WkIkWk[17][0] = ((IkWk[17][2]*wk[17][1])-(IkWk[17][1]*wk[17][2])); WkIkWk[17][1] = ((IkWk[17][0]*wk[17][2])-(IkWk[17][2]*wk[17][0])); WkIkWk[17][2] = ((IkWk[17][1]*wk[17][0])-(IkWk[17][0]*wk[17][1])); IkWk[18][0] = ((.00047517*wk[18][2])+((3.024e-5*wk[18][1])+(.00933861* wk[18][0]))); IkWk[18][1] = (((3.024e-5*wk[18][0])+(.00915219*wk[18][1]))-(.00106364* wk[18][2])); IkWk[18][2] = ((.00032466*wk[18][2])+((.00047517*wk[18][0])-(.00106364* wk[18][1]))); WkIkWk[18][0] = ((IkWk[18][2]*wk[18][1])-(IkWk[18][1]*wk[18][2])); WkIkWk[18][1] = ((IkWk[18][0]*wk[18][2])-(IkWk[18][2]*wk[18][0])); WkIkWk[18][2] = ((IkWk[18][1]*wk[18][0])-(IkWk[18][0]*wk[18][1])); IkWk[19][0] = ((7.331e-5*wk[19][2])+((.00086458*wk[19][0])-(2.761e-5* wk[19][1]))); IkWk[19][1] = (((.00082117*wk[19][1])-(2.761e-5*wk[19][0]))-(.00020674* wk[19][2])); IkWk[19][2] = ((3.059e-5*wk[19][2])+((7.331e-5*wk[19][0])-(.00020674* wk[19][1]))); WkIkWk[19][0] = ((IkWk[19][2]*wk[19][1])-(IkWk[19][1]*wk[19][2])); WkIkWk[19][1] = ((IkWk[19][0]*wk[19][2])-(IkWk[19][2]*wk[19][0])); WkIkWk[19][2] = ((IkWk[19][1]*wk[19][0])-(IkWk[19][0]*wk[19][1])); IkWk[20][0] = ((7.338e-5*wk[20][2])+((2.77e-5*wk[20][1])+(.00086575* wk[20][0]))); IkWk[20][1] = ((.00020754*wk[20][2])+((2.77e-5*wk[20][0])+(.00082196* wk[20][1]))); IkWk[20][2] = ((3.093e-5*wk[20][2])+((7.338e-5*wk[20][0])+(.00020754* wk[20][1]))); WkIkWk[20][0] = ((IkWk[20][2]*wk[20][1])-(IkWk[20][1]*wk[20][2])); WkIkWk[20][1] = ((IkWk[20][0]*wk[20][2])-(IkWk[20][2]*wk[20][0])); WkIkWk[20][2] = ((IkWk[20][1]*wk[20][0])-(IkWk[20][0]*wk[20][1])); WkIkWk[21][0] = -(.00013462*(wk[21][1]*wk[21][1])); WkIkWk[21][2] = -(1e-8*(wk[21][1]*wk[21][1])); WkIkWk[22][0] = (.00013693*(wk[22][1]*wk[22][1])); WkIkWk[22][2] = (1e-8*(wk[22][1]*wk[22][1])); /* Compute temporaries for use in SDRHS */ w0w0[0] = (u[3]*u[3]); w0w0[3] = (wk[8][0]*wk[8][0]); w0w0[4] = (wk[9][0]*wk[9][0]); w0w0[5] = (wk[10][0]*wk[10][0]); w0w0[6] = (wk[11][0]*wk[11][0]); w0w0[7] = (wk[12][0]*wk[12][0]); w0w0[8] = (wk[13][0]*wk[13][0]); w0w0[9] = (wk[14][0]*wk[14][0]); w0w0[10] = (wk[15][0]*wk[15][0]); w0w0[11] = (wk[16][0]*wk[16][0]); w0w0[12] = (wk[17][0]*wk[17][0]); w0w0[13] = (wk[18][0]*wk[18][0]); w0w0[14] = (wk[19][0]*wk[19][0]); w0w0[15] = (wk[20][0]*wk[20][0]); w1w1[0] = (u[4]*u[4]); w1w1[1] = (u[6]*u[6]); w1w1[2] = (u[7]*u[7]); w1w1[3] = (wk[8][1]*wk[8][1]); w1w1[4] = (wk[9][1]*wk[9][1]); w1w1[5] = (wk[10][1]*wk[10][1]); w1w1[6] = (wk[11][1]*wk[11][1]); w1w1[7] = (wk[12][1]*wk[12][1]); w1w1[8] = (wk[13][1]*wk[13][1]); w1w1[9] = (wk[14][1]*wk[14][1]); w1w1[10] = (wk[15][1]*wk[15][1]); w1w1[11] = (wk[16][1]*wk[16][1]); w1w1[12] = (wk[17][1]*wk[17][1]); w1w1[13] = (wk[18][1]*wk[18][1]); w1w1[14] = (wk[19][1]*wk[19][1]); w1w1[15] = (wk[20][1]*wk[20][1]); w1w1[16] = (wk[21][1]*wk[21][1]); w1w1[17] = (wk[22][1]*wk[22][1]); w2w2[0] = (u[5]*u[5]); w2w2[3] = (wk[8][2]*wk[8][2]); w2w2[4] = (wk[9][2]*wk[9][2]); w2w2[5] = (wk[10][2]*wk[10][2]); w2w2[6] = (wk[11][2]*wk[11][2]); w2w2[7] = (wk[12][2]*wk[12][2]); w2w2[8] = (wk[13][2]*wk[13][2]); w2w2[9] = (wk[14][2]*wk[14][2]); w2w2[10] = (wk[15][2]*wk[15][2]); w2w2[11] = (wk[16][2]*wk[16][2]); w2w2[12] = (wk[17][2]*wk[17][2]); w2w2[13] = (wk[18][2]*wk[18][2]); w2w2[14] = (wk[19][2]*wk[19][2]); w2w2[15] = (wk[20][2]*wk[20][2]); w0w1[0] = (u[3]*u[4]); w0w1[3] = (wk[8][0]*wk[8][1]); w0w1[4] = (wk[9][0]*wk[9][1]); w0w1[5] = (wk[10][0]*wk[10][1]); w0w1[6] = (wk[11][0]*wk[11][1]); w0w1[7] = (wk[12][0]*wk[12][1]); w0w1[8] = (wk[13][0]*wk[13][1]); w0w1[9] = (wk[14][0]*wk[14][1]); w0w1[10] = (wk[15][0]*wk[15][1]); w0w1[11] = (wk[16][0]*wk[16][1]); w0w1[12] = (wk[17][0]*wk[17][1]); w0w1[13] = (wk[18][0]*wk[18][1]); w0w1[14] = (wk[19][0]*wk[19][1]); w0w1[15] = (wk[20][0]*wk[20][1]); w0w2[0] = (u[3]*u[5]); w0w2[3] = (wk[8][0]*wk[8][2]); w0w2[4] = (wk[9][0]*wk[9][2]); w0w2[5] = (wk[10][0]*wk[10][2]); w0w2[6] = (wk[11][0]*wk[11][2]); w0w2[7] = (wk[12][0]*wk[12][2]); w0w2[8] = (wk[13][0]*wk[13][2]); w0w2[9] = (wk[14][0]*wk[14][2]); w0w2[10] = (wk[15][0]*wk[15][2]); w0w2[11] = (wk[16][0]*wk[16][2]); w0w2[12] = (wk[17][0]*wk[17][2]); w0w2[13] = (wk[18][0]*wk[18][2]); w0w2[14] = (wk[19][0]*wk[19][2]); w0w2[15] = (wk[20][0]*wk[20][2]); w1w2[0] = (u[4]*u[5]); w1w2[3] = (wk[8][1]*wk[8][2]); w1w2[4] = (wk[9][1]*wk[9][2]); w1w2[5] = (wk[10][1]*wk[10][2]); w1w2[6] = (wk[11][1]*wk[11][2]); w1w2[7] = (wk[12][1]*wk[12][2]); w1w2[8] = (wk[13][1]*wk[13][2]); w1w2[9] = (wk[14][1]*wk[14][2]); w1w2[10] = (wk[15][1]*wk[15][2]); w1w2[11] = (wk[16][1]*wk[16][2]); w1w2[12] = (wk[17][1]*wk[17][2]); w1w2[13] = (wk[18][1]*wk[18][2]); w1w2[14] = (wk[19][1]*wk[19][2]); w1w2[15] = (wk[20][1]*wk[20][2]); w00w11[0] = -(w0w0[0]+w1w1[0]); w00w11[3] = -(w0w0[3]+w1w1[3]); w00w11[4] = -(w0w0[4]+w1w1[4]); w00w11[5] = -(w0w0[5]+w1w1[5]); w00w11[6] = -(w0w0[6]+w1w1[6]); w00w11[7] = -(w0w0[7]+w1w1[7]); w00w11[8] = -(w0w0[8]+w1w1[8]); w00w11[9] = -(w0w0[9]+w1w1[9]); w00w11[10] = -(w0w0[10]+w1w1[10]); w00w11[11] = -(w0w0[11]+w1w1[11]); w00w11[12] = -(w0w0[12]+w1w1[12]); w00w11[13] = -(w0w0[13]+w1w1[13]); w00w11[14] = -(w0w0[14]+w1w1[14]); w00w11[15] = -(w0w0[15]+w1w1[15]); w00w22[0] = -(w0w0[0]+w2w2[0]); w00w22[3] = -(w0w0[3]+w2w2[3]); w00w22[4] = -(w0w0[4]+w2w2[4]); w00w22[5] = -(w0w0[5]+w2w2[5]); w00w22[6] = -(w0w0[6]+w2w2[6]); w00w22[7] = -(w0w0[7]+w2w2[7]); w00w22[8] = -(w0w0[8]+w2w2[8]); w00w22[9] = -(w0w0[9]+w2w2[9]); w00w22[10] = -(w0w0[10]+w2w2[10]); w00w22[11] = -(w0w0[11]+w2w2[11]); w00w22[12] = -(w0w0[12]+w2w2[12]); w00w22[13] = -(w0w0[13]+w2w2[13]); w00w22[14] = -(w0w0[14]+w2w2[14]); w00w22[15] = -(w0w0[15]+w2w2[15]); w11w22[0] = -(w1w1[0]+w2w2[0]); w11w22[3] = -(w1w1[3]+w2w2[3]); w11w22[4] = -(w1w1[4]+w2w2[4]); w11w22[5] = -(w1w1[5]+w2w2[5]); w11w22[6] = -(w1w1[6]+w2w2[6]); w11w22[7] = -(w1w1[7]+w2w2[7]); w11w22[8] = -(w1w1[8]+w2w2[8]); w11w22[9] = -(w1w1[9]+w2w2[9]); w11w22[10] = -(w1w1[10]+w2w2[10]); w11w22[11] = -(w1w1[11]+w2w2[11]); w11w22[12] = -(w1w1[12]+w2w2[12]); w11w22[13] = -(w1w1[13]+w2w2[13]); w11w22[14] = -(w1w1[14]+w2w2[14]); w11w22[15] = -(w1w1[15]+w2w2[15]); /* Compute vnk & vnb (mass center linear velocities in N) */ vnk[6][0] = -((5.201e-5*(u[6]*s6))+(1.2242604*(u[6]*c6))); vnk[6][2] = ((1.2242604*(u[6]*s6))-(5.201e-5*(u[6]*c6))); vnk[7][0] = -((5.197e-5*(u[7]*s7))+(1.22369868*(u[7]*c7))); vnk[7][2] = ((1.22369868*(u[7]*s7))-(5.197e-5*(u[7]*c7))); vnk[8][0] = ((u[0]+((Cik[3][0][2]*Wirk[8][2])+((Cik[3][0][0]*Wirk[8][0])+( Cik[3][0][1]*Wirk[8][1]))))+((cnk[8][0][2]*Wkrpk[8][2])+((Cik[3][0][1]* Wkrpk[8][1])+(cnk[8][0][0]*Wkrpk[8][0])))); vnk[8][1] = ((u[1]+((Cik[3][1][2]*Wirk[8][2])+((Cik[3][1][0]*Wirk[8][0])+( Cik[3][1][1]*Wirk[8][1]))))+((cnk[8][1][2]*Wkrpk[8][2])+((Cik[3][1][1]* Wkrpk[8][1])+(cnk[8][1][0]*Wkrpk[8][0])))); vnk[8][2] = ((u[2]+((Cik[3][2][2]*Wirk[8][2])+((Cik[3][2][0]*Wirk[8][0])+( Cik[3][2][1]*Wirk[8][1]))))+((cnk[8][2][2]*Wkrpk[8][2])+((Cik[3][2][1]* Wkrpk[8][1])+(cnk[8][2][0]*Wkrpk[8][0])))); vnk[9][0] = ((u[0]+((Cik[3][0][2]*Wirk[9][2])+((Cik[3][0][0]*Wirk[9][0])+( Cik[3][0][1]*Wirk[9][1]))))+((cnk[9][0][2]*Wkrpk[9][2])+((Cik[3][0][1]* Wkrpk[9][1])+(cnk[9][0][0]*Wkrpk[9][0])))); vnk[9][1] = ((u[1]+((Cik[3][1][2]*Wirk[9][2])+((Cik[3][1][0]*Wirk[9][0])+( Cik[3][1][1]*Wirk[9][1]))))+((cnk[9][1][2]*Wkrpk[9][2])+((Cik[3][1][1]* Wkrpk[9][1])+(cnk[9][1][0]*Wkrpk[9][0])))); vnk[9][2] = ((u[2]+((Cik[3][2][2]*Wirk[9][2])+((Cik[3][2][0]*Wirk[9][0])+( Cik[3][2][1]*Wirk[9][1]))))+((cnk[9][2][2]*Wkrpk[9][2])+((Cik[3][2][1]* Wkrpk[9][1])+(cnk[9][2][0]*Wkrpk[9][0])))); vnk[10][0] = ((u[0]+((Cik[3][0][2]*Wirk[10][2])+((Cik[3][0][0]*Wirk[10][0])+ (Cik[3][0][1]*Wirk[10][1]))))+((cnk[10][0][2]*Wkrpk[10][2])+((Cik[3][0][1] *Wkrpk[10][1])+(cnk[10][0][0]*Wkrpk[10][0])))); vnk[10][1] = ((u[1]+((Cik[3][1][2]*Wirk[10][2])+((Cik[3][1][0]*Wirk[10][0])+ (Cik[3][1][1]*Wirk[10][1]))))+((cnk[10][1][2]*Wkrpk[10][2])+((Cik[3][1][1] *Wkrpk[10][1])+(cnk[10][1][0]*Wkrpk[10][0])))); vnk[10][2] = ((u[2]+((Cik[3][2][2]*Wirk[10][2])+((Cik[3][2][0]*Wirk[10][0])+ (Cik[3][2][1]*Wirk[10][1]))))+((cnk[10][2][2]*Wkrpk[10][2])+((Cik[3][2][1] *Wkrpk[10][1])+(cnk[10][2][0]*Wkrpk[10][0])))); vnk[11][0] = ((vnk[9][0]+((cnk[9][0][2]*Wirk[11][2])+((Cik[3][0][1]* Wirk[11][1])+(cnk[9][0][0]*Wirk[11][0]))))+((cnk[11][0][2]*Wkrpk[11][2])+( (Cik[3][0][1]*Wkrpk[11][1])+(cnk[11][0][0]*Wkrpk[11][0])))); vnk[11][1] = ((vnk[9][1]+((cnk[9][1][2]*Wirk[11][2])+((Cik[3][1][1]* Wirk[11][1])+(cnk[9][1][0]*Wirk[11][0]))))+((cnk[11][1][2]*Wkrpk[11][2])+( (Cik[3][1][1]*Wkrpk[11][1])+(cnk[11][1][0]*Wkrpk[11][0])))); vnk[11][2] = ((vnk[9][2]+((cnk[9][2][2]*Wirk[11][2])+((Cik[3][2][1]* Wirk[11][1])+(cnk[9][2][0]*Wirk[11][0]))))+((cnk[11][2][2]*Wkrpk[11][2])+( (Cik[3][2][1]*Wkrpk[11][1])+(cnk[11][2][0]*Wkrpk[11][0])))); vnk[12][0] = ((vnk[10][0]+((cnk[10][0][2]*Wirk[12][2])+((Cik[3][0][1]* Wirk[12][1])+(cnk[10][0][0]*Wirk[12][0]))))+((cnk[12][0][2]*Wkrpk[12][2])+ ((Cik[3][0][1]*Wkrpk[12][1])+(cnk[12][0][0]*Wkrpk[12][0])))); vnk[12][1] = ((vnk[10][1]+((cnk[10][1][2]*Wirk[12][2])+((Cik[3][1][1]* Wirk[12][1])+(cnk[10][1][0]*Wirk[12][0]))))+((cnk[12][1][2]*Wkrpk[12][2])+ ((Cik[3][1][1]*Wkrpk[12][1])+(cnk[12][1][0]*Wkrpk[12][0])))); vnk[12][2] = ((vnk[10][2]+((cnk[10][2][2]*Wirk[12][2])+((Cik[3][2][1]* Wirk[12][1])+(cnk[10][2][0]*Wirk[12][0]))))+((cnk[12][2][2]*Wkrpk[12][2])+ ((Cik[3][2][1]*Wkrpk[12][1])+(cnk[12][2][0]*Wkrpk[12][0])))); vnk[13][0] = ((vnk[11][0]+((cnk[11][0][2]*Wirk[13][2])+((Cik[3][0][1]* Wirk[13][1])+(cnk[11][0][0]*Wirk[13][0]))))+((cnk[13][0][2]*Wkrpk[13][2])+ ((Cik[3][0][1]*Wkrpk[13][1])+(cnk[13][0][0]*Wkrpk[13][0])))); vnk[13][1] = ((vnk[11][1]+((cnk[11][1][2]*Wirk[13][2])+((Cik[3][1][1]* Wirk[13][1])+(cnk[11][1][0]*Wirk[13][0]))))+((cnk[13][1][2]*Wkrpk[13][2])+ ((Cik[3][1][1]*Wkrpk[13][1])+(cnk[13][1][0]*Wkrpk[13][0])))); vnk[13][2] = ((vnk[11][2]+((cnk[11][2][2]*Wirk[13][2])+((Cik[3][2][1]* Wirk[13][1])+(cnk[11][2][0]*Wirk[13][0]))))+((cnk[13][2][2]*Wkrpk[13][2])+ ((Cik[3][2][1]*Wkrpk[13][1])+(cnk[13][2][0]*Wkrpk[13][0])))); vnk[14][0] = ((vnk[12][0]+((cnk[12][0][2]*Wirk[14][2])+((Cik[3][0][1]* Wirk[14][1])+(cnk[12][0][0]*Wirk[14][0]))))+((cnk[14][0][2]*Wkrpk[14][2])+ ((Cik[3][0][1]*Wkrpk[14][1])+(cnk[14][0][0]*Wkrpk[14][0])))); vnk[14][1] = ((vnk[12][1]+((cnk[12][1][2]*Wirk[14][2])+((Cik[3][1][1]* Wirk[14][1])+(cnk[12][1][0]*Wirk[14][0]))))+((cnk[14][1][2]*Wkrpk[14][2])+ ((Cik[3][1][1]*Wkrpk[14][1])+(cnk[14][1][0]*Wkrpk[14][0])))); vnk[14][2] = ((vnk[12][2]+((cnk[12][2][2]*Wirk[14][2])+((Cik[3][2][1]* Wirk[14][1])+(cnk[12][2][0]*Wirk[14][0]))))+((cnk[14][2][2]*Wkrpk[14][2])+ ((Cik[3][2][1]*Wkrpk[14][1])+(cnk[14][2][0]*Wkrpk[14][0])))); vnk[15][0] = ((u[0]+((Cik[3][0][2]*Wirk[15][2])+((Cik[3][0][0]*Wirk[15][0])+ (Cik[3][0][1]*Wirk[15][1]))))+((cnk[15][0][2]*Wkrpk[15][2])+((Cik[3][0][1] *Wkrpk[15][1])+(cnk[15][0][0]*Wkrpk[15][0])))); vnk[15][1] = ((u[1]+((Cik[3][1][2]*Wirk[15][2])+((Cik[3][1][0]*Wirk[15][0])+ (Cik[3][1][1]*Wirk[15][1]))))+((cnk[15][1][2]*Wkrpk[15][2])+((Cik[3][1][1] *Wkrpk[15][1])+(cnk[15][1][0]*Wkrpk[15][0])))); vnk[15][2] = ((u[2]+((Cik[3][2][2]*Wirk[15][2])+((Cik[3][2][0]*Wirk[15][0])+ (Cik[3][2][1]*Wirk[15][1]))))+((cnk[15][2][2]*Wkrpk[15][2])+((Cik[3][2][1] *Wkrpk[15][1])+(cnk[15][2][0]*Wkrpk[15][0])))); vnk[16][0] = ((u[0]+((Cik[3][0][2]*Wirk[16][2])+((Cik[3][0][0]*Wirk[16][0])+ (Cik[3][0][1]*Wirk[16][1]))))+((cnk[16][0][2]*Wkrpk[16][2])+((Cik[3][0][1] *Wkrpk[16][1])+(cnk[16][0][0]*Wkrpk[16][0])))); vnk[16][1] = ((u[1]+((Cik[3][1][2]*Wirk[16][2])+((Cik[3][1][0]*Wirk[16][0])+ (Cik[3][1][1]*Wirk[16][1]))))+((cnk[16][1][2]*Wkrpk[16][2])+((Cik[3][1][1] *Wkrpk[16][1])+(cnk[16][1][0]*Wkrpk[16][0])))); vnk[16][2] = ((u[2]+((Cik[3][2][2]*Wirk[16][2])+((Cik[3][2][0]*Wirk[16][0])+ (Cik[3][2][1]*Wirk[16][1]))))+((cnk[16][2][2]*Wkrpk[16][2])+((Cik[3][2][1] *Wkrpk[16][1])+(cnk[16][2][0]*Wkrpk[16][0])))); vnk[17][0] = ((vnk[15][0]+((cnk[15][0][2]*Wirk[17][2])+((Cik[3][0][1]* Wirk[17][1])+(cnk[15][0][0]*Wirk[17][0]))))+((cnk[17][0][2]*Wkrpk[17][2])+ ((Cik[3][0][1]*Wkrpk[17][1])+(cnk[17][0][0]*Wkrpk[17][0])))); vnk[17][1] = ((vnk[15][1]+((cnk[15][1][2]*Wirk[17][2])+((Cik[3][1][1]* Wirk[17][1])+(cnk[15][1][0]*Wirk[17][0]))))+((cnk[17][1][2]*Wkrpk[17][2])+ ((Cik[3][1][1]*Wkrpk[17][1])+(cnk[17][1][0]*Wkrpk[17][0])))); vnk[17][2] = ((vnk[15][2]+((cnk[15][2][2]*Wirk[17][2])+((Cik[3][2][1]* Wirk[17][1])+(cnk[15][2][0]*Wirk[17][0]))))+((cnk[17][2][2]*Wkrpk[17][2])+ ((Cik[3][2][1]*Wkrpk[17][1])+(cnk[17][2][0]*Wkrpk[17][0])))); vnk[18][0] = ((vnk[16][0]+((cnk[16][0][2]*Wirk[18][2])+((Cik[3][0][1]* Wirk[18][1])+(cnk[16][0][0]*Wirk[18][0]))))+((cnk[18][0][2]*Wkrpk[18][2])+ ((Cik[3][0][1]*Wkrpk[18][1])+(cnk[18][0][0]*Wkrpk[18][0])))); vnk[18][1] = ((vnk[16][1]+((cnk[16][1][2]*Wirk[18][2])+((Cik[3][1][1]* Wirk[18][1])+(cnk[16][1][0]*Wirk[18][0]))))+((cnk[18][1][2]*Wkrpk[18][2])+ ((Cik[3][1][1]*Wkrpk[18][1])+(cnk[18][1][0]*Wkrpk[18][0])))); vnk[18][2] = ((vnk[16][2]+((cnk[16][2][2]*Wirk[18][2])+((Cik[3][2][1]* Wirk[18][1])+(cnk[16][2][0]*Wirk[18][0]))))+((cnk[18][2][2]*Wkrpk[18][2])+ ((Cik[3][2][1]*Wkrpk[18][1])+(cnk[18][2][0]*Wkrpk[18][0])))); vnk[19][0] = ((vnk[17][0]+((cnk[17][0][2]*Wirk[19][2])+((Cik[3][0][1]* Wirk[19][1])+(cnk[17][0][0]*Wirk[19][0]))))+((cnk[19][0][2]*Wkrpk[19][2])+ ((Cik[3][0][1]*Wkrpk[19][1])+(cnk[19][0][0]*Wkrpk[19][0])))); vnk[19][1] = ((vnk[17][1]+((cnk[17][1][2]*Wirk[19][2])+((Cik[3][1][1]* Wirk[19][1])+(cnk[17][1][0]*Wirk[19][0]))))+((cnk[19][1][2]*Wkrpk[19][2])+ ((Cik[3][1][1]*Wkrpk[19][1])+(cnk[19][1][0]*Wkrpk[19][0])))); vnk[19][2] = ((vnk[17][2]+((cnk[17][2][2]*Wirk[19][2])+((Cik[3][2][1]* Wirk[19][1])+(cnk[17][2][0]*Wirk[19][0]))))+((cnk[19][2][2]*Wkrpk[19][2])+ ((Cik[3][2][1]*Wkrpk[19][1])+(cnk[19][2][0]*Wkrpk[19][0])))); vnk[20][0] = ((vnk[18][0]+((cnk[18][0][2]*Wirk[20][2])+((Cik[3][0][1]* Wirk[20][1])+(cnk[18][0][0]*Wirk[20][0]))))+((cnk[20][0][2]*Wkrpk[20][2])+ ((Cik[3][0][1]*Wkrpk[20][1])+(cnk[20][0][0]*Wkrpk[20][0])))); vnk[20][1] = ((vnk[18][1]+((cnk[18][1][2]*Wirk[20][2])+((Cik[3][1][1]* Wirk[20][1])+(cnk[18][1][0]*Wirk[20][0]))))+((cnk[20][1][2]*Wkrpk[20][2])+ ((Cik[3][1][1]*Wkrpk[20][1])+(cnk[20][1][0]*Wkrpk[20][0])))); vnk[20][2] = ((vnk[18][2]+((cnk[18][2][2]*Wirk[20][2])+((Cik[3][2][1]* Wirk[20][1])+(cnk[18][2][0]*Wirk[20][0]))))+((cnk[20][2][2]*Wkrpk[20][2])+ ((Cik[3][2][1]*Wkrpk[20][1])+(cnk[20][2][0]*Wkrpk[20][0])))); vnk[21][0] = ((vnk[6][0]+((5.201e-5*(u[6]*s6))-(.4177396*(u[6]*c6))))-(( 5.2e-5*(cnk[21][0][2]*wk[21][1]))+(.28373605*(cnk[21][0][0]*wk[21][1])))); vnk[21][2] = ((vnk[6][2]+((5.201e-5*(u[6]*c6))+(.4177396*(u[6]*s6))))-(( 5.2e-5*(cnk[21][2][2]*wk[21][1]))+(.28373605*(cnk[21][2][0]*wk[21][1])))); vnk[22][0] = ((vnk[7][0]+((5.197e-5*(u[7]*s7))-(.41830132*(u[7]*c7))))-(( 5.194e-5*(cnk[22][0][2]*wk[22][1]))+(.28317448*(cnk[22][0][0]*wk[22][1]))) ); vnk[22][2] = ((vnk[7][2]+((5.197e-5*(u[7]*c7))+(.41830132*(u[7]*s7))))-(( 5.194e-5*(cnk[22][2][2]*wk[22][1]))+(.28317448*(cnk[22][2][0]*wk[22][1]))) ); vnb[0][0] = u[0]; vnb[0][1] = u[1]; vnb[0][2] = u[2]; vnb[1][0] = vnk[6][0]; vnb[1][1] = 0.; vnb[1][2] = vnk[6][2]; vnb[2][0] = vnk[7][0]; vnb[2][1] = 0.; vnb[2][2] = vnk[7][2]; vnb[3][0] = vnk[8][0]; vnb[3][1] = vnk[8][1]; vnb[3][2] = vnk[8][2]; vnb[4][0] = vnk[9][0]; vnb[4][1] = vnk[9][1]; vnb[4][2] = vnk[9][2]; vnb[5][0] = vnk[10][0]; vnb[5][1] = vnk[10][1]; vnb[5][2] = vnk[10][2]; vnb[6][0] = vnk[11][0]; vnb[6][1] = vnk[11][1]; vnb[6][2] = vnk[11][2]; vnb[7][0] = vnk[12][0]; vnb[7][1] = vnk[12][1]; vnb[7][2] = vnk[12][2]; vnb[8][0] = vnk[13][0]; vnb[8][1] = vnk[13][1]; vnb[8][2] = vnk[13][2]; vnb[9][0] = vnk[14][0]; vnb[9][1] = vnk[14][1]; vnb[9][2] = vnk[14][2]; vnb[10][0] = vnk[15][0]; vnb[10][1] = vnk[15][1]; vnb[10][2] = vnk[15][2]; vnb[11][0] = vnk[16][0]; vnb[11][1] = vnk[16][1]; vnb[11][2] = vnk[16][2]; vnb[12][0] = vnk[17][0]; vnb[12][1] = vnk[17][1]; vnb[12][2] = vnk[17][2]; vnb[13][0] = vnk[18][0]; vnb[13][1] = vnk[18][1]; vnb[13][2] = vnk[18][2]; vnb[14][0] = vnk[19][0]; vnb[14][1] = vnk[19][1]; vnb[14][2] = vnk[19][2]; vnb[15][0] = vnk[20][0]; vnb[15][1] = vnk[20][1]; vnb[15][2] = vnk[20][2]; vnb[16][0] = vnk[21][0]; vnb[16][1] = 0.; vnb[16][2] = vnk[21][2]; vnb[17][0] = vnk[22][0]; vnb[17][1] = 0.; vnb[17][2] = vnk[22][2]; /* Compute qdot (kinematical equations) */ qdot[0] = u[0]; qdot[1] = u[1]; qdot[2] = u[2]; qdot[3] = (.5*((q[23]*u[3])+((q[4]*u[5])-(q[5]*u[4])))); qdot[4] = (.5*((q[5]*u[3])+((q[23]*u[4])-(q[3]*u[5])))); qdot[5] = (.5*(((q[3]*u[4])+(q[23]*u[5]))-(q[4]*u[3]))); qdot[23] = -(.5*((q[3]*u[3])+((q[4]*u[4])+(q[5]*u[5])))); if (stabvel != 0.) { ee = ((q[23]*q[23])+((q[5]*q[5])+((q[3]*q[3])+(q[4]*q[4])))); stab = ((stabvel*(1.-ee))/ee); qdot[3] = (qdot[3]+(q[3]*stab)); qdot[4] = (qdot[4]+(q[4]*stab)); qdot[5] = (qdot[5]+(q[5]*stab)); qdot[23] = (qdot[23]+(q[23]*stab)); } qdot[6] = u[6]; qdot[7] = u[7]; qdot[8] = u[8]; qdot[9] = u[9]; qdot[10] = u[10]; qdot[11] = u[11]; qdot[12] = u[12]; qdot[13] = u[13]; qdot[14] = u[14]; qdot[15] = u[15]; qdot[16] = u[16]; qdot[17] = u[17]; qdot[18] = u[18]; qdot[19] = u[19]; qdot[20] = u[20]; qdot[21] = u[21]; qdot[22] = u[22]; skipus: ; /* Initialize applied forces and torques to zero */ for (i = 0; i < 18; i++) { for (j = 0; j < 3; j++) { ufk[i][j] = 0.; utk[i][j] = 0.; } } for (i = 0; i < 23; i++) { utau[i] = 0.; } ltauflg = 0; fs0flg = 0; /* Used 1.58 seconds CPU time, 98304 additional bytes of memory. Equations contain 1169 adds/subtracts/negates 1521 multiplies 1 divides 1151 assignments */ } void BOCfly :: sdqdot ( double oqdot[24] ) { /* Return position coordinate derivatives for tree joints. */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(63,23); return; } for (i = 0; i <= 23; i++) { oqdot[i] = qdot[i]; } } void BOCfly :: sdu2qdot ( double uin[23], double oqdot[24] ) { /* Convert velocities to qdots. */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(64,23); return; } for (i = 0; i <= 22; i++) { oqdot[i] = uin[i]; } oqdot[3] = (.5*((q[23]*uin[3])+((q[4]*uin[5])-(q[5]*uin[4])))); oqdot[4] = (.5*((q[5]*uin[3])+((q[23]*uin[4])-(q[3]*uin[5])))); oqdot[5] = (.5*(((q[3]*uin[4])+(q[23]*uin[5]))-(q[4]*uin[3]))); oqdot[23] = -(.5*((q[3]*uin[3])+((q[4]*uin[4])+(q[5]*uin[5])))); /* Used 0.01 seconds CPU time, 0 additional bytes of memory. Equations contain 9 adds/subtracts/negates 16 multiplies 0 divides 27 assignments */ } void BOCfly :: sdpsstate ( double lqin[1] ) { if (roustate != 2) { sdseterr(9,23); return; } } void BOCfly :: sddoping ( ) { if (vpkflg == 0) { /* Compute ping (jt pins in ground frame) */ ping[0][0] = 1.; ping[1][1] = 1.; ping[2][2] = 1.; ping[3][0] = Cik[3][0][0]; ping[3][1] = Cik[3][1][0]; ping[3][2] = Cik[3][2][0]; ping[4][0] = Cik[3][0][1]; ping[4][1] = Cik[3][1][1]; ping[4][2] = Cik[3][2][1]; ping[5][0] = Cik[3][0][2]; ping[5][1] = Cik[3][1][2]; ping[5][2] = Cik[3][2][2]; ping[6][1] = 1.; ping[7][1] = 1.; ping[8][0] = Cik[3][0][1]; ping[8][1] = Cik[3][1][1]; ping[8][2] = Cik[3][2][1]; ping[9][0] = Cik[3][0][1]; ping[9][1] = Cik[3][1][1]; ping[9][2] = Cik[3][2][1]; ping[10][0] = Cik[3][0][1]; ping[10][1] = Cik[3][1][1]; ping[10][2] = Cik[3][2][1]; ping[11][0] = Cik[3][0][1]; ping[11][1] = Cik[3][1][1]; ping[11][2] = Cik[3][2][1]; ping[12][0] = Cik[3][0][1]; ping[12][1] = Cik[3][1][1]; ping[12][2] = Cik[3][2][1]; ping[13][0] = Cik[3][0][1]; ping[13][1] = Cik[3][1][1]; ping[13][2] = Cik[3][2][1]; ping[14][0] = Cik[3][0][1]; ping[14][1] = Cik[3][1][1]; ping[14][2] = Cik[3][2][1]; ping[15][0] = Cik[3][0][1]; ping[15][1] = Cik[3][1][1]; ping[15][2] = Cik[3][2][1]; ping[16][0] = Cik[3][0][1]; ping[16][1] = Cik[3][1][1]; ping[16][2] = Cik[3][2][1]; ping[17][0] = Cik[3][0][1]; ping[17][1] = Cik[3][1][1]; ping[17][2] = Cik[3][2][1]; ping[18][0] = Cik[3][0][1]; ping[18][1] = Cik[3][1][1]; ping[18][2] = Cik[3][2][1]; ping[19][0] = Cik[3][0][1]; ping[19][1] = Cik[3][1][1]; ping[19][2] = Cik[3][2][1]; ping[20][0] = Cik[3][0][1]; ping[20][1] = Cik[3][1][1]; ping[20][2] = Cik[3][2][1]; ping[21][1] = 1.; ping[22][1] = 1.; /* Compute hngpt (hinge pts in ground frame) */ hngpt[0][0] = q[0]; hngpt[1][0] = q[0]; hngpt[1][1] = q[1]; hngpt[2][0] = q[0]; hngpt[2][1] = q[1]; hngpt[2][2] = q[2]; hngpt[3][0] = q[0]; hngpt[3][1] = q[1]; hngpt[3][2] = q[2]; hngpt[4][0] = q[0]; hngpt[4][1] = q[1]; hngpt[4][2] = q[2]; hngpt[5][0] = q[0]; hngpt[5][1] = q[1]; hngpt[5][2] = q[2]; hngpt[6][0] = (rnk[6][0]+((1.2242604*s6)-(5.201e-5*c6))); hngpt[6][1] = .156437; hngpt[6][2] = (rnk[6][2]+((5.201e-5*s6)+(1.2242604*c6))); hngpt[7][0] = (rnk[7][0]+((1.22369868*s7)-(5.197e-5*c7))); hngpt[7][1] = -.156437; hngpt[7][2] = (rnk[7][2]+((5.197e-5*s7)+(1.22369868*c7))); hngpt[8][0] = (rnk[8][0]+((.12000071*cnk[8][0][2])-((.0007183* Cik[3][0][1])+(.01691003*cnk[8][0][0])))); hngpt[8][1] = (rnk[8][1]+((.12000071*cnk[8][1][2])-((.0007183* Cik[3][1][1])+(.01691003*cnk[8][1][0])))); hngpt[8][2] = (rnk[8][2]+((.12000071*cnk[8][2][2])-((.0007183* Cik[3][2][1])+(.01691003*cnk[8][2][0])))); hngpt[9][0] = (rnk[9][0]+((.13168857*cnk[9][0][2])+((.00015877* Cik[3][0][1])-(.0094289*cnk[9][0][0])))); hngpt[9][1] = (rnk[9][1]+((.13168857*cnk[9][1][2])+((.00015877* Cik[3][1][1])-(.0094289*cnk[9][1][0])))); hngpt[9][2] = (rnk[9][2]+((.13168857*cnk[9][2][2])+((.00015877* Cik[3][2][1])-(.0094289*cnk[9][2][0])))); hngpt[10][0] = (rnk[10][0]+((.13190398*cnk[10][0][2])-((.00235524* Cik[3][0][1])+(.00943639*cnk[10][0][0])))); hngpt[10][1] = (rnk[10][1]+((.13190398*cnk[10][1][2])-((.00235524* Cik[3][1][1])+(.00943639*cnk[10][1][0])))); hngpt[10][2] = (rnk[10][2]+((.13190398*cnk[10][2][2])-((.00235524* Cik[3][2][1])+(.00943639*cnk[10][2][0])))); hngpt[11][0] = (rnk[11][0]+((.12618299*cnk[11][0][2])+((.01672166* cnk[11][0][0])-(.00561135*Cik[3][0][1])))); hngpt[11][1] = (rnk[11][1]+((.12618299*cnk[11][1][2])+((.01672166* cnk[11][1][0])-(.00561135*Cik[3][1][1])))); hngpt[11][2] = (rnk[11][2]+((.12618299*cnk[11][2][2])+((.01672166* cnk[11][2][0])-(.00561135*Cik[3][2][1])))); hngpt[12][0] = (rnk[12][0]+((.12587504*cnk[12][0][2])+((.00342896* Cik[3][0][1])+(.01674373*cnk[12][0][0])))); hngpt[12][1] = (rnk[12][1]+((.12587504*cnk[12][1][2])+((.00342896* Cik[3][1][1])+(.01674373*cnk[12][1][0])))); hngpt[12][2] = (rnk[12][2]+((.12587504*cnk[12][2][2])+((.00342896* Cik[3][2][1])+(.01674373*cnk[12][2][0])))); hngpt[13][0] = (rnk[13][0]+((.03983908*cnk[13][0][2])-((.00193095* Cik[3][0][1])+(.03543467*cnk[13][0][0])))); hngpt[13][1] = (rnk[13][1]+((.03983908*cnk[13][1][2])-((.00193095* Cik[3][1][1])+(.03543467*cnk[13][1][0])))); hngpt[13][2] = (rnk[13][2]+((.03983908*cnk[13][2][2])-((.00193095* Cik[3][2][1])+(.03543467*cnk[13][2][0])))); hngpt[14][0] = (rnk[14][0]+((.03987912*cnk[14][0][2])-((.00095499* Cik[3][0][1])+(.03539679*cnk[14][0][0])))); hngpt[14][1] = (rnk[14][1]+((.03987912*cnk[14][1][2])-((.00095499* Cik[3][1][1])+(.03539679*cnk[14][1][0])))); hngpt[14][2] = (rnk[14][2]+((.03987912*cnk[14][2][2])-((.00095499* Cik[3][2][1])+(.03539679*cnk[14][2][0])))); hngpt[15][0] = (rnk[15][0]+((.20905614*cnk[15][0][2])+((.00885657* Cik[3][0][1])-(.00307461*cnk[15][0][0])))); hngpt[15][1] = (rnk[15][1]+((.20905614*cnk[15][1][2])+((.00885657* Cik[3][1][1])-(.00307461*cnk[15][1][0])))); hngpt[15][2] = (rnk[15][2]+((.20905614*cnk[15][2][2])+((.00885657* Cik[3][2][1])-(.00307461*cnk[15][2][0])))); hngpt[16][0] = (rnk[16][0]+((.21008605*cnk[16][0][2])-((.00305957* cnk[16][0][0])+(.00995927*Cik[3][0][1])))); hngpt[16][1] = (rnk[16][1]+((.21008605*cnk[16][1][2])-((.00305957* cnk[16][1][0])+(.00995927*Cik[3][1][1])))); hngpt[16][2] = (rnk[16][2]+((.21008605*cnk[16][2][2])-((.00305957* cnk[16][2][0])+(.00995927*Cik[3][2][1])))); hngpt[17][0] = (rnk[17][0]+((.15249501*cnk[17][0][2])-((.00055916* Cik[3][0][1])+(.01407264*cnk[17][0][0])))); hngpt[17][1] = (rnk[17][1]+((.15249501*cnk[17][1][2])-((.00055916* Cik[3][1][1])+(.01407264*cnk[17][1][0])))); hngpt[17][2] = (rnk[17][2]+((.15249501*cnk[17][2][2])-((.00055916* Cik[3][2][1])+(.01407264*cnk[17][2][0])))); hngpt[18][0] = (rnk[18][0]+((.15260355*cnk[18][0][2])-((.00074575* Cik[3][0][1])+(.01407902*cnk[18][0][0])))); hngpt[18][1] = (rnk[18][1]+((.15260355*cnk[18][1][2])-((.00074575* Cik[3][1][1])+(.01407902*cnk[18][1][0])))); hngpt[18][2] = (rnk[18][2]+((.15260355*cnk[18][2][2])-((.00074575* Cik[3][2][1])+(.01407902*cnk[18][2][0])))); hngpt[19][0] = (rnk[19][0]+(((.00486049*Cik[3][0][1])-(.01360932* cnk[19][0][0]))-(.01387793*cnk[19][0][2]))); hngpt[19][1] = (rnk[19][1]+(((.00486049*Cik[3][1][1])-(.01360932* cnk[19][1][0]))-(.01387793*cnk[19][1][2]))); hngpt[19][2] = (rnk[19][2]+(((.00486049*Cik[3][2][1])-(.01360932* cnk[19][2][0]))-(.01387793*cnk[19][2][2]))); hngpt[20][0] = (rnk[20][0]-((.01382852*cnk[20][0][2])+((.00560346* Cik[3][0][1])+(.01361601*cnk[20][0][0])))); hngpt[20][1] = (rnk[20][1]-((.01382852*cnk[20][1][2])+((.00560346* Cik[3][1][1])+(.01361601*cnk[20][1][0])))); hngpt[20][2] = (rnk[20][2]-((.01382852*cnk[20][2][2])+((.00560346* Cik[3][2][1])+(.01361601*cnk[20][2][0])))); hngpt[21][0] = (rnk[21][0]+((.28373605*cnk[21][0][2])-(5.2e-5* cnk[21][0][0]))); hngpt[21][1] = .156437; hngpt[21][2] = (rnk[21][2]+((.28373605*cnk[21][2][2])-(5.2e-5* cnk[21][2][0]))); hngpt[22][0] = (rnk[22][0]+((.28317448*cnk[22][0][2])-(5.194e-5* cnk[22][0][0]))); hngpt[22][1] = -.156437; hngpt[22][2] = (rnk[22][2]+((.28317448*cnk[22][2][2])-(5.194e-5* cnk[22][2][0]))); vpkflg = 1; } /* Used 0.13 seconds CPU time, 0 additional bytes of memory. Equations contain 133 adds/subtracts/negates 133 multiplies 0 divides 121 assignments */ } void BOCfly :: sddoltau ( ) { /* Compute effect of loop hinge torques */ /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 0 assignments */ } void BOCfly :: sddoiner ( ) { /* Compute inertial accelerations and related temps */ if (inerflg == 0) { /* Compute Otk (inertial angular acceleration) */ Otk[8][0] = -(u[8]*wk[8][2]); Otk[8][2] = (u[8]*wk[8][0]); Otk[9][0] = -(u[9]*wk[9][2]); Otk[9][2] = (u[9]*wk[9][0]); Otk[10][0] = -(u[10]*wk[10][2]); Otk[10][2] = (u[10]*wk[10][0]); Otk[11][0] = (((Otk[9][0]*c11)-(Otk[9][2]*s11))-(u[11]*wk[11][2])); Otk[11][2] = ((u[11]*wk[11][0])+((Otk[9][0]*s11)+(Otk[9][2]*c11))); Otk[12][0] = (((Otk[10][0]*c12)-(Otk[10][2]*s12))-(u[12]*wk[12][2])); Otk[12][2] = ((u[12]*wk[12][0])+((Otk[10][0]*s12)+(Otk[10][2]*c12))); Otk[13][0] = (((Otk[11][0]*c13)-(Otk[11][2]*s13))-(u[13]*wk[13][2])); Otk[13][2] = ((u[13]*wk[13][0])+((Otk[11][0]*s13)+(Otk[11][2]*c13))); Otk[14][0] = (((Otk[12][0]*c14)-(Otk[12][2]*s14))-(u[14]*wk[14][2])); Otk[14][2] = ((u[14]*wk[14][0])+((Otk[12][0]*s14)+(Otk[12][2]*c14))); Otk[15][0] = -(u[15]*wk[15][2]); Otk[15][2] = (u[15]*wk[15][0]); Otk[16][0] = -(u[16]*wk[16][2]); Otk[16][2] = (u[16]*wk[16][0]); Otk[17][0] = (((Otk[15][0]*c17)-(Otk[15][2]*s17))-(u[17]*wk[17][2])); Otk[17][2] = ((u[17]*wk[17][0])+((Otk[15][0]*s17)+(Otk[15][2]*c17))); Otk[18][0] = (((Otk[16][0]*c18)-(Otk[16][2]*s18))-(u[18]*wk[18][2])); Otk[18][2] = ((u[18]*wk[18][0])+((Otk[16][0]*s18)+(Otk[16][2]*c18))); Otk[19][0] = (((Otk[17][0]*c19)-(Otk[17][2]*s19))-(u[19]*wk[19][2])); Otk[19][2] = ((u[19]*wk[19][0])+((Otk[17][0]*s19)+(Otk[17][2]*c19))); Otk[20][0] = (((Otk[18][0]*c20)-(Otk[18][2]*s20))-(u[20]*wk[20][2])); Otk[20][2] = ((u[20]*wk[20][0])+((Otk[18][0]*s20)+(Otk[18][2]*c20))); /* Compute Atk (inertial linear acceleration) */ Atk[6][0] = -(5.201e-5*(u[6]*u[6])); Atk[6][2] = (1.2242604*(u[6]*u[6])); Atk[7][0] = -(5.197e-5*(u[7]*u[7])); Atk[7][2] = (1.22369868*(u[7]*u[7])); AiOiWi[8][0] = ((u[4]*Wirk[8][2])-(u[5]*Wirk[8][1])); AiOiWi[8][1] = ((u[5]*Wirk[8][0])-(u[3]*Wirk[8][2])); AiOiWi[8][2] = ((u[3]*Wirk[8][1])-(u[4]*Wirk[8][0])); Atk[8][0] = (((AiOiWi[8][0]*c8)-(AiOiWi[8][2]*s8))+(((wk[8][1]* Wkrpk[8][2])-(wk[8][2]*Wkrpk[8][1]))-(.0007183*Otk[8][2]))); Atk[8][1] = (AiOiWi[8][1]+(((.01691003*Otk[8][2])+(.12000071*Otk[8][0])) +((wk[8][2]*Wkrpk[8][0])-(wk[8][0]*Wkrpk[8][2])))); Atk[8][2] = (((.0007183*Otk[8][0])+((wk[8][0]*Wkrpk[8][1])-(wk[8][1]* Wkrpk[8][0])))+((AiOiWi[8][0]*s8)+(AiOiWi[8][2]*c8))); AiOiWi[9][0] = ((u[4]*Wirk[9][2])-(u[5]*Wirk[9][1])); AiOiWi[9][1] = ((u[5]*Wirk[9][0])-(u[3]*Wirk[9][2])); AiOiWi[9][2] = ((u[3]*Wirk[9][1])-(u[4]*Wirk[9][0])); Atk[9][0] = (((.00015877*Otk[9][2])+((wk[9][1]*Wkrpk[9][2])-(wk[9][2]* Wkrpk[9][1])))+((AiOiWi[9][0]*c9)-(AiOiWi[9][2]*s9))); Atk[9][1] = (AiOiWi[9][1]+(((.0094289*Otk[9][2])+(.13168857*Otk[9][0]))+ ((wk[9][2]*Wkrpk[9][0])-(wk[9][0]*Wkrpk[9][2])))); Atk[9][2] = (((AiOiWi[9][0]*s9)+(AiOiWi[9][2]*c9))+(((wk[9][0]* Wkrpk[9][1])-(wk[9][1]*Wkrpk[9][0]))-(.00015877*Otk[9][0]))); AiOiWi[10][0] = ((u[4]*Wirk[10][2])-(u[5]*Wirk[10][1])); AiOiWi[10][1] = ((u[5]*Wirk[10][0])-(u[3]*Wirk[10][2])); AiOiWi[10][2] = ((u[3]*Wirk[10][1])-(u[4]*Wirk[10][0])); Atk[10][0] = (((AiOiWi[10][0]*c10)-(AiOiWi[10][2]*s10))+(((wk[10][1]* Wkrpk[10][2])-(wk[10][2]*Wkrpk[10][1]))-(.00235524*Otk[10][2]))); Atk[10][1] = (AiOiWi[10][1]+(((.00943639*Otk[10][2])+(.13190398* Otk[10][0]))+((wk[10][2]*Wkrpk[10][0])-(wk[10][0]*Wkrpk[10][2])))); Atk[10][2] = (((.00235524*Otk[10][0])+((wk[10][0]*Wkrpk[10][1])-( wk[10][1]*Wkrpk[10][0])))+((AiOiWi[10][0]*s10)+(AiOiWi[10][2]*c10))); AiOiWi[11][0] = (Atk[9][0]+((.00284123*Otk[9][2])+((Wirk[11][2]*wk[9][1] )-(Wirk[11][1]*wk[9][2])))); AiOiWi[11][1] = (Atk[9][1]+(((.0065711*Otk[9][2])+(.15931143*Otk[9][0])) +((Wirk[11][0]*wk[9][2])-(Wirk[11][2]*wk[9][0])))); AiOiWi[11][2] = (Atk[9][2]+(((Wirk[11][1]*wk[9][0])-(Wirk[11][0]* wk[9][1]))-(.00284123*Otk[9][0]))); Atk[11][0] = (((AiOiWi[11][0]*c11)-(AiOiWi[11][2]*s11))+(((wk[11][1]* Wkrpk[11][2])-(wk[11][2]*Wkrpk[11][1]))-(.00561135*Otk[11][2]))); Atk[11][1] = (AiOiWi[11][1]+(((.12618299*Otk[11][0])-(.01672166* Otk[11][2]))+((wk[11][2]*Wkrpk[11][0])-(wk[11][0]*Wkrpk[11][2])))); Atk[11][2] = (((.00561135*Otk[11][0])+((wk[11][0]*Wkrpk[11][1])-( wk[11][1]*Wkrpk[11][0])))+((AiOiWi[11][0]*s11)+(AiOiWi[11][2]*c11))); AiOiWi[12][0] = (Atk[10][0]+(((Wirk[12][2]*wk[10][1])-(Wirk[12][1]* wk[10][2]))-(.00064476*Otk[10][2]))); AiOiWi[12][1] = (Atk[10][1]+(((.00656361*Otk[10][2])+(.15909602* Otk[10][0]))+((Wirk[12][0]*wk[10][2])-(Wirk[12][2]*wk[10][0])))); AiOiWi[12][2] = (Atk[10][2]+((.00064476*Otk[10][0])+((Wirk[12][1]* wk[10][0])-(Wirk[12][0]*wk[10][1])))); Atk[12][0] = (((.00342896*Otk[12][2])+((wk[12][1]*Wkrpk[12][2])-( wk[12][2]*Wkrpk[12][1])))+((AiOiWi[12][0]*c12)-(AiOiWi[12][2]*s12))); Atk[12][1] = (AiOiWi[12][1]+(((.12587504*Otk[12][0])-(.01674373* Otk[12][2]))+((wk[12][2]*Wkrpk[12][0])-(wk[12][0]*Wkrpk[12][2])))); Atk[12][2] = (((AiOiWi[12][0]*s12)+(AiOiWi[12][2]*c12))+(((wk[12][0]* Wkrpk[12][1])-(wk[12][1]*Wkrpk[12][0]))-(.00342896*Otk[12][0]))); AiOiWi[13][0] = (Atk[11][0]+((.00061135*Otk[11][2])+((Wirk[13][2]* wk[11][1])-(Wirk[13][1]*wk[11][2])))); AiOiWi[13][1] = (Atk[11][1]+(((.00172166*Otk[11][2])+(.17081701* Otk[11][0]))+((Wirk[13][0]*wk[11][2])-(Wirk[13][2]*wk[11][0])))); AiOiWi[13][2] = (Atk[11][2]+(((Wirk[13][1]*wk[11][0])-(Wirk[13][0]* wk[11][1]))-(.00061135*Otk[11][0]))); Atk[13][0] = (((AiOiWi[13][0]*c13)-(AiOiWi[13][2]*s13))+(((wk[13][1]* Wkrpk[13][2])-(wk[13][2]*Wkrpk[13][1]))-(.00193095*Otk[13][2]))); Atk[13][1] = (AiOiWi[13][1]+(((.03543467*Otk[13][2])+(.03983908* Otk[13][0]))+((wk[13][2]*Wkrpk[13][0])-(wk[13][0]*Wkrpk[13][2])))); Atk[13][2] = (((.00193095*Otk[13][0])+((wk[13][0]*Wkrpk[13][1])-( wk[13][1]*Wkrpk[13][0])))+((AiOiWi[13][0]*s13)+(AiOiWi[13][2]*c13))); AiOiWi[14][0] = (Atk[12][0]+((.00157104*Otk[12][2])+((Wirk[14][2]* wk[12][1])-(Wirk[14][1]*wk[12][2])))); AiOiWi[14][1] = (Atk[12][1]+(((.00174373*Otk[12][2])+(.17112496* Otk[12][0]))+((Wirk[14][0]*wk[12][2])-(Wirk[14][2]*wk[12][0])))); AiOiWi[14][2] = (Atk[12][2]+(((Wirk[14][1]*wk[12][0])-(Wirk[14][0]* wk[12][1]))-(.00157104*Otk[12][0]))); Atk[14][0] = (((AiOiWi[14][0]*c14)-(AiOiWi[14][2]*s14))+(((wk[14][1]* Wkrpk[14][2])-(wk[14][2]*Wkrpk[14][1]))-(.00095499*Otk[14][2]))); Atk[14][1] = (AiOiWi[14][1]+(((.03539679*Otk[14][2])+(.03987912* Otk[14][0]))+((wk[14][2]*Wkrpk[14][0])-(wk[14][0]*Wkrpk[14][2])))); Atk[14][2] = (((.00095499*Otk[14][0])+((wk[14][0]*Wkrpk[14][1])-( wk[14][1]*Wkrpk[14][0])))+((AiOiWi[14][0]*s14)+(AiOiWi[14][2]*c14))); AiOiWi[15][0] = ((u[4]*Wirk[15][2])-(u[5]*Wirk[15][1])); AiOiWi[15][1] = ((u[5]*Wirk[15][0])-(u[3]*Wirk[15][2])); AiOiWi[15][2] = ((u[3]*Wirk[15][1])-(u[4]*Wirk[15][0])); Atk[15][0] = (((.00885657*Otk[15][2])+((wk[15][1]*Wkrpk[15][2])-( wk[15][2]*Wkrpk[15][1])))+((AiOiWi[15][0]*c15)-(AiOiWi[15][2]*s15))); Atk[15][1] = (AiOiWi[15][1]+(((.00307461*Otk[15][2])+(.20905614* Otk[15][0]))+((wk[15][2]*Wkrpk[15][0])-(wk[15][0]*Wkrpk[15][2])))); Atk[15][2] = (((AiOiWi[15][0]*s15)+(AiOiWi[15][2]*c15))+(((wk[15][0]* Wkrpk[15][1])-(wk[15][1]*Wkrpk[15][0]))-(.00885657*Otk[15][0]))); AiOiWi[16][0] = ((u[4]*Wirk[16][2])-(u[5]*Wirk[16][1])); AiOiWi[16][1] = ((u[5]*Wirk[16][0])-(u[3]*Wirk[16][2])); AiOiWi[16][2] = ((u[3]*Wirk[16][1])-(u[4]*Wirk[16][0])); Atk[16][0] = (((AiOiWi[16][0]*c16)-(AiOiWi[16][2]*s16))+(((wk[16][1]* Wkrpk[16][2])-(wk[16][2]*Wkrpk[16][1]))-(.00995927*Otk[16][2]))); Atk[16][1] = (AiOiWi[16][1]+(((.00305957*Otk[16][2])+(.21008605* Otk[16][0]))+((wk[16][2]*Wkrpk[16][0])-(wk[16][0]*Wkrpk[16][2])))); Atk[16][2] = (((.00995927*Otk[16][0])+((wk[16][0]*Wkrpk[16][1])-( wk[16][1]*Wkrpk[16][0])))+((AiOiWi[16][0]*s16)+(AiOiWi[16][2]*c16))); AiOiWi[17][0] = (Atk[15][0]+((.02714343*Otk[15][2])+((Wirk[17][2]* wk[15][1])-(Wirk[17][1]*wk[15][2])))); AiOiWi[17][1] = (Atk[15][1]+(((Wirk[17][0]*wk[15][2])-(Wirk[17][2]* wk[15][0]))-((.00307461*Otk[15][2])+(.01305614*Otk[15][0])))); AiOiWi[17][2] = (Atk[15][2]+(((Wirk[17][1]*wk[15][0])-(Wirk[17][0]* wk[15][1]))-(.02714343*Otk[15][0]))); Atk[17][0] = (((AiOiWi[17][0]*c17)-(AiOiWi[17][2]*s17))+(((wk[17][1]* Wkrpk[17][2])-(wk[17][2]*Wkrpk[17][1]))-(.00055916*Otk[17][2]))); Atk[17][1] = (AiOiWi[17][1]+(((.01407264*Otk[17][2])+(.15249501* Otk[17][0]))+((wk[17][2]*Wkrpk[17][0])-(wk[17][0]*Wkrpk[17][2])))); Atk[17][2] = (((.00055916*Otk[17][0])+((wk[17][0]*Wkrpk[17][1])-( wk[17][1]*Wkrpk[17][0])))+((AiOiWi[17][0]*s17)+(AiOiWi[17][2]*c17))); AiOiWi[18][0] = (Atk[16][0]+(((Wirk[18][2]*wk[16][1])-(Wirk[18][1]* wk[16][2]))-(.02604073*Otk[16][2]))); AiOiWi[18][1] = (Atk[16][1]+(((Wirk[18][0]*wk[16][2])-(Wirk[18][2]* wk[16][0]))-((.00305957*Otk[16][2])+(.01408605*Otk[16][0])))); AiOiWi[18][2] = (Atk[16][2]+((.02604073*Otk[16][0])+((Wirk[18][1]* wk[16][0])-(Wirk[18][0]*wk[16][1])))); Atk[18][0] = (((AiOiWi[18][0]*c18)-(AiOiWi[18][2]*s18))+(((wk[18][1]* Wkrpk[18][2])-(wk[18][2]*Wkrpk[18][1]))-(.00074575*Otk[18][2]))); Atk[18][1] = (AiOiWi[18][1]+(((.01407902*Otk[18][2])+(.15260355* Otk[18][0]))+((wk[18][2]*Wkrpk[18][0])-(wk[18][0]*Wkrpk[18][2])))); Atk[18][2] = (((.00074575*Otk[18][0])+((wk[18][0]*Wkrpk[18][1])-( wk[18][1]*Wkrpk[18][0])))+((AiOiWi[18][0]*s18)+(AiOiWi[18][2]*c18))); AiOiWi[19][0] = (Atk[17][0]+((.02455916*Otk[17][2])+((Wirk[19][2]* wk[17][1])-(Wirk[19][1]*wk[17][2])))); AiOiWi[19][1] = (Atk[17][1]+(((.00792736*Otk[17][2])+(.01750499* Otk[17][0]))+((Wirk[19][0]*wk[17][2])-(Wirk[19][2]*wk[17][0])))); AiOiWi[19][2] = (Atk[17][2]+(((Wirk[19][1]*wk[17][0])-(Wirk[19][0]* wk[17][1]))-(.02455916*Otk[17][0]))); Atk[19][0] = (((.00486049*Otk[19][2])+((wk[19][1]*Wkrpk[19][2])-( wk[19][2]*Wkrpk[19][1])))+((AiOiWi[19][0]*c19)-(AiOiWi[19][2]*s19))); Atk[19][1] = (AiOiWi[19][1]+(((.01360932*Otk[19][2])-(.01387793* Otk[19][0]))+((wk[19][2]*Wkrpk[19][0])-(wk[19][0]*Wkrpk[19][2])))); Atk[19][2] = (((AiOiWi[19][0]*s19)+(AiOiWi[19][2]*c19))+(((wk[19][0]* Wkrpk[19][1])-(wk[19][1]*Wkrpk[19][0]))-(.00486049*Otk[19][0]))); AiOiWi[20][0] = (Atk[18][0]+(((Wirk[20][2]*wk[18][1])-(Wirk[20][1]* wk[18][2]))-(.02325425*Otk[18][2]))); AiOiWi[20][1] = (Atk[18][1]+(((.00792098*Otk[18][2])+(.01739645* Otk[18][0]))+((Wirk[20][0]*wk[18][2])-(Wirk[20][2]*wk[18][0])))); AiOiWi[20][2] = (Atk[18][2]+((.02325425*Otk[18][0])+((Wirk[20][1]* wk[18][0])-(Wirk[20][0]*wk[18][1])))); Atk[20][0] = (((AiOiWi[20][0]*c20)-(AiOiWi[20][2]*s20))+(((wk[20][1]* Wkrpk[20][2])-(wk[20][2]*Wkrpk[20][1]))-(.00560346*Otk[20][2]))); Atk[20][1] = (AiOiWi[20][1]+(((.01361601*Otk[20][2])-(.01382852* Otk[20][0]))+((wk[20][2]*Wkrpk[20][0])-(wk[20][0]*Wkrpk[20][2])))); Atk[20][2] = (((.00560346*Otk[20][0])+((wk[20][0]*Wkrpk[20][1])-( wk[20][1]*Wkrpk[20][0])))+((AiOiWi[20][0]*s20)+(AiOiWi[20][2]*c20))); AiOiWi[21][0] = (Atk[6][0]+(5.201e-5*(u[6]*u[6]))); AiOiWi[21][2] = (Atk[6][2]+(.4177396*(u[6]*u[6]))); Atk[21][0] = (((AiOiWi[21][0]*c21)-(AiOiWi[21][2]*s21))-(5.2e-5*( wk[21][1]*wk[21][1]))); Atk[21][2] = ((.28373605*(wk[21][1]*wk[21][1]))+((AiOiWi[21][0]*s21)+( AiOiWi[21][2]*c21))); AiOiWi[22][0] = (Atk[7][0]+(5.197e-5*(u[7]*u[7]))); AiOiWi[22][2] = (Atk[7][2]+(.41830132*(u[7]*u[7]))); Atk[22][0] = (((AiOiWi[22][0]*c22)-(AiOiWi[22][2]*s22))-(5.194e-5*( wk[22][1]*wk[22][1]))); Atk[22][2] = ((.28317448*(wk[22][1]*wk[22][1]))+((AiOiWi[22][0]*s22)+( AiOiWi[22][2]*c22))); inerflg = 1; } /* Used 0.33 seconds CPU time, 16384 additional bytes of memory. Equations contain 302 adds/subtracts/negates 382 multiplies 0 divides 116 assignments */ } void BOCfly :: sddofs0 ( ) { /* Compute effect of all applied loads */ if (fs0flg == 0) { sddoltau(); sddoiner(); /* Compute Fstar (forces) */ Fstar[5][0] = ((108.164246577873*Cik[3][2][0])-ufk[0][0]); Fstar[5][1] = ((108.164246577873*Cik[3][2][1])-ufk[0][1]); Fstar[5][2] = ((108.164246577873*Cik[3][2][2])-ufk[0][2]); Fstar[6][0] = ((.18166784*(Atk[6][0]-(9.80665*s6)))-ufk[1][0]); Fstar[6][2] = ((.18166784*(Atk[6][2]+(9.80665*c6)))-ufk[1][2]); Fstar[7][0] = ((.1816698*(Atk[7][0]-(9.80665*s7)))-ufk[2][0]); Fstar[7][2] = ((.1816698*(Atk[7][2]+(9.80665*c7)))-ufk[2][2]); Fstar[8][0] = ((9.84983599*(Atk[8][0]-gk[8][0]))-ufk[3][0]); Fstar[8][1] = ((9.84983599*(Atk[8][1]+(9.80665*Cik[3][2][1])))-ufk[3][1] ); Fstar[8][2] = ((9.84983599*(Atk[8][2]-gk[8][2]))-ufk[3][2]); Fstar[9][0] = ((2.89586298*(Atk[9][0]-gk[9][0]))-ufk[4][0]); Fstar[9][1] = ((2.89586298*(Atk[9][1]+(9.80665*Cik[3][2][1])))-ufk[4][1] ); Fstar[9][2] = ((2.89586298*(Atk[9][2]-gk[9][2]))-ufk[4][2]); Fstar[10][0] = ((2.8984419*(Atk[10][0]-gk[10][0]))-ufk[5][0]); Fstar[10][1] = ((2.8984419*(Atk[10][1]+(9.80665*Cik[3][2][1])))- ufk[5][1]); Fstar[10][2] = ((2.8984419*(Atk[10][2]-gk[10][2]))-ufk[5][2]); Fstar[11][0] = ((1.02133193*(Atk[11][0]-gk[11][0]))-ufk[6][0]); Fstar[11][1] = ((1.02133193*(Atk[11][1]+(9.80665*Cik[3][2][1])))- ufk[6][1]); Fstar[11][2] = ((1.02133193*(Atk[11][2]-gk[11][2]))-ufk[6][2]); Fstar[12][0] = ((1.02007998*(Atk[12][0]-gk[12][0]))-ufk[7][0]); Fstar[12][1] = ((1.02007998*(Atk[12][1]+(9.80665*Cik[3][2][1])))- ufk[7][1]); Fstar[12][2] = ((1.02007998*(Atk[12][2]-gk[12][2]))-ufk[7][2]); Fstar[13][0] = ((.53380008*(Atk[13][0]-gk[13][0]))-ufk[8][0]); Fstar[13][1] = ((.53380008*(Atk[13][1]+(9.80665*Cik[3][2][1])))- ufk[8][1]); Fstar[13][2] = ((.53380008*(Atk[13][2]-gk[13][2]))-ufk[8][2]); Fstar[14][0] = ((.5325902*(Atk[14][0]-gk[14][0]))-ufk[9][0]); Fstar[14][1] = ((.5325902*(Atk[14][1]+(9.80665*Cik[3][2][1])))-ufk[9][1] ); Fstar[14][2] = ((.5325902*(Atk[14][2]-gk[14][2]))-ufk[9][2]); Fstar[15][0] = ((1.00985702*(Atk[15][0]-gk[15][0]))-ufk[10][0]); Fstar[15][1] = ((1.00985702*(Atk[15][1]+(9.80665*Cik[3][2][1])))- ufk[10][1]); Fstar[15][2] = ((1.00985702*(Atk[15][2]-gk[15][2]))-ufk[10][2]); Fstar[16][0] = ((1.01701701*(Atk[16][0]-gk[16][0]))-ufk[11][0]); Fstar[16][1] = ((1.01701701*(Atk[16][1]+(9.80665*Cik[3][2][1])))- ufk[11][1]); Fstar[16][2] = ((1.01701701*(Atk[16][2]-gk[16][2]))-ufk[11][2]); Fstar[17][0] = ((.43362906*(Atk[17][0]-gk[17][0]))-ufk[12][0]); Fstar[17][1] = ((.43362906*(Atk[17][1]+(9.80665*Cik[3][2][1])))- ufk[12][1]); Fstar[17][2] = ((.43362906*(Atk[17][2]-gk[17][2]))-ufk[12][2]); Fstar[18][0] = ((.43358398*(Atk[18][0]-gk[18][0]))-ufk[13][0]); Fstar[18][1] = ((.43358398*(Atk[18][1]+(9.80665*Cik[3][2][1])))- ufk[13][1]); Fstar[18][2] = ((.43358398*(Atk[18][2]-gk[18][2]))-ufk[13][2]); Fstar[19][0] = ((.05946345*(Atk[19][0]-gk[19][0]))-ufk[14][0]); Fstar[19][1] = ((.05946345*(Atk[19][1]+(9.80665*Cik[3][2][1])))- ufk[14][1]); Fstar[19][2] = ((.05946345*(Atk[19][2]-gk[19][2]))-ufk[14][2]); Fstar[20][0] = ((.05944511*(Atk[20][0]-gk[20][0]))-ufk[15][0]); Fstar[20][1] = ((.05944511*(Atk[20][1]+(9.80665*Cik[3][2][1])))- ufk[15][1]); Fstar[20][2] = ((.05944511*(Atk[20][2]-gk[20][2]))-ufk[15][2]); Fstar[21][0] = ((.0260106*(Atk[21][0]-gk[21][0]))-ufk[16][0]); Fstar[21][2] = ((.0260106*(Atk[21][2]-gk[21][2]))-ufk[16][2]); Fstar[22][0] = ((.02601109*(Atk[22][0]-gk[22][0]))-ufk[17][0]); Fstar[22][2] = ((.02601109*(Atk[22][2]-gk[22][2]))-ufk[17][2]); /* Compute Tstar (torques) */ Tstar[5][0] = (WkIkWk[5][0]-utk[0][0]); Tstar[5][1] = (WkIkWk[5][1]-utk[0][1]); Tstar[5][2] = (WkIkWk[5][2]-utk[0][2]); Tstar[6][0] = (WkIkWk[6][0]-utk[1][0]); Tstar[6][2] = (WkIkWk[6][2]-utk[1][2]); Tstar[7][0] = (WkIkWk[7][0]-utk[2][0]); Tstar[7][2] = (WkIkWk[7][2]-utk[2][2]); Tstar[8][0] = ((WkIkWk[8][0]+((.00823665*Otk[8][2])+(.52257967*Otk[8][0] )))-utk[3][0]); Tstar[8][1] = ((WkIkWk[8][1]+((1.657e-5*Otk[8][0])-(.00036016*Otk[8][2]) ))-utk[3][1]); Tstar[8][2] = ((WkIkWk[8][2]+((.00823665*Otk[8][0])+(.01799022*Otk[8][2] )))-utk[3][2]); Tstar[9][0] = ((WkIkWk[9][0]+((.00223918*Otk[9][2])+(.05459333*Otk[9][0] )))-utk[4][0]); Tstar[9][1] = ((WkIkWk[9][1]+((.00147758*Otk[9][2])-(5.918e-5*Otk[9][0]) ))-utk[4][1]); Tstar[9][2] = ((WkIkWk[9][2]+((.00223918*Otk[9][0])+(.00343412*Otk[9][2] )))-utk[4][2]); Tstar[10][0] = ((WkIkWk[10][0]+((.00224427*Otk[10][2])+(.05476817* Otk[10][0])))-utk[5][0]); Tstar[10][1] = ((WkIkWk[10][1]+((6.279e-5*Otk[10][0])-(.00156061* Otk[10][2])))-utk[5][1]); Tstar[10][2] = ((WkIkWk[10][2]+((.00224427*Otk[10][0])+(.00344556* Otk[10][2])))-utk[5][2]); Tstar[11][0] = ((WkIkWk[11][0]+((.00826684*Otk[11][0])-(.00026397* Otk[11][2])))-utk[6][0]); Tstar[11][1] = ((WkIkWk[11][1]+((1.273e-5*Otk[11][0])+(.00020104* Otk[11][2])))-utk[6][1]); Tstar[11][2] = ((WkIkWk[11][2]+((.00057599*Otk[11][2])-(.00026397* Otk[11][0])))-utk[6][2]); Tstar[12][0] = ((WkIkWk[12][0]+((.00821833*Otk[12][0])-(.00026918* Otk[12][2])))-utk[7][0]); Tstar[12][1] = ((WkIkWk[12][1]-((1.283e-5*Otk[12][0])+(.00019836* Otk[12][2])))-utk[7][1]); Tstar[12][2] = ((WkIkWk[12][2]+((.00057568*Otk[12][2])-(.00026918* Otk[12][0])))-utk[7][2]); Tstar[13][0] = ((WkIkWk[13][0]+((.00016083*Otk[13][2])+(.00031494* Otk[13][0])))-utk[8][0]); Tstar[13][1] = ((WkIkWk[13][1]+((4.29e-6*Otk[13][2])-(1.886e-5* Otk[13][0])))-utk[8][1]); Tstar[13][2] = ((WkIkWk[13][2]+((.00016083*Otk[13][0])+(.00103272* Otk[13][2])))-utk[8][2]); Tstar[14][0] = ((WkIkWk[14][0]+((.00016063*Otk[14][2])+(.00031249* Otk[14][0])))-utk[9][0]); Tstar[14][1] = ((WkIkWk[14][1]+((1.71e-5*Otk[14][0])-(4.13e-6*Otk[14][2] )))-utk[9][1]); Tstar[14][2] = ((WkIkWk[14][2]+((.00016063*Otk[14][0])+(.00102891* Otk[14][2])))-utk[9][2]); Tstar[15][0] = ((WkIkWk[15][0]+((.00098554*Otk[15][2])+(.03667794* Otk[15][0])))-utk[10][0]); Tstar[15][1] = ((WkIkWk[15][1]+((.00236551*Otk[15][2])-(9.282e-5* Otk[15][0])))-utk[10][1]); Tstar[15][2] = ((WkIkWk[15][2]+((.00072894*Otk[15][2])+(.00098554* Otk[15][0])))-utk[10][2]); Tstar[16][0] = ((WkIkWk[16][0]+((.00099076*Otk[16][2])+(.03712787* Otk[16][0])))-utk[11][0]); Tstar[16][1] = ((WkIkWk[16][1]+((9.583e-5*Otk[16][0])-(.00244367* Otk[16][2])))-utk[11][1]); Tstar[16][2] = ((WkIkWk[16][2]+((.00074417*Otk[16][2])+(.00099076* Otk[16][0])))-utk[11][2]); Tstar[17][0] = ((WkIkWk[17][0]+((.00047536*Otk[17][2])+(.00933952* Otk[17][0])))-utk[12][0]); Tstar[17][1] = ((WkIkWk[17][1]+((.00105064*Otk[17][2])-(2.97e-5* Otk[17][0])))-utk[12][1]); Tstar[17][2] = ((WkIkWk[17][2]+((.00032059*Otk[17][2])+(.00047536* Otk[17][0])))-utk[12][2]); Tstar[18][0] = ((WkIkWk[18][0]+((.00047517*Otk[18][2])+(.00933861* Otk[18][0])))-utk[13][0]); Tstar[18][1] = ((WkIkWk[18][1]+((3.024e-5*Otk[18][0])-(.00106364* Otk[18][2])))-utk[13][1]); Tstar[18][2] = ((WkIkWk[18][2]+((.00032466*Otk[18][2])+(.00047517* Otk[18][0])))-utk[13][2]); Tstar[19][0] = ((WkIkWk[19][0]+((7.331e-5*Otk[19][2])+(.00086458* Otk[19][0])))-utk[14][0]); Tstar[19][1] = ((WkIkWk[19][1]-((2.761e-5*Otk[19][0])+(.00020674* Otk[19][2])))-utk[14][1]); Tstar[19][2] = ((WkIkWk[19][2]+((3.059e-5*Otk[19][2])+(7.331e-5* Otk[19][0])))-utk[14][2]); Tstar[20][0] = ((WkIkWk[20][0]+((7.338e-5*Otk[20][2])+(.00086575* Otk[20][0])))-utk[15][0]); Tstar[20][1] = ((WkIkWk[20][1]+((2.77e-5*Otk[20][0])+(.00020754* Otk[20][2])))-utk[15][1]); Tstar[20][2] = ((WkIkWk[20][2]+((3.093e-5*Otk[20][2])+(7.338e-5* Otk[20][0])))-utk[15][2]); Tstar[21][0] = (WkIkWk[21][0]-utk[16][0]); Tstar[21][2] = (WkIkWk[21][2]-utk[16][2]); Tstar[22][0] = (WkIkWk[22][0]-utk[17][0]); Tstar[22][2] = (WkIkWk[22][2]-utk[17][2]); /* Compute fs0 (RHS ignoring constraints) */ fs0[22] = (utau[22]+(utk[17][1]+((5.194e-5*Fstar[22][2])+(.28317448* Fstar[22][0])))); Fstark[7][0] = (Fstar[7][0]+((Fstar[22][0]*c22)+(Fstar[22][2]*s22))); Fstark[7][1] = -(ufk[2][1]+ufk[17][1]); Fstark[7][2] = (Fstar[7][2]+((Fstar[22][2]*c22)-(Fstar[22][0]*s22))); tvec1[0] = (Tstar[22][0]+((rik[22][2]*ufk[17][1])-(4.88000000000008e-6* Fstar[22][2]))); tvec1[1] = (((Fstar[22][0]*rik[22][2])-(Fstar[22][2]*rik[22][0]))- utk[17][1]); tvec1[2] = (Tstar[22][2]+((4.88000000000008e-6*Fstar[22][0])-(rik[22][0] *ufk[17][1]))); Tstark[7][0] = (Tstar[7][0]+((tvec1[0]*c22)+(tvec1[2]*s22))); Tstark[7][1] = (tvec1[1]-utk[2][1]); Tstark[7][2] = (Tstar[7][2]+((tvec1[2]*c22)-(tvec1[0]*s22))); fs0[21] = (utau[21]+(utk[16][1]+((5.2e-5*Fstar[21][2])+(.28373605* Fstar[21][0])))); Fstark[6][0] = (Fstar[6][0]+((Fstar[21][0]*c21)+(Fstar[21][2]*s21))); Fstark[6][1] = -(ufk[1][1]+ufk[16][1]); Fstark[6][2] = (Fstar[6][2]+((Fstar[21][2]*c21)-(Fstar[21][0]*s21))); tvec1[0] = (Tstar[21][0]+((rik[21][2]*ufk[16][1])-(5.26e-6*Fstar[21][2]) )); tvec1[1] = (((Fstar[21][0]*rik[21][2])-(Fstar[21][2]*rik[21][0]))- utk[16][1]); tvec1[2] = (Tstar[21][2]+((5.26e-6*Fstar[21][0])-(rik[21][0]*ufk[16][1]) )); Tstark[6][0] = (Tstar[6][0]+((tvec1[0]*c21)+(tvec1[2]*s21))); Tstark[6][1] = (tvec1[1]-utk[1][1]); Tstark[6][2] = (Tstar[6][2]+((tvec1[2]*c21)-(tvec1[0]*s21))); fs0[20] = (utau[20]-(Tstar[20][1]+((.01382852*Fstar[20][0])-(.01361601* Fstar[20][2])))); Fstark[18][0] = (Fstar[18][0]+((Fstar[20][0]*c20)+(Fstar[20][2]*s20))); Fstark[18][1] = (Fstar[18][1]+Fstar[20][1]); Fstark[18][2] = (Fstar[18][2]+((Fstar[20][2]*c20)-(Fstar[20][0]*s20))); tvec1[0] = (Tstar[20][0]+((.02885771*Fstar[20][2])-(Fstar[20][1]* rik[20][2]))); tvec1[1] = (Tstar[20][1]+((Fstar[20][0]*rik[20][2])-(Fstar[20][2]* rik[20][0]))); tvec1[2] = (Tstar[20][2]+((Fstar[20][1]*rik[20][0])-(.02885771* Fstar[20][0]))); Tstark[18][0] = (Tstar[18][0]+((tvec1[0]*c20)+(tvec1[2]*s20))); Tstark[18][1] = (Tstar[18][1]+tvec1[1]); Tstark[18][2] = (Tstar[18][2]+((tvec1[2]*c20)-(tvec1[0]*s20))); fs0[19] = (utau[19]-(Tstar[19][1]+((.01387793*Fstar[19][0])-(.01360932* Fstar[19][2])))); Fstark[17][0] = (Fstar[17][0]+((Fstar[19][0]*c19)+(Fstar[19][2]*s19))); Fstark[17][1] = (Fstar[17][1]+Fstar[19][1]); Fstark[17][2] = (Fstar[17][2]+((Fstar[19][2]*c19)-(Fstar[19][0]*s19))); tvec1[0] = (Tstar[19][0]-((.02941965*Fstar[19][2])+(Fstar[19][1]* rik[19][2]))); tvec1[1] = (Tstar[19][1]+((Fstar[19][0]*rik[19][2])-(Fstar[19][2]* rik[19][0]))); tvec1[2] = (Tstar[19][2]+((.02941965*Fstar[19][0])+(Fstar[19][1]* rik[19][0]))); Tstark[17][0] = (Tstar[17][0]+((tvec1[0]*c19)+(tvec1[2]*s19))); Tstark[17][1] = (Tstar[17][1]+tvec1[1]); Tstark[17][2] = (Tstar[17][2]+((tvec1[2]*c19)-(tvec1[0]*s19))); fs0[18] = (utau[18]-(Tstark[18][1]-((.01407902*Fstark[18][2])+(.15260355 *Fstark[18][0])))); Fstark[16][0] = (Fstar[16][0]+((Fstark[18][0]*c18)+(Fstark[18][2]*s18))); Fstark[16][1] = (Fstar[16][1]+Fstark[18][1]); Fstark[16][2] = (Fstar[16][2]+((Fstark[18][2]*c18)-(Fstark[18][0]*s18))); tvec1[0] = (Tstark[18][0]+((.02678648*Fstark[18][2])-(Fstark[18][1]* rik[18][2]))); tvec1[1] = (Tstark[18][1]+((Fstark[18][0]*rik[18][2])-(Fstark[18][2]* rik[18][0]))); tvec1[2] = (Tstark[18][2]+((Fstark[18][1]*rik[18][0])-(.02678648* Fstark[18][0]))); Tstark[16][0] = (Tstar[16][0]+((tvec1[0]*c18)+(tvec1[2]*s18))); Tstark[16][1] = (Tstar[16][1]+tvec1[1]); Tstark[16][2] = (Tstar[16][2]+((tvec1[2]*c18)-(tvec1[0]*s18))); fs0[17] = (utau[17]-(Tstark[17][1]-((.01407264*Fstark[17][2])+(.15249501 *Fstark[17][0])))); Fstark[15][0] = (Fstar[15][0]+((Fstark[17][0]*c17)+(Fstark[17][2]*s17))); Fstark[15][1] = (Fstar[15][1]+Fstark[17][1]); Fstark[15][2] = (Fstar[15][2]+((Fstark[17][2]*c17)-(Fstark[17][0]*s17))); tvec1[0] = (Tstark[17][0]-((.02658427*Fstark[17][2])+(Fstark[17][1]* rik[17][2]))); tvec1[1] = (Tstark[17][1]+((Fstark[17][0]*rik[17][2])-(Fstark[17][2]* rik[17][0]))); tvec1[2] = (Tstark[17][2]+((.02658427*Fstark[17][0])+(Fstark[17][1]* rik[17][0]))); Tstark[15][0] = (Tstar[15][0]+((tvec1[0]*c17)+(tvec1[2]*s17))); Tstark[15][1] = (Tstar[15][1]+tvec1[1]); Tstark[15][2] = (Tstar[15][2]+((tvec1[2]*c17)-(tvec1[0]*s17))); fs0[16] = (utau[16]-(Tstark[16][1]-((.00305957*Fstark[16][2])+(.21008605 *Fstark[16][0])))); Fstark[5][0] = (Fstar[5][0]+((Fstark[16][0]*c16)+(Fstark[16][2]*s16))); Fstark[5][1] = (Fstar[5][1]+Fstark[16][1]); Fstark[5][2] = (Fstar[5][2]+((Fstark[16][2]*c16)-(Fstark[16][0]*s16))); tvec1[0] = (Tstark[16][0]+((.10875063*Fstark[16][2])-(Fstark[16][1]* rik[16][2]))); tvec1[1] = (Tstark[16][1]+((Fstark[16][0]*rik[16][2])-(Fstark[16][2]* rik[16][0]))); tvec1[2] = (Tstark[16][2]+((Fstark[16][1]*rik[16][0])-(.10875063* Fstark[16][0]))); Tstark[5][0] = (Tstar[5][0]+((tvec1[0]*c16)+(tvec1[2]*s16))); Tstark[5][1] = (Tstar[5][1]+tvec1[1]); Tstark[5][2] = (Tstar[5][2]+((tvec1[2]*c16)-(tvec1[0]*s16))); fs0[15] = (utau[15]-(Tstark[15][1]-((.00307461*Fstark[15][2])+(.20905614 *Fstark[15][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[15][0]*c15)+(Fstark[15][2]*s15))); Fstark[5][1] = (Fstark[5][1]+Fstark[15][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[15][2]*c15)-(Fstark[15][0]*s15))); tvec1[0] = (Tstark[15][0]-((.11006521*Fstark[15][2])+(Fstark[15][1]* rik[15][2]))); tvec1[1] = (Tstark[15][1]+((Fstark[15][0]*rik[15][2])-(Fstark[15][2]* rik[15][0]))); tvec1[2] = (Tstark[15][2]+((.11006521*Fstark[15][0])+(Fstark[15][1]* rik[15][0]))); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c15)+(tvec1[2]*s15))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c15)-(tvec1[0]*s15))); fs0[14] = (utau[14]-(Tstar[14][1]-((.03539679*Fstar[14][2])+(.03987912* Fstar[14][0])))); Fstark[12][0] = (Fstar[12][0]+((Fstar[14][0]*c14)+(Fstar[14][2]*s14))); Fstark[12][1] = (Fstar[12][1]+Fstar[14][1]); Fstark[12][2] = (Fstar[12][2]+((Fstar[14][2]*c14)-(Fstar[14][0]*s14))); tvec1[0] = (Tstar[14][0]-((.00061605*Fstar[14][2])+(Fstar[14][1]* rik[14][2]))); tvec1[1] = (Tstar[14][1]+((Fstar[14][0]*rik[14][2])-(Fstar[14][2]* rik[14][0]))); tvec1[2] = (Tstar[14][2]+((.00061605*Fstar[14][0])+(Fstar[14][1]* rik[14][0]))); Tstark[12][0] = (Tstar[12][0]+((tvec1[0]*c14)+(tvec1[2]*s14))); Tstark[12][1] = (Tstar[12][1]+tvec1[1]); Tstark[12][2] = (Tstar[12][2]+((tvec1[2]*c14)-(tvec1[0]*s14))); fs0[13] = (utau[13]-(Tstar[13][1]-((.03543467*Fstar[13][2])+(.03983908* Fstar[13][0])))); Fstark[11][0] = (Fstar[11][0]+((Fstar[13][0]*c13)+(Fstar[13][2]*s13))); Fstark[11][1] = (Fstar[11][1]+Fstar[13][1]); Fstark[11][2] = (Fstar[11][2]+((Fstar[13][2]*c13)-(Fstar[13][0]*s13))); tvec1[0] = (Tstar[13][0]+((.0013196*Fstar[13][2])-(Fstar[13][1]* rik[13][2]))); tvec1[1] = (Tstar[13][1]+((Fstar[13][0]*rik[13][2])-(Fstar[13][2]* rik[13][0]))); tvec1[2] = (Tstar[13][2]+((Fstar[13][1]*rik[13][0])-(.0013196* Fstar[13][0]))); Tstark[11][0] = (Tstar[11][0]+((tvec1[0]*c13)+(tvec1[2]*s13))); Tstark[11][1] = (Tstar[11][1]+tvec1[1]); Tstark[11][2] = (Tstar[11][2]+((tvec1[2]*c13)-(tvec1[0]*s13))); fs0[12] = (utau[12]-(Tstark[12][1]+((.01674373*Fstark[12][2])-(.12587504 *Fstark[12][0])))); Fstark[10][0] = (Fstar[10][0]+((Fstark[12][0]*c12)+(Fstark[12][2]*s12))); Fstark[10][1] = (Fstar[10][1]+Fstark[12][1]); Fstark[10][2] = (Fstar[10][2]+((Fstark[12][2]*c12)-(Fstark[12][0]*s12))); tvec1[0] = (Tstark[12][0]-((.0027842*Fstark[12][2])+(Fstark[12][1]* rik[12][2]))); tvec1[1] = (Tstark[12][1]+((Fstark[12][0]*rik[12][2])-(Fstark[12][2]* rik[12][0]))); tvec1[2] = (Tstark[12][2]+((.0027842*Fstark[12][0])+(Fstark[12][1]* rik[12][0]))); Tstark[10][0] = (Tstar[10][0]+((tvec1[0]*c12)+(tvec1[2]*s12))); Tstark[10][1] = (Tstar[10][1]+tvec1[1]); Tstark[10][2] = (Tstar[10][2]+((tvec1[2]*c12)-(tvec1[0]*s12))); fs0[11] = (utau[11]-(Tstark[11][1]+((.01672166*Fstark[11][2])-(.12618299 *Fstark[11][0])))); Fstark[9][0] = (Fstar[9][0]+((Fstark[11][0]*c11)+(Fstark[11][2]*s11))); Fstark[9][1] = (Fstar[9][1]+Fstark[11][1]); Fstark[9][2] = (Fstar[9][2]+((Fstark[11][2]*c11)-(Fstark[11][0]*s11))); tvec1[0] = (Tstark[11][0]+((.00277012*Fstark[11][2])-(Fstark[11][1]* rik[11][2]))); tvec1[1] = (Tstark[11][1]+((Fstark[11][0]*rik[11][2])-(Fstark[11][2]* rik[11][0]))); tvec1[2] = (Tstark[11][2]+((Fstark[11][1]*rik[11][0])-(.00277012* Fstark[11][0]))); Tstark[9][0] = (Tstar[9][0]+((tvec1[0]*c11)+(tvec1[2]*s11))); Tstark[9][1] = (Tstar[9][1]+tvec1[1]); Tstark[9][2] = (Tstar[9][2]+((tvec1[2]*c11)-(tvec1[0]*s11))); fs0[10] = (utau[10]-(Tstark[10][1]-((.00943639*Fstark[10][2])+(.13190398 *Fstark[10][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[10][0]*c10)+(Fstark[10][2]*s10))); Fstark[5][1] = (Fstark[5][1]+Fstark[10][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[10][2]*c10)-(Fstark[10][0]*s10))); tvec1[0] = (Tstark[10][0]+((.0481466*Fstark[10][2])-(Fstark[10][1]* rik[10][2]))); tvec1[1] = (Tstark[10][1]+((Fstark[10][0]*rik[10][2])-(Fstark[10][2]* rik[10][0]))); tvec1[2] = (Tstark[10][2]+((Fstark[10][1]*rik[10][0])-(.0481466* Fstark[10][0]))); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c10)+(tvec1[2]*s10))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c10)-(tvec1[0]*s10))); fs0[9] = (utau[9]-(Tstark[9][1]-((.0094289*Fstark[9][2])+(.13168857* Fstark[9][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[9][0]*c9)+(Fstark[9][2]*s9))); Fstark[5][1] = (Fstark[5][1]+Fstark[9][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[9][2]*c9)-(Fstark[9][0]*s9))); tvec1[0] = (Tstark[9][0]-((.04836741*Fstark[9][2])+(Fstark[9][1]* rik[9][2]))); tvec1[1] = (Tstark[9][1]+((Fstark[9][0]*rik[9][2])-(Fstark[9][2]* rik[9][0]))); tvec1[2] = (Tstark[9][2]+((.04836741*Fstark[9][0])+(Fstark[9][1]* rik[9][0]))); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c9)+(tvec1[2]*s9))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c9)-(tvec1[0]*s9))); fs0[8] = (utau[8]-(Tstar[8][1]-((.01691003*Fstar[8][2])+(.12000071* Fstar[8][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstar[8][0]*c8)+(Fstar[8][2]*s8))); Fstark[5][1] = (Fstar[8][1]+Fstark[5][1]); Fstark[5][2] = (Fstark[5][2]+((Fstar[8][2]*c8)-(Fstar[8][0]*s8))); tvec1[0] = (Tstar[8][0]-((.00049034*Fstar[8][2])+(Fstar[8][1]*rik[8][2]) )); tvec1[1] = (Tstar[8][1]+((Fstar[8][0]*rik[8][2])-(Fstar[8][2]*rik[8][0]) )); tvec1[2] = (Tstar[8][2]+((.00049034*Fstar[8][0])+(Fstar[8][1]*rik[8][0]) )); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c8)+(tvec1[2]*s8))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c8)-(tvec1[0]*s8))); fs0[7] = (utau[7]-(Tstark[7][1]-((5.197e-5*Fstark[7][2])+(1.22369868* Fstark[7][0])))); fs0[6] = (utau[6]-(Tstark[6][1]-((5.201e-5*Fstark[6][2])+(1.2242604* Fstark[6][0])))); fs0[5] = (utau[5]-Tstark[5][2]); Fstark[4][0] = Fstark[5][0]; Fstark[4][1] = Fstark[5][1]; Fstark[4][2] = Fstark[5][2]; Tstark[4][0] = Tstark[5][0]; Tstark[4][1] = Tstark[5][1]; Tstark[4][2] = Tstark[5][2]; fs0[4] = (utau[4]-Tstark[4][1]); Fstark[3][0] = Fstark[4][0]; Fstark[3][1] = Fstark[4][1]; Fstark[3][2] = Fstark[4][2]; Tstark[3][0] = Tstark[4][0]; Tstark[3][1] = Tstark[4][1]; Tstark[3][2] = Tstark[4][2]; fs0[3] = (utau[3]-Tstark[3][0]); Fstark[2][0] = ((Cik[3][0][2]*Fstark[3][2])+((Cik[3][0][0]*Fstark[3][0]) +(Cik[3][0][1]*Fstark[3][1]))); Fstark[2][1] = ((Cik[3][1][2]*Fstark[3][2])+((Cik[3][1][0]*Fstark[3][0]) +(Cik[3][1][1]*Fstark[3][1]))); Fstark[2][2] = ((Cik[3][2][2]*Fstark[3][2])+((Cik[3][2][0]*Fstark[3][0]) +(Cik[3][2][1]*Fstark[3][1]))); Tstark[2][0] = ((Cik[3][0][2]*Tstark[3][2])+((Cik[3][0][0]*Tstark[3][0]) +(Cik[3][0][1]*Tstark[3][1]))); Tstark[2][1] = ((Cik[3][1][2]*Tstark[3][2])+((Cik[3][1][0]*Tstark[3][0]) +(Cik[3][1][1]*Tstark[3][1]))); Tstark[2][2] = ((Cik[3][2][2]*Tstark[3][2])+((Cik[3][2][0]*Tstark[3][0]) +(Cik[3][2][1]*Tstark[3][1]))); fs0[2] = (utau[2]-Fstark[2][2]); Fstark[1][0] = Fstark[2][0]; Fstark[1][1] = Fstark[2][1]; Fstark[1][2] = Fstark[2][2]; tvec1[0] = (Tstark[2][0]-(Fstark[2][1]*q[2])); tvec1[1] = (Tstark[2][1]+(Fstark[2][0]*q[2])); Tstark[1][0] = tvec1[0]; Tstark[1][1] = tvec1[1]; Tstark[1][2] = Tstark[2][2]; fs0[1] = (utau[1]-Fstark[1][1]); Fstark[0][0] = Fstark[1][0]; Fstark[0][1] = Fstark[1][1]; Fstark[0][2] = Fstark[1][2]; tvec1[0] = (Tstark[1][0]+(Fstark[1][2]*q[1])); tvec1[2] = (Tstark[1][2]-(Fstark[1][0]*q[1])); Tstark[0][0] = tvec1[0]; Tstark[0][1] = Tstark[1][1]; Tstark[0][2] = tvec1[2]; fs0[0] = (utau[0]-Fstark[0][0]); fs0flg = 1; } /* Used 0.54 seconds CPU time, 24576 additional bytes of memory. Equations contain 540 adds/subtracts/negates 411 multiplies 0 divides 292 assignments */ } void BOCfly :: sddomm ( int routine ) { if (mmflg == 0) { /* Compute gains (DD, G) */ DD[22] = 386.601655908384; N11[22][0][0] = (c22-(.000763817365399425*s22)); N11[22][2][0] = -(s22+(.000763817365399425*c22)); N21[22][0][0] = ((.0799178893461792*s22)-(3.74224432514814e-6*c22)); N21[22][0][1] = (1.642+((5.194e-5*s22)+(.28317448*c22))); N21[22][0][2] = ((1.41995811288358e-5*s22)-(.00250244103699563*c22)); N21[22][1][0] = ((.00125418811398586*s22)-(1.642*c22)); N21[22][2][0] = ((3.74224432514814e-6*s22)+(.0799178893461792*c22)); N21[22][2][1] = ((5.194e-5*c22)-(.28317448*s22)); N21[22][2][2] = ((1.41995811288358e-5*c22)+(.00250244103699563*s22)); N22[22][0][0] = (c22-(7.98349512692128e-5*s22)); N22[22][2][0] = -(s22+(7.98349512692128e-5*c22)); P11[7][0][0] = (.1816698+((.00503671459813887*(N11[22][0][0]* N11[22][0][0]))+(.0260110863558528*(s22*s22)))); P11[7][0][2] = ((.00503671459813887*(N11[22][0][0]*N11[22][2][0]))+( .0260110863558528*(s22*c22))); P11[7][2][0] = P11[7][0][2]; P11[7][2][2] = (.1816698+((.00503671459813887*(N11[22][2][0]* N11[22][2][0]))+(.0260110863558528*(c22*c22)))); Pd[7][0][0] = ((.00503671459813887*(N11[22][0][0]*N21[22][0][0]))+( .0260110863558528*(N21[22][0][2]*s22))); Pd[7][0][1] = (((.00503671459813887*(N11[22][0][0]*N21[22][1][0]))-( .0427102037963103*(s22*s22)))-.222309094455864); Pd[7][0][2] = (.000453731225688+((.00503671459813887*(N11[22][0][0]* N21[22][2][0]))+(.0260110863558528*(N21[22][2][2]*s22)))); Pd[7][1][0] = (.222309094455864+(.02601109*N21[22][0][1])); Pd[7][1][2] = (9.441379506e-6+(.02601109*N21[22][2][1])); Pd[7][2][0] = (((.00503671459813887*(N11[22][2][0]*N21[22][0][0]))+( .0260110863558528*(N21[22][0][2]*c22)))-.000453731225688); Pd[7][2][1] = (((.00503671459813887*(N11[22][2][0]*N21[22][1][0]))-( .0427102037963103*(s22*c22)))-9.441379506e-6); Pd[7][2][2] = ((.00503671459813887*(N11[22][2][0]*N21[22][2][0]))+( .0260110863558528*(N21[22][2][2]*c22))); P22[7][0][0] = (.313875488658596+(((.000466789999800347*(N22[22][0][0]* N22[22][0][0]))-(4.17451672122366e-6*(s22*s22)))+((.0260110863558528*( N21[22][0][2]*N21[22][0][2]))+((.00503671459813887*(N21[22][0][0]* N21[22][0][0]))+(.02601109*(N21[22][0][1]*N21[22][0][1])))))); P22[7][0][1] = (((.00503671459813887*(N21[22][0][0]*N21[22][1][0]))-( .0427102037963103*(N21[22][0][2]*s22)))-5.64195882009946e-8); P22[7][0][2] = (1.08034036388713e-5+(((.000466789999800347*( N22[22][0][0]*N22[22][2][0]))-(4.17451672122366e-6*(s22*c22)))+(( .0260110863558528*(N21[22][0][2]*N21[22][2][2]))+((.00503671459813887* (N21[22][0][0]*N21[22][2][0]))+(.02601109*(N21[22][0][1]*N21[22][2][1] )))))); P22[7][1][0] = P22[7][0][1]; P22[7][1][1] = (.314112335928305+((.00503671459813887*(N21[22][1][0]* N21[22][1][0]))+(.0701301546335415*(s22*s22)))); P22[7][1][2] = (.00175123969805081+((.00503671459813887*(N21[22][1][0]* N21[22][2][0]))-(.0427102037963103*(N21[22][2][2]*s22)))); P22[7][2][0] = P22[7][0][2]; P22[7][2][1] = P22[7][1][2]; P22[7][2][2] = (.000233373711628522+(((.000466789999800347*( N22[22][2][0]*N22[22][2][0]))-(4.17451672122366e-6*(c22*c22)))+(( .0260110863558528*(N21[22][2][2]*N21[22][2][2]))+((.00503671459813887* (N21[22][2][0]*N21[22][2][0]))+(.02601109*(N21[22][2][1]*N21[22][2][1] )))))); DD[21] = 385.517906754109; N11[21][0][0] = (c21-(.000767688499534275*s21)); N11[21][2][0] = -(s21+(.000767688499534275*c21)); N21[21][0][0] = ((5.6801467513909e-6*c21)-(.076402775026998*s21)); N21[21][0][1] = (1.642+((5.2e-5*s21)+(.28373605*c21))); N21[21][0][2] = -((5.58895979195839e-6*c21)+(1.40032806561312e-5*s21)); N21[21][1][0] = ((.00126054451623528*s21)-(1.642*c21)); N21[21][2][0] = -((5.6801467513909e-6*s21)+(.076402775026998*c21)); N21[21][2][1] = ((5.2e-5*c21)-(.28373605*s21)); N21[21][2][2] = ((5.58895979195839e-6*s21)-(1.40032806561312e-5*c21)); N22[21][0][0] = (c21-(7.99003285964743e-5*s21)); N22[21][2][0] = -(s21+(7.99003285964743e-5*c21)); P11[6][0][0] = (.18166784+((.0050127739827671*(N11[21][0][0]* N11[21][0][0]))+(.0260105963404791*(s21*s21)))); P11[6][0][2] = ((.0050127739827671*(N11[21][0][0]*N11[21][2][0]))+( .0260105963404791*(s21*c21))); P11[6][2][0] = P11[6][0][2]; P11[6][2][2] = (.18166784+((.0050127739827671*(N11[21][2][0]* N11[21][2][0]))+(.0260105963404791*(c21*c21)))); Pd[6][0][0] = ((.0050127739827671*(N11[21][0][0]*N21[21][0][0]))+( .0260105963404791*(N21[21][0][2]*s21))); Pd[6][0][1] = (((.0050127739827671*(N11[21][0][0]*N21[21][1][0]))-( .0427093991910667*(s21*s21)))-.222408742465536); Pd[6][0][2] = (5.99503872e-8+((.0050127739827671*(N11[21][0][0]* N21[21][2][0]))+(.0260105963404791*(N21[21][2][2]*s21)))); Pd[6][1][0] = (.222408742465536+(.0260106*N21[21][0][1])); Pd[6][1][2] = (9.4485443584e-6+(.0260106*N21[21][2][1])); Pd[6][2][0] = (((.0050127739827671*(N11[21][2][0]*N21[21][0][0]))+( .0260105963404791*(N21[21][0][2]*c21)))-5.99503872e-8); Pd[6][2][1] = (((.0050127739827671*(N11[21][2][0]*N21[21][1][0]))-( .0427093991910667*(s21*c21)))-9.4485443584e-6); Pd[6][2][2] = ((.0050127739827671*(N11[21][2][0]*N21[21][2][0]))+( .0260105963404791*(N21[21][2][2]*c21))); P22[6][0][0] = (.314112136014374+(((.00046691999979996*(N22[21][0][0]* N22[21][0][0]))-(4.08234224870007e-6*(s21*s21)))+((.0260105963404791*( N21[21][0][2]*N21[21][0][2]))+((.0050127739827671*(N21[21][0][0]* N21[21][0][0]))+(.0260106*(N21[21][0][1]*N21[21][0][1])))))); P22[6][0][1] = (8.00031180196383e-8+((.0050127739827671*(N21[21][0][0]* N21[21][1][0]))-(.0427093991910667*(N21[21][0][2]*s21)))); P22[6][0][2] = (1.08174786956325e-5+(((.00046691999979996*(N22[21][0][0] *N22[21][2][0]))-(4.08234224870007e-6*(s21*c21)))+((.0260105963404791* (N21[21][0][2]*N21[21][2][2]))+((.0050127739827671*(N21[21][0][0]* N21[21][2][0]))+(.0260106*(N21[21][0][1]*N21[21][2][1])))))); P22[6][1][0] = P22[6][0][1]; P22[6][1][1] = (.314342606505773+((.0050127739827671*(N21[21][1][0]* N21[21][1][0]))+(.0701288334717315*(s21*s21)))); P22[6][1][2] = (((.0050127739827671*(N21[21][1][0]*N21[21][2][0]))-( .0427093991910667*(N21[21][2][2]*s21)))-.00226946339488501); P22[6][2][0] = P22[6][0][2]; P22[6][2][1] = P22[6][1][2]; P22[6][2][2] = (.000224750491438576+(((.00046691999979996*(N22[21][2][0] *N22[21][2][0]))-(4.08234224870007e-6*(c21*c21)))+((.0260105963404791* (N21[21][2][2]*N21[21][2][2]))+((.0050127739827671*(N21[21][2][0]* N21[21][2][0]))+(.0260106*(N21[21][2][1]*N21[21][2][1])))))); DD[20] = 1184.34517824184; N11[20][0][0] = (c20+(.0134371347236032*s20)); N11[20][2][0] = ((.0134371347236032*c20)-s20); N21[20][0][0] = (((.000322491233366477*c20)-(.024*s20))-(( .000384560056379028*c20)+(.00904888251330262*s20))); N21[20][0][1] = (.17+((.01361601*s20)-(.01382852*c20))); N21[20][0][2] = ((.024*c20)+((.00343796135505377*s20)+( .00606231867560465*c20))); N21[20][1][0] = (rikt[20][1][0]+(.0134371347236032*rikt[20][1][2])); N21[20][2][0] = (((.000384560056379028*s20)-(.00904888251330262*c20))-(( .000322491233366477*s20)+(.024*c20))); N21[20][2][1] = (.022+((.01361601*c20)+(.01382852*s20))); N21[20][2][2] = (((.00343796135505377*c20)-(.00606231867560465*s20))-( .024*s20)); N22[20][0][0] = (c20+(.0767630060568233*s20)); N22[20][2][0] = ((.0767630060568233*c20)-s20); P11[18][0][0] = (.43358398+((.0586447931317092*(N11[20][0][0]* N11[20][0][0]))+(.058658613188883*(s20*s20)))); P11[18][0][2] = ((.0586447931317092*(N11[20][0][0]*N11[20][2][0]))+( .058658613188883*(s20*c20))); P11[18][2][0] = P11[18][0][2]; P11[18][2][2] = (.43358398+((.0586447931317092*(N11[20][2][0]* N11[20][2][0]))+(.058658613188883*(c20*c20)))); Pd[18][0][0] = ((.0586447931317092*(N11[20][0][0]*N21[20][0][0]))+( .058658613188883*(N21[20][0][2]*s20))); Pd[18][0][1] = (((.0586447931317092*(N11[20][0][0]*N21[20][1][0]))+( .058658613188883*(rikt[20][1][2]*s20)))-.066166454571129); Pd[18][0][2] = (((.0586447931317092*(N11[20][0][0]*N21[20][2][0]))+( .058658613188883*(N21[20][2][2]*s20)))-.000323345253085); Pd[18][1][0] = (.066166454571129+(.05944511*N21[20][0][1])); Pd[18][1][2] = (.0061044375260996+(.05944511*N21[20][2][1])); Pd[18][2][0] = (.000323345253085+((.0586447931317092*(N11[20][2][0]* N21[20][0][0]))+(.058658613188883*(N21[20][0][2]*c20)))); Pd[18][2][1] = (((.0586447931317092*(N11[20][2][0]*N21[20][1][0]))+( .058658613188883*(rikt[20][1][2]*c20)))-.0061044375260996); Pd[18][2][2] = ((.0586447931317092*(N11[20][2][0]*N21[20][2][0]))+( .058658613188883*(N21[20][2][2]*c20))); P22[18][0][0] = (.0194360869931905+(((.000864816511752397*(N22[20][0][0] *N22[20][0][0]))-(2.65685951843988e-5*(s20*s20)))+((.058658613188883*( N21[20][0][2]*N21[20][0][2]))+((.0586447931317092*(N21[20][0][0]* N21[20][0][0]))+(.05944511*(N21[20][0][1]*N21[20][0][1])))))); P22[18][0][1] = (2.56876157149112e-5+((.0586447931317092*(N21[20][0][0]* N21[20][1][0]))+(.058658613188883*(N21[20][0][2]*rikt[20][1][2])))); P22[18][0][2] = (.00140672883723602+(((.000864816511752397*( N22[20][0][0]*N22[20][2][0]))-(2.65685951843988e-5*(s20*c20)))+(( .058658613188883*(N21[20][0][2]*N21[20][2][2]))+((.0586447931317092*( N21[20][0][0]*N21[20][2][0]))+(.05944511*(N21[20][0][1]*N21[20][2][1]) ))))); P22[18][1][0] = P22[18][0][1]; P22[18][1][1] = (.0193353703564867+((.0586447931317092*(N21[20][1][0]* N21[20][1][0]))+(.058658613188883*(rikt[20][1][2]*rikt[20][1][2])))); P22[18][1][2] = (((.0586447931317092*(N21[20][1][0]*N21[20][2][0]))+( .058658613188883*(N21[20][2][2]*rikt[20][1][2])))-.00101429636650358); P22[18][2][0] = P22[18][0][2]; P22[18][2][1] = P22[18][1][2]; P22[18][2][2] = (.000410845632741195+(((.000864816511752397*( N22[20][2][0]*N22[20][2][0]))-(2.65685951843988e-5*(c20*c20)))+(( .058658613188883*(N21[20][2][2]*N21[20][2][2]))+((.0586447931317092*( N21[20][2][0]*N21[20][2][0]))+(.05944511*(N21[20][2][1]*N21[20][2][1]) ))))); DD[19] = 1185.34545417985; N11[19][0][0] = (c19+(.0134955985984873*s19)); N11[19][2][0] = ((.0134955985984873*c19)-s19); N21[19][0][0] = (((.00039484370200454*c19)+(.00830819529356415*s19))+(( .024*s19)-(.000323894366363695*c19))); N21[19][0][1] = (.17+((.01360932*s19)-(.01387793*c19))); N21[19][0][2] = -((.024*c19)+((.0034263195401683*s19)+( .00531807286980771*c19))); N21[19][1][0] = (rikt[19][1][0]+(.0134955985984873*rikt[19][1][2])); N21[19][2][0] = (((.000323894366363695*s19)+(.024*c19))+(( .00830819529356415*c19)-(.00039484370200454*s19))); N21[19][2][1] = (.022+((.01360932*c19)+(.01387793*s19))); N21[19][2][2] = ((.024*s19)+((.00531807286980771*s19)-(.0034263195401683 *c19))); N22[19][0][0] = (c19+(.0768351789598212*s19)); N22[19][2][0] = ((.0768351789598212*c19)-s19); P11[17][0][0] = (.43362906+((.0586562251380273*(N11[19][0][0]* N11[19][0][0]))+(.0586764875894842*(s19*s19)))); P11[17][0][2] = ((.0586562251380273*(N11[19][0][0]*N11[19][2][0]))+( .0586764875894842*(s19*c19))); P11[17][2][0] = P11[17][0][2]; P11[17][2][2] = (.43362906+((.0586562251380273*(N11[19][2][0]* N11[19][2][0]))+(.0586764875894842*(c19*c19)))); Pd[17][0][0] = ((.0586562251380273*(N11[19][0][0]*N21[19][0][0]))+( .0586764875894842*(N21[19][0][2]*s19))); Pd[17][0][1] = (((.0586562251380273*(N11[19][0][0]*N21[19][1][0]))+( .0586764875894842*(rikt[19][1][2]*s19)))-.0661262678409906); Pd[17][0][2] = (((.0586562251380273*(N11[19][0][0]*N21[19][2][0]))+( .0586764875894842*(N21[19][2][2]*s19)))-.0002424680251896); Pd[17][1][0] = (.0661262678409906+(.05946345*N21[19][0][1])); Pd[17][1][2] = (.0061023056549184+(.05946345*N21[19][2][1])); Pd[17][2][0] = (.0002424680251896+((.0586562251380273*(N11[19][2][0]* N21[19][0][0]))+(.0586764875894842*(N21[19][0][2]*c19)))); Pd[17][2][1] = (((.0586562251380273*(N11[19][2][0]*N21[19][1][0]))+( .0586764875894842*(rikt[19][1][2]*c19)))-.0061023056549184); Pd[17][2][2] = ((.0586562251380273*(N11[19][2][0]*N21[19][2][0]))+( .0586764875894842*(N21[19][2][2]*c19))); P22[17][0][0] = (.0194235814540955+(((.000863651675657903*(N22[19][0][0] *N22[19][0][0]))-(2.65581192098382e-5*(s19*s19)))+((.0586764875894842* (N21[19][0][2]*N21[19][0][2]))+((.0586562251380273*(N21[19][0][0]* N21[19][0][0]))+(.05946345*(N21[19][0][1]*N21[19][0][1])))))); P22[17][0][1] = (((.0586562251380273*(N21[19][0][0]*N21[19][1][0]))+( .0586764875894842*(N21[19][0][2]*rikt[19][1][2])))-3.31121652300042e-5 ); P22[17][0][2] = (.00140593116186984+(((.000863651675657903*( N22[19][0][0]*N22[19][2][0]))-(2.65581192098382e-5*(s19*c19)))+(( .0586764875894842*(N21[19][0][2]*N21[19][2][2]))+((.0586562251380273*( N21[19][0][0]*N21[19][2][0]))+(.05946345*(N21[19][0][1]*N21[19][2][1]) ))))); P22[17][1][0] = P22[17][0][1]; P22[17][1][1] = (.0193269914263262+((.0586562251380273*(N21[19][1][0]* N21[19][1][0]))+(.0586764875894842*(rikt[19][1][2]*rikt[19][1][2])))); P22[17][1][2] = (.00108761516392597+((.0586562251380273*(N21[19][1][0]* N21[19][2][0]))+(.0586764875894842*(N21[19][2][2]*rikt[19][1][2])))); P22[17][2][0] = P22[17][0][2]; P22[17][2][1] = P22[17][1][2]; P22[17][2][2] = (.000406601129072596+(((.000863651675657903*( N22[19][2][0]*N22[19][2][0]))-(2.65581192098382e-5*(c19*c19)))+(( .0586764875894842*(N21[19][2][2]*N21[19][2][2]))+((.0586562251380273*( N21[19][2][0]*N21[19][2][0]))+(.05946345*(N21[19][2][1]*N21[19][2][1]) ))))); if (P22[18][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[18] = (1./P22[18][1][1]); } G1[18][0] = (DD[18]*Pd[18][0][1]); G1[18][2] = (DD[18]*Pd[18][2][1]); G2[18][0] = (DD[18]*P22[18][0][1]); G2[18][2] = (DD[18]*P22[18][2][1]); P11[18][0][0] = (P11[18][0][0]-(G1[18][0]*Pd[18][0][1])); P11[18][0][2] = (P11[18][0][2]-(G1[18][2]*Pd[18][0][1])); P11[18][2][2] = (P11[18][2][2]-(G1[18][2]*Pd[18][2][1])); Pd[18][0][0] = (Pd[18][0][0]-(G2[18][0]*Pd[18][0][1])); Pd[18][0][2] = (Pd[18][0][2]-(G2[18][2]*Pd[18][0][1])); Pd[18][2][0] = (Pd[18][2][0]-(G2[18][0]*Pd[18][2][1])); Pd[18][2][2] = (Pd[18][2][2]-(G2[18][2]*Pd[18][2][1])); P22[18][0][0] = (P22[18][0][0]-(G2[18][0]*P22[18][0][1])); P22[18][0][2] = (P22[18][0][2]-(G2[18][2]*P22[18][0][1])); P22[18][1][2] = (P22[18][1][2]-P22[18][2][1]); P22[18][2][2] = (P22[18][2][2]-(G2[18][2]*P22[18][2][1])); sD1INV[0][0] = 0.; if (P11[18][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[18][0][0]); } sL11[2][0] = (P11[18][0][2]*sD1INV[0][0]); sL11D1[2][2] = (P11[18][2][2]-(P11[18][0][2]*sL11[2][0])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[2][0] = (Pd[18][2][0]-(Pd[18][0][0]*sL11[2][0])); sD1L21[2][2] = (Pd[18][2][2]-(Pd[18][0][2]*sL11[2][0])); sL21[0][0] = (Pd[18][0][0]*sD1INV[0][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[2][0] = (Pd[18][0][2]*sD1INV[0][0]); sL21[2][2] = (sD1INV[2][2]*sD1L21[2][2]); sL22D2[0][0] = (P22[18][0][0]-((sD1L21[2][0]*sL21[0][2])+(( 2.02827788518523*(Pd[18][1][0]*Pd[18][1][0]))+(Pd[18][0][0]*sL21[0][0] )))); sL22D2[2][0] = (P22[18][0][2]-((sD1L21[2][0]*sL21[2][2])+(( 2.02827788518523*(Pd[18][1][0]*Pd[18][1][2]))+(Pd[18][0][0]*sL21[2][0] )))); sL22D2[2][2] = (P22[18][2][2]-((sD1L21[2][2]*sL21[2][2])+(( 2.02827788518523*(Pd[18][1][2]*Pd[18][1][2]))+(Pd[18][0][2]*sL21[2][0] )))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[2][0] = (sD2INV[0][0]*sL22D2[2][0]); sL22D2[2][2] = (sL22D2[2][2]-(sL22[2][0]*sL22D2[2][0])); N11[18][0][0] = (c18+(sL11[2][0]*s18)); N11[18][2][0] = ((sL11[2][0]*c18)-s18); N21[18][0][0] = ((.036*((sL11[2][0]*c18)-s18))+((sL21[0][0]*c18)+( sL21[2][0]*s18))); N21[18][0][1] = (.196+(2.02827788518523*((Pd[18][1][0]*c18)+( Pd[18][1][2]*s18)))); N21[18][0][2] = ((.036*c18)+((sL21[0][2]*c18)+(sL21[2][2]*s18))); N21[18][1][0] = -(.196*(c18+(sL11[2][0]*s18))); N21[18][2][0] = (((sL21[2][0]*c18)-(sL21[0][0]*s18))-(.036*(c18+( sL11[2][0]*s18)))); N21[18][2][1] = (2.02827788518523*((Pd[18][1][2]*c18)-(Pd[18][1][0]*s18) )); N21[18][2][2] = (((sL21[2][2]*c18)-(sL21[0][2]*s18))-(.036*s18)); N22[18][0][0] = (c18+(sL22[2][0]*s18)); N22[18][2][0] = ((sL22[2][0]*c18)-s18); psiD11[18][0][0] = (N11[18][0][0]*P11[18][0][0]); psiD11[18][0][2] = (sL11D1[2][2]*s18); psiD11[18][2][0] = (N11[18][2][0]*P11[18][0][0]); psiD11[18][2][2] = (sL11D1[2][2]*c18); psiD21[18][0][0] = (N21[18][0][0]*P11[18][0][0]); psiD21[18][0][2] = (N21[18][0][2]*sL11D1[2][2]); psiD21[18][1][0] = (N21[18][1][0]*P11[18][0][0]); psiD21[18][1][2] = -(.196*(sL11D1[2][2]*s18)); psiD21[18][2][0] = (N21[18][2][0]*P11[18][0][0]); psiD21[18][2][2] = (N21[18][2][2]*sL11D1[2][2]); psiD22[18][0][0] = (N22[18][0][0]*sL22D2[0][0]); psiD22[18][0][2] = (sL22D2[2][2]*s18); psiD22[18][2][0] = (N22[18][2][0]*sL22D2[0][0]); psiD22[18][2][2] = (sL22D2[2][2]*c18); P11[16][0][0] = (1.01701701+((N11[18][0][0]*psiD11[18][0][0])+( psiD11[18][0][2]*s18))); P11[16][0][2] = ((N11[18][2][0]*psiD11[18][0][0])+(psiD11[18][0][2]*c18) ); P11[16][2][0] = P11[16][0][2]; P11[16][2][2] = (1.01701701+((N11[18][2][0]*psiD11[18][2][0])+( psiD11[18][2][2]*c18))); Pd[16][0][0] = ((N21[18][0][0]*psiD11[18][0][0])+(N21[18][0][2]* psiD11[18][0][2])); Pd[16][0][1] = (((N21[18][1][0]*psiD11[18][0][0])-(.196*( psiD11[18][0][2]*s18)))-.21366108641371); Pd[16][0][2] = (((N21[18][2][0]*psiD11[18][0][0])+(N21[18][2][2]* psiD11[18][0][2]))-.0101287469971827); Pd[16][1][0] = (.21366108641371+(.49302909*N21[18][0][1])); Pd[16][1][2] = (.0031116347332857+(.49302909*N21[18][2][1])); Pd[16][2][0] = (.0101287469971827+((N21[18][0][0]*psiD11[18][2][0])+( N21[18][0][2]*psiD11[18][2][2]))); Pd[16][2][1] = (((N21[18][1][0]*psiD11[18][2][0])-(.196*( psiD11[18][2][2]*s18)))-.0031116347332857); Pd[16][2][2] = ((N21[18][2][0]*psiD11[18][2][0])+(N21[18][2][2]* psiD11[18][2][2])); P22[16][0][0] = (.0821159586094717+(((N21[18][0][2]*psiD21[18][0][2])+(( .49302909*(N21[18][0][1]*N21[18][0][1]))+(N21[18][0][0]* psiD21[18][0][0])))+((N22[18][0][0]*psiD22[18][0][0])+( psiD22[18][0][2]*s18)))); P22[16][0][1] = (6.48403895498297e-5+((N21[18][1][0]*psiD21[18][0][0])-( .196*(psiD21[18][0][2]*s18)))); P22[16][0][2] = (.0016444710501588+(((N21[18][2][2]*psiD21[18][0][2])+(( .49302909*(N21[18][0][1]*N21[18][2][1]))+(N21[18][2][0]* psiD21[18][0][0])))+((N22[18][2][0]*psiD22[18][0][0])+( psiD22[18][0][2]*c18)))); P22[16][1][0] = P22[16][0][1]; P22[16][1][1] = (.081693043947646+((N21[18][1][0]*psiD21[18][1][0])-( .196*(psiD21[18][1][2]*s18)))); P22[16][1][2] = (((N21[18][2][0]*psiD21[18][1][0])+(N21[18][2][2]* psiD21[18][1][2]))-.000315761551912526); P22[16][2][0] = P22[16][0][2]; P22[16][2][1] = P22[16][1][2]; P22[16][2][2] = (.000854565190387551+(((N21[18][2][2]*psiD21[18][2][2])+ ((.49302909*(N21[18][2][1]*N21[18][2][1]))+(N21[18][2][0]* psiD21[18][2][0])))+((N22[18][2][0]*psiD22[18][2][0])+( psiD22[18][2][2]*c18)))); if (P22[17][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[17] = (1./P22[17][1][1]); } G1[17][0] = (DD[17]*Pd[17][0][1]); G1[17][2] = (DD[17]*Pd[17][2][1]); G2[17][0] = (DD[17]*P22[17][0][1]); G2[17][2] = (DD[17]*P22[17][2][1]); P11[17][0][0] = (P11[17][0][0]-(G1[17][0]*Pd[17][0][1])); P11[17][0][2] = (P11[17][0][2]-(G1[17][2]*Pd[17][0][1])); P11[17][2][2] = (P11[17][2][2]-(G1[17][2]*Pd[17][2][1])); Pd[17][0][0] = (Pd[17][0][0]-(G2[17][0]*Pd[17][0][1])); Pd[17][0][2] = (Pd[17][0][2]-(G2[17][2]*Pd[17][0][1])); Pd[17][2][0] = (Pd[17][2][0]-(G2[17][0]*Pd[17][2][1])); Pd[17][2][2] = (Pd[17][2][2]-(G2[17][2]*Pd[17][2][1])); P22[17][0][0] = (P22[17][0][0]-(G2[17][0]*P22[17][0][1])); P22[17][0][2] = (P22[17][0][2]-(G2[17][2]*P22[17][0][1])); P22[17][1][2] = (P22[17][1][2]-P22[17][2][1]); P22[17][2][2] = (P22[17][2][2]-(G2[17][2]*P22[17][2][1])); sD1INV[0][0] = 0.; if (P11[17][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[17][0][0]); } sL11[2][0] = (P11[17][0][2]*sD1INV[0][0]); sL11D1[2][2] = (P11[17][2][2]-(P11[17][0][2]*sL11[2][0])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[2][0] = (Pd[17][2][0]-(Pd[17][0][0]*sL11[2][0])); sD1L21[2][2] = (Pd[17][2][2]-(Pd[17][0][2]*sL11[2][0])); sL21[0][0] = (Pd[17][0][0]*sD1INV[0][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[2][0] = (Pd[17][0][2]*sD1INV[0][0]); sL21[2][2] = (sD1INV[2][2]*sD1L21[2][2]); sL22D2[0][0] = (P22[17][0][0]-((sD1L21[2][0]*sL21[0][2])+(( 2.02801701449491*(Pd[17][1][0]*Pd[17][1][0]))+(Pd[17][0][0]*sL21[0][0] )))); sL22D2[2][0] = (P22[17][0][2]-((sD1L21[2][0]*sL21[2][2])+(( 2.02801701449491*(Pd[17][1][0]*Pd[17][1][2]))+(Pd[17][0][0]*sL21[2][0] )))); sL22D2[2][2] = (P22[17][2][2]-((sD1L21[2][2]*sL21[2][2])+(( 2.02801701449491*(Pd[17][1][2]*Pd[17][1][2]))+(Pd[17][0][2]*sL21[2][0] )))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[2][0] = (sD2INV[0][0]*sL22D2[2][0]); sL22D2[2][2] = (sL22D2[2][2]-(sL22[2][0]*sL22D2[2][0])); N11[17][0][0] = (c17+(sL11[2][0]*s17)); N11[17][2][0] = ((sL11[2][0]*c17)-s17); N21[17][0][0] = ((.036*(s17-(sL11[2][0]*c17)))+((sL21[0][0]*c17)+( sL21[2][0]*s17))); N21[17][0][1] = (.196+(2.02801701449491*((Pd[17][1][0]*c17)+( Pd[17][1][2]*s17)))); N21[17][0][2] = (((sL21[0][2]*c17)+(sL21[2][2]*s17))-(.036*c17)); N21[17][1][0] = -(.196*(c17+(sL11[2][0]*s17))); N21[17][2][0] = ((.036*(c17+(sL11[2][0]*s17)))+((sL21[2][0]*c17)-( sL21[0][0]*s17))); N21[17][2][1] = (2.02801701449491*((Pd[17][1][2]*c17)-(Pd[17][1][0]*s17) )); N21[17][2][2] = ((.036*s17)+((sL21[2][2]*c17)-(sL21[0][2]*s17))); N22[17][0][0] = (c17+(sL22[2][0]*s17)); N22[17][2][0] = ((sL22[2][0]*c17)-s17); psiD11[17][0][0] = (N11[17][0][0]*P11[17][0][0]); psiD11[17][0][2] = (sL11D1[2][2]*s17); psiD11[17][2][0] = (N11[17][2][0]*P11[17][0][0]); psiD11[17][2][2] = (sL11D1[2][2]*c17); psiD21[17][0][0] = (N21[17][0][0]*P11[17][0][0]); psiD21[17][0][2] = (N21[17][0][2]*sL11D1[2][2]); psiD21[17][1][0] = (N21[17][1][0]*P11[17][0][0]); psiD21[17][1][2] = -(.196*(sL11D1[2][2]*s17)); psiD21[17][2][0] = (N21[17][2][0]*P11[17][0][0]); psiD21[17][2][2] = (N21[17][2][2]*sL11D1[2][2]); psiD22[17][0][0] = (N22[17][0][0]*sL22D2[0][0]); psiD22[17][0][2] = (sL22D2[2][2]*s17); psiD22[17][2][0] = (N22[17][2][0]*sL22D2[0][0]); psiD22[17][2][2] = (sL22D2[2][2]*c17); P11[15][0][0] = (1.00985702+((N11[17][0][0]*psiD11[17][0][0])+( psiD11[17][0][2]*s17))); P11[15][0][2] = ((N11[17][2][0]*psiD11[17][0][0])+(psiD11[17][0][2]*c17) ); P11[15][2][0] = P11[15][0][2]; P11[15][2][2] = (1.00985702+((N11[17][2][0]*psiD11[17][2][0])+( psiD11[17][2][2]*c17))); Pd[15][0][0] = ((N21[17][0][0]*psiD11[17][0][0])+(N21[17][0][2]* psiD11[17][0][2])); Pd[15][0][1] = (((N21[17][1][0]*psiD11[17][0][0])-(.196*( psiD11[17][0][2]*s17)))-.211116810553103); Pd[15][0][2] = (.0089438693876214+((N21[17][2][0]*psiD11[17][0][0])+( N21[17][2][2]*psiD11[17][0][2]))); Pd[15][1][0] = (.211116810553103+(.49309251*N21[17][0][1])); Pd[15][1][2] = (.0031049164922622+(.49309251*N21[17][2][1])); Pd[15][2][0] = (((N21[17][0][0]*psiD11[17][2][0])+(N21[17][0][2]* psiD11[17][2][2]))-.0089438693876214); Pd[15][2][1] = (((N21[17][1][0]*psiD11[17][2][0])-(.196*( psiD11[17][2][2]*s17)))-.0031049164922622); Pd[15][2][2] = ((N21[17][2][0]*psiD11[17][2][0])+(N21[17][2][2]* psiD11[17][2][2])); P22[15][0][0] = (.0808924175086453+(((N21[17][0][2]*psiD21[17][0][2])+(( .49309251*(N21[17][0][1]*N21[17][0][1]))+(N21[17][0][0]* psiD21[17][0][0])))+((N22[17][0][0]*psiD22[17][0][0])+( psiD22[17][0][2]*s17)))); P22[15][0][1] = (((N21[17][1][0]*psiD21[17][0][0])-(.196*( psiD21[17][0][2]*s17)))-6.53210897421254e-5); P22[15][0][2] = (.00163464185689468+(((N21[17][2][2]*psiD21[17][0][2])+( (.49309251*(N21[17][0][1]*N21[17][2][1]))+(N21[17][2][0]* psiD21[17][0][0])))+((N22[17][2][0]*psiD22[17][0][0])+( psiD22[17][0][2]*c17)))); P22[15][1][0] = P22[15][0][1]; P22[15][1][1] = (.0805048419106392+((N21[17][1][0]*psiD21[17][1][0])-( .196*(psiD21[17][1][2]*s17)))); P22[15][1][2] = (.000495739189159706+((N21[17][2][0]*psiD21[17][1][0])+( N21[17][2][2]*psiD21[17][1][2]))); P22[15][2][0] = P22[15][0][2]; P22[15][2][1] = P22[15][1][2]; P22[15][2][2] = (.0008176984125986+(((N21[17][2][2]*psiD21[17][2][2])+(( .49309251*(N21[17][2][1]*N21[17][2][1]))+(N21[17][2][0]* psiD21[17][2][0])))+((N22[17][2][0]*psiD22[17][2][0])+( psiD22[17][2][2]*c17)))); if (P22[16][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[16] = (1./P22[16][1][1]); } G1[16][0] = (DD[16]*Pd[16][0][1]); G1[16][2] = (DD[16]*Pd[16][2][1]); G2[16][0] = (DD[16]*P22[16][0][1]); G2[16][2] = (DD[16]*P22[16][2][1]); P11[16][0][0] = (P11[16][0][0]-(G1[16][0]*Pd[16][0][1])); P11[16][0][2] = (P11[16][0][2]-(G1[16][2]*Pd[16][0][1])); P11[16][2][2] = (P11[16][2][2]-(G1[16][2]*Pd[16][2][1])); Pd[16][0][0] = (Pd[16][0][0]-(G2[16][0]*Pd[16][0][1])); Pd[16][0][2] = (Pd[16][0][2]-(G2[16][2]*Pd[16][0][1])); Pd[16][2][0] = (Pd[16][2][0]-(G2[16][0]*Pd[16][2][1])); Pd[16][2][2] = (Pd[16][2][2]-(G2[16][2]*Pd[16][2][1])); P22[16][0][0] = (P22[16][0][0]-(G2[16][0]*P22[16][0][1])); P22[16][0][2] = (P22[16][0][2]-(G2[16][2]*P22[16][0][1])); P22[16][1][2] = (P22[16][1][2]-P22[16][2][1]); P22[16][2][2] = (P22[16][2][2]-(G2[16][2]*P22[16][2][1])); sD1INV[0][0] = 0.; if (P11[16][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[16][0][0]); } sL11[2][0] = (P11[16][0][2]*sD1INV[0][0]); sL11D1[2][2] = (P11[16][2][2]-(P11[16][0][2]*sL11[2][0])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[2][0] = (Pd[16][2][0]-(Pd[16][0][0]*sL11[2][0])); sD1L21[2][2] = (Pd[16][2][2]-(Pd[16][0][2]*sL11[2][0])); sL21[0][0] = (Pd[16][0][0]*sD1INV[0][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[2][0] = (Pd[16][0][2]*sD1INV[0][0]); sL21[2][2] = (sD1INV[2][2]*sD1L21[2][2]); sL22D2[0][0] = (P22[16][0][0]-((sD1L21[2][0]*sL21[0][2])+(( .662231437834911*(Pd[16][1][0]*Pd[16][1][0]))+(Pd[16][0][0]*sL21[0][0] )))); sL22D2[2][0] = (P22[16][0][2]-((sD1L21[2][0]*sL21[2][2])+(( .662231437834911*(Pd[16][1][0]*Pd[16][1][2]))+(Pd[16][0][0]*sL21[2][0] )))); sL22D2[2][2] = (P22[16][2][2]-((sD1L21[2][2]*sL21[2][2])+(( .662231437834911*(Pd[16][1][2]*Pd[16][1][2]))+(Pd[16][0][2]*sL21[2][0] )))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[2][0] = (sD2INV[0][0]*sL22D2[2][0]); sL22D2[2][2] = (sL22D2[2][2]-(sL22[2][0]*sL22D2[2][0])); N11[16][0][0] = (c16+(sL11[2][0]*s16)); N11[16][2][0] = ((sL11[2][0]*c16)-s16); N21[16][0][0] = ((.09879136*((sL11[2][0]*c16)-s16))+((sL21[0][0]*c16)+( sL21[2][0]*s16))); N21[16][0][1] = ((.662231437834911*((Pd[16][1][0]*c16)+(Pd[16][1][2]*s16 )))-.2639286); N21[16][0][2] = ((.09879136*c16)+((sL21[0][2]*c16)+(sL21[2][2]*s16))); N21[16][1][0] = (rikt[16][1][0]+(rikt[16][1][2]*sL11[2][0])); N21[16][2][0] = (((sL21[2][0]*c16)-(sL21[0][0]*s16))-(.09879136*(c16+( sL11[2][0]*s16)))); N21[16][2][1] = ((.662231437834911*((Pd[16][1][2]*c16)-(Pd[16][1][0]*s16 )))-.01052618); N21[16][2][2] = (((sL21[2][2]*c16)-(sL21[0][2]*s16))-(.09879136*s16)); N22[16][0][0] = (c16+(sL22[2][0]*s16)); N22[16][2][0] = ((sL22[2][0]*c16)-s16); psiD11[16][0][0] = (N11[16][0][0]*P11[16][0][0]); psiD11[16][0][2] = (sL11D1[2][2]*s16); psiD11[16][2][0] = (N11[16][2][0]*P11[16][0][0]); psiD11[16][2][2] = (sL11D1[2][2]*c16); psiD21[16][0][0] = (N21[16][0][0]*P11[16][0][0]); psiD21[16][0][2] = (N21[16][0][2]*sL11D1[2][2]); psiD21[16][1][0] = (N21[16][1][0]*P11[16][0][0]); psiD21[16][1][2] = (rikt[16][1][2]*sL11D1[2][2]); psiD21[16][2][0] = (N21[16][2][0]*P11[16][0][0]); psiD21[16][2][2] = (N21[16][2][2]*sL11D1[2][2]); psiD22[16][0][0] = (N22[16][0][0]*sL22D2[0][0]); psiD22[16][0][2] = (sL22D2[2][2]*s16); psiD22[16][2][0] = (N22[16][2][0]*sL22D2[0][0]); psiD22[16][2][2] = (sL22D2[2][2]*c16); P11[5][0][0] = (11.02968359+((N11[16][0][0]*psiD11[16][0][0])+( psiD11[16][0][2]*s16))); P11[5][0][2] = ((N11[16][2][0]*psiD11[16][0][0])+(psiD11[16][0][2]*c16)); P11[5][2][0] = P11[5][0][2]; P11[5][2][2] = (11.02968359+((N11[16][2][0]*psiD11[16][2][0])+( psiD11[16][2][2]*c16))); Pd[5][0][0] = ((N21[16][0][0]*psiD11[16][0][0])+(N21[16][0][2]* psiD11[16][0][2])); Pd[5][0][1] = ((N21[16][1][0]*psiD11[16][0][0])+(psiD11[16][0][2]* rikt[16][1][2])); Pd[5][0][2] = ((N21[16][2][0]*psiD11[16][0][0])+(N21[16][2][2]* psiD11[16][0][2])); Pd[5][1][0] = (1.5100461*N21[16][0][1]); Pd[5][1][2] = (1.5100461*N21[16][2][1]); Pd[5][2][0] = ((N21[16][0][0]*psiD11[16][2][0])+(N21[16][0][2]* psiD11[16][2][2])); Pd[5][2][1] = ((N21[16][1][0]*psiD11[16][2][0])+(psiD11[16][2][2]* rikt[16][1][2])); Pd[5][2][2] = ((N21[16][2][0]*psiD11[16][2][0])+(N21[16][2][2]* psiD11[16][2][2])); P22[5][0][0] = (.48506516+(((N21[16][0][2]*psiD21[16][0][2])+((1.5100461 *(N21[16][0][1]*N21[16][0][1]))+(N21[16][0][0]*psiD21[16][0][0])))+(( N22[16][0][0]*psiD22[16][0][0])+(psiD22[16][0][2]*s16)))); P22[5][0][1] = (((N21[16][1][0]*psiD21[16][0][0])+(psiD21[16][0][2]* rikt[16][1][2]))-5.312e-5); P22[5][0][2] = ((((N21[16][2][2]*psiD21[16][0][2])+((1.5100461*( N21[16][0][1]*N21[16][2][1]))+(N21[16][2][0]*psiD21[16][0][0])))+(( N22[16][2][0]*psiD22[16][0][0])+(psiD22[16][0][2]*c16)))-.01450199); P22[5][1][0] = P22[5][0][1]; P22[5][1][1] = (.47268161+((N21[16][1][0]*psiD21[16][1][0])+( psiD21[16][1][2]*rikt[16][1][2]))); P22[5][1][2] = (((N21[16][2][0]*psiD21[16][1][0])+(N21[16][2][2]* psiD21[16][1][2]))-.00031516); P22[5][2][0] = P22[5][0][2]; P22[5][2][1] = P22[5][1][2]; P22[5][2][2] = (.03863225+(((N21[16][2][2]*psiD21[16][2][2])+((1.5100461 *(N21[16][2][1]*N21[16][2][1]))+(N21[16][2][0]*psiD21[16][2][0])))+(( N22[16][2][0]*psiD22[16][2][0])+(psiD22[16][2][2]*c16)))); if (P22[15][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[15] = (1./P22[15][1][1]); } G1[15][0] = (DD[15]*Pd[15][0][1]); G1[15][2] = (DD[15]*Pd[15][2][1]); G2[15][0] = (DD[15]*P22[15][0][1]); G2[15][2] = (DD[15]*P22[15][2][1]); P11[15][0][0] = (P11[15][0][0]-(G1[15][0]*Pd[15][0][1])); P11[15][0][2] = (P11[15][0][2]-(G1[15][2]*Pd[15][0][1])); P11[15][2][2] = (P11[15][2][2]-(G1[15][2]*Pd[15][2][1])); Pd[15][0][0] = (Pd[15][0][0]-(G2[15][0]*Pd[15][0][1])); Pd[15][0][2] = (Pd[15][0][2]-(G2[15][2]*Pd[15][0][1])); Pd[15][2][0] = (Pd[15][2][0]-(G2[15][0]*Pd[15][2][1])); Pd[15][2][2] = (Pd[15][2][2]-(G2[15][2]*Pd[15][2][1])); P22[15][0][0] = (P22[15][0][0]-(G2[15][0]*P22[15][0][1])); P22[15][0][2] = (P22[15][0][2]-(G2[15][2]*P22[15][0][1])); P22[15][1][2] = (P22[15][1][2]-P22[15][2][1]); P22[15][2][2] = (P22[15][2][2]-(G2[15][2]*P22[15][2][1])); sD1INV[0][0] = 0.; if (P11[15][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[15][0][0]); } sL11[2][0] = (P11[15][0][2]*sD1INV[0][0]); sL11D1[2][2] = (P11[15][2][2]-(P11[15][0][2]*sL11[2][0])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[2][0] = (Pd[15][2][0]-(Pd[15][0][0]*sL11[2][0])); sD1L21[2][2] = (Pd[15][2][2]-(Pd[15][0][2]*sL11[2][0])); sL21[0][0] = (Pd[15][0][0]*sD1INV[0][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[2][0] = (Pd[15][0][2]*sD1INV[0][0]); sL21[2][2] = (sD1INV[2][2]*sD1L21[2][2]); sL22D2[0][0] = (P22[15][0][0]-((sD1L21[2][0]*sL21[0][2])+(( .665358337082683*(Pd[15][1][0]*Pd[15][1][0]))+(Pd[15][0][0]*sL21[0][0] )))); sL22D2[2][0] = (P22[15][0][2]-((sD1L21[2][0]*sL21[2][2])+(( .665358337082683*(Pd[15][1][0]*Pd[15][1][2]))+(Pd[15][0][0]*sL21[2][0] )))); sL22D2[2][2] = (P22[15][2][2]-((sD1L21[2][2]*sL21[2][2])+(( .665358337082683*(Pd[15][1][2]*Pd[15][1][2]))+(Pd[15][0][2]*sL21[2][0] )))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[2][0] = (sD2INV[0][0]*sL22D2[2][0]); sL22D2[2][2] = (sL22D2[2][2]-(sL22[2][0]*sL22D2[2][0])); N11[15][0][0] = (c15+(sL11[2][0]*s15)); N11[15][2][0] = ((sL11[2][0]*c15)-s15); N21[15][0][0] = ((.10120864*(s15-(sL11[2][0]*c15)))+((sL21[0][0]*c15)+( sL21[2][0]*s15))); N21[15][0][1] = ((.665358337082683*((Pd[15][1][0]*c15)+(Pd[15][1][2]*s15 )))-.2639286); N21[15][0][2] = (((sL21[0][2]*c15)+(sL21[2][2]*s15))-(.10120864*c15)); N21[15][1][0] = (rikt[15][1][0]+(rikt[15][1][2]*sL11[2][0])); N21[15][2][0] = ((.10120864*(c15+(sL11[2][0]*s15)))+((sL21[2][0]*c15)-( sL21[0][0]*s15))); N21[15][2][1] = ((.665358337082683*((Pd[15][1][2]*c15)-(Pd[15][1][0]*s15 )))-.01052618); N21[15][2][2] = ((.10120864*s15)+((sL21[2][2]*c15)-(sL21[0][2]*s15))); N22[15][0][0] = (c15+(sL22[2][0]*s15)); N22[15][2][0] = ((sL22[2][0]*c15)-s15); psiD11[15][0][0] = (N11[15][0][0]*P11[15][0][0]); psiD11[15][0][2] = (sL11D1[2][2]*s15); psiD11[15][2][0] = (N11[15][2][0]*P11[15][0][0]); psiD11[15][2][2] = (sL11D1[2][2]*c15); psiD21[15][0][0] = (N21[15][0][0]*P11[15][0][0]); psiD21[15][0][2] = (N21[15][0][2]*sL11D1[2][2]); psiD21[15][1][0] = (N21[15][1][0]*P11[15][0][0]); psiD21[15][1][2] = (rikt[15][1][2]*sL11D1[2][2]); psiD21[15][2][0] = (N21[15][2][0]*P11[15][0][0]); psiD21[15][2][2] = (N21[15][2][2]*sL11D1[2][2]); psiD22[15][0][0] = (N22[15][0][0]*sL22D2[0][0]); psiD22[15][0][2] = (sL22D2[2][2]*s15); psiD22[15][2][0] = (N22[15][2][0]*sL22D2[0][0]); psiD22[15][2][2] = (sL22D2[2][2]*c15); P11[5][0][0] = (P11[5][0][0]+((N11[15][0][0]*psiD11[15][0][0])+( psiD11[15][0][2]*s15))); P11[5][0][2] = (P11[5][0][2]+((N11[15][2][0]*psiD11[15][0][0])+( psiD11[15][0][2]*c15))); P11[5][2][0] = P11[5][0][2]; P11[5][2][2] = (P11[5][2][2]+((N11[15][2][0]*psiD11[15][2][0])+( psiD11[15][2][2]*c15))); Pd[5][0][0] = (Pd[5][0][0]+((N21[15][0][0]*psiD11[15][0][0])+( N21[15][0][2]*psiD11[15][0][2]))); Pd[5][0][1] = (Pd[5][0][1]+((N21[15][1][0]*psiD11[15][0][0])+( psiD11[15][0][2]*rikt[15][1][2]))); Pd[5][0][2] = (Pd[5][0][2]+((N21[15][2][0]*psiD11[15][0][0])+( N21[15][2][2]*psiD11[15][0][2]))); Pd[5][1][0] = (Pd[5][1][0]+(1.50294953*N21[15][0][1])); Pd[5][1][2] = (Pd[5][1][2]+(1.50294953*N21[15][2][1])); Pd[5][2][0] = (Pd[5][2][0]+((N21[15][0][0]*psiD11[15][2][0])+( N21[15][0][2]*psiD11[15][2][2]))); Pd[5][2][1] = (Pd[5][2][1]+((N21[15][1][0]*psiD11[15][2][0])+( psiD11[15][2][2]*rikt[15][1][2]))); Pd[5][2][2] = (Pd[5][2][2]+((N21[15][2][0]*psiD11[15][2][0])+( N21[15][2][2]*psiD11[15][2][2]))); P22[5][0][0] = (P22[5][0][0]+(((N21[15][0][2]*psiD21[15][0][2])+(( 1.50294953*(N21[15][0][1]*N21[15][0][1]))+(N21[15][0][0]* psiD21[15][0][0])))+((N22[15][0][0]*psiD22[15][0][0])+( psiD22[15][0][2]*s15)))); P22[5][0][1] = (P22[5][0][1]+((N21[15][1][0]*psiD21[15][0][0])+( psiD21[15][0][2]*rikt[15][1][2]))); P22[5][0][2] = (P22[5][0][2]+(((N21[15][2][2]*psiD21[15][0][2])+(( 1.50294953*(N21[15][0][1]*N21[15][2][1]))+(N21[15][2][0]* psiD21[15][0][0])))+((N22[15][2][0]*psiD22[15][0][0])+( psiD22[15][0][2]*c15)))); P22[5][1][0] = P22[5][0][1]; P22[5][1][1] = (P22[5][1][1]+((N21[15][1][0]*psiD21[15][1][0])+( psiD21[15][1][2]*rikt[15][1][2]))); P22[5][1][2] = (P22[5][1][2]+((N21[15][2][0]*psiD21[15][1][0])+( N21[15][2][2]*psiD21[15][1][2]))); P22[5][2][0] = P22[5][0][2]; P22[5][2][1] = P22[5][1][2]; P22[5][2][2] = (P22[5][2][2]+(((N21[15][2][2]*psiD21[15][2][2])+(( 1.50294953*(N21[15][2][1]*N21[15][2][1]))+(N21[15][2][0]* psiD21[15][2][0])))+((N22[15][2][0]*psiD22[15][2][0])+( psiD22[15][2][2]*c15)))); DD[14] = 378.678658433892; N11[14][0][0] = (c14-(.419119969601573*s14)); N11[14][2][0] = -(s14+(.419119969601573*c14)); N21[14][0][0] = (((.00209559984800787*c14)+(.005*s14))-(( 2.00857550022735e-5*c14)+(.00104680874563099*s14))); N21[14][0][1] = (.297+((.03539679*s14)+(.03987912*c14))); N21[14][0][2] = (((.0014923238680468*c14)-(.000129777127194929*s14))-( .005*c14)); N21[14][1][0] = (rikt[14][1][0]-(.419119969601573*rikt[14][1][2])); N21[14][2][0] = (((2.00857550022735e-5*s14)-(.00104680874563099*c14))+(( .005*c14)-(.00209559984800787*s14))); N21[14][2][1] = (((.03539679*c14)-(.03987912*s14))-.015); N21[14][2][2] = ((.005*s14)-((.000129777127194929*c14)+( .0014923238680468*s14))); N22[14][0][0] = (c14+(.514660603029699*s14)); N22[14][2][0] = ((.514660603029699*c14)-s14); P11[12][0][0] = (1.02007998+((.334460383269538*(s14*s14))+( .361766429248675*(N11[14][0][0]*N11[14][0][0])))); P11[12][0][2] = ((.334460383269538*(s14*c14))+(.361766429248675*( N11[14][0][0]*N11[14][2][0]))); P11[12][2][0] = P11[12][0][2]; P11[12][2][2] = (1.02007998+((.334460383269538*(c14*c14))+( .361766429248675*(N11[14][2][0]*N11[14][2][0])))); Pd[12][0][0] = ((.334460383269538*(N21[14][0][2]*s14))+(.361766429248675 *(N11[14][0][0]*N21[14][0][0]))); Pd[12][0][1] = (((.334460383269538*(rikt[14][1][2]*s14))+( .361766429248675*(N11[14][0][0]*N21[14][1][0])))-.128402608285699); Pd[12][0][2] = (.0034978134482208+((.334460383269538*(N21[14][2][2]*s14) )+(.361766429248675*(N11[14][0][0]*N21[14][2][0])))); Pd[12][1][0] = (.128402608285699+(.5325902*N21[14][0][1])); Pd[12][1][2] = ((.5325902*N21[14][2][1])-.0170799437635254); Pd[12][2][0] = (((.334460383269538*(N21[14][0][2]*c14))+( .361766429248675*(N11[14][2][0]*N21[14][0][0])))-.0034978134482208); Pd[12][2][1] = (.0170799437635254+((.334460383269538*(rikt[14][1][2]*c14 ))+(.361766429248675*(N11[14][2][0]*N21[14][1][0])))); Pd[12][2][2] = ((.334460383269538*(N21[14][2][2]*c14))+(.361766429248675 *(N11[14][2][0]*N21[14][2][0]))); P22[12][0][0] = (.0243930073164681+(((.000312230416881203*(N22[14][0][0] *N22[14][0][0]))+(.000946192658838129*(s14*s14)))+((.334460383269538*( N21[14][0][2]*N21[14][0][2]))+((.361766429248675*(N21[14][0][0]* N21[14][0][0]))+(.5325902*(N21[14][0][1]*N21[14][0][1])))))); P22[12][0][1] = (((.334460383269538*(N21[14][0][2]*rikt[14][1][2]))+( .361766429248675*(N21[14][0][0]*N21[14][1][0])))-7.13964439673781e-5); P22[12][0][2] = ((((.000312230416881203*(N22[14][0][0]*N22[14][2][0]))+( .000946192658838129*(s14*c14)))+((.334460383269538*(N21[14][0][2]* N21[14][2][2]))+((.361766429248675*(N21[14][0][0]*N21[14][2][0]))+( .5325902*(N21[14][0][1]*N21[14][2][1])))))-.00241911860443151); P22[12][1][0] = P22[12][0][1]; P22[12][1][1] = (.0248331154208584+((.334460383269538*(rikt[14][1][2]* rikt[14][1][2]))+(.361766429248675*(N21[14][1][0]*N21[14][1][0])))); P22[12][1][2] = (((.334460383269538*(N21[14][2][2]*rikt[14][1][2]))+( .361766429248675*(N21[14][1][0]*N21[14][2][0])))-.000638647407707331); P22[12][2][0] = P22[12][0][2]; P22[12][2][1] = P22[12][1][2]; P22[12][2][2] = (.000873655829193064+(((.000312230416881203*( N22[14][2][0]*N22[14][2][0]))+(.000946192658838129*(c14*c14)))+(( .334460383269538*(N21[14][2][2]*N21[14][2][2]))+((.361766429248675*( N21[14][2][0]*N21[14][2][0]))+(.5325902*(N21[14][2][1]*N21[14][2][1])) )))); DD[13] = 377.831870134533; N11[13][0][0] = (c13-(.418769123727484*s13)); N11[13][2][0] = -(s13+(.418769123727484*c13)); N21[13][0][0] = -(((.0012261730340833*c13)+(.00183597158489252*s13))+(( .00209384561863742*c13)+(.005*s13))); N21[13][0][1] = (.297+((.03543467*s13)+(.03983908*c13))); N21[13][0][2] = ((.005*c13)+((.000134620428706795*s13)+( .00133912219454309*c13))); N21[13][1][0] = (rikt[13][1][0]-(.418769123727484*rikt[13][1][2])); N21[13][2][0] = (((.0012261730340833*s13)-(.00183597158489252*c13))+(( .00209384561863742*s13)-(.005*c13))); N21[13][2][1] = (((.03543467*c13)-(.03983908*s13))-.015); N21[13][2][2] = (((.000134620428706795*c13)-(.00133912219454309*s13))-( .005*s13)); N22[13][0][0] = (c13+(.511407709589138*s13)); N22[13][2][0] = ((.511407709589138*c13)-s13); P11[11][0][0] = (1.02133193+((.334974444350359*(s13*s13))+( .362926681002944*(N11[13][0][0]*N11[13][0][0])))); P11[11][0][2] = ((.334974444350359*(s13*c13))+(.362926681002944*( N11[13][0][0]*N11[13][2][0]))); P11[11][2][0] = P11[11][0][2]; P11[11][2][2] = (1.02133193+((.334974444350359*(c13*c13))+( .362926681002944*(N11[13][2][0]*N11[13][2][0])))); Pd[11][0][0] = ((.334974444350359*(N21[13][0][2]*s13))+(.362926681002944 *(N11[13][0][0]*N21[13][0][0]))); Pd[11][0][1] = (((.334974444350359*(rikt[13][1][2]*s13))+( .362926681002944*(N11[13][0][0]*N21[13][1][0])))-.128874716709871); Pd[11][0][2] = (((.334974444350359*(N21[13][2][2]*s13))+( .362926681002944*(N11[13][0][0]*N21[13][2][0])))-.0057310509254055); Pd[11][1][0] = (.128874716709871+(.53380008*N21[13][0][1])); Pd[11][1][2] = ((.53380008*N21[13][2][1])-.0170783652806038); Pd[11][2][0] = (.0057310509254055+((.334974444350359*(N21[13][0][2]*c13) )+(.362926681002944*(N11[13][2][0]*N21[13][0][0])))); Pd[11][2][1] = (.0170783652806038+((.334974444350359*(rikt[13][1][2]*c13 ))+(.362926681002944*(N11[13][2][0]*N21[13][1][0])))); Pd[11][2][2] = ((.334974444350359*(N21[13][2][2]*c13))+(.362926681002944 *(N11[13][2][0]*N21[13][2][0]))); P22[11][0][0] = (.0245607960224647+(((.000314625001372641*(N22[13][0][0] *N22[13][0][0]))+(.000950417356814985*(s13*s13)))+((.334974444350359*( N21[13][0][2]*N21[13][0][2]))+((.362926681002944*(N21[13][0][0]* N21[13][0][0]))+(.53380008*(N21[13][0][1]*N21[13][0][1])))))); P22[11][0][1] = (.000108562685017316+((.334974444350359*(N21[13][0][2]* rikt[13][1][2]))+(.362926681002944*(N21[13][0][0]*N21[13][1][0])))); P22[11][0][2] = ((((.000314625001372641*(N22[13][0][0]*N22[13][2][0]))+( .000950417356814985*(s13*c13)))+((.334974444350359*(N21[13][0][2]* N21[13][2][2]))+((.362926681002944*(N21[13][0][0]*N21[13][2][0]))+( .53380008*(N21[13][0][1]*N21[13][2][1])))))-.00241896919541878); P22[11][1][0] = P22[11][0][1]; P22[11][1][1] = (.0249808957074325+((.334974444350359*(rikt[13][1][2]* rikt[13][1][2]))+(.362926681002944*(N21[13][1][0]*N21[13][1][0])))); P22[11][1][2] = (.000924201141609933+((.334974444350359*(N21[13][2][2]* rikt[13][1][2]))+(.362926681002944*(N21[13][1][0]*N21[13][2][0])))); P22[11][2][0] = P22[11][0][2]; P22[11][2][1] = P22[11][1][2]; P22[11][2][2] = (.000893727550188336+(((.000314625001372641*( N22[13][2][0]*N22[13][2][0]))+(.000950417356814985*(c13*c13)))+(( .334974444350359*(N21[13][2][2]*N21[13][2][2]))+((.362926681002944*( N21[13][2][0]*N21[13][2][0]))+(.53380008*(N21[13][2][1]*N21[13][2][1]) ))))); if (P22[12][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[12] = (1./P22[12][1][1]); } G1[12][0] = (DD[12]*Pd[12][0][1]); G1[12][2] = (DD[12]*Pd[12][2][1]); G2[12][0] = (DD[12]*P22[12][0][1]); G2[12][2] = (DD[12]*P22[12][2][1]); P11[12][0][0] = (P11[12][0][0]-(G1[12][0]*Pd[12][0][1])); P11[12][0][2] = (P11[12][0][2]-(G1[12][2]*Pd[12][0][1])); P11[12][2][2] = (P11[12][2][2]-(G1[12][2]*Pd[12][2][1])); Pd[12][0][0] = (Pd[12][0][0]-(G2[12][0]*Pd[12][0][1])); Pd[12][0][2] = (Pd[12][0][2]-(G2[12][2]*Pd[12][0][1])); Pd[12][2][0] = (Pd[12][2][0]-(G2[12][0]*Pd[12][2][1])); Pd[12][2][2] = (Pd[12][2][2]-(G2[12][2]*Pd[12][2][1])); P22[12][0][0] = (P22[12][0][0]-(G2[12][0]*P22[12][0][1])); P22[12][0][2] = (P22[12][0][2]-(G2[12][2]*P22[12][0][1])); P22[12][1][2] = (P22[12][1][2]-P22[12][2][1]); P22[12][2][2] = (P22[12][2][2]-(G2[12][2]*P22[12][2][1])); sD1INV[0][0] = 0.; if (P11[12][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[12][0][0]); } sL11[2][0] = (P11[12][0][2]*sD1INV[0][0]); sL11D1[2][2] = (P11[12][2][2]-(P11[12][0][2]*sL11[2][0])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[2][0] = (Pd[12][2][0]-(Pd[12][0][0]*sL11[2][0])); sD1L21[2][2] = (Pd[12][2][2]-(Pd[12][0][2]*sL11[2][0])); sL21[0][0] = (Pd[12][0][0]*sD1INV[0][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[2][0] = (Pd[12][0][2]*sD1INV[0][0]); sL21[2][2] = (sD1INV[2][2]*sD1L21[2][2]); sL22D2[0][0] = (P22[12][0][0]-((sD1L21[2][0]*sL21[0][2])+(( .644051784391196*(Pd[12][1][0]*Pd[12][1][0]))+(Pd[12][0][0]*sL21[0][0] )))); sL22D2[2][0] = (P22[12][0][2]-((sD1L21[2][0]*sL21[2][2])+(( .644051784391196*(Pd[12][1][0]*Pd[12][1][2]))+(Pd[12][0][0]*sL21[2][0] )))); sL22D2[2][2] = (P22[12][2][2]-((sD1L21[2][2]*sL21[2][2])+(( .644051784391196*(Pd[12][1][2]*Pd[12][1][2]))+(Pd[12][0][2]*sL21[2][0] )))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[2][0] = (sD2INV[0][0]*sL22D2[2][0]); sL22D2[2][2] = (sL22D2[2][2]-(sL22[2][0]*sL22D2[2][0])); N11[12][0][0] = (c12+(sL11[2][0]*s12)); N11[12][2][0] = ((sL11[2][0]*c12)-s12); N21[12][0][0] = ((.003*((sL11[2][0]*c12)-s12))+((sL21[0][0]*c12)+( sL21[2][0]*s12))); N21[12][0][1] = (.291+(.644051784391196*((Pd[12][1][0]*c12)+( Pd[12][1][2]*s12)))); N21[12][0][2] = ((.003*c12)+((sL21[0][2]*c12)+(sL21[2][2]*s12))); N21[12][1][0] = (rikt[12][1][0]+(rikt[12][1][2]*sL11[2][0])); N21[12][2][0] = (((sL21[2][0]*c12)-(sL21[0][0]*s12))-(.003*(c12+( sL11[2][0]*s12)))); N21[12][2][1] = (.016+(.644051784391196*((Pd[12][1][2]*c12)-( Pd[12][1][0]*s12)))); N21[12][2][2] = (((sL21[2][2]*c12)-(sL21[0][2]*s12))-(.003*s12)); N22[12][0][0] = (c12+(sL22[2][0]*s12)); N22[12][2][0] = ((sL22[2][0]*c12)-s12); psiD11[12][0][0] = (N11[12][0][0]*P11[12][0][0]); psiD11[12][0][2] = (sL11D1[2][2]*s12); psiD11[12][2][0] = (N11[12][2][0]*P11[12][0][0]); psiD11[12][2][2] = (sL11D1[2][2]*c12); psiD21[12][0][0] = (N21[12][0][0]*P11[12][0][0]); psiD21[12][0][2] = (N21[12][0][2]*sL11D1[2][2]); psiD21[12][1][0] = (N21[12][1][0]*P11[12][0][0]); psiD21[12][1][2] = (rikt[12][1][2]*sL11D1[2][2]); psiD21[12][2][0] = (N21[12][2][0]*P11[12][0][0]); psiD21[12][2][2] = (N21[12][2][2]*sL11D1[2][2]); psiD22[12][0][0] = (N22[12][0][0]*sL22D2[0][0]); psiD22[12][0][2] = (sL22D2[2][2]*s12); psiD22[12][2][0] = (N22[12][2][0]*sL22D2[0][0]); psiD22[12][2][2] = (sL22D2[2][2]*c12); P11[10][0][0] = (2.8984419+((N11[12][0][0]*psiD11[12][0][0])+( psiD11[12][0][2]*s12))); P11[10][0][2] = ((N11[12][2][0]*psiD11[12][0][0])+(psiD11[12][0][2]*c12) ); P11[10][2][0] = P11[10][0][2]; P11[10][2][2] = (2.8984419+((N11[12][2][0]*psiD11[12][2][0])+( psiD11[12][2][2]*c12))); Pd[10][0][0] = ((N21[12][0][0]*psiD11[12][0][0])+(N21[12][0][2]* psiD11[12][0][2])); Pd[10][0][1] = (((N21[12][1][0]*psiD11[12][0][0])+(psiD11[12][0][2]* rikt[12][1][2]))-.382316022408762); Pd[10][0][2] = (((N21[12][2][0]*psiD11[12][0][0])+(N21[12][2][2]* psiD11[12][0][2]))-.006826526300556); Pd[10][1][0] = (.382316022408762+(1.55267018*N21[12][0][1])); Pd[10][1][2] = (.027350828160741+(1.55267018*N21[12][2][1])); Pd[10][2][0] = (.006826526300556+((N21[12][0][0]*psiD11[12][2][0])+( N21[12][0][2]*psiD11[12][2][2]))); Pd[10][2][1] = (((N21[12][1][0]*psiD11[12][2][0])+(psiD11[12][2][2]* rikt[12][1][2]))-.027350828160741); Pd[10][2][2] = ((N21[12][2][0]*psiD11[12][2][0])+(N21[12][2][2]* psiD11[12][2][2])); P22[10][0][0] = (.105213253081289+(((N21[12][0][2]*psiD21[12][0][2])+(( 1.55267018*(N21[12][0][1]*N21[12][0][1]))+(N21[12][0][0]* psiD21[12][0][0])))+((N22[12][0][0]*psiD22[12][0][0])+( psiD22[12][0][2]*s12)))); P22[10][0][1] = (((N21[12][1][0]*psiD21[12][0][0])+(psiD21[12][0][2]* rikt[12][1][2]))-1.62776451730363e-6); P22[10][0][2] = (.00585195309069782+(((N21[12][2][2]*psiD21[12][0][2])+( (1.55267018*(N21[12][0][1]*N21[12][2][1]))+(N21[12][2][0]* psiD21[12][0][0])))+((N22[12][2][0]*psiD22[12][0][0])+( psiD22[12][0][2]*c12)))); P22[10][1][0] = P22[10][0][1]; P22[10][1][1] = (.105623028054833+((N21[12][1][0]*psiD21[12][1][0])+( psiD21[12][1][2]*rikt[12][1][2]))); P22[10][1][2] = (((N21[12][2][0]*psiD21[12][1][0])+(N21[12][2][2]* psiD21[12][1][2]))-.000660164011381987); P22[10][2][0] = P22[10][0][2]; P22[10][2][1] = P22[10][1][2]; P22[10][2][2] = (.00371973118915186+(((N21[12][2][2]*psiD21[12][2][2])+( (1.55267018*(N21[12][2][1]*N21[12][2][1]))+(N21[12][2][0]* psiD21[12][2][0])))+((N22[12][2][0]*psiD22[12][2][0])+( psiD22[12][2][2]*c12)))); if (P22[11][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[11] = (1./P22[11][1][1]); } G1[11][0] = (DD[11]*Pd[11][0][1]); G1[11][2] = (DD[11]*Pd[11][2][1]); G2[11][0] = (DD[11]*P22[11][0][1]); G2[11][2] = (DD[11]*P22[11][2][1]); P11[11][0][0] = (P11[11][0][0]-(G1[11][0]*Pd[11][0][1])); P11[11][0][2] = (P11[11][0][2]-(G1[11][2]*Pd[11][0][1])); P11[11][2][2] = (P11[11][2][2]-(G1[11][2]*Pd[11][2][1])); Pd[11][0][0] = (Pd[11][0][0]-(G2[11][0]*Pd[11][0][1])); Pd[11][0][2] = (Pd[11][0][2]-(G2[11][2]*Pd[11][0][1])); Pd[11][2][0] = (Pd[11][2][0]-(G2[11][0]*Pd[11][2][1])); Pd[11][2][2] = (Pd[11][2][2]-(G2[11][2]*Pd[11][2][1])); P22[11][0][0] = (P22[11][0][0]-(G2[11][0]*P22[11][0][1])); P22[11][0][2] = (P22[11][0][2]-(G2[11][2]*P22[11][0][1])); P22[11][1][2] = (P22[11][1][2]-P22[11][2][1]); P22[11][2][2] = (P22[11][2][2]-(G2[11][2]*P22[11][2][1])); sD1INV[0][0] = 0.; if (P11[11][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[11][0][0]); } sL11[2][0] = (P11[11][0][2]*sD1INV[0][0]); sL11D1[2][2] = (P11[11][2][2]-(P11[11][0][2]*sL11[2][0])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[2][0] = (Pd[11][2][0]-(Pd[11][0][0]*sL11[2][0])); sD1L21[2][2] = (Pd[11][2][2]-(Pd[11][0][2]*sL11[2][0])); sL21[0][0] = (Pd[11][0][0]*sD1INV[0][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[2][0] = (Pd[11][0][2]*sD1INV[0][0]); sL21[2][2] = (sD1INV[2][2]*sD1L21[2][2]); sL22D2[0][0] = (P22[11][0][0]-((sD1L21[2][0]*sL21[0][2])+(( .64303222721266*(Pd[11][1][0]*Pd[11][1][0]))+(Pd[11][0][0]*sL21[0][0]) ))); sL22D2[2][0] = (P22[11][0][2]-((sD1L21[2][0]*sL21[2][2])+(( .64303222721266*(Pd[11][1][0]*Pd[11][1][2]))+(Pd[11][0][0]*sL21[2][0]) ))); sL22D2[2][2] = (P22[11][2][2]-((sD1L21[2][2]*sL21[2][2])+(( .64303222721266*(Pd[11][1][2]*Pd[11][1][2]))+(Pd[11][0][2]*sL21[2][0]) ))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[2][0] = (sD2INV[0][0]*sL22D2[2][0]); sL22D2[2][2] = (sL22D2[2][2]-(sL22[2][0]*sL22D2[2][0])); N11[11][0][0] = (c11+(sL11[2][0]*s11)); N11[11][2][0] = ((sL11[2][0]*c11)-s11); N21[11][0][0] = ((.003*(s11-(sL11[2][0]*c11)))+((sL21[0][0]*c11)+( sL21[2][0]*s11))); N21[11][0][1] = (.291+(.64303222721266*((Pd[11][1][0]*c11)+(Pd[11][1][2] *s11)))); N21[11][0][2] = (((sL21[0][2]*c11)+(sL21[2][2]*s11))-(.003*c11)); N21[11][1][0] = (rikt[11][1][0]+(rikt[11][1][2]*sL11[2][0])); N21[11][2][0] = ((.003*(c11+(sL11[2][0]*s11)))+((sL21[2][0]*c11)-( sL21[0][0]*s11))); N21[11][2][1] = (.016+(.64303222721266*((Pd[11][1][2]*c11)-(Pd[11][1][0] *s11)))); N21[11][2][2] = ((.003*s11)+((sL21[2][2]*c11)-(sL21[0][2]*s11))); N22[11][0][0] = (c11+(sL22[2][0]*s11)); N22[11][2][0] = ((sL22[2][0]*c11)-s11); psiD11[11][0][0] = (N11[11][0][0]*P11[11][0][0]); psiD11[11][0][2] = (sL11D1[2][2]*s11); psiD11[11][2][0] = (N11[11][2][0]*P11[11][0][0]); psiD11[11][2][2] = (sL11D1[2][2]*c11); psiD21[11][0][0] = (N21[11][0][0]*P11[11][0][0]); psiD21[11][0][2] = (N21[11][0][2]*sL11D1[2][2]); psiD21[11][1][0] = (N21[11][1][0]*P11[11][0][0]); psiD21[11][1][2] = (rikt[11][1][2]*sL11D1[2][2]); psiD21[11][2][0] = (N21[11][2][0]*P11[11][0][0]); psiD21[11][2][2] = (N21[11][2][2]*sL11D1[2][2]); psiD22[11][0][0] = (N22[11][0][0]*sL22D2[0][0]); psiD22[11][0][2] = (sL22D2[2][2]*s11); psiD22[11][2][0] = (N22[11][2][0]*sL22D2[0][0]); psiD22[11][2][2] = (sL22D2[2][2]*c11); P11[9][0][0] = (2.89586298+((N11[11][0][0]*psiD11[11][0][0])+( psiD11[11][0][2]*s11))); P11[9][0][2] = ((N11[11][2][0]*psiD11[11][0][0])+(psiD11[11][0][2]*c11)); P11[9][2][0] = P11[9][0][2]; P11[9][2][2] = (2.89586298+((N11[11][2][0]*psiD11[11][2][0])+( psiD11[11][2][2]*c11))); Pd[9][0][0] = ((N21[11][0][0]*psiD11[11][0][0])+(N21[11][0][2]* psiD11[11][0][2])); Pd[9][0][1] = (((N21[11][1][0]*psiD11[11][0][0])+(psiD11[11][0][2]* rikt[11][1][2]))-.381352054752139); Pd[9][0][2] = (.0004597761653346+((N21[11][2][0]*psiD11[11][0][0])+( N21[11][2][2]*psiD11[11][0][2]))); Pd[9][1][0] = (.381352054752139+(1.55513201*N21[11][0][1])); Pd[9][1][2] = (.027304802452122+(1.55513201*N21[11][2][1])); Pd[9][2][0] = (((N21[11][0][0]*psiD11[11][2][0])+(N21[11][0][2]* psiD11[11][2][2]))-.0004597761653346); Pd[9][2][1] = (((N21[11][1][0]*psiD11[11][2][0])+(psiD11[11][2][2]* rikt[11][1][2]))-.027304802452122); Pd[9][2][2] = ((N21[11][2][0]*psiD11[11][2][0])+(N21[11][2][2]* psiD11[11][2][2])); P22[9][0][0] = (.104813109755533+(((N21[11][0][2]*psiD21[11][0][2])+(( 1.55513201*(N21[11][0][1]*N21[11][0][1]))+(N21[11][0][0]* psiD21[11][0][0])))+((N22[11][0][0]*psiD22[11][0][0])+( psiD22[11][0][2]*s11)))); P22[9][0][1] = (((N21[11][1][0]*psiD21[11][0][0])+(psiD21[11][0][2]* rikt[11][1][2]))-5.48448165146766e-5); P22[9][0][2] = (.00583491038905244+(((N21[11][2][2]*psiD21[11][0][2])+(( 1.55513201*(N21[11][0][1]*N21[11][2][1]))+(N21[11][2][0]* psiD21[11][0][0])))+((N22[11][2][0]*psiD22[11][0][0])+( psiD22[11][0][2]*c11)))); P22[9][1][0] = P22[9][0][1]; P22[9][1][1] = (.105247251008712+((N21[11][1][0]*psiD21[11][1][0])+( psiD21[11][1][2]*rikt[11][1][2]))); P22[9][1][2] = (.001417032734267+((N21[11][2][0]*psiD21[11][1][0])+( N21[11][2][2]*psiD21[11][1][2]))); P22[9][2][0] = P22[9][0][2]; P22[9][2][1] = P22[9][1][2]; P22[9][2][2] = (.00369164725050258+(((N21[11][2][2]*psiD21[11][2][2])+(( 1.55513201*(N21[11][2][1]*N21[11][2][1]))+(N21[11][2][0]* psiD21[11][2][0])))+((N22[11][2][0]*psiD22[11][2][0])+( psiD22[11][2][2]*c11)))); if (P22[10][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[10] = (1./P22[10][1][1]); } G1[10][0] = (DD[10]*Pd[10][0][1]); G1[10][2] = (DD[10]*Pd[10][2][1]); G2[10][0] = (DD[10]*P22[10][0][1]); G2[10][2] = (DD[10]*P22[10][2][1]); P11[10][0][0] = (P11[10][0][0]-(G1[10][0]*Pd[10][0][1])); P11[10][0][2] = (P11[10][0][2]-(G1[10][2]*Pd[10][0][1])); P11[10][2][2] = (P11[10][2][2]-(G1[10][2]*Pd[10][2][1])); Pd[10][0][0] = (Pd[10][0][0]-(G2[10][0]*Pd[10][0][1])); Pd[10][0][2] = (Pd[10][0][2]-(G2[10][2]*Pd[10][0][1])); Pd[10][2][0] = (Pd[10][2][0]-(G2[10][0]*Pd[10][2][1])); Pd[10][2][2] = (Pd[10][2][2]-(G2[10][2]*Pd[10][2][1])); P22[10][0][0] = (P22[10][0][0]-(G2[10][0]*P22[10][0][1])); P22[10][0][2] = (P22[10][0][2]-(G2[10][2]*P22[10][0][1])); P22[10][1][2] = (P22[10][1][2]-P22[10][2][1]); P22[10][2][2] = (P22[10][2][2]-(G2[10][2]*P22[10][2][1])); sD1INV[0][0] = 0.; if (P11[10][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[10][0][0]); } sL11[2][0] = (P11[10][0][2]*sD1INV[0][0]); sL11D1[2][2] = (P11[10][2][2]-(P11[10][0][2]*sL11[2][0])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[2][0] = (Pd[10][2][0]-(Pd[10][0][0]*sL11[2][0])); sD1L21[2][2] = (Pd[10][2][2]-(Pd[10][0][2]*sL11[2][0])); sL21[0][0] = (Pd[10][0][0]*sD1INV[0][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[2][0] = (Pd[10][0][2]*sD1INV[0][0]); sL21[2][2] = (sD1INV[2][2]*sD1L21[2][2]); sL22D2[0][0] = (P22[10][0][0]-((sD1L21[2][0]*sL21[0][2])+(( .224662956588593*(Pd[10][1][0]*Pd[10][1][0]))+(Pd[10][0][0]*sL21[0][0] )))); sL22D2[2][0] = (P22[10][0][2]-((sD1L21[2][0]*sL21[2][2])+(( .224662956588593*(Pd[10][1][0]*Pd[10][1][2]))+(Pd[10][0][0]*sL21[2][0] )))); sL22D2[2][2] = (P22[10][2][2]-((sD1L21[2][2]*sL21[2][2])+(( .224662956588593*(Pd[10][1][2]*Pd[10][1][2]))+(Pd[10][0][2]*sL21[2][0] )))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[2][0] = (sD2INV[0][0]*sL22D2[2][0]); sL22D2[2][2] = (sL22D2[2][2]-(sL22[2][0]*sL22D2[2][0])); N11[10][0][0] = (c10+(sL11[2][0]*s10)); N11[10][2][0] = ((sL11[2][0]*c10)-s10); N21[10][0][0] = ((.04579136*((sL11[2][0]*c10)-s10))+((sL21[0][0]*c10)+( sL21[2][0]*s10))); N21[10][0][1] = (.0620714+(.224662956588593*((Pd[10][1][0]*c10)+( Pd[10][1][2]*s10)))); N21[10][0][2] = ((.04579136*c10)+((sL21[0][2]*c10)+(sL21[2][2]*s10))); N21[10][1][0] = (rikt[10][1][0]+(rikt[10][1][2]*sL11[2][0])); N21[10][2][0] = (((sL21[2][0]*c10)-(sL21[0][0]*s10))-(.04579136*(c10+( sL11[2][0]*s10)))); N21[10][2][1] = ((.224662956588593*((Pd[10][1][2]*c10)-(Pd[10][1][0]*s10 )))-.01952618); N21[10][2][2] = (((sL21[2][2]*c10)-(sL21[0][2]*s10))-(.04579136*s10)); N22[10][0][0] = (c10+(sL22[2][0]*s10)); N22[10][2][0] = ((sL22[2][0]*c10)-s10); psiD11[10][0][0] = (N11[10][0][0]*P11[10][0][0]); psiD11[10][0][2] = (sL11D1[2][2]*s10); psiD11[10][2][0] = (N11[10][2][0]*P11[10][0][0]); psiD11[10][2][2] = (sL11D1[2][2]*c10); psiD21[10][0][0] = (N21[10][0][0]*P11[10][0][0]); psiD21[10][0][2] = (N21[10][0][2]*sL11D1[2][2]); psiD21[10][1][0] = (N21[10][1][0]*P11[10][0][0]); psiD21[10][1][2] = (rikt[10][1][2]*sL11D1[2][2]); psiD21[10][2][0] = (N21[10][2][0]*P11[10][0][0]); psiD21[10][2][2] = (N21[10][2][2]*sL11D1[2][2]); psiD22[10][0][0] = (N22[10][0][0]*sL22D2[0][0]); psiD22[10][0][2] = (sL22D2[2][2]*s10); psiD22[10][2][0] = (N22[10][2][0]*sL22D2[0][0]); psiD22[10][2][2] = (sL22D2[2][2]*c10); P11[5][0][0] = (P11[5][0][0]+((N11[10][0][0]*psiD11[10][0][0])+( psiD11[10][0][2]*s10))); P11[5][0][2] = (P11[5][0][2]+((N11[10][2][0]*psiD11[10][0][0])+( psiD11[10][0][2]*c10))); P11[5][2][0] = P11[5][0][2]; P11[5][2][2] = (P11[5][2][2]+((N11[10][2][0]*psiD11[10][2][0])+( psiD11[10][2][2]*c10))); Pd[5][0][0] = (Pd[5][0][0]+((N21[10][0][0]*psiD11[10][0][0])+( N21[10][0][2]*psiD11[10][0][2]))); Pd[5][0][1] = (Pd[5][0][1]+((N21[10][1][0]*psiD11[10][0][0])+( psiD11[10][0][2]*rikt[10][1][2]))); Pd[5][0][2] = (Pd[5][0][2]+((N21[10][2][0]*psiD11[10][0][0])+( N21[10][2][2]*psiD11[10][0][2]))); Pd[5][1][0] = (Pd[5][1][0]+(4.45111208*N21[10][0][1])); Pd[5][1][2] = (Pd[5][1][2]+(4.45111208*N21[10][2][1])); Pd[5][2][0] = (Pd[5][2][0]+((N21[10][0][0]*psiD11[10][2][0])+( N21[10][0][2]*psiD11[10][2][2]))); Pd[5][2][1] = (Pd[5][2][1]+((N21[10][1][0]*psiD11[10][2][0])+( psiD11[10][2][2]*rikt[10][1][2]))); Pd[5][2][2] = (Pd[5][2][2]+((N21[10][2][0]*psiD11[10][2][0])+( N21[10][2][2]*psiD11[10][2][2]))); P22[5][0][0] = (P22[5][0][0]+(((N21[10][0][2]*psiD21[10][0][2])+(( 4.45111208*(N21[10][0][1]*N21[10][0][1]))+(N21[10][0][0]* psiD21[10][0][0])))+((N22[10][0][0]*psiD22[10][0][0])+( psiD22[10][0][2]*s10)))); P22[5][0][1] = (P22[5][0][1]+((N21[10][1][0]*psiD21[10][0][0])+( psiD21[10][0][2]*rikt[10][1][2]))); P22[5][0][2] = (P22[5][0][2]+(((N21[10][2][2]*psiD21[10][0][2])+(( 4.45111208*(N21[10][0][1]*N21[10][2][1]))+(N21[10][2][0]* psiD21[10][0][0])))+((N22[10][2][0]*psiD22[10][0][0])+( psiD22[10][0][2]*c10)))); P22[5][1][0] = P22[5][0][1]; P22[5][1][1] = (P22[5][1][1]+((N21[10][1][0]*psiD21[10][1][0])+( psiD21[10][1][2]*rikt[10][1][2]))); P22[5][1][2] = (P22[5][1][2]+((N21[10][2][0]*psiD21[10][1][0])+( N21[10][2][2]*psiD21[10][1][2]))); P22[5][2][0] = P22[5][0][2]; P22[5][2][1] = P22[5][1][2]; P22[5][2][2] = (P22[5][2][2]+(((N21[10][2][2]*psiD21[10][2][2])+(( 4.45111208*(N21[10][2][1]*N21[10][2][1]))+(N21[10][2][0]* psiD21[10][2][0])))+((N22[10][2][0]*psiD22[10][2][0])+( psiD22[10][2][2]*c10)))); if (P22[9][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[9] = (1./P22[9][1][1]); } G1[9][0] = (DD[9]*Pd[9][0][1]); G1[9][2] = (DD[9]*Pd[9][2][1]); G2[9][0] = (DD[9]*P22[9][0][1]); G2[9][2] = (DD[9]*P22[9][2][1]); P11[9][0][0] = (P11[9][0][0]-(G1[9][0]*Pd[9][0][1])); P11[9][0][2] = (P11[9][0][2]-(G1[9][2]*Pd[9][0][1])); P11[9][2][2] = (P11[9][2][2]-(G1[9][2]*Pd[9][2][1])); Pd[9][0][0] = (Pd[9][0][0]-(G2[9][0]*Pd[9][0][1])); Pd[9][0][2] = (Pd[9][0][2]-(G2[9][2]*Pd[9][0][1])); Pd[9][2][0] = (Pd[9][2][0]-(G2[9][0]*Pd[9][2][1])); Pd[9][2][2] = (Pd[9][2][2]-(G2[9][2]*Pd[9][2][1])); P22[9][0][0] = (P22[9][0][0]-(G2[9][0]*P22[9][0][1])); P22[9][0][2] = (P22[9][0][2]-(G2[9][2]*P22[9][0][1])); P22[9][1][2] = (P22[9][1][2]-P22[9][2][1]); P22[9][2][2] = (P22[9][2][2]-(G2[9][2]*P22[9][2][1])); sD1INV[0][0] = 0.; if (P11[9][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[9][0][0]); } sL11[2][0] = (P11[9][0][2]*sD1INV[0][0]); sL11D1[2][2] = (P11[9][2][2]-(P11[9][0][2]*sL11[2][0])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[2][0] = (Pd[9][2][0]-(Pd[9][0][0]*sL11[2][0])); sD1L21[2][2] = (Pd[9][2][2]-(Pd[9][0][2]*sL11[2][0])); sL21[0][0] = (Pd[9][0][0]*sD1INV[0][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[2][0] = (Pd[9][0][2]*sD1INV[0][0]); sL21[2][2] = (sD1INV[2][2]*sD1L21[2][2]); sL22D2[0][0] = (P22[9][0][0]-((sD1L21[2][0]*sL21[0][2])+(( .224668866679628*(Pd[9][1][0]*Pd[9][1][0]))+(Pd[9][0][0]*sL21[0][0]))) ); sL22D2[2][0] = (P22[9][0][2]-((sD1L21[2][0]*sL21[2][2])+(( .224668866679628*(Pd[9][1][0]*Pd[9][1][2]))+(Pd[9][0][0]*sL21[2][0]))) ); sL22D2[2][2] = (P22[9][2][2]-((sD1L21[2][2]*sL21[2][2])+(( .224668866679628*(Pd[9][1][2]*Pd[9][1][2]))+(Pd[9][0][2]*sL21[2][0]))) ); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[2][0] = (sD2INV[0][0]*sL22D2[2][0]); sL22D2[2][2] = (sL22D2[2][2]-(sL22[2][0]*sL22D2[2][0])); N11[9][0][0] = (c9+(sL11[2][0]*s9)); N11[9][2][0] = ((sL11[2][0]*c9)-s9); N21[9][0][0] = ((.04820864*(s9-(sL11[2][0]*c9)))+((sL21[0][0]*c9)+( sL21[2][0]*s9))); N21[9][0][1] = (.0620714+(.224668866679628*((Pd[9][1][0]*c9)+( Pd[9][1][2]*s9)))); N21[9][0][2] = (((sL21[0][2]*c9)+(sL21[2][2]*s9))-(.04820864*c9)); N21[9][1][0] = (rikt[9][1][0]+(rikt[9][1][2]*sL11[2][0])); N21[9][2][0] = ((.04820864*(c9+(sL11[2][0]*s9)))+((sL21[2][0]*c9)-( sL21[0][0]*s9))); N21[9][2][1] = ((.224668866679628*((Pd[9][1][2]*c9)-(Pd[9][1][0]*s9)))- .01952618); N21[9][2][2] = ((.04820864*s9)+((sL21[2][2]*c9)-(sL21[0][2]*s9))); N22[9][0][0] = (c9+(sL22[2][0]*s9)); N22[9][2][0] = ((sL22[2][0]*c9)-s9); psiD11[9][0][0] = (N11[9][0][0]*P11[9][0][0]); psiD11[9][0][2] = (sL11D1[2][2]*s9); psiD11[9][2][0] = (N11[9][2][0]*P11[9][0][0]); psiD11[9][2][2] = (sL11D1[2][2]*c9); psiD21[9][0][0] = (N21[9][0][0]*P11[9][0][0]); psiD21[9][0][2] = (N21[9][0][2]*sL11D1[2][2]); psiD21[9][1][0] = (N21[9][1][0]*P11[9][0][0]); psiD21[9][1][2] = (rikt[9][1][2]*sL11D1[2][2]); psiD21[9][2][0] = (N21[9][2][0]*P11[9][0][0]); psiD21[9][2][2] = (N21[9][2][2]*sL11D1[2][2]); psiD22[9][0][0] = (N22[9][0][0]*sL22D2[0][0]); psiD22[9][0][2] = (sL22D2[2][2]*s9); psiD22[9][2][0] = (N22[9][2][0]*sL22D2[0][0]); psiD22[9][2][2] = (sL22D2[2][2]*c9); P11[5][0][0] = (P11[5][0][0]+((N11[9][0][0]*psiD11[9][0][0])+( psiD11[9][0][2]*s9))); P11[5][0][2] = (P11[5][0][2]+((N11[9][2][0]*psiD11[9][0][0])+( psiD11[9][0][2]*c9))); P11[5][2][0] = P11[5][0][2]; P11[5][2][2] = (P11[5][2][2]+((N11[9][2][0]*psiD11[9][2][0])+( psiD11[9][2][2]*c9))); Pd[5][0][0] = (Pd[5][0][0]+((N21[9][0][0]*psiD11[9][0][0])+(N21[9][0][2] *psiD11[9][0][2]))); Pd[5][0][1] = (Pd[5][0][1]+((N21[9][1][0]*psiD11[9][0][0])+( psiD11[9][0][2]*rikt[9][1][2]))); Pd[5][0][2] = (Pd[5][0][2]+((N21[9][2][0]*psiD11[9][0][0])+(N21[9][2][2] *psiD11[9][0][2]))); Pd[5][1][0] = (Pd[5][1][0]+(4.45099499*N21[9][0][1])); Pd[5][1][2] = (Pd[5][1][2]+(4.45099499*N21[9][2][1])); Pd[5][2][0] = (Pd[5][2][0]+((N21[9][0][0]*psiD11[9][2][0])+(N21[9][0][2] *psiD11[9][2][2]))); Pd[5][2][1] = (Pd[5][2][1]+((N21[9][1][0]*psiD11[9][2][0])+( psiD11[9][2][2]*rikt[9][1][2]))); Pd[5][2][2] = (Pd[5][2][2]+((N21[9][2][0]*psiD11[9][2][0])+(N21[9][2][2] *psiD11[9][2][2]))); P22[5][0][0] = (P22[5][0][0]+(((N21[9][0][2]*psiD21[9][0][2])+(( 4.45099499*(N21[9][0][1]*N21[9][0][1]))+(N21[9][0][0]*psiD21[9][0][0]) ))+((N22[9][0][0]*psiD22[9][0][0])+(psiD22[9][0][2]*s9)))); P22[5][0][1] = (P22[5][0][1]+((N21[9][1][0]*psiD21[9][0][0])+( psiD21[9][0][2]*rikt[9][1][2]))); P22[5][0][2] = (P22[5][0][2]+(((N21[9][2][2]*psiD21[9][0][2])+(( 4.45099499*(N21[9][0][1]*N21[9][2][1]))+(N21[9][2][0]*psiD21[9][0][0]) ))+((N22[9][2][0]*psiD22[9][0][0])+(psiD22[9][0][2]*c9)))); P22[5][1][0] = P22[5][0][1]; P22[5][1][1] = (P22[5][1][1]+((N21[9][1][0]*psiD21[9][1][0])+( psiD21[9][1][2]*rikt[9][1][2]))); P22[5][1][2] = (P22[5][1][2]+((N21[9][2][0]*psiD21[9][1][0])+( N21[9][2][2]*psiD21[9][1][2]))); P22[5][2][0] = P22[5][0][2]; P22[5][2][1] = P22[5][1][2]; P22[5][2][2] = (P22[5][2][2]+(((N21[9][2][2]*psiD21[9][2][2])+(( 4.45099499*(N21[9][2][1]*N21[9][2][1]))+(N21[9][2][0]*psiD21[9][2][0]) ))+((N22[9][2][0]*psiD22[9][2][0])+(psiD22[9][2][2]*c9)))); DD[8] = 1.49386933528469; N11[8][0][0] = (c8-(.0378863388980916*s8)); N11[8][2][0] = -(s8+(.0378863388980916*c8)); N21[8][0][0] = (((4.57909446457894e-5*c8)+(.00120864*s8))-(( 2.3444708339713e-5*c8)+(.000800222791098706*s8))); N21[8][0][1] = (((.01691003*s8)+(.12000071*c8))-.3829286); N21[8][0][2] = (((.000718833970364026*c8)-(1.16062019497711e-5*s8))-( .00120864*c8)); N21[8][1][0] = (rikt[8][1][0]-(.0378863388980916*rikt[8][1][2])); N21[8][2][0] = (((2.3444708339713e-5*s8)-(.000800222791098706*c8))+(( .00120864*c8)-(4.57909446457894e-5*s8))); N21[8][2][1] = (((.01691003*c8)-(.12000071*s8))-.00252618); N21[8][2][2] = ((.00120864*s8)-((1.16062019497711e-5*c8)+( .000718833970364026*s8))); N22[8][0][0] = (c8+(.0157615419311457*s8)); N22[8][2][0] = ((.0157615419311457*c8)-s8); P11[5][0][0] = (P11[5][0][0]+((7.76276009568321*(N11[8][0][0]* N11[8][0][0]))+(9.7972497400523*(s8*s8)))); P11[5][0][2] = (P11[5][0][2]+((7.76276009568321*(N11[8][0][0]* N11[8][2][0]))+(9.7972497400523*(s8*c8)))); P11[5][2][0] = P11[5][0][2]; P11[5][2][2] = (P11[5][2][2]+((7.76276009568321*(N11[8][2][0]* N11[8][2][0]))+(9.7972497400523*(c8*c8)))); Pd[5][0][0] = (Pd[5][0][0]+((7.76276009568321*(N11[8][0][0]*N21[8][0][0] ))+(9.7972497400523*(N21[8][0][2]*s8)))); Pd[5][0][1] = (Pd[5][0][1]+((7.76276009568321*(N11[8][0][0]*N21[8][1][0] ))+(9.7972497400523*(rikt[8][1][2]*s8)))); Pd[5][0][2] = (Pd[5][0][2]+((7.76276009568321*(N11[8][0][0]*N21[8][2][0] ))+(9.7972497400523*(N21[8][2][2]*s8)))); Pd[5][1][0] = (Pd[5][1][0]+(9.84983599*N21[8][0][1])); Pd[5][1][2] = (Pd[5][1][2]+(9.84983599*N21[8][2][1])); Pd[5][2][0] = (Pd[5][2][0]+((7.76276009568321*(N11[8][2][0]*N21[8][0][0] ))+(9.7972497400523*(N21[8][0][2]*c8)))); Pd[5][2][1] = (Pd[5][2][1]+((7.76276009568321*(N11[8][2][0]*N21[8][1][0] ))+(9.7972497400523*(rikt[8][1][2]*c8)))); Pd[5][2][2] = (Pd[5][2][2]+((7.76276009568321*(N11[8][2][0]*N21[8][2][0] ))+(9.7972497400523*(N21[8][2][2]*c8)))); P22[5][0][0] = (P22[5][0][0]+(((.0178601503205227*(s8*s8))+( .522579669476767*(N22[8][0][0]*N22[8][0][0])))+((9.7972497400523*( N21[8][0][2]*N21[8][0][2]))+((7.76276009568321*(N21[8][0][0]* N21[8][0][0]))+(9.84983599*(N21[8][0][1]*N21[8][0][1])))))); P22[5][0][1] = (P22[5][0][1]+((7.76276009568321*(N21[8][0][0]* N21[8][1][0]))+(9.7972497400523*(N21[8][0][2]*rikt[8][1][2])))); P22[5][0][2] = (P22[5][0][2]+(((.0178601503205227*(s8*c8))+( .522579669476767*(N22[8][0][0]*N22[8][2][0])))+((9.7972497400523*( N21[8][0][2]*N21[8][2][2]))+((7.76276009568321*(N21[8][0][0]* N21[8][2][0]))+(9.84983599*(N21[8][0][1]*N21[8][2][1])))))); P22[5][1][0] = P22[5][0][1]; P22[5][1][1] = (P22[5][1][1]+((7.76276009568321*(N21[8][1][0]* N21[8][1][0]))+(9.7972497400523*(rikt[8][1][2]*rikt[8][1][2])))); P22[5][1][2] = (P22[5][1][2]+((7.76276009568321*(N21[8][1][0]* N21[8][2][0]))+(9.7972497400523*(N21[8][2][2]*rikt[8][1][2])))); P22[5][2][0] = P22[5][0][2]; P22[5][2][1] = P22[5][1][2]; P22[5][2][2] = (P22[5][2][2]+(((.0178601503205227*(c8*c8))+( .522579669476767*(N22[8][2][0]*N22[8][2][0])))+((9.7972497400523*( N21[8][2][2]*N21[8][2][2]))+((7.76276009568321*(N21[8][2][0]* N21[8][2][0]))+(9.84983599*(N21[8][2][1]*N21[8][2][1])))))); if (P22[7][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[7] = (1./P22[7][1][1]); } if (P22[6][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[6] = (1./P22[6][1][1]); } if (P22[5][2][2] < 1e-13) { sdseterr(routine,47); } else { DD[5] = (1./P22[5][2][2]); } G1[5][0] = (DD[5]*Pd[5][0][2]); G1[5][1] = (DD[5]*Pd[5][1][2]); G1[5][2] = (DD[5]*Pd[5][2][2]); G2[5][0] = (DD[5]*P22[5][0][2]); G2[5][1] = (DD[5]*P22[5][1][2]); P11[5][0][0] = (P11[5][0][0]-(G1[5][0]*Pd[5][0][2])); P11[5][0][1] = -(G1[5][1]*Pd[5][0][2]); P11[5][0][2] = (P11[5][0][2]-(G1[5][2]*Pd[5][0][2])); P11[5][1][1] = (32.79462228-(G1[5][1]*Pd[5][1][2])); P11[5][1][2] = -(G1[5][2]*Pd[5][1][2]); P11[5][2][2] = (P11[5][2][2]-(G1[5][2]*Pd[5][2][2])); Pd[5][0][0] = (Pd[5][0][0]-(G2[5][0]*Pd[5][0][2])); Pd[5][0][1] = (Pd[5][0][1]-(G2[5][1]*Pd[5][0][2])); Pd[5][1][0] = (Pd[5][1][0]-(G2[5][0]*Pd[5][1][2])); Pd[5][1][1] = -(G2[5][1]*Pd[5][1][2]); Pd[5][2][0] = (Pd[5][2][0]-(G2[5][0]*Pd[5][2][2])); Pd[5][2][1] = (Pd[5][2][1]-(G2[5][1]*Pd[5][2][2])); P22[5][0][0] = (P22[5][0][0]-(G2[5][0]*P22[5][0][2])); P22[5][0][1] = (P22[5][0][1]-(G2[5][1]*P22[5][0][2])); P22[5][1][1] = (P22[5][1][1]-(G2[5][1]*P22[5][1][2])); sD1INV[0][0] = 0.; if (P11[5][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[5][0][0]); } sL11[1][0] = (P11[5][0][1]*sD1INV[0][0]); sL11[2][0] = (P11[5][0][2]*sD1INV[0][0]); sL11D1[1][1] = (P11[5][1][1]-(P11[5][0][1]*sL11[1][0])); sL11D1[2][1] = (P11[5][1][2]-(P11[5][0][2]*sL11[1][0])); sL11D1[2][2] = (P11[5][2][2]-(P11[5][0][2]*sL11[2][0])); sD1INV[1][1] = 0.; if (sL11D1[1][1] >= 1e-13) { sD1INV[1][1] = (1./sL11D1[1][1]); } sL11[2][1] = (sD1INV[1][1]*sL11D1[2][1]); sL11D1[2][2] = (sL11D1[2][2]-(sL11[2][1]*sL11D1[2][1])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[1][0] = (Pd[5][1][0]-(Pd[5][0][0]*sL11[1][0])); sD1L21[1][1] = (Pd[5][1][1]-(Pd[5][0][1]*sL11[1][0])); sD1L21[2][0] = (Pd[5][2][0]-((Pd[5][0][0]*sL11[2][0])+(sD1L21[1][0]* sL11[2][1]))); sD1L21[2][1] = (Pd[5][2][1]-((Pd[5][0][1]*sL11[2][0])+(sD1L21[1][1]* sL11[2][1]))); sL21[0][0] = (Pd[5][0][0]*sD1INV[0][0]); sL21[0][1] = (sD1INV[1][1]*sD1L21[1][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL21[1][0] = (Pd[5][0][1]*sD1INV[0][0]); sL21[1][1] = (sD1INV[1][1]*sD1L21[1][1]); sL21[1][2] = (sD1INV[2][2]*sD1L21[2][1]); sL22D2[0][0] = (P22[5][0][0]-((sD1L21[2][0]*sL21[0][2])+((Pd[5][0][0]* sL21[0][0])+(sD1L21[1][0]*sL21[0][1])))); sL22D2[1][0] = (P22[5][0][1]-((sD1L21[2][0]*sL21[1][2])+((Pd[5][0][0]* sL21[1][0])+(sD1L21[1][0]*sL21[1][1])))); sL22D2[1][1] = (P22[5][1][1]-((sD1L21[2][1]*sL21[1][2])+((Pd[5][0][1]* sL21[1][0])+(sD1L21[1][1]*sL21[1][1])))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[1][0] = (sD2INV[0][0]*sL22D2[1][0]); sL22D2[1][1] = (sL22D2[1][1]-(sL22[1][0]*sL22D2[1][0])); sD2INV[1][1] = 0.; if (sL22D2[1][1] >= 1e-13) { sD2INV[1][1] = (1./sL22D2[1][1]); } psiD11[5][1][0] = (P11[5][0][0]*sL11[1][0]); psiD11[5][2][0] = (P11[5][0][0]*sL11[2][0]); psiD11[5][2][1] = (sL11[2][1]*sL11D1[1][1]); psiD21[5][0][0] = (P11[5][0][0]*sL21[0][0]); psiD21[5][0][1] = (sL11D1[1][1]*sL21[0][1]); psiD21[5][0][2] = (sL11D1[2][2]*sL21[0][2]); psiD21[5][1][0] = (P11[5][0][0]*sL21[1][0]); psiD21[5][1][1] = (sL11D1[1][1]*sL21[1][1]); psiD21[5][1][2] = (sL11D1[2][2]*sL21[1][2]); psiD22[5][1][0] = (sL22[1][0]*sL22D2[0][0]); P11[4][0][0] = P11[5][0][0]; P11[4][0][1] = (P11[5][0][0]*sL11[1][0]); P11[4][0][2] = (P11[5][0][0]*sL11[2][0]); P11[4][1][0] = P11[4][0][1]; P11[4][1][1] = (sL11D1[1][1]+(psiD11[5][1][0]*sL11[1][0])); P11[4][1][2] = ((psiD11[5][1][0]*sL11[2][0])+(sL11[2][1]*sL11D1[1][1])); P11[4][2][0] = P11[4][0][2]; P11[4][2][1] = P11[4][1][2]; P11[4][2][2] = (sL11D1[2][2]+((psiD11[5][2][0]*sL11[2][0])+( psiD11[5][2][1]*sL11[2][1]))); Pd[4][0][0] = (P11[5][0][0]*sL21[0][0]); Pd[4][0][1] = (P11[5][0][0]*sL21[1][0]); Pd[4][1][0] = ((psiD11[5][1][0]*sL21[0][0])+(sL11D1[1][1]*sL21[0][1])); Pd[4][1][1] = ((psiD11[5][1][0]*sL21[1][0])+(sL11D1[1][1]*sL21[1][1])); Pd[4][2][0] = ((sL11D1[2][2]*sL21[0][2])+((psiD11[5][2][0]*sL21[0][0])+( psiD11[5][2][1]*sL21[0][1]))); Pd[4][2][1] = ((sL11D1[2][2]*sL21[1][2])+((psiD11[5][2][0]*sL21[1][0])+( psiD11[5][2][1]*sL21[1][1]))); P22[4][0][0] = (sL22D2[0][0]+((psiD21[5][0][2]*sL21[0][2])+(( psiD21[5][0][0]*sL21[0][0])+(psiD21[5][0][1]*sL21[0][1])))); P22[4][0][1] = ((sL22[1][0]*sL22D2[0][0])+((psiD21[5][0][2]*sL21[1][2])+ ((psiD21[5][0][0]*sL21[1][0])+(psiD21[5][0][1]*sL21[1][1])))); P22[4][1][0] = P22[4][0][1]; P22[4][1][1] = ((sL22D2[1][1]+(psiD22[5][1][0]*sL22[1][0]))+(( psiD21[5][1][2]*sL21[1][2])+((psiD21[5][1][0]*sL21[1][0])+( psiD21[5][1][1]*sL21[1][1])))); if (P22[4][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[4] = (1./P22[4][1][1]); } G1[4][0] = (DD[4]*Pd[4][0][1]); G1[4][1] = (DD[4]*Pd[4][1][1]); G1[4][2] = (DD[4]*Pd[4][2][1]); G2[4][0] = (DD[4]*P22[4][0][1]); P11[4][0][0] = (P11[4][0][0]-(G1[4][0]*Pd[4][0][1])); P11[4][0][1] = (P11[4][0][1]-(G1[4][1]*Pd[4][0][1])); P11[4][0][2] = (P11[4][0][2]-(G1[4][2]*Pd[4][0][1])); P11[4][1][1] = (P11[4][1][1]-(G1[4][1]*Pd[4][1][1])); P11[4][1][2] = (P11[4][1][2]-(G1[4][2]*Pd[4][1][1])); P11[4][2][2] = (P11[4][2][2]-(G1[4][2]*Pd[4][2][1])); Pd[4][0][0] = (Pd[4][0][0]-(G2[4][0]*Pd[4][0][1])); Pd[4][1][0] = (Pd[4][1][0]-(G2[4][0]*Pd[4][1][1])); Pd[4][2][0] = (Pd[4][2][0]-(G2[4][0]*Pd[4][2][1])); P22[4][0][0] = (P22[4][0][0]-(G2[4][0]*P22[4][0][1])); sD1INV[0][0] = 0.; if (P11[4][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[4][0][0]); } sL11[1][0] = (P11[4][0][1]*sD1INV[0][0]); sL11[2][0] = (P11[4][0][2]*sD1INV[0][0]); sL11D1[1][1] = (P11[4][1][1]-(P11[4][0][1]*sL11[1][0])); sL11D1[2][1] = (P11[4][1][2]-(P11[4][0][2]*sL11[1][0])); sL11D1[2][2] = (P11[4][2][2]-(P11[4][0][2]*sL11[2][0])); sD1INV[1][1] = 0.; if (sL11D1[1][1] >= 1e-13) { sD1INV[1][1] = (1./sL11D1[1][1]); } sL11[2][1] = (sD1INV[1][1]*sL11D1[2][1]); sL11D1[2][2] = (sL11D1[2][2]-(sL11[2][1]*sL11D1[2][1])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } sD1L21[1][0] = (Pd[4][1][0]-(Pd[4][0][0]*sL11[1][0])); sD1L21[2][0] = (Pd[4][2][0]-((Pd[4][0][0]*sL11[2][0])+(sD1L21[1][0]* sL11[2][1]))); sL21[0][0] = (Pd[4][0][0]*sD1INV[0][0]); sL21[0][1] = (sD1INV[1][1]*sD1L21[1][0]); sL21[0][2] = (sD1INV[2][2]*sD1L21[2][0]); sL22D2[0][0] = (P22[4][0][0]-((sD1L21[2][0]*sL21[0][2])+((Pd[4][0][0]* sL21[0][0])+(sD1L21[1][0]*sL21[0][1])))); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } psiD11[4][1][0] = (P11[4][0][0]*sL11[1][0]); psiD11[4][2][0] = (P11[4][0][0]*sL11[2][0]); psiD11[4][2][1] = (sL11[2][1]*sL11D1[1][1]); psiD21[4][0][0] = (P11[4][0][0]*sL21[0][0]); psiD21[4][0][1] = (sL11D1[1][1]*sL21[0][1]); psiD21[4][0][2] = (sL11D1[2][2]*sL21[0][2]); P11[3][0][0] = P11[4][0][0]; P11[3][0][1] = (P11[4][0][0]*sL11[1][0]); P11[3][0][2] = (P11[4][0][0]*sL11[2][0]); P11[3][1][0] = P11[3][0][1]; P11[3][1][1] = (sL11D1[1][1]+(psiD11[4][1][0]*sL11[1][0])); P11[3][1][2] = ((psiD11[4][1][0]*sL11[2][0])+(sL11[2][1]*sL11D1[1][1])); P11[3][2][0] = P11[3][0][2]; P11[3][2][1] = P11[3][1][2]; P11[3][2][2] = (sL11D1[2][2]+((psiD11[4][2][0]*sL11[2][0])+( psiD11[4][2][1]*sL11[2][1]))); Pd[3][0][0] = (P11[4][0][0]*sL21[0][0]); Pd[3][1][0] = ((psiD11[4][1][0]*sL21[0][0])+(sL11D1[1][1]*sL21[0][1])); Pd[3][2][0] = ((sL11D1[2][2]*sL21[0][2])+((psiD11[4][2][0]*sL21[0][0])+( psiD11[4][2][1]*sL21[0][1]))); P22[3][0][0] = (sL22D2[0][0]+((psiD21[4][0][2]*sL21[0][2])+(( psiD21[4][0][0]*sL21[0][0])+(psiD21[4][0][1]*sL21[0][1])))); if (P22[3][0][0] < 1e-13) { sdseterr(routine,47); } else { DD[3] = (1./P22[3][0][0]); } G1[3][0] = (DD[3]*Pd[3][0][0]); G1[3][1] = (DD[3]*Pd[3][1][0]); G1[3][2] = (DD[3]*Pd[3][2][0]); P11[3][0][0] = (P11[3][0][0]-(G1[3][0]*Pd[3][0][0])); P11[3][0][1] = (P11[3][0][1]-(G1[3][1]*Pd[3][0][0])); P11[3][0][2] = (P11[3][0][2]-(G1[3][2]*Pd[3][0][0])); P11[3][1][1] = (P11[3][1][1]-(G1[3][1]*Pd[3][1][0])); P11[3][1][2] = (P11[3][1][2]-(G1[3][2]*Pd[3][1][0])); P11[3][2][2] = (P11[3][2][2]-(G1[3][2]*Pd[3][2][0])); sD1INV[0][0] = 0.; if (P11[3][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[3][0][0]); } sL11[1][0] = (P11[3][0][1]*sD1INV[0][0]); sL11[2][0] = (P11[3][0][2]*sD1INV[0][0]); sL11D1[1][1] = (P11[3][1][1]-(P11[3][0][1]*sL11[1][0])); sL11D1[2][1] = (P11[3][1][2]-(P11[3][0][2]*sL11[1][0])); sL11D1[2][2] = (P11[3][2][2]-(P11[3][0][2]*sL11[2][0])); sD1INV[1][1] = 0.; if (sL11D1[1][1] >= 1e-13) { sD1INV[1][1] = (1./sL11D1[1][1]); } sL11[2][1] = (sD1INV[1][1]*sL11D1[2][1]); sL11D1[2][2] = (sL11D1[2][2]-(sL11[2][1]*sL11D1[2][1])); sD1INV[2][2] = 0.; if (sL11D1[2][2] >= 1e-13) { sD1INV[2][2] = (1./sL11D1[2][2]); } N11[3][0][0] = ((Cik[3][0][0]+(Cik[3][0][1]*sL11[1][0]))+(Cik[3][0][2]* sL11[2][0])); N11[3][0][1] = (Cik[3][0][1]+(Cik[3][0][2]*sL11[2][1])); N11[3][1][0] = ((Cik[3][1][0]+(Cik[3][1][1]*sL11[1][0]))+(Cik[3][1][2]* sL11[2][0])); N11[3][1][1] = (Cik[3][1][1]+(Cik[3][1][2]*sL11[2][1])); N11[3][2][0] = ((Cik[3][2][0]+(Cik[3][2][1]*sL11[1][0]))+(Cik[3][2][2]* sL11[2][0])); N11[3][2][1] = (Cik[3][2][1]+(Cik[3][2][2]*sL11[2][1])); psiD11[3][0][0] = (N11[3][0][0]*P11[3][0][0]); psiD11[3][0][1] = (N11[3][0][1]*sL11D1[1][1]); psiD11[3][0][2] = (Cik[3][0][2]*sL11D1[2][2]); psiD11[3][1][0] = (N11[3][1][0]*P11[3][0][0]); psiD11[3][1][1] = (N11[3][1][1]*sL11D1[1][1]); psiD11[3][1][2] = (Cik[3][1][2]*sL11D1[2][2]); psiD11[3][2][0] = (N11[3][2][0]*P11[3][0][0]); psiD11[3][2][1] = (N11[3][2][1]*sL11D1[1][1]); psiD11[3][2][2] = (Cik[3][2][2]*sL11D1[2][2]); P11[2][0][0] = ((Cik[3][0][2]*psiD11[3][0][2])+((N11[3][0][0]* psiD11[3][0][0])+(N11[3][0][1]*psiD11[3][0][1]))); P11[2][0][1] = ((Cik[3][1][2]*psiD11[3][0][2])+((N11[3][1][0]* psiD11[3][0][0])+(N11[3][1][1]*psiD11[3][0][1]))); P11[2][0][2] = ((Cik[3][2][2]*psiD11[3][0][2])+((N11[3][2][0]* psiD11[3][0][0])+(N11[3][2][1]*psiD11[3][0][1]))); P11[2][1][0] = P11[2][0][1]; P11[2][1][1] = ((Cik[3][1][2]*psiD11[3][1][2])+((N11[3][1][0]* psiD11[3][1][0])+(N11[3][1][1]*psiD11[3][1][1]))); P11[2][1][2] = ((Cik[3][2][2]*psiD11[3][1][2])+((N11[3][2][0]* psiD11[3][1][0])+(N11[3][2][1]*psiD11[3][1][1]))); P11[2][2][0] = P11[2][0][2]; P11[2][2][1] = P11[2][1][2]; P11[2][2][2] = ((Cik[3][2][2]*psiD11[3][2][2])+((N11[3][2][0]* psiD11[3][2][0])+(N11[3][2][1]*psiD11[3][2][1]))); if (P11[2][2][2] < 1e-13) { sdseterr(routine,47); } else { DD[2] = (1./P11[2][2][2]); } G1[2][0] = (DD[2]*P11[2][0][2]); G1[2][1] = (DD[2]*P11[2][1][2]); P11[2][0][0] = (P11[2][0][0]-(G1[2][0]*P11[2][0][2])); P11[2][0][1] = (P11[2][0][1]-(G1[2][1]*P11[2][0][2])); P11[2][1][1] = (P11[2][1][1]-(G1[2][1]*P11[2][1][2])); sD1INV[0][0] = 0.; if (P11[2][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[2][0][0]); } sL11[1][0] = (P11[2][0][1]*sD1INV[0][0]); sL11D1[1][1] = (P11[2][1][1]-(P11[2][0][1]*sL11[1][0])); sD1INV[1][1] = 0.; if (sL11D1[1][1] >= 1e-13) { sD1INV[1][1] = (1./sL11D1[1][1]); } N21[2][0][0] = -(q[2]*sL11[1][0]); psiD11[2][1][0] = (P11[2][0][0]*sL11[1][0]); psiD21[2][0][0] = (N21[2][0][0]*P11[2][0][0]); psiD21[2][0][1] = -(q[2]*sL11D1[1][1]); psiD21[2][1][0] = (P11[2][0][0]*q[2]); P11[1][0][0] = P11[2][0][0]; P11[1][0][1] = (P11[2][0][0]*sL11[1][0]); P11[1][1][0] = P11[1][0][1]; P11[1][1][1] = (sL11D1[1][1]+(psiD11[2][1][0]*sL11[1][0])); Pd[1][0][0] = (N21[2][0][0]*P11[2][0][0]); Pd[1][0][1] = (P11[2][0][0]*q[2]); Pd[1][1][0] = ((N21[2][0][0]*psiD11[2][1][0])-(q[2]*sL11D1[1][1])); Pd[1][1][1] = (psiD11[2][1][0]*q[2]); P22[1][0][0] = ((N21[2][0][0]*psiD21[2][0][0])-(psiD21[2][0][1]*q[2])); P22[1][0][1] = (psiD21[2][0][0]*q[2]); P22[1][1][0] = P22[1][0][1]; P22[1][1][1] = (psiD21[2][1][0]*q[2]); if (P11[1][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[1] = (1./P11[1][1][1]); } G1[1][0] = (DD[1]*P11[1][0][1]); G2[1][0] = (DD[1]*Pd[1][1][0]); G2[1][1] = (DD[1]*Pd[1][1][1]); P11[1][0][0] = (P11[1][0][0]-(G1[1][0]*P11[1][0][1])); Pd[1][0][0] = (Pd[1][0][0]-(G2[1][0]*P11[1][0][1])); Pd[1][0][1] = (Pd[1][0][1]-(G2[1][1]*P11[1][0][1])); P22[1][0][0] = (P22[1][0][0]-(G2[1][0]*Pd[1][1][0])); P22[1][0][1] = (P22[1][0][1]-(G2[1][1]*Pd[1][1][0])); P22[1][1][1] = (P22[1][1][1]-(G2[1][1]*Pd[1][1][1])); sD1INV[0][0] = 0.; if (P11[1][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[1][0][0]); } sL21[0][0] = (Pd[1][0][0]*sD1INV[0][0]); sL21[1][0] = (Pd[1][0][1]*sD1INV[0][0]); sL22D2[0][0] = (P22[1][0][0]-(Pd[1][0][0]*sL21[0][0])); sL22D2[1][0] = (P22[1][0][1]-(Pd[1][0][0]*sL21[1][0])); sL22D2[1][1] = (P22[1][1][1]-(Pd[1][0][1]*sL21[1][0])); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[1][0] = (sD2INV[0][0]*sL22D2[1][0]); sL22D2[1][1] = (sL22D2[1][1]-(sL22[1][0]*sL22D2[1][0])); sD2INV[1][1] = 0.; if (sL22D2[1][1] >= 1e-13) { sD2INV[1][1] = (1./sL22D2[1][1]); } psiD21[1][0][0] = (P11[1][0][0]*sL21[0][0]); psiD21[1][1][0] = (P11[1][0][0]*sL21[1][0]); psiD21[1][2][0] = -(P11[1][0][0]*q[1]); psiD22[1][1][0] = (sL22[1][0]*sL22D2[0][0]); P11[0][0][0] = P11[1][0][0]; Pd[0][0][0] = (P11[1][0][0]*sL21[0][0]); Pd[0][0][1] = (P11[1][0][0]*sL21[1][0]); Pd[0][0][2] = -(P11[1][0][0]*q[1]); P22[0][0][0] = (sL22D2[0][0]+(psiD21[1][0][0]*sL21[0][0])); P22[0][0][1] = ((psiD21[1][0][0]*sL21[1][0])+(sL22[1][0]*sL22D2[0][0])); P22[0][0][2] = -(psiD21[1][0][0]*q[1]); P22[0][1][0] = P22[0][0][1]; P22[0][1][1] = ((psiD21[1][1][0]*sL21[1][0])+(sL22D2[1][1]+( psiD22[1][1][0]*sL22[1][0]))); P22[0][1][2] = -(psiD21[1][1][0]*q[1]); P22[0][2][0] = P22[0][0][2]; P22[0][2][1] = P22[0][1][2]; P22[0][2][2] = -(psiD21[1][2][0]*q[1]); if (P11[0][0][0] < 1e-13) { sdseterr(routine,47); } else { DD[0] = (1./P11[0][0][0]); } mmflg = 1; } /* Used 3.14 seconds CPU time, 106496 additional bytes of memory. Equations contain 1196 adds/subtracts/negates 1923 multiplies 57 divides 1135 assignments */ } void BOCfly :: sdlhs ( int routine ) { /* Compute all remaining state- and force-dependent quantities */ roustate = 2; sddomm(routine); sddofs0(); } void BOCfly :: sdmassmat ( double mmat[23][23] ) { /* Calculate the system mass matrix */ int i,j; double udotin[23],mmrow[23],biastrq[23]; if ((roustate != 2) && (roustate != 3)) { sdseterr(57,23); return; } for (i = 0; i < 23; i++) { udotin[i] = 0.; } sdcomptrq(udotin,biastrq); for (i = 0; i < 23; i++) { udotin[i] = 1.; sdcomptrq(udotin,mmrow); udotin[i] = 0.; for (j = i; j <= 22; j++) { mmat[i][j] = mmrow[j]-biastrq[j]; mmat[j][i] = mmat[i][j]; } } /* Check for singular mass matrix */ for (i = 0; i < 23; i++) { if (mmat[i][i] <= 1e-13) { sdseterr(57,47); } } } void BOCfly :: sdfrcmat ( double fmat[23] ) { /* Return the system force matrix (RHS), excluding constraints */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(58,23); return; } sddofs0(); for (i = 0; i < 23; i++) { fmat[i] = fs0[i]; } } void BOCfly :: sdmfrc ( double imult[1] ) { } void BOCfly :: sdequivht ( double tau[23] ) { /* Compute tree hinge torques to match effect of applied loads */ double fstareq[23][3],tstareq[23][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(56,23); return; } /* Compute fstareq (forces) */ fstareq[5][0] = ((108.164246577873*Cik[3][2][0])-ufk[0][0]); fstareq[5][1] = ((108.164246577873*Cik[3][2][1])-ufk[0][1]); fstareq[5][2] = ((108.164246577873*Cik[3][2][2])-ufk[0][2]); fstareq[6][0] = -(ufk[1][0]+(1.781552923136*s6)); fstareq[6][2] = ((1.781552923136*c6)-ufk[1][2]); fstareq[7][0] = -(ufk[2][0]+(1.78157214417*s7)); fstareq[7][2] = ((1.78157214417*c7)-ufk[2][2]); fstareq[8][0] = -(ufk[3][0]+(9.84983599*gk[8][0])); fstareq[8][1] = ((96.5938941113335*Cik[3][2][1])-ufk[3][1]); fstareq[8][2] = -(ufk[3][2]+(9.84983599*gk[8][2])); fstareq[9][0] = -(ufk[4][0]+(2.89586298*gk[9][0])); fstareq[9][1] = ((28.398714692817*Cik[3][2][1])-ufk[4][1]); fstareq[9][2] = -(ufk[4][2]+(2.89586298*gk[9][2])); fstareq[10][0] = -(ufk[5][0]+(2.8984419*gk[10][0])); fstareq[10][1] = ((28.424005258635*Cik[3][2][1])-ufk[5][1]); fstareq[10][2] = -(ufk[5][2]+(2.8984419*gk[10][2])); fstareq[11][0] = -(ufk[6][0]+(1.02133193*gk[11][0])); fstareq[11][1] = ((10.0158447713345*Cik[3][2][1])-ufk[6][1]); fstareq[11][2] = -(ufk[6][2]+(1.02133193*gk[11][2])); fstareq[12][0] = -(ufk[7][0]+(1.02007998*gk[12][0])); fstareq[12][1] = ((10.003567335867*Cik[3][2][1])-ufk[7][1]); fstareq[12][2] = -(ufk[7][2]+(1.02007998*gk[12][2])); fstareq[13][0] = -(ufk[8][0]+(.53380008*gk[13][0])); fstareq[13][1] = ((5.234790554532*Cik[3][2][1])-ufk[8][1]); fstareq[13][2] = -(ufk[8][2]+(.53380008*gk[13][2])); fstareq[14][0] = -(ufk[9][0]+(.5325902*gk[14][0])); fstareq[14][1] = ((5.22292568483*Cik[3][2][1])-ufk[9][1]); fstareq[14][2] = -(ufk[9][2]+(.5325902*gk[14][2])); fstareq[15][0] = -(ufk[10][0]+(1.00985702*gk[15][0])); fstareq[15][1] = ((9.903314345183*Cik[3][2][1])-ufk[10][1]); fstareq[15][2] = -(ufk[10][2]+(1.00985702*gk[15][2])); fstareq[16][0] = -(ufk[11][0]+(1.01701701*gk[16][0])); fstareq[16][1] = ((9.9735298611165*Cik[3][2][1])-ufk[11][1]); fstareq[16][2] = -(ufk[11][2]+(1.01701701*gk[16][2])); fstareq[17][0] = -(ufk[12][0]+(.43362906*gk[17][0])); fstareq[17][1] = ((4.252448421249*Cik[3][2][1])-ufk[12][1]); fstareq[17][2] = -(ufk[12][2]+(.43362906*gk[17][2])); fstareq[18][0] = -(ufk[13][0]+(.43358398*gk[18][0])); fstareq[18][1] = ((4.252006337467*Cik[3][2][1])-ufk[13][1]); fstareq[18][2] = -(ufk[13][2]+(.43358398*gk[18][2])); fstareq[19][0] = -(ufk[14][0]+(.05946345*gk[19][0])); fstareq[19][1] = ((.5831372419425*Cik[3][2][1])-ufk[14][1]); fstareq[19][2] = -(ufk[14][2]+(.05946345*gk[19][2])); fstareq[20][0] = -(ufk[15][0]+(.05944511*gk[20][0])); fstareq[20][1] = ((.5829573879815*Cik[3][2][1])-ufk[15][1]); fstareq[20][2] = -(ufk[15][2]+(.05944511*gk[20][2])); fstareq[21][0] = -(ufk[16][0]+(.0260106*gk[21][0])); fstareq[21][2] = -(ufk[16][2]+(.0260106*gk[21][2])); fstareq[22][0] = -(ufk[17][0]+(.02601109*gk[22][0])); fstareq[22][2] = -(ufk[17][2]+(.02601109*gk[22][2])); /* Compute tstareq (torques) */ /* Compute taus (RHS ignoring constraints and inertial forces) */ tau[22] = (utau[22]+(utk[17][1]+((5.194e-5*fstareq[22][2])+(.28317448* fstareq[22][0])))); Fstark[7][0] = (fstareq[7][0]+((fstareq[22][0]*c22)+(fstareq[22][2]*s22))); Fstark[7][1] = -(ufk[2][1]+ufk[17][1]); Fstark[7][2] = (fstareq[7][2]+((fstareq[22][2]*c22)-(fstareq[22][0]*s22))); tvec1[0] = (((rik[22][2]*ufk[17][1])-(4.88000000000008e-6*fstareq[22][2]))- utk[17][0]); tvec1[1] = (((fstareq[22][0]*rik[22][2])-(fstareq[22][2]*rik[22][0]))- utk[17][1]); tvec1[2] = (((4.88000000000008e-6*fstareq[22][0])-(rik[22][0]*ufk[17][1]))- utk[17][2]); Tstark[7][0] = (((tvec1[0]*c22)+(tvec1[2]*s22))-utk[2][0]); Tstark[7][1] = (tvec1[1]-utk[2][1]); Tstark[7][2] = (((tvec1[2]*c22)-(tvec1[0]*s22))-utk[2][2]); tau[21] = (utau[21]+(utk[16][1]+((5.2e-5*fstareq[21][2])+(.28373605* fstareq[21][0])))); Fstark[6][0] = (fstareq[6][0]+((fstareq[21][0]*c21)+(fstareq[21][2]*s21))); Fstark[6][1] = -(ufk[1][1]+ufk[16][1]); Fstark[6][2] = (fstareq[6][2]+((fstareq[21][2]*c21)-(fstareq[21][0]*s21))); tvec1[0] = (((rik[21][2]*ufk[16][1])-(5.26e-6*fstareq[21][2]))-utk[16][0]); tvec1[1] = (((fstareq[21][0]*rik[21][2])-(fstareq[21][2]*rik[21][0]))- utk[16][1]); tvec1[2] = (((5.26e-6*fstareq[21][0])-(rik[21][0]*ufk[16][1]))-utk[16][2]); Tstark[6][0] = (((tvec1[0]*c21)+(tvec1[2]*s21))-utk[1][0]); Tstark[6][1] = (tvec1[1]-utk[1][1]); Tstark[6][2] = (((tvec1[2]*c21)-(tvec1[0]*s21))-utk[1][2]); tau[20] = (utau[20]-(((.01382852*fstareq[20][0])-(.01361601*fstareq[20][2])) -utk[15][1])); Fstark[18][0] = (fstareq[18][0]+((fstareq[20][0]*c20)+(fstareq[20][2]*s20))); Fstark[18][1] = (fstareq[18][1]+fstareq[20][1]); Fstark[18][2] = (fstareq[18][2]+((fstareq[20][2]*c20)-(fstareq[20][0]*s20))); tvec1[0] = (((.02885771*fstareq[20][2])-(fstareq[20][1]*rik[20][2]))- utk[15][0]); tvec1[1] = (((fstareq[20][0]*rik[20][2])-(fstareq[20][2]*rik[20][0]))- utk[15][1]); tvec1[2] = (((fstareq[20][1]*rik[20][0])-(.02885771*fstareq[20][0]))- utk[15][2]); Tstark[18][0] = (((tvec1[0]*c20)+(tvec1[2]*s20))-utk[13][0]); Tstark[18][1] = (tvec1[1]-utk[13][1]); Tstark[18][2] = (((tvec1[2]*c20)-(tvec1[0]*s20))-utk[13][2]); tau[19] = (utau[19]-(((.01387793*fstareq[19][0])-(.01360932*fstareq[19][2])) -utk[14][1])); Fstark[17][0] = (fstareq[17][0]+((fstareq[19][0]*c19)+(fstareq[19][2]*s19))); Fstark[17][1] = (fstareq[17][1]+fstareq[19][1]); Fstark[17][2] = (fstareq[17][2]+((fstareq[19][2]*c19)-(fstareq[19][0]*s19))); tvec1[0] = -(utk[14][0]+((.02941965*fstareq[19][2])+(fstareq[19][1]* rik[19][2]))); tvec1[1] = (((fstareq[19][0]*rik[19][2])-(fstareq[19][2]*rik[19][0]))- utk[14][1]); tvec1[2] = (((.02941965*fstareq[19][0])+(fstareq[19][1]*rik[19][0]))- utk[14][2]); Tstark[17][0] = (((tvec1[0]*c19)+(tvec1[2]*s19))-utk[12][0]); Tstark[17][1] = (tvec1[1]-utk[12][1]); Tstark[17][2] = (((tvec1[2]*c19)-(tvec1[0]*s19))-utk[12][2]); tau[18] = (utau[18]-(Tstark[18][1]-((.01407902*Fstark[18][2])+(.15260355* Fstark[18][0])))); Fstark[16][0] = (fstareq[16][0]+((Fstark[18][0]*c18)+(Fstark[18][2]*s18))); Fstark[16][1] = (fstareq[16][1]+Fstark[18][1]); Fstark[16][2] = (fstareq[16][2]+((Fstark[18][2]*c18)-(Fstark[18][0]*s18))); tvec1[0] = (Tstark[18][0]+((.02678648*Fstark[18][2])-(Fstark[18][1]* rik[18][2]))); tvec1[1] = (Tstark[18][1]+((Fstark[18][0]*rik[18][2])-(Fstark[18][2]* rik[18][0]))); tvec1[2] = (Tstark[18][2]+((Fstark[18][1]*rik[18][0])-(.02678648* Fstark[18][0]))); Tstark[16][0] = (((tvec1[0]*c18)+(tvec1[2]*s18))-utk[11][0]); Tstark[16][1] = (tvec1[1]-utk[11][1]); Tstark[16][2] = (((tvec1[2]*c18)-(tvec1[0]*s18))-utk[11][2]); tau[17] = (utau[17]-(Tstark[17][1]-((.01407264*Fstark[17][2])+(.15249501* Fstark[17][0])))); Fstark[15][0] = (fstareq[15][0]+((Fstark[17][0]*c17)+(Fstark[17][2]*s17))); Fstark[15][1] = (fstareq[15][1]+Fstark[17][1]); Fstark[15][2] = (fstareq[15][2]+((Fstark[17][2]*c17)-(Fstark[17][0]*s17))); tvec1[0] = (Tstark[17][0]-((.02658427*Fstark[17][2])+(Fstark[17][1]* rik[17][2]))); tvec1[1] = (Tstark[17][1]+((Fstark[17][0]*rik[17][2])-(Fstark[17][2]* rik[17][0]))); tvec1[2] = (Tstark[17][2]+((.02658427*Fstark[17][0])+(Fstark[17][1]* rik[17][0]))); Tstark[15][0] = (((tvec1[0]*c17)+(tvec1[2]*s17))-utk[10][0]); Tstark[15][1] = (tvec1[1]-utk[10][1]); Tstark[15][2] = (((tvec1[2]*c17)-(tvec1[0]*s17))-utk[10][2]); tau[16] = (utau[16]-(Tstark[16][1]-((.00305957*Fstark[16][2])+(.21008605* Fstark[16][0])))); Fstark[5][0] = (fstareq[5][0]+((Fstark[16][0]*c16)+(Fstark[16][2]*s16))); Fstark[5][1] = (fstareq[5][1]+Fstark[16][1]); Fstark[5][2] = (fstareq[5][2]+((Fstark[16][2]*c16)-(Fstark[16][0]*s16))); tvec1[0] = (Tstark[16][0]+((.10875063*Fstark[16][2])-(Fstark[16][1]* rik[16][2]))); tvec1[1] = (Tstark[16][1]+((Fstark[16][0]*rik[16][2])-(Fstark[16][2]* rik[16][0]))); tvec1[2] = (Tstark[16][2]+((Fstark[16][1]*rik[16][0])-(.10875063* Fstark[16][0]))); Tstark[5][0] = (((tvec1[0]*c16)+(tvec1[2]*s16))-utk[0][0]); Tstark[5][1] = (tvec1[1]-utk[0][1]); Tstark[5][2] = (((tvec1[2]*c16)-(tvec1[0]*s16))-utk[0][2]); tau[15] = (utau[15]-(Tstark[15][1]-((.00307461*Fstark[15][2])+(.20905614* Fstark[15][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[15][0]*c15)+(Fstark[15][2]*s15))); Fstark[5][1] = (Fstark[5][1]+Fstark[15][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[15][2]*c15)-(Fstark[15][0]*s15))); tvec1[0] = (Tstark[15][0]-((.11006521*Fstark[15][2])+(Fstark[15][1]* rik[15][2]))); tvec1[1] = (Tstark[15][1]+((Fstark[15][0]*rik[15][2])-(Fstark[15][2]* rik[15][0]))); tvec1[2] = (Tstark[15][2]+((.11006521*Fstark[15][0])+(Fstark[15][1]* rik[15][0]))); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c15)+(tvec1[2]*s15))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c15)-(tvec1[0]*s15))); tau[14] = (utau[14]+(utk[9][1]+((.03539679*fstareq[14][2])+(.03987912* fstareq[14][0])))); Fstark[12][0] = (fstareq[12][0]+((fstareq[14][0]*c14)+(fstareq[14][2]*s14))); Fstark[12][1] = (fstareq[12][1]+fstareq[14][1]); Fstark[12][2] = (fstareq[12][2]+((fstareq[14][2]*c14)-(fstareq[14][0]*s14))); tvec1[0] = -(utk[9][0]+((.00061605*fstareq[14][2])+(fstareq[14][1]* rik[14][2]))); tvec1[1] = (((fstareq[14][0]*rik[14][2])-(fstareq[14][2]*rik[14][0]))- utk[9][1]); tvec1[2] = (((.00061605*fstareq[14][0])+(fstareq[14][1]*rik[14][0]))- utk[9][2]); Tstark[12][0] = (((tvec1[0]*c14)+(tvec1[2]*s14))-utk[7][0]); Tstark[12][1] = (tvec1[1]-utk[7][1]); Tstark[12][2] = (((tvec1[2]*c14)-(tvec1[0]*s14))-utk[7][2]); tau[13] = (utau[13]+(utk[8][1]+((.03543467*fstareq[13][2])+(.03983908* fstareq[13][0])))); Fstark[11][0] = (fstareq[11][0]+((fstareq[13][0]*c13)+(fstareq[13][2]*s13))); Fstark[11][1] = (fstareq[11][1]+fstareq[13][1]); Fstark[11][2] = (fstareq[11][2]+((fstareq[13][2]*c13)-(fstareq[13][0]*s13))); tvec1[0] = (((.0013196*fstareq[13][2])-(fstareq[13][1]*rik[13][2]))- utk[8][0]); tvec1[1] = (((fstareq[13][0]*rik[13][2])-(fstareq[13][2]*rik[13][0]))- utk[8][1]); tvec1[2] = (((fstareq[13][1]*rik[13][0])-(.0013196*fstareq[13][0]))- utk[8][2]); Tstark[11][0] = (((tvec1[0]*c13)+(tvec1[2]*s13))-utk[6][0]); Tstark[11][1] = (tvec1[1]-utk[6][1]); Tstark[11][2] = (((tvec1[2]*c13)-(tvec1[0]*s13))-utk[6][2]); tau[12] = (utau[12]-(Tstark[12][1]+((.01674373*Fstark[12][2])-(.12587504* Fstark[12][0])))); Fstark[10][0] = (fstareq[10][0]+((Fstark[12][0]*c12)+(Fstark[12][2]*s12))); Fstark[10][1] = (fstareq[10][1]+Fstark[12][1]); Fstark[10][2] = (fstareq[10][2]+((Fstark[12][2]*c12)-(Fstark[12][0]*s12))); tvec1[0] = (Tstark[12][0]-((.0027842*Fstark[12][2])+(Fstark[12][1]* rik[12][2]))); tvec1[1] = (Tstark[12][1]+((Fstark[12][0]*rik[12][2])-(Fstark[12][2]* rik[12][0]))); tvec1[2] = (Tstark[12][2]+((.0027842*Fstark[12][0])+(Fstark[12][1]* rik[12][0]))); Tstark[10][0] = (((tvec1[0]*c12)+(tvec1[2]*s12))-utk[5][0]); Tstark[10][1] = (tvec1[1]-utk[5][1]); Tstark[10][2] = (((tvec1[2]*c12)-(tvec1[0]*s12))-utk[5][2]); tau[11] = (utau[11]-(Tstark[11][1]+((.01672166*Fstark[11][2])-(.12618299* Fstark[11][0])))); Fstark[9][0] = (fstareq[9][0]+((Fstark[11][0]*c11)+(Fstark[11][2]*s11))); Fstark[9][1] = (fstareq[9][1]+Fstark[11][1]); Fstark[9][2] = (fstareq[9][2]+((Fstark[11][2]*c11)-(Fstark[11][0]*s11))); tvec1[0] = (Tstark[11][0]+((.00277012*Fstark[11][2])-(Fstark[11][1]* rik[11][2]))); tvec1[1] = (Tstark[11][1]+((Fstark[11][0]*rik[11][2])-(Fstark[11][2]* rik[11][0]))); tvec1[2] = (Tstark[11][2]+((Fstark[11][1]*rik[11][0])-(.00277012* Fstark[11][0]))); Tstark[9][0] = (((tvec1[0]*c11)+(tvec1[2]*s11))-utk[4][0]); Tstark[9][1] = (tvec1[1]-utk[4][1]); Tstark[9][2] = (((tvec1[2]*c11)-(tvec1[0]*s11))-utk[4][2]); tau[10] = (utau[10]-(Tstark[10][1]-((.00943639*Fstark[10][2])+(.13190398* Fstark[10][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[10][0]*c10)+(Fstark[10][2]*s10))); Fstark[5][1] = (Fstark[5][1]+Fstark[10][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[10][2]*c10)-(Fstark[10][0]*s10))); tvec1[0] = (Tstark[10][0]+((.0481466*Fstark[10][2])-(Fstark[10][1]* rik[10][2]))); tvec1[1] = (Tstark[10][1]+((Fstark[10][0]*rik[10][2])-(Fstark[10][2]* rik[10][0]))); tvec1[2] = (Tstark[10][2]+((Fstark[10][1]*rik[10][0])-(.0481466* Fstark[10][0]))); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c10)+(tvec1[2]*s10))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c10)-(tvec1[0]*s10))); tau[9] = (utau[9]-(Tstark[9][1]-((.0094289*Fstark[9][2])+(.13168857* Fstark[9][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[9][0]*c9)+(Fstark[9][2]*s9))); Fstark[5][1] = (Fstark[5][1]+Fstark[9][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[9][2]*c9)-(Fstark[9][0]*s9))); tvec1[0] = (Tstark[9][0]-((.04836741*Fstark[9][2])+(Fstark[9][1]*rik[9][2])) ); tvec1[1] = (Tstark[9][1]+((Fstark[9][0]*rik[9][2])-(Fstark[9][2]*rik[9][0])) ); tvec1[2] = (Tstark[9][2]+((.04836741*Fstark[9][0])+(Fstark[9][1]*rik[9][0])) ); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c9)+(tvec1[2]*s9))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c9)-(tvec1[0]*s9))); tau[8] = (utau[8]+(utk[3][1]+((.01691003*fstareq[8][2])+(.12000071* fstareq[8][0])))); Fstark[5][0] = (Fstark[5][0]+((fstareq[8][0]*c8)+(fstareq[8][2]*s8))); Fstark[5][1] = (fstareq[8][1]+Fstark[5][1]); Fstark[5][2] = (Fstark[5][2]+((fstareq[8][2]*c8)-(fstareq[8][0]*s8))); tvec1[0] = -(utk[3][0]+((.00049034*fstareq[8][2])+(fstareq[8][1]*rik[8][2])) ); tvec1[1] = (((fstareq[8][0]*rik[8][2])-(fstareq[8][2]*rik[8][0]))-utk[3][1]); tvec1[2] = (((.00049034*fstareq[8][0])+(fstareq[8][1]*rik[8][0]))-utk[3][2]); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c8)+(tvec1[2]*s8))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c8)-(tvec1[0]*s8))); tau[7] = (utau[7]-(Tstark[7][1]-((5.197e-5*Fstark[7][2])+(1.22369868* Fstark[7][0])))); tau[6] = (utau[6]-(Tstark[6][1]-((5.201e-5*Fstark[6][2])+(1.2242604* Fstark[6][0])))); tau[5] = (utau[5]-Tstark[5][2]); Fstark[4][0] = Fstark[5][0]; Fstark[4][1] = Fstark[5][1]; Fstark[4][2] = Fstark[5][2]; Tstark[4][0] = Tstark[5][0]; Tstark[4][1] = Tstark[5][1]; Tstark[4][2] = Tstark[5][2]; tau[4] = (utau[4]-Tstark[4][1]); Fstark[3][0] = Fstark[4][0]; Fstark[3][1] = Fstark[4][1]; Fstark[3][2] = Fstark[4][2]; Tstark[3][0] = Tstark[4][0]; Tstark[3][1] = Tstark[4][1]; Tstark[3][2] = Tstark[4][2]; tau[3] = (utau[3]-Tstark[3][0]); Fstark[2][0] = ((Cik[3][0][2]*Fstark[3][2])+((Cik[3][0][0]*Fstark[3][0])+( Cik[3][0][1]*Fstark[3][1]))); Fstark[2][1] = ((Cik[3][1][2]*Fstark[3][2])+((Cik[3][1][0]*Fstark[3][0])+( Cik[3][1][1]*Fstark[3][1]))); Fstark[2][2] = ((Cik[3][2][2]*Fstark[3][2])+((Cik[3][2][0]*Fstark[3][0])+( Cik[3][2][1]*Fstark[3][1]))); Tstark[2][0] = ((Cik[3][0][2]*Tstark[3][2])+((Cik[3][0][0]*Tstark[3][0])+( Cik[3][0][1]*Tstark[3][1]))); Tstark[2][1] = ((Cik[3][1][2]*Tstark[3][2])+((Cik[3][1][0]*Tstark[3][0])+( Cik[3][1][1]*Tstark[3][1]))); Tstark[2][2] = ((Cik[3][2][2]*Tstark[3][2])+((Cik[3][2][0]*Tstark[3][0])+( Cik[3][2][1]*Tstark[3][1]))); tau[2] = (utau[2]-Fstark[2][2]); Fstark[1][0] = Fstark[2][0]; Fstark[1][1] = Fstark[2][1]; Fstark[1][2] = Fstark[2][2]; tvec1[0] = (Tstark[2][0]-(Fstark[2][1]*q[2])); tvec1[1] = (Tstark[2][1]+(Fstark[2][0]*q[2])); Tstark[1][0] = tvec1[0]; Tstark[1][1] = tvec1[1]; Tstark[1][2] = Tstark[2][2]; tau[1] = (utau[1]-Fstark[1][1]); Fstark[0][0] = Fstark[1][0]; Fstark[0][1] = Fstark[1][1]; Fstark[0][2] = Fstark[1][2]; tvec1[0] = (Tstark[1][0]+(Fstark[1][2]*q[1])); tvec1[2] = (Tstark[1][2]-(Fstark[1][0]*q[1])); Tstark[0][0] = tvec1[0]; Tstark[0][1] = Tstark[1][1]; Tstark[0][2] = tvec1[2]; tau[0] = (utau[0]-Fstark[0][0]); /* Op counts below do not include called subroutines */ /* Used 0.40 seconds CPU time, 0 additional bytes of memory. Equations contain 400 adds/subtracts/negates 316 multiplies 0 divides 242 assignments */ } void BOCfly :: sdfulltrq ( double udotin[23], double multin[1], double trqout[23] ) { /* Compute hinge torques which would produce indicated udots */ double fstarr[23][3],tstarr[23][3],Otkr[23][3],Atir[23][3],Atkr[23][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(61,23); return; } /* Account for inertial accelerations and supplied udots */ Otkr[8][0] = (((udotin[3]*c8)-(udotin[5]*s8))-(u[8]*wk[8][2])); Otkr[8][1] = (udotin[4]+udotin[8]); Otkr[8][2] = ((u[8]*wk[8][0])+((udotin[3]*s8)+(udotin[5]*c8))); Otkr[9][0] = (((udotin[3]*c9)-(udotin[5]*s9))-(u[9]*wk[9][2])); Otkr[9][1] = (udotin[4]+udotin[9]); Otkr[9][2] = ((u[9]*wk[9][0])+((udotin[3]*s9)+(udotin[5]*c9))); Otkr[10][0] = (((udotin[3]*c10)-(udotin[5]*s10))-(u[10]*wk[10][2])); Otkr[10][1] = (udotin[4]+udotin[10]); Otkr[10][2] = ((u[10]*wk[10][0])+((udotin[3]*s10)+(udotin[5]*c10))); Otkr[11][0] = (((Otkr[9][0]*c11)-(Otkr[9][2]*s11))-(u[11]*wk[11][2])); Otkr[11][1] = (Otkr[9][1]+udotin[11]); Otkr[11][2] = ((u[11]*wk[11][0])+((Otkr[9][0]*s11)+(Otkr[9][2]*c11))); Otkr[12][0] = (((Otkr[10][0]*c12)-(Otkr[10][2]*s12))-(u[12]*wk[12][2])); Otkr[12][1] = (Otkr[10][1]+udotin[12]); Otkr[12][2] = ((u[12]*wk[12][0])+((Otkr[10][0]*s12)+(Otkr[10][2]*c12))); Otkr[13][0] = (((Otkr[11][0]*c13)-(Otkr[11][2]*s13))-(u[13]*wk[13][2])); Otkr[13][1] = (Otkr[11][1]+udotin[13]); Otkr[13][2] = ((u[13]*wk[13][0])+((Otkr[11][0]*s13)+(Otkr[11][2]*c13))); Otkr[14][0] = (((Otkr[12][0]*c14)-(Otkr[12][2]*s14))-(u[14]*wk[14][2])); Otkr[14][1] = (Otkr[12][1]+udotin[14]); Otkr[14][2] = ((u[14]*wk[14][0])+((Otkr[12][0]*s14)+(Otkr[12][2]*c14))); Otkr[15][0] = (((udotin[3]*c15)-(udotin[5]*s15))-(u[15]*wk[15][2])); Otkr[15][1] = (udotin[4]+udotin[15]); Otkr[15][2] = ((u[15]*wk[15][0])+((udotin[3]*s15)+(udotin[5]*c15))); Otkr[16][0] = (((udotin[3]*c16)-(udotin[5]*s16))-(u[16]*wk[16][2])); Otkr[16][1] = (udotin[4]+udotin[16]); Otkr[16][2] = ((u[16]*wk[16][0])+((udotin[3]*s16)+(udotin[5]*c16))); Otkr[17][0] = (((Otkr[15][0]*c17)-(Otkr[15][2]*s17))-(u[17]*wk[17][2])); Otkr[17][1] = (Otkr[15][1]+udotin[17]); Otkr[17][2] = ((u[17]*wk[17][0])+((Otkr[15][0]*s17)+(Otkr[15][2]*c17))); Otkr[18][0] = (((Otkr[16][0]*c18)-(Otkr[16][2]*s18))-(u[18]*wk[18][2])); Otkr[18][1] = (Otkr[16][1]+udotin[18]); Otkr[18][2] = ((u[18]*wk[18][0])+((Otkr[16][0]*s18)+(Otkr[16][2]*c18))); Otkr[19][0] = (((Otkr[17][0]*c19)-(Otkr[17][2]*s19))-(u[19]*wk[19][2])); Otkr[19][1] = (Otkr[17][1]+udotin[19]); Otkr[19][2] = ((u[19]*wk[19][0])+((Otkr[17][0]*s19)+(Otkr[17][2]*c19))); Otkr[20][0] = (((Otkr[18][0]*c20)-(Otkr[18][2]*s20))-(u[20]*wk[20][2])); Otkr[20][1] = (Otkr[18][1]+udotin[20]); Otkr[20][2] = ((u[20]*wk[20][0])+((Otkr[18][0]*s20)+(Otkr[18][2]*c20))); Otkr[21][1] = (udotin[6]+udotin[21]); Otkr[22][1] = (udotin[7]+udotin[22]); Atkr[3][0] = ((Cik[3][2][0]*udotin[2])+((Cik[3][0][0]*udotin[0])+( Cik[3][1][0]*udotin[1]))); Atkr[3][1] = ((Cik[3][2][1]*udotin[2])+((Cik[3][0][1]*udotin[0])+( Cik[3][1][1]*udotin[1]))); Atkr[3][2] = ((Cik[3][2][2]*udotin[2])+((Cik[3][0][2]*udotin[0])+( Cik[3][1][2]*udotin[1]))); Atkr[6][0] = -((5.201e-5*(u[6]*u[6]))+(1.2242604*udotin[6])); Atkr[6][2] = ((1.2242604*(u[6]*u[6]))-(5.201e-5*udotin[6])); Atkr[7][0] = -((5.197e-5*(u[7]*u[7]))+(1.22369868*udotin[7])); Atkr[7][2] = ((1.22369868*(u[7]*u[7]))-(5.197e-5*udotin[7])); Atir[8][0] = (Atkr[3][0]+(((.00120864*udotin[5])+(.3829286*udotin[4]))+(( u[4]*Wirk[8][2])-(u[5]*Wirk[8][1])))); Atir[8][1] = (Atkr[3][1]+(((u[5]*Wirk[8][0])-(u[3]*Wirk[8][2]))-((.00252618* udotin[5])+(.3829286*udotin[3])))); Atir[8][2] = (Atkr[3][2]+(((.00252618*udotin[4])-(.00120864*udotin[3]))+(( u[3]*Wirk[8][1])-(u[4]*Wirk[8][0])))); Atkr[8][0] = (((Atir[8][0]*c8)-(Atir[8][2]*s8))+(((wk[8][1]*Wkrpk[8][2])-( wk[8][2]*Wkrpk[8][1]))-((.0007183*Otkr[8][2])+(.12000071*Otkr[8][1])))); Atkr[8][1] = (Atir[8][1]+(((.01691003*Otkr[8][2])+(.12000071*Otkr[8][0]))+(( wk[8][2]*Wkrpk[8][0])-(wk[8][0]*Wkrpk[8][2])))); Atkr[8][2] = (((Atir[8][0]*s8)+(Atir[8][2]*c8))+(((.0007183*Otkr[8][0])-( .01691003*Otkr[8][1]))+((wk[8][0]*Wkrpk[8][1])-(wk[8][1]*Wkrpk[8][0])))); Atir[9][0] = (Atkr[3][0]+(((.04820864*udotin[5])-(.0620714*udotin[4]))+(( u[4]*Wirk[9][2])-(u[5]*Wirk[9][1])))); Atir[9][1] = (Atkr[3][1]+(((.0620714*udotin[3])-(.01952618*udotin[5]))+(( u[5]*Wirk[9][0])-(u[3]*Wirk[9][2])))); Atir[9][2] = (Atkr[3][2]+(((.01952618*udotin[4])-(.04820864*udotin[3]))+(( u[3]*Wirk[9][1])-(u[4]*Wirk[9][0])))); Atkr[9][0] = (((Atir[9][0]*c9)-(Atir[9][2]*s9))+(((.00015877*Otkr[9][2])-( .13168857*Otkr[9][1]))+((wk[9][1]*Wkrpk[9][2])-(wk[9][2]*Wkrpk[9][1])))); Atkr[9][1] = (Atir[9][1]+(((.0094289*Otkr[9][2])+(.13168857*Otkr[9][0]))+(( wk[9][2]*Wkrpk[9][0])-(wk[9][0]*Wkrpk[9][2])))); Atkr[9][2] = (((Atir[9][0]*s9)+(Atir[9][2]*c9))+(((wk[9][0]*Wkrpk[9][1])-( wk[9][1]*Wkrpk[9][0]))-((.00015877*Otkr[9][0])+(.0094289*Otkr[9][1])))); Atir[10][0] = (Atkr[3][0]+(((u[4]*Wirk[10][2])-(u[5]*Wirk[10][1]))-(( .04579136*udotin[5])+(.0620714*udotin[4])))); Atir[10][1] = (Atkr[3][1]+(((.0620714*udotin[3])-(.01952618*udotin[5]))+(( u[5]*Wirk[10][0])-(u[3]*Wirk[10][2])))); Atir[10][2] = (Atkr[3][2]+(((.01952618*udotin[4])+(.04579136*udotin[3]))+(( u[3]*Wirk[10][1])-(u[4]*Wirk[10][0])))); Atkr[10][0] = (((Atir[10][0]*c10)-(Atir[10][2]*s10))+(((wk[10][1]* Wkrpk[10][2])-(wk[10][2]*Wkrpk[10][1]))-((.00235524*Otkr[10][2])+( .13190398*Otkr[10][1])))); Atkr[10][1] = (Atir[10][1]+(((.00943639*Otkr[10][2])+(.13190398*Otkr[10][0]) )+((wk[10][2]*Wkrpk[10][0])-(wk[10][0]*Wkrpk[10][2])))); Atkr[10][2] = (((Atir[10][0]*s10)+(Atir[10][2]*c10))+(((.00235524* Otkr[10][0])-(.00943639*Otkr[10][1]))+((wk[10][0]*Wkrpk[10][1])-(wk[10][1] *Wkrpk[10][0])))); Atir[11][0] = (Atkr[9][0]+(((.00284123*Otkr[9][2])-(.15931143*Otkr[9][1]))+( (Wirk[11][2]*wk[9][1])-(Wirk[11][1]*wk[9][2])))); Atir[11][1] = (Atkr[9][1]+(((.0065711*Otkr[9][2])+(.15931143*Otkr[9][0]))+(( Wirk[11][0]*wk[9][2])-(Wirk[11][2]*wk[9][0])))); Atir[11][2] = (Atkr[9][2]+(((Wirk[11][1]*wk[9][0])-(Wirk[11][0]*wk[9][1]))-( (.00284123*Otkr[9][0])+(.0065711*Otkr[9][1])))); Atkr[11][0] = (((Atir[11][0]*c11)-(Atir[11][2]*s11))+(((wk[11][1]* Wkrpk[11][2])-(wk[11][2]*Wkrpk[11][1]))-((.00561135*Otkr[11][2])+( .12618299*Otkr[11][1])))); Atkr[11][1] = (Atir[11][1]+(((.12618299*Otkr[11][0])-(.01672166*Otkr[11][2]) )+((wk[11][2]*Wkrpk[11][0])-(wk[11][0]*Wkrpk[11][2])))); Atkr[11][2] = (((Atir[11][0]*s11)+(Atir[11][2]*c11))+(((.00561135* Otkr[11][0])+(.01672166*Otkr[11][1]))+((wk[11][0]*Wkrpk[11][1])-(wk[11][1] *Wkrpk[11][0])))); Atir[12][0] = (Atkr[10][0]+(((Wirk[12][2]*wk[10][1])-(Wirk[12][1]*wk[10][2]) )-((.00064476*Otkr[10][2])+(.15909602*Otkr[10][1])))); Atir[12][1] = (Atkr[10][1]+(((.00656361*Otkr[10][2])+(.15909602*Otkr[10][0]) )+((Wirk[12][0]*wk[10][2])-(Wirk[12][2]*wk[10][0])))); Atir[12][2] = (Atkr[10][2]+(((.00064476*Otkr[10][0])-(.00656361*Otkr[10][1]) )+((Wirk[12][1]*wk[10][0])-(Wirk[12][0]*wk[10][1])))); Atkr[12][0] = (((Atir[12][0]*c12)-(Atir[12][2]*s12))+(((.00342896* Otkr[12][2])-(.12587504*Otkr[12][1]))+((wk[12][1]*Wkrpk[12][2])-(wk[12][2] *Wkrpk[12][1])))); Atkr[12][1] = (Atir[12][1]+(((.12587504*Otkr[12][0])-(.01674373*Otkr[12][2]) )+((wk[12][2]*Wkrpk[12][0])-(wk[12][0]*Wkrpk[12][2])))); Atkr[12][2] = (((Atir[12][0]*s12)+(Atir[12][2]*c12))+(((.01674373* Otkr[12][1])-(.00342896*Otkr[12][0]))+((wk[12][0]*Wkrpk[12][1])-(wk[12][1] *Wkrpk[12][0])))); Atir[13][0] = (Atkr[11][0]+(((.00061135*Otkr[11][2])-(.17081701*Otkr[11][1]) )+((Wirk[13][2]*wk[11][1])-(Wirk[13][1]*wk[11][2])))); Atir[13][1] = (Atkr[11][1]+(((.00172166*Otkr[11][2])+(.17081701*Otkr[11][0]) )+((Wirk[13][0]*wk[11][2])-(Wirk[13][2]*wk[11][0])))); Atir[13][2] = (Atkr[11][2]+(((Wirk[13][1]*wk[11][0])-(Wirk[13][0]*wk[11][1]) )-((.00061135*Otkr[11][0])+(.00172166*Otkr[11][1])))); Atkr[13][0] = (((Atir[13][0]*c13)-(Atir[13][2]*s13))+(((wk[13][1]* Wkrpk[13][2])-(wk[13][2]*Wkrpk[13][1]))-((.00193095*Otkr[13][2])+( .03983908*Otkr[13][1])))); Atkr[13][1] = (Atir[13][1]+(((.03543467*Otkr[13][2])+(.03983908*Otkr[13][0]) )+((wk[13][2]*Wkrpk[13][0])-(wk[13][0]*Wkrpk[13][2])))); Atkr[13][2] = (((Atir[13][0]*s13)+(Atir[13][2]*c13))+(((.00193095* Otkr[13][0])-(.03543467*Otkr[13][1]))+((wk[13][0]*Wkrpk[13][1])-(wk[13][1] *Wkrpk[13][0])))); Atir[14][0] = (Atkr[12][0]+(((.00157104*Otkr[12][2])-(.17112496*Otkr[12][1]) )+((Wirk[14][2]*wk[12][1])-(Wirk[14][1]*wk[12][2])))); Atir[14][1] = (Atkr[12][1]+(((.00174373*Otkr[12][2])+(.17112496*Otkr[12][0]) )+((Wirk[14][0]*wk[12][2])-(Wirk[14][2]*wk[12][0])))); Atir[14][2] = (Atkr[12][2]+(((Wirk[14][1]*wk[12][0])-(Wirk[14][0]*wk[12][1]) )-((.00157104*Otkr[12][0])+(.00174373*Otkr[12][1])))); Atkr[14][0] = (((Atir[14][0]*c14)-(Atir[14][2]*s14))+(((wk[14][1]* Wkrpk[14][2])-(wk[14][2]*Wkrpk[14][1]))-((.00095499*Otkr[14][2])+( .03987912*Otkr[14][1])))); Atkr[14][1] = (Atir[14][1]+(((.03539679*Otkr[14][2])+(.03987912*Otkr[14][0]) )+((wk[14][2]*Wkrpk[14][0])-(wk[14][0]*Wkrpk[14][2])))); Atkr[14][2] = (((Atir[14][0]*s14)+(Atir[14][2]*c14))+(((.00095499* Otkr[14][0])-(.03539679*Otkr[14][1]))+((wk[14][0]*Wkrpk[14][1])-(wk[14][1] *Wkrpk[14][0])))); Atir[15][0] = (Atkr[3][0]+(((.10120864*udotin[5])+(.2639286*udotin[4]))+(( u[4]*Wirk[15][2])-(u[5]*Wirk[15][1])))); Atir[15][1] = (Atkr[3][1]+(((u[5]*Wirk[15][0])-(u[3]*Wirk[15][2]))-(( .01052618*udotin[5])+(.2639286*udotin[3])))); Atir[15][2] = (Atkr[3][2]+(((.01052618*udotin[4])-(.10120864*udotin[3]))+(( u[3]*Wirk[15][1])-(u[4]*Wirk[15][0])))); Atkr[15][0] = (((Atir[15][0]*c15)-(Atir[15][2]*s15))+(((.00885657* Otkr[15][2])-(.20905614*Otkr[15][1]))+((wk[15][1]*Wkrpk[15][2])-(wk[15][2] *Wkrpk[15][1])))); Atkr[15][1] = (Atir[15][1]+(((.00307461*Otkr[15][2])+(.20905614*Otkr[15][0]) )+((wk[15][2]*Wkrpk[15][0])-(wk[15][0]*Wkrpk[15][2])))); Atkr[15][2] = (((Atir[15][0]*s15)+(Atir[15][2]*c15))+(((wk[15][0]* Wkrpk[15][1])-(wk[15][1]*Wkrpk[15][0]))-((.00307461*Otkr[15][1])+( .00885657*Otkr[15][0])))); Atir[16][0] = (Atkr[3][0]+(((.2639286*udotin[4])-(.09879136*udotin[5]))+(( u[4]*Wirk[16][2])-(u[5]*Wirk[16][1])))); Atir[16][1] = (Atkr[3][1]+(((u[5]*Wirk[16][0])-(u[3]*Wirk[16][2]))-(( .01052618*udotin[5])+(.2639286*udotin[3])))); Atir[16][2] = (Atkr[3][2]+(((.01052618*udotin[4])+(.09879136*udotin[3]))+(( u[3]*Wirk[16][1])-(u[4]*Wirk[16][0])))); Atkr[16][0] = (((Atir[16][0]*c16)-(Atir[16][2]*s16))+(((wk[16][1]* Wkrpk[16][2])-(wk[16][2]*Wkrpk[16][1]))-((.00995927*Otkr[16][2])+( .21008605*Otkr[16][1])))); Atkr[16][1] = (Atir[16][1]+(((.00305957*Otkr[16][2])+(.21008605*Otkr[16][0]) )+((wk[16][2]*Wkrpk[16][0])-(wk[16][0]*Wkrpk[16][2])))); Atkr[16][2] = (((Atir[16][0]*s16)+(Atir[16][2]*c16))+(((.00995927* Otkr[16][0])-(.00305957*Otkr[16][1]))+((wk[16][0]*Wkrpk[16][1])-(wk[16][1] *Wkrpk[16][0])))); Atir[17][0] = (Atkr[15][0]+(((.01305614*Otkr[15][1])+(.02714343*Otkr[15][2]) )+((Wirk[17][2]*wk[15][1])-(Wirk[17][1]*wk[15][2])))); Atir[17][1] = (Atkr[15][1]+(((Wirk[17][0]*wk[15][2])-(Wirk[17][2]*wk[15][0]) )-((.00307461*Otkr[15][2])+(.01305614*Otkr[15][0])))); Atir[17][2] = (Atkr[15][2]+(((.00307461*Otkr[15][1])-(.02714343*Otkr[15][0]) )+((Wirk[17][1]*wk[15][0])-(Wirk[17][0]*wk[15][1])))); Atkr[17][0] = (((Atir[17][0]*c17)-(Atir[17][2]*s17))+(((wk[17][1]* Wkrpk[17][2])-(wk[17][2]*Wkrpk[17][1]))-((.00055916*Otkr[17][2])+( .15249501*Otkr[17][1])))); Atkr[17][1] = (Atir[17][1]+(((.01407264*Otkr[17][2])+(.15249501*Otkr[17][0]) )+((wk[17][2]*Wkrpk[17][0])-(wk[17][0]*Wkrpk[17][2])))); Atkr[17][2] = (((Atir[17][0]*s17)+(Atir[17][2]*c17))+(((.00055916* Otkr[17][0])-(.01407264*Otkr[17][1]))+((wk[17][0]*Wkrpk[17][1])-(wk[17][1] *Wkrpk[17][0])))); Atir[18][0] = (Atkr[16][0]+(((.01408605*Otkr[16][1])-(.02604073*Otkr[16][2]) )+((Wirk[18][2]*wk[16][1])-(Wirk[18][1]*wk[16][2])))); Atir[18][1] = (Atkr[16][1]+(((Wirk[18][0]*wk[16][2])-(Wirk[18][2]*wk[16][0]) )-((.00305957*Otkr[16][2])+(.01408605*Otkr[16][0])))); Atir[18][2] = (Atkr[16][2]+(((.00305957*Otkr[16][1])+(.02604073*Otkr[16][0]) )+((Wirk[18][1]*wk[16][0])-(Wirk[18][0]*wk[16][1])))); Atkr[18][0] = (((Atir[18][0]*c18)-(Atir[18][2]*s18))+(((wk[18][1]* Wkrpk[18][2])-(wk[18][2]*Wkrpk[18][1]))-((.00074575*Otkr[18][2])+( .15260355*Otkr[18][1])))); Atkr[18][1] = (Atir[18][1]+(((.01407902*Otkr[18][2])+(.15260355*Otkr[18][0]) )+((wk[18][2]*Wkrpk[18][0])-(wk[18][0]*Wkrpk[18][2])))); Atkr[18][2] = (((Atir[18][0]*s18)+(Atir[18][2]*c18))+(((.00074575* Otkr[18][0])-(.01407902*Otkr[18][1]))+((wk[18][0]*Wkrpk[18][1])-(wk[18][1] *Wkrpk[18][0])))); Atir[19][0] = (Atkr[17][0]+(((.02455916*Otkr[17][2])-(.01750499*Otkr[17][1]) )+((Wirk[19][2]*wk[17][1])-(Wirk[19][1]*wk[17][2])))); Atir[19][1] = (Atkr[17][1]+(((.00792736*Otkr[17][2])+(.01750499*Otkr[17][0]) )+((Wirk[19][0]*wk[17][2])-(Wirk[19][2]*wk[17][0])))); Atir[19][2] = (Atkr[17][2]+(((Wirk[19][1]*wk[17][0])-(Wirk[19][0]*wk[17][1]) )-((.00792736*Otkr[17][1])+(.02455916*Otkr[17][0])))); Atkr[19][0] = (((Atir[19][0]*c19)-(Atir[19][2]*s19))+(((.00486049* Otkr[19][2])+(.01387793*Otkr[19][1]))+((wk[19][1]*Wkrpk[19][2])-(wk[19][2] *Wkrpk[19][1])))); Atkr[19][1] = (Atir[19][1]+(((.01360932*Otkr[19][2])-(.01387793*Otkr[19][0]) )+((wk[19][2]*Wkrpk[19][0])-(wk[19][0]*Wkrpk[19][2])))); Atkr[19][2] = (((Atir[19][0]*s19)+(Atir[19][2]*c19))+(((wk[19][0]* Wkrpk[19][1])-(wk[19][1]*Wkrpk[19][0]))-((.00486049*Otkr[19][0])+( .01360932*Otkr[19][1])))); Atir[20][0] = (Atkr[18][0]+(((Wirk[20][2]*wk[18][1])-(Wirk[20][1]*wk[18][2]) )-((.01739645*Otkr[18][1])+(.02325425*Otkr[18][2])))); Atir[20][1] = (Atkr[18][1]+(((.00792098*Otkr[18][2])+(.01739645*Otkr[18][0]) )+((Wirk[20][0]*wk[18][2])-(Wirk[20][2]*wk[18][0])))); Atir[20][2] = (Atkr[18][2]+(((.02325425*Otkr[18][0])-(.00792098*Otkr[18][1]) )+((Wirk[20][1]*wk[18][0])-(Wirk[20][0]*wk[18][1])))); Atkr[20][0] = (((Atir[20][0]*c20)-(Atir[20][2]*s20))+(((.01382852* Otkr[20][1])-(.00560346*Otkr[20][2]))+((wk[20][1]*Wkrpk[20][2])-(wk[20][2] *Wkrpk[20][1])))); Atkr[20][1] = (Atir[20][1]+(((.01361601*Otkr[20][2])-(.01382852*Otkr[20][0]) )+((wk[20][2]*Wkrpk[20][0])-(wk[20][0]*Wkrpk[20][2])))); Atkr[20][2] = (((Atir[20][0]*s20)+(Atir[20][2]*c20))+(((.00560346* Otkr[20][0])-(.01361601*Otkr[20][1]))+((wk[20][0]*Wkrpk[20][1])-(wk[20][1] *Wkrpk[20][0])))); Atir[21][0] = (Atkr[6][0]+((5.201e-5*(u[6]*u[6]))-(.4177396*udotin[6]))); Atir[21][2] = (Atkr[6][2]+((5.201e-5*udotin[6])+(.4177396*(u[6]*u[6])))); Atkr[21][0] = (((Atir[21][0]*c21)-(Atir[21][2]*s21))-((5.2e-5*(wk[21][1]* wk[21][1]))+(.28373605*Otkr[21][1]))); Atkr[21][2] = (((.28373605*(wk[21][1]*wk[21][1]))-(5.2e-5*Otkr[21][1]))+(( Atir[21][0]*s21)+(Atir[21][2]*c21))); Atir[22][0] = (Atkr[7][0]+((5.197e-5*(u[7]*u[7]))-(.41830132*udotin[7]))); Atir[22][2] = (Atkr[7][2]+((5.197e-5*udotin[7])+(.41830132*(u[7]*u[7])))); Atkr[22][0] = (((Atir[22][0]*c22)-(Atir[22][2]*s22))-((5.194e-5*(wk[22][1]* wk[22][1]))+(.28317448*Otkr[22][1]))); Atkr[22][2] = (((.28317448*(wk[22][1]*wk[22][1]))-(5.194e-5*Otkr[22][1]))+(( Atir[22][0]*s22)+(Atir[22][2]*c22))); /* Accumulate all forces and torques */ fstarr[5][0] = (ufk[0][0]-(11.02968359*(Atkr[3][0]+(9.80665*Cik[3][2][0])))); fstarr[5][1] = (ufk[0][1]-(11.02968359*(Atkr[3][1]+(9.80665*Cik[3][2][1])))); fstarr[5][2] = (ufk[0][2]-(11.02968359*(Atkr[3][2]+(9.80665*Cik[3][2][2])))); fstarr[6][0] = (ufk[1][0]+(.18166784*((9.80665*s6)-Atkr[6][0]))); fstarr[6][2] = (ufk[1][2]-(.18166784*(Atkr[6][2]+(9.80665*c6)))); fstarr[7][0] = (ufk[2][0]+(.1816698*((9.80665*s7)-Atkr[7][0]))); fstarr[7][2] = (ufk[2][2]-(.1816698*(Atkr[7][2]+(9.80665*c7)))); fstarr[8][0] = (ufk[3][0]+(9.84983599*(gk[8][0]-Atkr[8][0]))); fstarr[8][1] = (ufk[3][1]-(9.84983599*(Atkr[8][1]+(9.80665*Cik[3][2][1])))); fstarr[8][2] = (ufk[3][2]+(9.84983599*(gk[8][2]-Atkr[8][2]))); fstarr[9][0] = (ufk[4][0]+(2.89586298*(gk[9][0]-Atkr[9][0]))); fstarr[9][1] = (ufk[4][1]-(2.89586298*(Atkr[9][1]+(9.80665*Cik[3][2][1])))); fstarr[9][2] = (ufk[4][2]+(2.89586298*(gk[9][2]-Atkr[9][2]))); fstarr[10][0] = (ufk[5][0]+(2.8984419*(gk[10][0]-Atkr[10][0]))); fstarr[10][1] = (ufk[5][1]-(2.8984419*(Atkr[10][1]+(9.80665*Cik[3][2][1])))); fstarr[10][2] = (ufk[5][2]+(2.8984419*(gk[10][2]-Atkr[10][2]))); fstarr[11][0] = (ufk[6][0]+(1.02133193*(gk[11][0]-Atkr[11][0]))); fstarr[11][1] = (ufk[6][1]-(1.02133193*(Atkr[11][1]+(9.80665*Cik[3][2][1]))) ); fstarr[11][2] = (ufk[6][2]+(1.02133193*(gk[11][2]-Atkr[11][2]))); fstarr[12][0] = (ufk[7][0]+(1.02007998*(gk[12][0]-Atkr[12][0]))); fstarr[12][1] = (ufk[7][1]-(1.02007998*(Atkr[12][1]+(9.80665*Cik[3][2][1]))) ); fstarr[12][2] = (ufk[7][2]+(1.02007998*(gk[12][2]-Atkr[12][2]))); fstarr[13][0] = (ufk[8][0]+(.53380008*(gk[13][0]-Atkr[13][0]))); fstarr[13][1] = (ufk[8][1]-(.53380008*(Atkr[13][1]+(9.80665*Cik[3][2][1])))); fstarr[13][2] = (ufk[8][2]+(.53380008*(gk[13][2]-Atkr[13][2]))); fstarr[14][0] = (ufk[9][0]+(.5325902*(gk[14][0]-Atkr[14][0]))); fstarr[14][1] = (ufk[9][1]-(.5325902*(Atkr[14][1]+(9.80665*Cik[3][2][1])))); fstarr[14][2] = (ufk[9][2]+(.5325902*(gk[14][2]-Atkr[14][2]))); fstarr[15][0] = (ufk[10][0]+(1.00985702*(gk[15][0]-Atkr[15][0]))); fstarr[15][1] = (ufk[10][1]-(1.00985702*(Atkr[15][1]+(9.80665*Cik[3][2][1])) )); fstarr[15][2] = (ufk[10][2]+(1.00985702*(gk[15][2]-Atkr[15][2]))); fstarr[16][0] = (ufk[11][0]+(1.01701701*(gk[16][0]-Atkr[16][0]))); fstarr[16][1] = (ufk[11][1]-(1.01701701*(Atkr[16][1]+(9.80665*Cik[3][2][1])) )); fstarr[16][2] = (ufk[11][2]+(1.01701701*(gk[16][2]-Atkr[16][2]))); fstarr[17][0] = (ufk[12][0]+(.43362906*(gk[17][0]-Atkr[17][0]))); fstarr[17][1] = (ufk[12][1]-(.43362906*(Atkr[17][1]+(9.80665*Cik[3][2][1]))) ); fstarr[17][2] = (ufk[12][2]+(.43362906*(gk[17][2]-Atkr[17][2]))); fstarr[18][0] = (ufk[13][0]+(.43358398*(gk[18][0]-Atkr[18][0]))); fstarr[18][1] = (ufk[13][1]-(.43358398*(Atkr[18][1]+(9.80665*Cik[3][2][1]))) ); fstarr[18][2] = (ufk[13][2]+(.43358398*(gk[18][2]-Atkr[18][2]))); fstarr[19][0] = (ufk[14][0]+(.05946345*(gk[19][0]-Atkr[19][0]))); fstarr[19][1] = (ufk[14][1]-(.05946345*(Atkr[19][1]+(9.80665*Cik[3][2][1]))) ); fstarr[19][2] = (ufk[14][2]+(.05946345*(gk[19][2]-Atkr[19][2]))); fstarr[20][0] = (ufk[15][0]+(.05944511*(gk[20][0]-Atkr[20][0]))); fstarr[20][1] = (ufk[15][1]-(.05944511*(Atkr[20][1]+(9.80665*Cik[3][2][1]))) ); fstarr[20][2] = (ufk[15][2]+(.05944511*(gk[20][2]-Atkr[20][2]))); fstarr[21][0] = (ufk[16][0]+(.0260106*(gk[21][0]-Atkr[21][0]))); fstarr[21][2] = (ufk[16][2]+(.0260106*(gk[21][2]-Atkr[21][2]))); fstarr[22][0] = (ufk[17][0]+(.02601109*(gk[22][0]-Atkr[22][0]))); fstarr[22][2] = (ufk[17][2]+(.02601109*(gk[22][2]-Atkr[22][2]))); tstarr[5][0] = (utk[0][0]-(WkIkWk[5][0]+(((.48506516*udotin[3])-(5.312e-5* udotin[4]))-(.01450199*udotin[5])))); tstarr[5][1] = (utk[0][1]-(WkIkWk[5][1]+(((.47268161*udotin[4])-(5.312e-5* udotin[3]))-(.00031516*udotin[5])))); tstarr[5][2] = (utk[0][2]-(WkIkWk[5][2]+((.03863225*udotin[5])-((.00031516* udotin[4])+(.01450199*udotin[3]))))); tstarr[6][0] = (utk[1][0]-(WkIkWk[6][0]+(8e-8*udotin[6]))); tstarr[6][1] = (utk[1][1]-(.04205639*udotin[6])); tstarr[6][2] = (utk[1][2]-(WkIkWk[6][2]-(.00226939*udotin[6]))); tstarr[7][0] = (utk[2][0]-(WkIkWk[7][0]-(8e-8*udotin[7]))); tstarr[7][1] = (utk[2][1]-(.04207299*udotin[7])); tstarr[7][2] = (utk[2][2]-(WkIkWk[7][2]+(.00230647*udotin[7]))); tstarr[8][0] = (utk[3][0]-(WkIkWk[8][0]+((.00823665*Otkr[8][2])+((1.657e-5* Otkr[8][1])+(.52257967*Otkr[8][0]))))); tstarr[8][1] = (utk[3][1]-(WkIkWk[8][1]+(((1.657e-5*Otkr[8][0])+(.52474672* Otkr[8][1]))-(.00036016*Otkr[8][2])))); tstarr[8][2] = (utk[3][2]-(WkIkWk[8][2]+((.01799022*Otkr[8][2])+((.00823665* Otkr[8][0])-(.00036016*Otkr[8][1]))))); tstarr[9][0] = (utk[4][0]-(WkIkWk[9][0]+((.00223918*Otkr[9][2])+((.05459333* Otkr[9][0])-(5.918e-5*Otkr[9][1]))))); tstarr[9][1] = (utk[4][1]-(WkIkWk[9][1]+((.00147758*Otkr[9][2])+((.05477009* Otkr[9][1])-(5.918e-5*Otkr[9][0]))))); tstarr[9][2] = (utk[4][2]-(WkIkWk[9][2]+((.00343412*Otkr[9][2])+((.00147758* Otkr[9][1])+(.00223918*Otkr[9][0]))))); tstarr[10][0] = (utk[5][0]-(WkIkWk[10][0]+((.00224427*Otkr[10][2])+(( 6.279e-5*Otkr[10][1])+(.05476817*Otkr[10][0]))))); tstarr[10][1] = (utk[5][1]-(WkIkWk[10][1]+(((6.279e-5*Otkr[10][0])+( .05493593*Otkr[10][1]))-(.00156061*Otkr[10][2])))); tstarr[10][2] = (utk[5][2]-(WkIkWk[10][2]+((.00344556*Otkr[10][2])+(( .00224427*Otkr[10][0])-(.00156061*Otkr[10][1]))))); tstarr[11][0] = (utk[6][0]-(WkIkWk[11][0]+(((1.273e-5*Otkr[11][1])+( .00826684*Otkr[11][0]))-(.00026397*Otkr[11][2])))); tstarr[11][1] = (utk[6][1]-(WkIkWk[11][1]+((.00020104*Otkr[11][2])+(( 1.273e-5*Otkr[11][0])+(.00843352*Otkr[11][1]))))); tstarr[11][2] = (utk[6][2]-(WkIkWk[11][2]+((.00057599*Otkr[11][2])+(( .00020104*Otkr[11][1])-(.00026397*Otkr[11][0]))))); tstarr[12][0] = (utk[7][0]-(WkIkWk[12][0]+(((.00821833*Otkr[12][0])-( 1.283e-5*Otkr[12][1]))-(.00026918*Otkr[12][2])))); tstarr[12][1] = (utk[7][1]-(WkIkWk[12][1]+(((.00838445*Otkr[12][1])-( 1.283e-5*Otkr[12][0]))-(.00019836*Otkr[12][2])))); tstarr[12][2] = (utk[7][2]-(WkIkWk[12][2]+((.00057568*Otkr[12][2])-(( .00019836*Otkr[12][1])+(.00026918*Otkr[12][0]))))); tstarr[13][0] = (utk[8][0]-(WkIkWk[13][0]+((.00016083*Otkr[13][2])+(( .00031494*Otkr[13][0])-(1.886e-5*Otkr[13][1]))))); tstarr[13][1] = (utk[8][1]-(WkIkWk[13][1]+((4.29e-6*Otkr[13][2])+((.00112921 *Otkr[13][1])-(1.886e-5*Otkr[13][0]))))); tstarr[13][2] = (utk[8][2]-(WkIkWk[13][2]+((.00103272*Otkr[13][2])+((4.29e-6 *Otkr[13][1])+(.00016083*Otkr[13][0]))))); tstarr[14][0] = (utk[9][0]-(WkIkWk[14][0]+((.00016063*Otkr[14][2])+((1.71e-5 *Otkr[14][1])+(.00031249*Otkr[14][0]))))); tstarr[14][1] = (utk[9][1]-(WkIkWk[14][1]+(((1.71e-5*Otkr[14][0])+(.00112646 *Otkr[14][1]))-(4.13e-6*Otkr[14][2])))); tstarr[14][2] = (utk[9][2]-(WkIkWk[14][2]+((.00102891*Otkr[14][2])+(( .00016063*Otkr[14][0])-(4.13e-6*Otkr[14][1]))))); tstarr[15][0] = (utk[10][0]-(WkIkWk[15][0]+((.00098554*Otkr[15][2])+(( .03667794*Otkr[15][0])-(9.282e-5*Otkr[15][1]))))); tstarr[15][1] = (utk[10][1]-(WkIkWk[15][1]+((.00236551*Otkr[15][2])+(( .03636003*Otkr[15][1])-(9.282e-5*Otkr[15][0]))))); tstarr[15][2] = (utk[10][2]-(WkIkWk[15][2]+((.00072894*Otkr[15][2])+(( .00098554*Otkr[15][0])+(.00236551*Otkr[15][1]))))); tstarr[16][0] = (utk[11][0]-(WkIkWk[16][0]+((.00099076*Otkr[16][2])+(( 9.583e-5*Otkr[16][1])+(.03712787*Otkr[16][0]))))); tstarr[16][1] = (utk[11][1]-(WkIkWk[16][1]+(((9.583e-5*Otkr[16][0])+( .03679631*Otkr[16][1]))-(.00244367*Otkr[16][2])))); tstarr[16][2] = (utk[11][2]-(WkIkWk[16][2]+((.00074417*Otkr[16][2])+(( .00099076*Otkr[16][0])-(.00244367*Otkr[16][1]))))); tstarr[17][0] = (utk[12][0]-(WkIkWk[17][0]+((.00047536*Otkr[17][2])+(( .00933952*Otkr[17][0])-(2.97e-5*Otkr[17][1]))))); tstarr[17][1] = (utk[12][1]-(WkIkWk[17][1]+((.00105064*Otkr[17][2])+(( .00915719*Otkr[17][1])-(2.97e-5*Otkr[17][0]))))); tstarr[17][2] = (utk[12][2]-(WkIkWk[17][2]+((.00032059*Otkr[17][2])+(( .00047536*Otkr[17][0])+(.00105064*Otkr[17][1]))))); tstarr[18][0] = (utk[13][0]-(WkIkWk[18][0]+((.00047517*Otkr[18][2])+(( 3.024e-5*Otkr[18][1])+(.00933861*Otkr[18][0]))))); tstarr[18][1] = (utk[13][1]-(WkIkWk[18][1]+(((3.024e-5*Otkr[18][0])+( .00915219*Otkr[18][1]))-(.00106364*Otkr[18][2])))); tstarr[18][2] = (utk[13][2]-(WkIkWk[18][2]+((.00032466*Otkr[18][2])+(( .00047517*Otkr[18][0])-(.00106364*Otkr[18][1]))))); tstarr[19][0] = (utk[14][0]-(WkIkWk[19][0]+((7.331e-5*Otkr[19][2])+(( .00086458*Otkr[19][0])-(2.761e-5*Otkr[19][1]))))); tstarr[19][1] = (utk[14][1]-(WkIkWk[19][1]+(((.00082117*Otkr[19][1])-( 2.761e-5*Otkr[19][0]))-(.00020674*Otkr[19][2])))); tstarr[19][2] = (utk[14][2]-(WkIkWk[19][2]+((3.059e-5*Otkr[19][2])+(( 7.331e-5*Otkr[19][0])-(.00020674*Otkr[19][1]))))); tstarr[20][0] = (utk[15][0]-(WkIkWk[20][0]+((7.338e-5*Otkr[20][2])+((2.77e-5 *Otkr[20][1])+(.00086575*Otkr[20][0]))))); tstarr[20][1] = (utk[15][1]-(WkIkWk[20][1]+((.00020754*Otkr[20][2])+(( 2.77e-5*Otkr[20][0])+(.00082196*Otkr[20][1]))))); tstarr[20][2] = (utk[15][2]-(WkIkWk[20][2]+((3.093e-5*Otkr[20][2])+(( 7.338e-5*Otkr[20][0])+(.00020754*Otkr[20][1]))))); tstarr[21][0] = (utk[16][0]-(WkIkWk[21][0]+(1e-8*Otkr[21][1]))); tstarr[21][1] = (utk[16][1]-(.0004999*Otkr[21][1])); tstarr[21][2] = (utk[16][2]-(WkIkWk[21][2]-(.00013462*Otkr[21][1]))); tstarr[22][0] = (utk[17][0]-(WkIkWk[22][0]-(1e-8*Otkr[22][1]))); tstarr[22][1] = (utk[17][1]-(.00050087*Otkr[22][1])); tstarr[22][2] = (utk[17][2]-(WkIkWk[22][2]+(.00013693*Otkr[22][1]))); /* Now calculate the torques */ trqout[22] = -(utau[22]+(tstarr[22][1]-((5.194e-5*fstarr[22][2])+(.28317448* fstarr[22][0])))); Fstark[7][0] = (fstarr[7][0]+((fstarr[22][0]*c22)+(fstarr[22][2]*s22))); Fstark[7][1] = (ufk[2][1]+ufk[17][1]); Fstark[7][2] = (fstarr[7][2]+((fstarr[22][2]*c22)-(fstarr[22][0]*s22))); tvec1[0] = (tstarr[22][0]-((4.88000000000008e-6*fstarr[22][2])+(rik[22][2]* ufk[17][1]))); tvec1[1] = (tstarr[22][1]+((fstarr[22][0]*rik[22][2])-(fstarr[22][2]* rik[22][0]))); tvec1[2] = (tstarr[22][2]+((4.88000000000008e-6*fstarr[22][0])+(rik[22][0]* ufk[17][1]))); Tstark[7][0] = (tstarr[7][0]+((tvec1[0]*c22)+(tvec1[2]*s22))); Tstark[7][1] = (tstarr[7][1]+tvec1[1]); Tstark[7][2] = (tstarr[7][2]+((tvec1[2]*c22)-(tvec1[0]*s22))); trqout[21] = -(utau[21]+(tstarr[21][1]-((5.2e-5*fstarr[21][2])+(.28373605* fstarr[21][0])))); Fstark[6][0] = (fstarr[6][0]+((fstarr[21][0]*c21)+(fstarr[21][2]*s21))); Fstark[6][1] = (ufk[1][1]+ufk[16][1]); Fstark[6][2] = (fstarr[6][2]+((fstarr[21][2]*c21)-(fstarr[21][0]*s21))); tvec1[0] = (tstarr[21][0]-((5.26e-6*fstarr[21][2])+(rik[21][2]*ufk[16][1]))); tvec1[1] = (tstarr[21][1]+((fstarr[21][0]*rik[21][2])-(fstarr[21][2]* rik[21][0]))); tvec1[2] = (tstarr[21][2]+((5.26e-6*fstarr[21][0])+(rik[21][0]*ufk[16][1]))); Tstark[6][0] = (tstarr[6][0]+((tvec1[0]*c21)+(tvec1[2]*s21))); Tstark[6][1] = (tstarr[6][1]+tvec1[1]); Tstark[6][2] = (tstarr[6][2]+((tvec1[2]*c21)-(tvec1[0]*s21))); trqout[20] = -(utau[20]+(tstarr[20][1]+((.01382852*fstarr[20][0])-(.01361601 *fstarr[20][2])))); Fstark[18][0] = (fstarr[18][0]+((fstarr[20][0]*c20)+(fstarr[20][2]*s20))); Fstark[18][1] = (fstarr[18][1]+fstarr[20][1]); Fstark[18][2] = (fstarr[18][2]+((fstarr[20][2]*c20)-(fstarr[20][0]*s20))); tvec1[0] = (tstarr[20][0]+((.02885771*fstarr[20][2])-(fstarr[20][1]* rik[20][2]))); tvec1[1] = (tstarr[20][1]+((fstarr[20][0]*rik[20][2])-(fstarr[20][2]* rik[20][0]))); tvec1[2] = (tstarr[20][2]+((fstarr[20][1]*rik[20][0])-(.02885771* fstarr[20][0]))); Tstark[18][0] = (tstarr[18][0]+((tvec1[0]*c20)+(tvec1[2]*s20))); Tstark[18][1] = (tstarr[18][1]+tvec1[1]); Tstark[18][2] = (tstarr[18][2]+((tvec1[2]*c20)-(tvec1[0]*s20))); trqout[19] = -(utau[19]+(tstarr[19][1]+((.01387793*fstarr[19][0])-(.01360932 *fstarr[19][2])))); Fstark[17][0] = (fstarr[17][0]+((fstarr[19][0]*c19)+(fstarr[19][2]*s19))); Fstark[17][1] = (fstarr[17][1]+fstarr[19][1]); Fstark[17][2] = (fstarr[17][2]+((fstarr[19][2]*c19)-(fstarr[19][0]*s19))); tvec1[0] = (tstarr[19][0]-((.02941965*fstarr[19][2])+(fstarr[19][1]* rik[19][2]))); tvec1[1] = (tstarr[19][1]+((fstarr[19][0]*rik[19][2])-(fstarr[19][2]* rik[19][0]))); tvec1[2] = (tstarr[19][2]+((.02941965*fstarr[19][0])+(fstarr[19][1]* rik[19][0]))); Tstark[17][0] = (tstarr[17][0]+((tvec1[0]*c19)+(tvec1[2]*s19))); Tstark[17][1] = (tstarr[17][1]+tvec1[1]); Tstark[17][2] = (tstarr[17][2]+((tvec1[2]*c19)-(tvec1[0]*s19))); trqout[18] = -(utau[18]+(Tstark[18][1]-((.01407902*Fstark[18][2])+(.15260355 *Fstark[18][0])))); Fstark[16][0] = (fstarr[16][0]+((Fstark[18][0]*c18)+(Fstark[18][2]*s18))); Fstark[16][1] = (Fstark[18][1]+fstarr[16][1]); Fstark[16][2] = (fstarr[16][2]+((Fstark[18][2]*c18)-(Fstark[18][0]*s18))); tvec1[0] = (Tstark[18][0]+((.02678648*Fstark[18][2])-(Fstark[18][1]* rik[18][2]))); tvec1[1] = (Tstark[18][1]+((Fstark[18][0]*rik[18][2])-(Fstark[18][2]* rik[18][0]))); tvec1[2] = (Tstark[18][2]+((Fstark[18][1]*rik[18][0])-(.02678648* Fstark[18][0]))); Tstark[16][0] = (tstarr[16][0]+((tvec1[0]*c18)+(tvec1[2]*s18))); Tstark[16][1] = (tstarr[16][1]+tvec1[1]); Tstark[16][2] = (tstarr[16][2]+((tvec1[2]*c18)-(tvec1[0]*s18))); trqout[17] = -(utau[17]+(Tstark[17][1]-((.01407264*Fstark[17][2])+(.15249501 *Fstark[17][0])))); Fstark[15][0] = (fstarr[15][0]+((Fstark[17][0]*c17)+(Fstark[17][2]*s17))); Fstark[15][1] = (Fstark[17][1]+fstarr[15][1]); Fstark[15][2] = (fstarr[15][2]+((Fstark[17][2]*c17)-(Fstark[17][0]*s17))); tvec1[0] = (Tstark[17][0]-((.02658427*Fstark[17][2])+(Fstark[17][1]* rik[17][2]))); tvec1[1] = (Tstark[17][1]+((Fstark[17][0]*rik[17][2])-(Fstark[17][2]* rik[17][0]))); tvec1[2] = (Tstark[17][2]+((.02658427*Fstark[17][0])+(Fstark[17][1]* rik[17][0]))); Tstark[15][0] = (tstarr[15][0]+((tvec1[0]*c17)+(tvec1[2]*s17))); Tstark[15][1] = (tstarr[15][1]+tvec1[1]); Tstark[15][2] = (tstarr[15][2]+((tvec1[2]*c17)-(tvec1[0]*s17))); trqout[16] = -(utau[16]+(Tstark[16][1]-((.00305957*Fstark[16][2])+(.21008605 *Fstark[16][0])))); Fstark[5][0] = (fstarr[5][0]+((Fstark[16][0]*c16)+(Fstark[16][2]*s16))); Fstark[5][1] = (Fstark[16][1]+fstarr[5][1]); Fstark[5][2] = (fstarr[5][2]+((Fstark[16][2]*c16)-(Fstark[16][0]*s16))); tvec1[0] = (Tstark[16][0]+((.10875063*Fstark[16][2])-(Fstark[16][1]* rik[16][2]))); tvec1[1] = (Tstark[16][1]+((Fstark[16][0]*rik[16][2])-(Fstark[16][2]* rik[16][0]))); tvec1[2] = (Tstark[16][2]+((Fstark[16][1]*rik[16][0])-(.10875063* Fstark[16][0]))); Tstark[5][0] = (tstarr[5][0]+((tvec1[0]*c16)+(tvec1[2]*s16))); Tstark[5][1] = (tstarr[5][1]+tvec1[1]); Tstark[5][2] = (tstarr[5][2]+((tvec1[2]*c16)-(tvec1[0]*s16))); trqout[15] = -(utau[15]+(Tstark[15][1]-((.00307461*Fstark[15][2])+(.20905614 *Fstark[15][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[15][0]*c15)+(Fstark[15][2]*s15))); Fstark[5][1] = (Fstark[5][1]+Fstark[15][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[15][2]*c15)-(Fstark[15][0]*s15))); tvec1[0] = (Tstark[15][0]-((.11006521*Fstark[15][2])+(Fstark[15][1]* rik[15][2]))); tvec1[1] = (Tstark[15][1]+((Fstark[15][0]*rik[15][2])-(Fstark[15][2]* rik[15][0]))); tvec1[2] = (Tstark[15][2]+((.11006521*Fstark[15][0])+(Fstark[15][1]* rik[15][0]))); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c15)+(tvec1[2]*s15))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c15)-(tvec1[0]*s15))); trqout[14] = -(utau[14]+(tstarr[14][1]-((.03539679*fstarr[14][2])+(.03987912 *fstarr[14][0])))); Fstark[12][0] = (fstarr[12][0]+((fstarr[14][0]*c14)+(fstarr[14][2]*s14))); Fstark[12][1] = (fstarr[12][1]+fstarr[14][1]); Fstark[12][2] = (fstarr[12][2]+((fstarr[14][2]*c14)-(fstarr[14][0]*s14))); tvec1[0] = (tstarr[14][0]-((.00061605*fstarr[14][2])+(fstarr[14][1]* rik[14][2]))); tvec1[1] = (tstarr[14][1]+((fstarr[14][0]*rik[14][2])-(fstarr[14][2]* rik[14][0]))); tvec1[2] = (tstarr[14][2]+((.00061605*fstarr[14][0])+(fstarr[14][1]* rik[14][0]))); Tstark[12][0] = (tstarr[12][0]+((tvec1[0]*c14)+(tvec1[2]*s14))); Tstark[12][1] = (tstarr[12][1]+tvec1[1]); Tstark[12][2] = (tstarr[12][2]+((tvec1[2]*c14)-(tvec1[0]*s14))); trqout[13] = -(utau[13]+(tstarr[13][1]-((.03543467*fstarr[13][2])+(.03983908 *fstarr[13][0])))); Fstark[11][0] = (fstarr[11][0]+((fstarr[13][0]*c13)+(fstarr[13][2]*s13))); Fstark[11][1] = (fstarr[11][1]+fstarr[13][1]); Fstark[11][2] = (fstarr[11][2]+((fstarr[13][2]*c13)-(fstarr[13][0]*s13))); tvec1[0] = (tstarr[13][0]+((.0013196*fstarr[13][2])-(fstarr[13][1]* rik[13][2]))); tvec1[1] = (tstarr[13][1]+((fstarr[13][0]*rik[13][2])-(fstarr[13][2]* rik[13][0]))); tvec1[2] = (tstarr[13][2]+((fstarr[13][1]*rik[13][0])-(.0013196* fstarr[13][0]))); Tstark[11][0] = (tstarr[11][0]+((tvec1[0]*c13)+(tvec1[2]*s13))); Tstark[11][1] = (tstarr[11][1]+tvec1[1]); Tstark[11][2] = (tstarr[11][2]+((tvec1[2]*c13)-(tvec1[0]*s13))); trqout[12] = -(utau[12]+(Tstark[12][1]+((.01674373*Fstark[12][2])-(.12587504 *Fstark[12][0])))); Fstark[10][0] = (fstarr[10][0]+((Fstark[12][0]*c12)+(Fstark[12][2]*s12))); Fstark[10][1] = (Fstark[12][1]+fstarr[10][1]); Fstark[10][2] = (fstarr[10][2]+((Fstark[12][2]*c12)-(Fstark[12][0]*s12))); tvec1[0] = (Tstark[12][0]-((.0027842*Fstark[12][2])+(Fstark[12][1]* rik[12][2]))); tvec1[1] = (Tstark[12][1]+((Fstark[12][0]*rik[12][2])-(Fstark[12][2]* rik[12][0]))); tvec1[2] = (Tstark[12][2]+((.0027842*Fstark[12][0])+(Fstark[12][1]* rik[12][0]))); Tstark[10][0] = (tstarr[10][0]+((tvec1[0]*c12)+(tvec1[2]*s12))); Tstark[10][1] = (tstarr[10][1]+tvec1[1]); Tstark[10][2] = (tstarr[10][2]+((tvec1[2]*c12)-(tvec1[0]*s12))); trqout[11] = -(utau[11]+(Tstark[11][1]+((.01672166*Fstark[11][2])-(.12618299 *Fstark[11][0])))); Fstark[9][0] = (fstarr[9][0]+((Fstark[11][0]*c11)+(Fstark[11][2]*s11))); Fstark[9][1] = (Fstark[11][1]+fstarr[9][1]); Fstark[9][2] = (fstarr[9][2]+((Fstark[11][2]*c11)-(Fstark[11][0]*s11))); tvec1[0] = (Tstark[11][0]+((.00277012*Fstark[11][2])-(Fstark[11][1]* rik[11][2]))); tvec1[1] = (Tstark[11][1]+((Fstark[11][0]*rik[11][2])-(Fstark[11][2]* rik[11][0]))); tvec1[2] = (Tstark[11][2]+((Fstark[11][1]*rik[11][0])-(.00277012* Fstark[11][0]))); Tstark[9][0] = (tstarr[9][0]+((tvec1[0]*c11)+(tvec1[2]*s11))); Tstark[9][1] = (tstarr[9][1]+tvec1[1]); Tstark[9][2] = (tstarr[9][2]+((tvec1[2]*c11)-(tvec1[0]*s11))); trqout[10] = -(utau[10]+(Tstark[10][1]-((.00943639*Fstark[10][2])+(.13190398 *Fstark[10][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[10][0]*c10)+(Fstark[10][2]*s10))); Fstark[5][1] = (Fstark[5][1]+Fstark[10][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[10][2]*c10)-(Fstark[10][0]*s10))); tvec1[0] = (Tstark[10][0]+((.0481466*Fstark[10][2])-(Fstark[10][1]* rik[10][2]))); tvec1[1] = (Tstark[10][1]+((Fstark[10][0]*rik[10][2])-(Fstark[10][2]* rik[10][0]))); tvec1[2] = (Tstark[10][2]+((Fstark[10][1]*rik[10][0])-(.0481466* Fstark[10][0]))); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c10)+(tvec1[2]*s10))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c10)-(tvec1[0]*s10))); trqout[9] = -(utau[9]+(Tstark[9][1]-((.0094289*Fstark[9][2])+(.13168857* Fstark[9][0])))); Fstark[5][0] = (Fstark[5][0]+((Fstark[9][0]*c9)+(Fstark[9][2]*s9))); Fstark[5][1] = (Fstark[5][1]+Fstark[9][1]); Fstark[5][2] = (Fstark[5][2]+((Fstark[9][2]*c9)-(Fstark[9][0]*s9))); tvec1[0] = (Tstark[9][0]-((.04836741*Fstark[9][2])+(Fstark[9][1]*rik[9][2])) ); tvec1[1] = (Tstark[9][1]+((Fstark[9][0]*rik[9][2])-(Fstark[9][2]*rik[9][0])) ); tvec1[2] = (Tstark[9][2]+((.04836741*Fstark[9][0])+(Fstark[9][1]*rik[9][0])) ); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c9)+(tvec1[2]*s9))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c9)-(tvec1[0]*s9))); trqout[8] = -(utau[8]+(tstarr[8][1]-((.01691003*fstarr[8][2])+(.12000071* fstarr[8][0])))); Fstark[5][0] = (Fstark[5][0]+((fstarr[8][0]*c8)+(fstarr[8][2]*s8))); Fstark[5][1] = (Fstark[5][1]+fstarr[8][1]); Fstark[5][2] = (Fstark[5][2]+((fstarr[8][2]*c8)-(fstarr[8][0]*s8))); tvec1[0] = (tstarr[8][0]-((.00049034*fstarr[8][2])+(fstarr[8][1]*rik[8][2])) ); tvec1[1] = (tstarr[8][1]+((fstarr[8][0]*rik[8][2])-(fstarr[8][2]*rik[8][0])) ); tvec1[2] = (tstarr[8][2]+((.00049034*fstarr[8][0])+(fstarr[8][1]*rik[8][0])) ); Tstark[5][0] = (Tstark[5][0]+((tvec1[0]*c8)+(tvec1[2]*s8))); Tstark[5][1] = (Tstark[5][1]+tvec1[1]); Tstark[5][2] = (Tstark[5][2]+((tvec1[2]*c8)-(tvec1[0]*s8))); trqout[7] = -(utau[7]+(Tstark[7][1]-((5.197e-5*Fstark[7][2])+(1.22369868* Fstark[7][0])))); trqout[6] = -(utau[6]+(Tstark[6][1]-((5.201e-5*Fstark[6][2])+(1.2242604* Fstark[6][0])))); trqout[5] = -(Tstark[5][2]+utau[5]); Fstark[4][0] = Fstark[5][0]; Fstark[4][1] = Fstark[5][1]; Fstark[4][2] = Fstark[5][2]; Tstark[4][0] = Tstark[5][0]; Tstark[4][1] = Tstark[5][1]; Tstark[4][2] = Tstark[5][2]; trqout[4] = -(Tstark[4][1]+utau[4]); Fstark[3][0] = Fstark[4][0]; Fstark[3][1] = Fstark[4][1]; Fstark[3][2] = Fstark[4][2]; Tstark[3][0] = Tstark[4][0]; Tstark[3][1] = Tstark[4][1]; Tstark[3][2] = Tstark[4][2]; trqout[3] = -(Tstark[3][0]+utau[3]); Fstark[2][0] = ((Cik[3][0][2]*Fstark[3][2])+((Cik[3][0][0]*Fstark[3][0])+( Cik[3][0][1]*Fstark[3][1]))); Fstark[2][1] = ((Cik[3][1][2]*Fstark[3][2])+((Cik[3][1][0]*Fstark[3][0])+( Cik[3][1][1]*Fstark[3][1]))); Fstark[2][2] = ((Cik[3][2][2]*Fstark[3][2])+((Cik[3][2][0]*Fstark[3][0])+( Cik[3][2][1]*Fstark[3][1]))); Tstark[2][0] = ((Cik[3][0][2]*Tstark[3][2])+((Cik[3][0][0]*Tstark[3][0])+( Cik[3][0][1]*Tstark[3][1]))); Tstark[2][1] = ((Cik[3][1][2]*Tstark[3][2])+((Cik[3][1][0]*Tstark[3][0])+( Cik[3][1][1]*Tstark[3][1]))); Tstark[2][2] = ((Cik[3][2][2]*Tstark[3][2])+((Cik[3][2][0]*Tstark[3][0])+( Cik[3][2][1]*Tstark[3][1]))); trqout[2] = -(Fstark[2][2]+utau[2]); Fstark[1][0] = Fstark[2][0]; Fstark[1][1] = Fstark[2][1]; Fstark[1][2] = Fstark[2][2]; tvec1[0] = (Tstark[2][0]-(Fstark[2][1]*q[2])); tvec1[1] = (Tstark[2][1]+(Fstark[2][0]*q[2])); Tstark[1][0] = tvec1[0]; Tstark[1][1] = tvec1[1]; Tstark[1][2] = Tstark[2][2]; trqout[1] = -(Fstark[1][1]+utau[1]); Fstark[0][0] = Fstark[1][0]; Fstark[0][1] = Fstark[1][1]; Fstark[0][2] = Fstark[1][2]; tvec1[0] = (Tstark[1][0]+(Fstark[1][2]*q[1])); tvec1[2] = (Tstark[1][2]-(Fstark[1][0]*q[1])); Tstark[0][0] = tvec1[0]; Tstark[0][1] = Tstark[1][1]; Tstark[0][2] = tvec1[2]; trqout[0] = -(Fstark[0][0]+utau[0]); /* Op counts below do not include called subroutines */ /* Used 1.04 seconds CPU time, 0 additional bytes of memory. Equations contain 1061 adds/subtracts/negates 969 multiplies 0 divides 430 assignments */ } void BOCfly :: sdcomptrq ( double udotin[23], double trqout[23] ) { /* Compute hinge torques to produce these udots, ignoring constraints */ double multin[1]; if ((roustate != 2) && (roustate != 3)) { sdseterr(60,23); return; } sdfulltrq(udotin,multin,trqout); } void BOCfly :: sdmulttrq ( double multin[1], double trqout[23] ) { /* Compute hinge trqs which would be produced by these mults. */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(65,23); return; } for (i = 0; i < 23; i++) { trqout[i] = 0.; } } void BOCfly :: sdfs0 ( ) { /* Compute Fs (ignoring multiplier forces) */ fs[0] = fs0[0]; fs[1] = fs0[1]; fs[2] = fs0[2]; fs[3] = fs0[3]; fs[4] = fs0[4]; fs[5] = fs0[5]; fs[6] = fs0[6]; fs[7] = fs0[7]; fs[8] = fs0[8]; fs[9] = fs0[9]; fs[10] = fs0[10]; fs[11] = fs0[11]; fs[12] = fs0[12]; fs[13] = fs0[13]; fs[14] = fs0[14]; fs[15] = fs0[15]; fs[16] = fs0[16]; fs[17] = fs0[17]; fs[18] = fs0[18]; fs[19] = fs0[19]; fs[20] = fs0[20]; fs[21] = fs0[21]; fs[22] = fs0[22]; /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 23 assignments */ } void BOCfly :: sdfsmult ( ) { int i; /* Compute Fs (multiplier-generated forces only) */ for (i = 0; i < 23; i++) { fs[i] = 0.; } /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 23 assignments */ } void BOCfly :: sdfsfull ( ) { /* Compute Fs (including all forces) */ sdfsmult(); fs[0] = (fs[0]+fs0[0]); fs[1] = (fs[1]+fs0[1]); fs[2] = (fs[2]+fs0[2]); fs[3] = (fs[3]+fs0[3]); fs[4] = (fs[4]+fs0[4]); fs[5] = (fs[5]+fs0[5]); fs[6] = (fs[6]+fs0[6]); fs[7] = (fs[7]+fs0[7]); fs[8] = (fs[8]+fs0[8]); fs[9] = (fs[9]+fs0[9]); fs[10] = (fs[10]+fs0[10]); fs[11] = (fs[11]+fs0[11]); fs[12] = (fs[12]+fs0[12]); fs[13] = (fs[13]+fs0[13]); fs[14] = (fs[14]+fs0[14]); fs[15] = (fs[15]+fs0[15]); fs[16] = (fs[16]+fs0[16]); fs[17] = (fs[17]+fs0[17]); fs[18] = (fs[18]+fs0[18]); fs[19] = (fs[19]+fs0[19]); fs[20] = (fs[20]+fs0[20]); fs[21] = (fs[21]+fs0[21]); fs[22] = (fs[22]+fs0[22]); /* Used 0.01 seconds CPU time, 0 additional bytes of memory. Equations contain 23 adds/subtracts/negates 0 multiplies 0 divides 23 assignments */ } void BOCfly :: sdfsgenmult ( ) { int i; /* Compute Fs (generic multiplier-generated forces) */ for (i = 0; i < 23; i++) { fs[i] = 0.; } /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 23 assignments */ } void BOCfly :: sdfsgenfull ( ) { /* Compute Fs (incl generic mult & other forces) */ sdfsgenmult(); fs[0] = (fs[0]+fs0[0]); fs[1] = (fs[1]+fs0[1]); fs[2] = (fs[2]+fs0[2]); fs[3] = (fs[3]+fs0[3]); fs[4] = (fs[4]+fs0[4]); fs[5] = (fs[5]+fs0[5]); fs[6] = (fs[6]+fs0[6]); fs[7] = (fs[7]+fs0[7]); fs[8] = (fs[8]+fs0[8]); fs[9] = (fs[9]+fs0[9]); fs[10] = (fs[10]+fs0[10]); fs[11] = (fs[11]+fs0[11]); fs[12] = (fs[12]+fs0[12]); fs[13] = (fs[13]+fs0[13]); fs[14] = (fs[14]+fs0[14]); fs[15] = (fs[15]+fs0[15]); fs[16] = (fs[16]+fs0[16]); fs[17] = (fs[17]+fs0[17]); fs[18] = (fs[18]+fs0[18]); fs[19] = (fs[19]+fs0[19]); fs[20] = (fs[20]+fs0[20]); fs[21] = (fs[21]+fs0[21]); fs[22] = (fs[22]+fs0[22]); /* Used 0.01 seconds CPU time, 0 additional bytes of memory. Equations contain 23 adds/subtracts/negates 0 multiplies 0 divides 23 assignments */ } void BOCfly :: sdrhs ( ) { /* Generated 27-Mar-2000 00:59:01 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 69086664 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 */ /* Compute hinge torques for tree hinges */ tauc[0] = utau[0]; tauc[1] = utau[1]; tauc[2] = utau[2]; tauc[3] = utau[3]; tauc[4] = utau[4]; tauc[5] = utau[5]; tauc[6] = utau[6]; tauc[7] = utau[7]; tauc[8] = utau[8]; tauc[9] = utau[9]; tauc[10] = utau[10]; tauc[11] = utau[11]; tauc[12] = utau[12]; tauc[13] = utau[13]; tauc[14] = utau[14]; tauc[15] = utau[15]; tauc[16] = utau[16]; tauc[17] = utau[17]; tauc[18] = utau[18]; tauc[19] = utau[19]; tauc[20] = utau[20]; tauc[21] = utau[21]; tauc[22] = utau[22]; sddoiner(); /* Compute onk & onb (angular accels in N) */ Onkb[8][0] = ((udot[3]*c8)-(udot[5]*s8)); Onkb[8][1] = (udot[4]+udot[8]); Onkb[8][2] = ((udot[3]*s8)+(udot[5]*c8)); Onkb[9][0] = ((udot[3]*c9)-(udot[5]*s9)); Onkb[9][1] = (udot[4]+udot[9]); Onkb[9][2] = ((udot[3]*s9)+(udot[5]*c9)); Onkb[10][0] = ((udot[3]*c10)-(udot[5]*s10)); Onkb[10][1] = (udot[4]+udot[10]); Onkb[10][2] = ((udot[3]*s10)+(udot[5]*c10)); Onkb[11][0] = ((Onkb[9][0]*c11)-(Onkb[9][2]*s11)); Onkb[11][1] = (Onkb[9][1]+udot[11]); Onkb[11][2] = ((Onkb[9][0]*s11)+(Onkb[9][2]*c11)); Onkb[12][0] = ((Onkb[10][0]*c12)-(Onkb[10][2]*s12)); Onkb[12][1] = (Onkb[10][1]+udot[12]); Onkb[12][2] = ((Onkb[10][0]*s12)+(Onkb[10][2]*c12)); Onkb[13][0] = ((Onkb[11][0]*c13)-(Onkb[11][2]*s13)); Onkb[13][1] = (Onkb[11][1]+udot[13]); Onkb[13][2] = ((Onkb[11][0]*s13)+(Onkb[11][2]*c13)); Onkb[14][0] = ((Onkb[12][0]*c14)-(Onkb[12][2]*s14)); Onkb[14][1] = (Onkb[12][1]+udot[14]); Onkb[14][2] = ((Onkb[12][0]*s14)+(Onkb[12][2]*c14)); Onkb[15][0] = ((udot[3]*c15)-(udot[5]*s15)); Onkb[15][1] = (udot[4]+udot[15]); Onkb[15][2] = ((udot[3]*s15)+(udot[5]*c15)); Onkb[16][0] = ((udot[3]*c16)-(udot[5]*s16)); Onkb[16][1] = (udot[4]+udot[16]); Onkb[16][2] = ((udot[3]*s16)+(udot[5]*c16)); Onkb[17][0] = ((Onkb[15][0]*c17)-(Onkb[15][2]*s17)); Onkb[17][1] = (Onkb[15][1]+udot[17]); Onkb[17][2] = ((Onkb[15][0]*s17)+(Onkb[15][2]*c17)); Onkb[18][0] = ((Onkb[16][0]*c18)-(Onkb[16][2]*s18)); Onkb[18][1] = (Onkb[16][1]+udot[18]); Onkb[18][2] = ((Onkb[16][0]*s18)+(Onkb[16][2]*c18)); Onkb[19][0] = ((Onkb[17][0]*c19)-(Onkb[17][2]*s19)); Onkb[19][1] = (Onkb[17][1]+udot[19]); Onkb[19][2] = ((Onkb[17][0]*s19)+(Onkb[17][2]*c19)); Onkb[20][0] = ((Onkb[18][0]*c20)-(Onkb[18][2]*s20)); Onkb[20][1] = (Onkb[18][1]+udot[20]); Onkb[20][2] = ((Onkb[18][0]*s20)+(Onkb[18][2]*c20)); Onkb[21][1] = (udot[6]+udot[21]); Onkb[22][1] = (udot[7]+udot[22]); onk[8][0] = (Onkb[8][0]+Otk[8][0]); onk[8][2] = (Onkb[8][2]+Otk[8][2]); onk[9][0] = (Onkb[9][0]+Otk[9][0]); onk[9][2] = (Onkb[9][2]+Otk[9][2]); onk[10][0] = (Onkb[10][0]+Otk[10][0]); onk[10][2] = (Onkb[10][2]+Otk[10][2]); onk[11][0] = (Onkb[11][0]+Otk[11][0]); onk[11][2] = (Onkb[11][2]+Otk[11][2]); onk[12][0] = (Onkb[12][0]+Otk[12][0]); onk[12][2] = (Onkb[12][2]+Otk[12][2]); onk[13][0] = (Onkb[13][0]+Otk[13][0]); onk[13][2] = (Onkb[13][2]+Otk[13][2]); onk[14][0] = (Onkb[14][0]+Otk[14][0]); onk[14][2] = (Onkb[14][2]+Otk[14][2]); onk[15][0] = (Onkb[15][0]+Otk[15][0]); onk[15][2] = (Onkb[15][2]+Otk[15][2]); onk[16][0] = (Onkb[16][0]+Otk[16][0]); onk[16][2] = (Onkb[16][2]+Otk[16][2]); onk[17][0] = (Onkb[17][0]+Otk[17][0]); onk[17][2] = (Onkb[17][2]+Otk[17][2]); onk[18][0] = (Onkb[18][0]+Otk[18][0]); onk[18][2] = (Onkb[18][2]+Otk[18][2]); onk[19][0] = (Onkb[19][0]+Otk[19][0]); onk[19][2] = (Onkb[19][2]+Otk[19][2]); onk[20][0] = (Onkb[20][0]+Otk[20][0]); onk[20][2] = (Onkb[20][2]+Otk[20][2]); onb[0][0] = udot[3]; onb[0][1] = udot[4]; onb[0][2] = udot[5]; onb[1][0] = 0.; onb[1][1] = udot[6]; onb[1][2] = 0.; onb[2][0] = 0.; onb[2][1] = udot[7]; onb[2][2] = 0.; onb[3][0] = onk[8][0]; onb[3][1] = Onkb[8][1]; onb[3][2] = onk[8][2]; onb[4][0] = onk[9][0]; onb[4][1] = Onkb[9][1]; onb[4][2] = onk[9][2]; onb[5][0] = onk[10][0]; onb[5][1] = Onkb[10][1]; onb[5][2] = onk[10][2]; onb[6][0] = onk[11][0]; onb[6][1] = Onkb[11][1]; onb[6][2] = onk[11][2]; onb[7][0] = onk[12][0]; onb[7][1] = Onkb[12][1]; onb[7][2] = onk[12][2]; onb[8][0] = onk[13][0]; onb[8][1] = Onkb[13][1]; onb[8][2] = onk[13][2]; onb[9][0] = onk[14][0]; onb[9][1] = Onkb[14][1]; onb[9][2] = onk[14][2]; onb[10][0] = onk[15][0]; onb[10][1] = Onkb[15][1]; onb[10][2] = onk[15][2]; onb[11][0] = onk[16][0]; onb[11][1] = Onkb[16][1]; onb[11][2] = onk[16][2]; onb[12][0] = onk[17][0]; onb[12][1] = Onkb[17][1]; onb[12][2] = onk[17][2]; onb[13][0] = onk[18][0]; onb[13][1] = Onkb[18][1]; onb[13][2] = onk[18][2]; onb[14][0] = onk[19][0]; onb[14][1] = Onkb[19][1]; onb[14][2] = onk[19][2]; onb[15][0] = onk[20][0]; onb[15][1] = Onkb[20][1]; onb[15][2] = onk[20][2]; onb[16][0] = 0.; onb[16][1] = Onkb[21][1]; onb[16][2] = 0.; onb[17][0] = 0.; onb[17][1] = Onkb[22][1]; onb[17][2] = 0.; /* Compute acceleration dyadics */ dyad[0][0][0] = w11w22[0]; dyad[0][0][1] = (w0w1[0]-udot[5]); dyad[0][0][2] = (udot[4]+w0w2[0]); dyad[0][1][0] = (udot[5]+w0w1[0]); dyad[0][1][1] = w00w22[0]; dyad[0][1][2] = (w1w2[0]-udot[3]); dyad[0][2][0] = (w0w2[0]-udot[4]); dyad[0][2][1] = (udot[3]+w1w2[0]); dyad[0][2][2] = w00w11[0]; dyad[1][0][0] = -w1w1[1]; dyad[1][0][1] = 0.; dyad[1][0][2] = udot[6]; dyad[1][1][0] = 0.; dyad[1][1][1] = 0.; dyad[1][1][2] = 0.; dyad[1][2][0] = -udot[6]; dyad[1][2][1] = 0.; dyad[1][2][2] = -w1w1[1]; dyad[2][0][0] = -w1w1[2]; dyad[2][0][1] = 0.; dyad[2][0][2] = udot[7]; dyad[2][1][0] = 0.; dyad[2][1][1] = 0.; dyad[2][1][2] = 0.; dyad[2][2][0] = -udot[7]; dyad[2][2][1] = 0.; dyad[2][2][2] = -w1w1[2]; dyad[3][0][0] = w11w22[3]; dyad[3][0][1] = (w0w1[3]-onk[8][2]); dyad[3][0][2] = (Onkb[8][1]+w0w2[3]); dyad[3][1][0] = (onk[8][2]+w0w1[3]); dyad[3][1][1] = w00w22[3]; dyad[3][1][2] = (w1w2[3]-onk[8][0]); dyad[3][2][0] = (w0w2[3]-Onkb[8][1]); dyad[3][2][1] = (onk[8][0]+w1w2[3]); dyad[3][2][2] = w00w11[3]; dyad[4][0][0] = w11w22[4]; dyad[4][0][1] = (w0w1[4]-onk[9][2]); dyad[4][0][2] = (Onkb[9][1]+w0w2[4]); dyad[4][1][0] = (onk[9][2]+w0w1[4]); dyad[4][1][1] = w00w22[4]; dyad[4][1][2] = (w1w2[4]-onk[9][0]); dyad[4][2][0] = (w0w2[4]-Onkb[9][1]); dyad[4][2][1] = (onk[9][0]+w1w2[4]); dyad[4][2][2] = w00w11[4]; dyad[5][0][0] = w11w22[5]; dyad[5][0][1] = (w0w1[5]-onk[10][2]); dyad[5][0][2] = (Onkb[10][1]+w0w2[5]); dyad[5][1][0] = (onk[10][2]+w0w1[5]); dyad[5][1][1] = w00w22[5]; dyad[5][1][2] = (w1w2[5]-onk[10][0]); dyad[5][2][0] = (w0w2[5]-Onkb[10][1]); dyad[5][2][1] = (onk[10][0]+w1w2[5]); dyad[5][2][2] = w00w11[5]; dyad[6][0][0] = w11w22[6]; dyad[6][0][1] = (w0w1[6]-onk[11][2]); dyad[6][0][2] = (Onkb[11][1]+w0w2[6]); dyad[6][1][0] = (onk[11][2]+w0w1[6]); dyad[6][1][1] = w00w22[6]; dyad[6][1][2] = (w1w2[6]-onk[11][0]); dyad[6][2][0] = (w0w2[6]-Onkb[11][1]); dyad[6][2][1] = (onk[11][0]+w1w2[6]); dyad[6][2][2] = w00w11[6]; dyad[7][0][0] = w11w22[7]; dyad[7][0][1] = (w0w1[7]-onk[12][2]); dyad[7][0][2] = (Onkb[12][1]+w0w2[7]); dyad[7][1][0] = (onk[12][2]+w0w1[7]); dyad[7][1][1] = w00w22[7]; dyad[7][1][2] = (w1w2[7]-onk[12][0]); dyad[7][2][0] = (w0w2[7]-Onkb[12][1]); dyad[7][2][1] = (onk[12][0]+w1w2[7]); dyad[7][2][2] = w00w11[7]; dyad[8][0][0] = w11w22[8]; dyad[8][0][1] = (w0w1[8]-onk[13][2]); dyad[8][0][2] = (Onkb[13][1]+w0w2[8]); dyad[8][1][0] = (onk[13][2]+w0w1[8]); dyad[8][1][1] = w00w22[8]; dyad[8][1][2] = (w1w2[8]-onk[13][0]); dyad[8][2][0] = (w0w2[8]-Onkb[13][1]); dyad[8][2][1] = (onk[13][0]+w1w2[8]); dyad[8][2][2] = w00w11[8]; dyad[9][0][0] = w11w22[9]; dyad[9][0][1] = (w0w1[9]-onk[14][2]); dyad[9][0][2] = (Onkb[14][1]+w0w2[9]); dyad[9][1][0] = (onk[14][2]+w0w1[9]); dyad[9][1][1] = w00w22[9]; dyad[9][1][2] = (w1w2[9]-onk[14][0]); dyad[9][2][0] = (w0w2[9]-Onkb[14][1]); dyad[9][2][1] = (onk[14][0]+w1w2[9]); dyad[9][2][2] = w00w11[9]; dyad[10][0][0] = w11w22[10]; dyad[10][0][1] = (w0w1[10]-onk[15][2]); dyad[10][0][2] = (Onkb[15][1]+w0w2[10]); dyad[10][1][0] = (onk[15][2]+w0w1[10]); dyad[10][1][1] = w00w22[10]; dyad[10][1][2] = (w1w2[10]-onk[15][0]); dyad[10][2][0] = (w0w2[10]-Onkb[15][1]); dyad[10][2][1] = (onk[15][0]+w1w2[10]); dyad[10][2][2] = w00w11[10]; dyad[11][0][0] = w11w22[11]; dyad[11][0][1] = (w0w1[11]-onk[16][2]); dyad[11][0][2] = (Onkb[16][1]+w0w2[11]); dyad[11][1][0] = (onk[16][2]+w0w1[11]); dyad[11][1][1] = w00w22[11]; dyad[11][1][2] = (w1w2[11]-onk[16][0]); dyad[11][2][0] = (w0w2[11]-Onkb[16][1]); dyad[11][2][1] = (onk[16][0]+w1w2[11]); dyad[11][2][2] = w00w11[11]; dyad[12][0][0] = w11w22[12]; dyad[12][0][1] = (w0w1[12]-onk[17][2]); dyad[12][0][2] = (Onkb[17][1]+w0w2[12]); dyad[12][1][0] = (onk[17][2]+w0w1[12]); dyad[12][1][1] = w00w22[12]; dyad[12][1][2] = (w1w2[12]-onk[17][0]); dyad[12][2][0] = (w0w2[12]-Onkb[17][1]); dyad[12][2][1] = (onk[17][0]+w1w2[12]); dyad[12][2][2] = w00w11[12]; dyad[13][0][0] = w11w22[13]; dyad[13][0][1] = (w0w1[13]-onk[18][2]); dyad[13][0][2] = (Onkb[18][1]+w0w2[13]); dyad[13][1][0] = (onk[18][2]+w0w1[13]); dyad[13][1][1] = w00w22[13]; dyad[13][1][2] = (w1w2[13]-onk[18][0]); dyad[13][2][0] = (w0w2[13]-Onkb[18][1]); dyad[13][2][1] = (onk[18][0]+w1w2[13]); dyad[13][2][2] = w00w11[13]; dyad[14][0][0] = w11w22[14]; dyad[14][0][1] = (w0w1[14]-onk[19][2]); dyad[14][0][2] = (Onkb[19][1]+w0w2[14]); dyad[14][1][0] = (onk[19][2]+w0w1[14]); dyad[14][1][1] = w00w22[14]; dyad[14][1][2] = (w1w2[14]-onk[19][0]); dyad[14][2][0] = (w0w2[14]-Onkb[19][1]); dyad[14][2][1] = (onk[19][0]+w1w2[14]); dyad[14][2][2] = w00w11[14]; dyad[15][0][0] = w11w22[15]; dyad[15][0][1] = (w0w1[15]-onk[20][2]); dyad[15][0][2] = (Onkb[20][1]+w0w2[15]); dyad[15][1][0] = (onk[20][2]+w0w1[15]); dyad[15][1][1] = w00w22[15]; dyad[15][1][2] = (w1w2[15]-onk[20][0]); dyad[15][2][0] = (w0w2[15]-Onkb[20][1]); dyad[15][2][1] = (onk[20][0]+w1w2[15]); dyad[15][2][2] = w00w11[15]; dyad[16][0][0] = -w1w1[16]; dyad[16][0][1] = 0.; dyad[16][0][2] = Onkb[21][1]; dyad[16][1][0] = 0.; dyad[16][1][1] = 0.; dyad[16][1][2] = 0.; dyad[16][2][0] = -Onkb[21][1]; dyad[16][2][1] = 0.; dyad[16][2][2] = -w1w1[16]; dyad[17][0][0] = -w1w1[17]; dyad[17][0][1] = 0.; dyad[17][0][2] = Onkb[22][1]; dyad[17][1][0] = 0.; dyad[17][1][1] = 0.; dyad[17][1][2] = 0.; dyad[17][2][0] = -Onkb[22][1]; dyad[17][2][1] = 0.; dyad[17][2][2] = -w1w1[17]; /* Compute ank & anb (mass center linear accels in N) */ Ankb[3][0] = ((Cik[3][2][0]*udot[2])+((Cik[3][0][0]*udot[0])+(Cik[3][1][0]* udot[1]))); Ankb[3][1] = ((Cik[3][2][1]*udot[2])+((Cik[3][0][1]*udot[0])+(Cik[3][1][1]* udot[1]))); Ankb[3][2] = ((Cik[3][2][2]*udot[2])+((Cik[3][0][2]*udot[0])+(Cik[3][1][2]* udot[1]))); AOnkri[8][0] = (Ankb[3][0]+((.00120864*udot[5])+(.3829286*udot[4]))); AOnkri[8][1] = (Ankb[3][1]-((.00252618*udot[5])+(.3829286*udot[3]))); AOnkri[8][2] = (Ankb[3][2]+((.00252618*udot[4])-(.00120864*udot[3]))); Ankb[8][0] = (((AOnkri[8][0]*c8)-(AOnkri[8][2]*s8))-((.0007183*Onkb[8][2])+( .12000071*Onkb[8][1]))); Ankb[8][1] = (AOnkri[8][1]+((.01691003*Onkb[8][2])+(.12000071*Onkb[8][0]))); Ankb[8][2] = (((.0007183*Onkb[8][0])-(.01691003*Onkb[8][1]))+((AOnkri[8][0]* s8)+(AOnkri[8][2]*c8))); AOnkri[9][0] = (Ankb[3][0]+((.04820864*udot[5])-(.0620714*udot[4]))); AOnkri[9][1] = (Ankb[3][1]+((.0620714*udot[3])-(.01952618*udot[5]))); AOnkri[9][2] = (Ankb[3][2]+((.01952618*udot[4])-(.04820864*udot[3]))); Ankb[9][0] = (((.00015877*Onkb[9][2])-(.13168857*Onkb[9][1]))+((AOnkri[9][0] *c9)-(AOnkri[9][2]*s9))); Ankb[9][1] = (AOnkri[9][1]+((.0094289*Onkb[9][2])+(.13168857*Onkb[9][0]))); Ankb[9][2] = (((AOnkri[9][0]*s9)+(AOnkri[9][2]*c9))-((.00015877*Onkb[9][0])+ (.0094289*Onkb[9][1]))); AOnkri[10][0] = (Ankb[3][0]-((.04579136*udot[5])+(.0620714*udot[4]))); AOnkri[10][1] = (Ankb[3][1]+((.0620714*udot[3])-(.01952618*udot[5]))); AOnkri[10][2] = (Ankb[3][2]+((.01952618*udot[4])+(.04579136*udot[3]))); Ankb[10][0] = (((AOnkri[10][0]*c10)-(AOnkri[10][2]*s10))-((.00235524* Onkb[10][2])+(.13190398*Onkb[10][1]))); Ankb[10][1] = (AOnkri[10][1]+((.00943639*Onkb[10][2])+(.13190398*Onkb[10][0] ))); Ankb[10][2] = (((.00235524*Onkb[10][0])-(.00943639*Onkb[10][1]))+(( AOnkri[10][0]*s10)+(AOnkri[10][2]*c10))); AOnkri[11][0] = (Ankb[9][0]+((.00284123*Onkb[9][2])-(.15931143*Onkb[9][1]))); AOnkri[11][1] = (Ankb[9][1]+((.0065711*Onkb[9][2])+(.15931143*Onkb[9][0]))); AOnkri[11][2] = (Ankb[9][2]-((.00284123*Onkb[9][0])+(.0065711*Onkb[9][1]))); Ankb[11][0] = (((AOnkri[11][0]*c11)-(AOnkri[11][2]*s11))-((.00561135* Onkb[11][2])+(.12618299*Onkb[11][1]))); Ankb[11][1] = (AOnkri[11][1]+((.12618299*Onkb[11][0])-(.01672166*Onkb[11][2] ))); Ankb[11][2] = (((.00561135*Onkb[11][0])+(.01672166*Onkb[11][1]))+(( AOnkri[11][0]*s11)+(AOnkri[11][2]*c11))); AOnkri[12][0] = (Ankb[10][0]-((.00064476*Onkb[10][2])+(.15909602*Onkb[10][1] ))); AOnkri[12][1] = (Ankb[10][1]+((.00656361*Onkb[10][2])+(.15909602*Onkb[10][0] ))); AOnkri[12][2] = (Ankb[10][2]+((.00064476*Onkb[10][0])-(.00656361*Onkb[10][1] ))); Ankb[12][0] = (((.00342896*Onkb[12][2])-(.12587504*Onkb[12][1]))+(( AOnkri[12][0]*c12)-(AOnkri[12][2]*s12))); Ankb[12][1] = (AOnkri[12][1]+((.12587504*Onkb[12][0])-(.01674373*Onkb[12][2] ))); Ankb[12][2] = (((.01674373*Onkb[12][1])-(.00342896*Onkb[12][0]))+(( AOnkri[12][0]*s12)+(AOnkri[12][2]*c12))); AOnkri[13][0] = (Ankb[11][0]+((.00061135*Onkb[11][2])-(.17081701*Onkb[11][1] ))); AOnkri[13][1] = (Ankb[11][1]+((.00172166*Onkb[11][2])+(.17081701*Onkb[11][0] ))); AOnkri[13][2] = (Ankb[11][2]-((.00061135*Onkb[11][0])+(.00172166*Onkb[11][1] ))); Ankb[13][0] = (((AOnkri[13][0]*c13)-(AOnkri[13][2]*s13))-((.00193095* Onkb[13][2])+(.03983908*Onkb[13][1]))); Ankb[13][1] = (AOnkri[13][1]+((.03543467*Onkb[13][2])+(.03983908*Onkb[13][0] ))); Ankb[13][2] = (((.00193095*Onkb[13][0])-(.03543467*Onkb[13][1]))+(( AOnkri[13][0]*s13)+(AOnkri[13][2]*c13))); AOnkri[14][0] = (Ankb[12][0]+((.00157104*Onkb[12][2])-(.17112496*Onkb[12][1] ))); AOnkri[14][1] = (Ankb[12][1]+((.00174373*Onkb[12][2])+(.17112496*Onkb[12][0] ))); AOnkri[14][2] = (Ankb[12][2]-((.00157104*Onkb[12][0])+(.00174373*Onkb[12][1] ))); Ankb[14][0] = (((AOnkri[14][0]*c14)-(AOnkri[14][2]*s14))-((.00095499* Onkb[14][2])+(.03987912*Onkb[14][1]))); Ankb[14][1] = (AOnkri[14][1]+((.03539679*Onkb[14][2])+(.03987912*Onkb[14][0] ))); Ankb[14][2] = (((.00095499*Onkb[14][0])-(.03539679*Onkb[14][1]))+(( AOnkri[14][0]*s14)+(AOnkri[14][2]*c14))); AOnkri[15][0] = (Ankb[3][0]+((.10120864*udot[5])+(.2639286*udot[4]))); AOnkri[15][1] = (Ankb[3][1]-((.01052618*udot[5])+(.2639286*udot[3]))); AOnkri[15][2] = (Ankb[3][2]+((.01052618*udot[4])-(.10120864*udot[3]))); Ankb[15][0] = (((.00885657*Onkb[15][2])-(.20905614*Onkb[15][1]))+(( AOnkri[15][0]*c15)-(AOnkri[15][2]*s15))); Ankb[15][1] = (AOnkri[15][1]+((.00307461*Onkb[15][2])+(.20905614*Onkb[15][0] ))); Ankb[15][2] = (((AOnkri[15][0]*s15)+(AOnkri[15][2]*c15))-((.00307461* Onkb[15][1])+(.00885657*Onkb[15][0]))); AOnkri[16][0] = (Ankb[3][0]+((.2639286*udot[4])-(.09879136*udot[5]))); AOnkri[16][1] = (Ankb[3][1]-((.01052618*udot[5])+(.2639286*udot[3]))); AOnkri[16][2] = (Ankb[3][2]+((.01052618*udot[4])+(.09879136*udot[3]))); Ankb[16][0] = (((AOnkri[16][0]*c16)-(AOnkri[16][2]*s16))-((.00995927* Onkb[16][2])+(.21008605*Onkb[16][1]))); Ankb[16][1] = (AOnkri[16][1]+((.00305957*Onkb[16][2])+(.21008605*Onkb[16][0] ))); Ankb[16][2] = (((.00995927*Onkb[16][0])-(.00305957*Onkb[16][1]))+(( AOnkri[16][0]*s16)+(AOnkri[16][2]*c16))); AOnkri[17][0] = (Ankb[15][0]+((.01305614*Onkb[15][1])+(.02714343*Onkb[15][2] ))); AOnkri[17][1] = (Ankb[15][1]-((.00307461*Onkb[15][2])+(.01305614*Onkb[15][0] ))); AOnkri[17][2] = (Ankb[15][2]+((.00307461*Onkb[15][1])-(.02714343*Onkb[15][0] ))); Ankb[17][0] = (((AOnkri[17][0]*c17)-(AOnkri[17][2]*s17))-((.00055916* Onkb[17][2])+(.15249501*Onkb[17][1]))); Ankb[17][1] = (AOnkri[17][1]+((.01407264*Onkb[17][2])+(.15249501*Onkb[17][0] ))); Ankb[17][2] = (((.00055916*Onkb[17][0])-(.01407264*Onkb[17][1]))+(( AOnkri[17][0]*s17)+(AOnkri[17][2]*c17))); AOnkri[18][0] = (Ankb[16][0]+((.01408605*Onkb[16][1])-(.02604073*Onkb[16][2] ))); AOnkri[18][1] = (Ankb[16][1]-((.00305957*Onkb[16][2])+(.01408605*Onkb[16][0] ))); AOnkri[18][2] = (Ankb[16][2]+((.00305957*Onkb[16][1])+(.02604073*Onkb[16][0] ))); Ankb[18][0] = (((AOnkri[18][0]*c18)-(AOnkri[18][2]*s18))-((.00074575* Onkb[18][2])+(.15260355*Onkb[18][1]))); Ankb[18][1] = (AOnkri[18][1]+((.01407902*Onkb[18][2])+(.15260355*Onkb[18][0] ))); Ankb[18][2] = (((.00074575*Onkb[18][0])-(.01407902*Onkb[18][1]))+(( AOnkri[18][0]*s18)+(AOnkri[18][2]*c18))); AOnkri[19][0] = (Ankb[17][0]+((.02455916*Onkb[17][2])-(.01750499*Onkb[17][1] ))); AOnkri[19][1] = (Ankb[17][1]+((.00792736*Onkb[17][2])+(.01750499*Onkb[17][0] ))); AOnkri[19][2] = (Ankb[17][2]-((.00792736*Onkb[17][1])+(.02455916*Onkb[17][0] ))); Ankb[19][0] = (((.00486049*Onkb[19][2])+(.01387793*Onkb[19][1]))+(( AOnkri[19][0]*c19)-(AOnkri[19][2]*s19))); Ankb[19][1] = (AOnkri[19][1]+((.01360932*Onkb[19][2])-(.01387793*Onkb[19][0] ))); Ankb[19][2] = (((AOnkri[19][0]*s19)+(AOnkri[19][2]*c19))-((.00486049* Onkb[19][0])+(.01360932*Onkb[19][1]))); AOnkri[20][0] = (Ankb[18][0]-((.01739645*Onkb[18][1])+(.02325425*Onkb[18][2] ))); AOnkri[20][1] = (Ankb[18][1]+((.00792098*Onkb[18][2])+(.01739645*Onkb[18][0] ))); AOnkri[20][2] = (Ankb[18][2]+((.02325425*Onkb[18][0])-(.00792098*Onkb[18][1] ))); Ankb[20][0] = (((.01382852*Onkb[20][1])-(.00560346*Onkb[20][2]))+(( AOnkri[20][0]*c20)-(AOnkri[20][2]*s20))); Ankb[20][1] = (AOnkri[20][1]+((.01361601*Onkb[20][2])-(.01382852*Onkb[20][0] ))); Ankb[20][2] = (((.00560346*Onkb[20][0])-(.01361601*Onkb[20][1]))+(( AOnkri[20][0]*s20)+(AOnkri[20][2]*c20))); Ankb[21][0] = -((.28373605*Onkb[21][1])+(1.642*(udot[6]*c21))); Ankb[21][2] = -((5.2e-5*Onkb[21][1])+(1.642*(udot[6]*s21))); Ankb[22][0] = -((.28317448*Onkb[22][1])+(1.642*(udot[7]*c22))); Ankb[22][2] = -((5.194e-5*Onkb[22][1])+(1.642*(udot[7]*s22))); ank[5][0] = ((Ankb[3][2]*Cik[3][0][2])+((Ankb[3][0]*Cik[3][0][0])+( Ankb[3][1]*Cik[3][0][1]))); ank[5][1] = ((Ankb[3][2]*Cik[3][1][2])+((Ankb[3][0]*Cik[3][1][0])+( Ankb[3][1]*Cik[3][1][1]))); ank[5][2] = ((Ankb[3][2]*Cik[3][2][2])+((Ankb[3][0]*Cik[3][2][0])+( Ankb[3][1]*Cik[3][2][1]))); AnkAtk[6][0] = (Atk[6][0]-(1.2242604*udot[6])); AnkAtk[6][2] = (Atk[6][2]-(5.201e-5*udot[6])); ank[6][0] = ((AnkAtk[6][0]*c6)+(AnkAtk[6][2]*s6)); ank[6][2] = ((AnkAtk[6][2]*c6)-(AnkAtk[6][0]*s6)); AnkAtk[7][0] = (Atk[7][0]-(1.22369868*udot[7])); AnkAtk[7][2] = (Atk[7][2]-(5.197e-5*udot[7])); ank[7][0] = ((AnkAtk[7][0]*c7)+(AnkAtk[7][2]*s7)); ank[7][2] = ((AnkAtk[7][2]*c7)-(AnkAtk[7][0]*s7)); AnkAtk[8][0] = (Ankb[8][0]+Atk[8][0]); AnkAtk[8][1] = (Ankb[8][1]+Atk[8][1]); AnkAtk[8][2] = (Ankb[8][2]+Atk[8][2]); ank[8][0] = ((AnkAtk[8][2]*cnk[8][0][2])+((AnkAtk[8][0]*cnk[8][0][0])+( AnkAtk[8][1]*Cik[3][0][1]))); ank[8][1] = ((AnkAtk[8][2]*cnk[8][1][2])+((AnkAtk[8][0]*cnk[8][1][0])+( AnkAtk[8][1]*Cik[3][1][1]))); ank[8][2] = ((AnkAtk[8][2]*cnk[8][2][2])+((AnkAtk[8][0]*cnk[8][2][0])+( AnkAtk[8][1]*Cik[3][2][1]))); AnkAtk[9][0] = (Ankb[9][0]+Atk[9][0]); AnkAtk[9][1] = (Ankb[9][1]+Atk[9][1]); AnkAtk[9][2] = (Ankb[9][2]+Atk[9][2]); ank[9][0] = ((AnkAtk[9][2]*cnk[9][0][2])+((AnkAtk[9][0]*cnk[9][0][0])+( AnkAtk[9][1]*Cik[3][0][1]))); ank[9][1] = ((AnkAtk[9][2]*cnk[9][1][2])+((AnkAtk[9][0]*cnk[9][1][0])+( AnkAtk[9][1]*Cik[3][1][1]))); ank[9][2] = ((AnkAtk[9][2]*cnk[9][2][2])+((AnkAtk[9][0]*cnk[9][2][0])+( AnkAtk[9][1]*Cik[3][2][1]))); AnkAtk[10][0] = (Ankb[10][0]+Atk[10][0]); AnkAtk[10][1] = (Ankb[10][1]+Atk[10][1]); AnkAtk[10][2] = (Ankb[10][2]+Atk[10][2]); ank[10][0] = ((AnkAtk[10][2]*cnk[10][0][2])+((AnkAtk[10][0]*cnk[10][0][0])+( AnkAtk[10][1]*Cik[3][0][1]))); ank[10][1] = ((AnkAtk[10][2]*cnk[10][1][2])+((AnkAtk[10][0]*cnk[10][1][0])+( AnkAtk[10][1]*Cik[3][1][1]))); ank[10][2] = ((AnkAtk[10][2]*cnk[10][2][2])+((AnkAtk[10][0]*cnk[10][2][0])+( AnkAtk[10][1]*Cik[3][2][1]))); AnkAtk[11][0] = (Ankb[11][0]+Atk[11][0]); AnkAtk[11][1] = (Ankb[11][1]+Atk[11][1]); AnkAtk[11][2] = (Ankb[11][2]+Atk[11][2]); ank[11][0] = ((AnkAtk[11][2]*cnk[11][0][2])+((AnkAtk[11][0]*cnk[11][0][0])+( AnkAtk[11][1]*Cik[3][0][1]))); ank[11][1] = ((AnkAtk[11][2]*cnk[11][1][2])+((AnkAtk[11][0]*cnk[11][1][0])+( AnkAtk[11][1]*Cik[3][1][1]))); ank[11][2] = ((AnkAtk[11][2]*cnk[11][2][2])+((AnkAtk[11][0]*cnk[11][2][0])+( AnkAtk[11][1]*Cik[3][2][1]))); AnkAtk[12][0] = (Ankb[12][0]+Atk[12][0]); AnkAtk[12][1] = (Ankb[12][1]+Atk[12][1]); AnkAtk[12][2] = (Ankb[12][2]+Atk[12][2]); ank[12][0] = ((AnkAtk[12][2]*cnk[12][0][2])+((AnkAtk[12][0]*cnk[12][0][0])+( AnkAtk[12][1]*Cik[3][0][1]))); ank[12][1] = ((AnkAtk[12][2]*cnk[12][1][2])+((AnkAtk[12][0]*cnk[12][1][0])+( AnkAtk[12][1]*Cik[3][1][1]))); ank[12][2] = ((AnkAtk[12][2]*cnk[12][2][2])+((AnkAtk[12][0]*cnk[12][2][0])+( AnkAtk[12][1]*Cik[3][2][1]))); AnkAtk[13][0] = (Ankb[13][0]+Atk[13][0]); AnkAtk[13][1] = (Ankb[13][1]+Atk[13][1]); AnkAtk[13][2] = (Ankb[13][2]+Atk[13][2]); ank[13][0] = ((AnkAtk[13][2]*cnk[13][0][2])+((AnkAtk[13][0]*cnk[13][0][0])+( AnkAtk[13][1]*Cik[3][0][1]))); ank[13][1] = ((AnkAtk[13][2]*cnk[13][1][2])+((AnkAtk[13][0]*cnk[13][1][0])+( AnkAtk[13][1]*Cik[3][1][1]))); ank[13][2] = ((AnkAtk[13][2]*cnk[13][2][2])+((AnkAtk[13][0]*cnk[13][2][0])+( AnkAtk[13][1]*Cik[3][2][1]))); AnkAtk[14][0] = (Ankb[14][0]+Atk[14][0]); AnkAtk[14][1] = (Ankb[14][1]+Atk[14][1]); AnkAtk[14][2] = (Ankb[14][2]+Atk[14][2]); ank[14][0] = ((AnkAtk[14][2]*cnk[14][0][2])+((AnkAtk[14][0]*cnk[14][0][0])+( AnkAtk[14][1]*Cik[3][0][1]))); ank[14][1] = ((AnkAtk[14][2]*cnk[14][1][2])+((AnkAtk[14][0]*cnk[14][1][0])+( AnkAtk[14][1]*Cik[3][1][1]))); ank[14][2] = ((AnkAtk[14][2]*cnk[14][2][2])+((AnkAtk[14][0]*cnk[14][2][0])+( AnkAtk[14][1]*Cik[3][2][1]))); AnkAtk[15][0] = (Ankb[15][0]+Atk[15][0]); AnkAtk[15][1] = (Ankb[15][1]+Atk[15][1]); AnkAtk[15][2] = (Ankb[15][2]+Atk[15][2]); ank[15][0] = ((AnkAtk[15][2]*cnk[15][0][2])+((AnkAtk[15][0]*cnk[15][0][0])+( AnkAtk[15][1]*Cik[3][0][1]))); ank[15][1] = ((AnkAtk[15][2]*cnk[15][1][2])+((AnkAtk[15][0]*cnk[15][1][0])+( AnkAtk[15][1]*Cik[3][1][1]))); ank[15][2] = ((AnkAtk[15][2]*cnk[15][2][2])+((AnkAtk[15][0]*cnk[15][2][0])+( AnkAtk[15][1]*Cik[3][2][1]))); AnkAtk[16][0] = (Ankb[16][0]+Atk[16][0]); AnkAtk[16][1] = (Ankb[16][1]+Atk[16][1]); AnkAtk[16][2] = (Ankb[16][2]+Atk[16][2]); ank[16][0] = ((AnkAtk[16][2]*cnk[16][0][2])+((AnkAtk[16][0]*cnk[16][0][0])+( AnkAtk[16][1]*Cik[3][0][1]))); ank[16][1] = ((AnkAtk[16][2]*cnk[16][1][2])+((AnkAtk[16][0]*cnk[16][1][0])+( AnkAtk[16][1]*Cik[3][1][1]))); ank[16][2] = ((AnkAtk[16][2]*cnk[16][2][2])+((AnkAtk[16][0]*cnk[16][2][0])+( AnkAtk[16][1]*Cik[3][2][1]))); AnkAtk[17][0] = (Ankb[17][0]+Atk[17][0]); AnkAtk[17][1] = (Ankb[17][1]+Atk[17][1]); AnkAtk[17][2] = (Ankb[17][2]+Atk[17][2]); ank[17][0] = ((AnkAtk[17][2]*cnk[17][0][2])+((AnkAtk[17][0]*cnk[17][0][0])+( AnkAtk[17][1]*Cik[3][0][1]))); ank[17][1] = ((AnkAtk[17][2]*cnk[17][1][2])+((AnkAtk[17][0]*cnk[17][1][0])+( AnkAtk[17][1]*Cik[3][1][1]))); ank[17][2] = ((AnkAtk[17][2]*cnk[17][2][2])+((AnkAtk[17][0]*cnk[17][2][0])+( AnkAtk[17][1]*Cik[3][2][1]))); AnkAtk[18][0] = (Ankb[18][0]+Atk[18][0]); AnkAtk[18][1] = (Ankb[18][1]+Atk[18][1]); AnkAtk[18][2] = (Ankb[18][2]+Atk[18][2]); ank[18][0] = ((AnkAtk[18][2]*cnk[18][0][2])+((AnkAtk[18][0]*cnk[18][0][0])+( AnkAtk[18][1]*Cik[3][0][1]))); ank[18][1] = ((AnkAtk[18][2]*cnk[18][1][2])+((AnkAtk[18][0]*cnk[18][1][0])+( AnkAtk[18][1]*Cik[3][1][1]))); ank[18][2] = ((AnkAtk[18][2]*cnk[18][2][2])+((AnkAtk[18][0]*cnk[18][2][0])+( AnkAtk[18][1]*Cik[3][2][1]))); AnkAtk[19][0] = (Ankb[19][0]+Atk[19][0]); AnkAtk[19][1] = (Ankb[19][1]+Atk[19][1]); AnkAtk[19][2] = (Ankb[19][2]+Atk[19][2]); ank[19][0] = ((AnkAtk[19][2]*cnk[19][0][2])+((AnkAtk[19][0]*cnk[19][0][0])+( AnkAtk[19][1]*Cik[3][0][1]))); ank[19][1] = ((AnkAtk[19][2]*cnk[19][1][2])+((AnkAtk[19][0]*cnk[19][1][0])+( AnkAtk[19][1]*Cik[3][1][1]))); ank[19][2] = ((AnkAtk[19][2]*cnk[19][2][2])+((AnkAtk[19][0]*cnk[19][2][0])+( AnkAtk[19][1]*Cik[3][2][1]))); AnkAtk[20][0] = (Ankb[20][0]+Atk[20][0]); AnkAtk[20][1] = (Ankb[20][1]+Atk[20][1]); AnkAtk[20][2] = (Ankb[20][2]+Atk[20][2]); ank[20][0] = ((AnkAtk[20][2]*cnk[20][0][2])+((AnkAtk[20][0]*cnk[20][0][0])+( AnkAtk[20][1]*Cik[3][0][1]))); ank[20][1] = ((AnkAtk[20][2]*cnk[20][1][2])+((AnkAtk[20][0]*cnk[20][1][0])+( AnkAtk[20][1]*Cik[3][1][1]))); ank[20][2] = ((AnkAtk[20][2]*cnk[20][2][2])+((AnkAtk[20][0]*cnk[20][2][0])+( AnkAtk[20][1]*Cik[3][2][1]))); AnkAtk[21][0] = (Ankb[21][0]+Atk[21][0]); AnkAtk[21][2] = (Ankb[21][2]+Atk[21][2]); ank[21][0] = ((AnkAtk[21][0]*cnk[21][0][0])+(AnkAtk[21][2]*cnk[21][0][2])); ank[21][2] = ((AnkAtk[21][0]*cnk[21][2][0])+(AnkAtk[21][2]*cnk[21][2][2])); AnkAtk[22][0] = (Ankb[22][0]+Atk[22][0]); AnkAtk[22][2] = (Ankb[22][2]+Atk[22][2]); ank[22][0] = ((AnkAtk[22][0]*cnk[22][0][0])+(AnkAtk[22][2]*cnk[22][0][2])); ank[22][2] = ((AnkAtk[22][0]*cnk[22][2][0])+(AnkAtk[22][2]*cnk[22][2][2])); anb[0][0] = ank[5][0]; anb[0][1] = ank[5][1]; anb[0][2] = ank[5][2]; anb[1][0] = ank[6][0]; anb[1][1] = 0.; anb[1][2] = ank[6][2]; anb[2][0] = ank[7][0]; anb[2][1] = 0.; anb[2][2] = ank[7][2]; anb[3][0] = ank[8][0]; anb[3][1] = ank[8][1]; anb[3][2] = ank[8][2]; anb[4][0] = ank[9][0]; anb[4][1] = ank[9][1]; anb[4][2] = ank[9][2]; anb[5][0] = ank[10][0]; anb[5][1] = ank[10][1]; anb[5][2] = ank[10][2]; anb[6][0] = ank[11][0]; anb[6][1] = ank[11][1]; anb[6][2] = ank[11][2]; anb[7][0] = ank[12][0]; anb[7][1] = ank[12][1]; anb[7][2] = ank[12][2]; anb[8][0] = ank[13][0]; anb[8][1] = ank[13][1]; anb[8][2] = ank[13][2]; anb[9][0] = ank[14][0]; anb[9][1] = ank[14][1]; anb[9][2] = ank[14][2]; anb[10][0] = ank[15][0]; anb[10][1] = ank[15][1]; anb[10][2] = ank[15][2]; anb[11][0] = ank[16][0]; anb[11][1] = ank[16][1]; anb[11][2] = ank[16][2]; anb[12][0] = ank[17][0]; anb[12][1] = ank[17][1]; anb[12][2] = ank[17][2]; anb[13][0] = ank[18][0]; anb[13][1] = ank[18][1]; anb[13][2] = ank[18][2]; anb[14][0] = ank[19][0]; anb[14][1] = ank[19][1]; anb[14][2] = ank[19][2]; anb[15][0] = ank[20][0]; anb[15][1] = ank[20][1]; anb[15][2] = ank[20][2]; anb[16][0] = ank[21][0]; anb[16][1] = 0.; anb[16][2] = ank[21][2]; anb[17][0] = ank[22][0]; anb[17][1] = 0.; anb[17][2] = ank[22][2]; roustate = 3; /* Used 0.54 seconds CPU time, 0 additional bytes of memory. Equations contain 498 adds/subtracts/negates 427 multiplies 0 divides 542 assignments */ } void BOCfly :: sdpseudo ( double lqout[1], double luout[1] ) { /* Return pseudo-coordinates for loop joints. */ /* There are no loop joints in this system. */ } void BOCfly :: sdpsqdot ( double lqdout[1] ) { /* Return pseudo-coordinate derivatives for loop joints. */ /* There are no loop joints in this system. */ } void BOCfly :: sdpsudot ( double ludout[1] ) { /* Return pseudo-coordinate accelerations for loop joints. */ /* There are no loop joints in this system. */ } void BOCfly :: sdperr ( double errs[1] ) { } void BOCfly :: sdverr ( double errs[1] ) { } void BOCfly :: sdaerr ( double errs[1] ) { } int BOCfly :: sdindx ( int joint, int axis ) { int offs,gotit; if (sdchkjaxis(36,joint,axis) != 0) { return 0; } gotit = 0; if (jtype[joint] == 4) { if (axis == 3) { offs = ballq[joint]; gotit = 1; } } else { if ((jtype[joint] == 6) || (jtype[joint] == 21)) { if (axis == 6) { offs = ballq[joint]; gotit = 1; } } } if (gotit == 0) { offs = firstq[joint]+axis; } return offs; } void BOCfly :: sdpresacc ( int joint, int axis, double prval ) { } void BOCfly :: sdpresvel ( int joint, int axis, double prval ) { } void BOCfly :: sdprespos ( int joint, int axis, double prval ) { } void BOCfly :: sdgetht ( int joint, int axis, double *torque ) { if (sdchkjaxis(30,joint,axis) != 0) { return; } if (roustate != 3) { sdseterr(30,24); return; } *torque = tauc[sdindx(joint,axis)]; } void BOCfly :: sdhinget ( int joint, int axis, double torque ) { if (sdchkjaxis(10,joint,axis) != 0) { return; } if (roustate != 2) { sdseterr(10,23); return; } if (mfrcflg != 0) { mtau[sdindx(joint,axis)] = mtau[sdindx(joint,axis)]+torque; } else { fs0flg = 0; utau[sdindx(joint,axis)] = utau[sdindx(joint,axis)]+torque; } } void BOCfly :: sdpointf ( int body, double point[3], double force[3] ) { double torque[3]; if (sdchkbnum(11,body) != 0) { return; } if (roustate != 2) { sdseterr(11,23); return; } if (body == -1) { return; } torque[0] = point[1]*force[2]-point[2]*force[1]; torque[1] = point[2]*force[0]-point[0]*force[2]; torque[2] = point[0]*force[1]-point[1]*force[0]; if (mfrcflg != 0) { mfk[body][0] = mfk[body][0]+force[0]; mtk[body][0] = mtk[body][0]+torque[0]; mfk[body][1] = mfk[body][1]+force[1]; mtk[body][1] = mtk[body][1]+torque[1]; mfk[body][2] = mfk[body][2]+force[2]; mtk[body][2] = mtk[body][2]+torque[2]; } else { fs0flg = 0; ufk[body][0] = ufk[body][0]+force[0]; utk[body][0] = utk[body][0]+torque[0]; ufk[body][1] = ufk[body][1]+force[1]; utk[body][1] = utk[body][1]+torque[1]; ufk[body][2] = ufk[body][2]+force[2]; utk[body][2] = utk[body][2]+torque[2]; } } void BOCfly :: sdbodyt ( int body, double torque[3] ) { if (sdchkbnum(12,body) != 0) { return; } if (roustate != 2) { sdseterr(12,23); return; } if (body == -1) { return; } if (mfrcflg != 0) { mtk[body][0] = mtk[body][0]+torque[0]; mtk[body][1] = mtk[body][1]+torque[1]; mtk[body][2] = mtk[body][2]+torque[2]; } else { fs0flg = 0; utk[body][0] = utk[body][0]+torque[0]; utk[body][1] = utk[body][1]+torque[1]; utk[body][2] = utk[body][2]+torque[2]; } } void BOCfly :: sddoww ( int routine ) { roustate = 2; if (wwflg == 0) { wwflg = 1; } /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 0 assignments */ } void BOCfly :: sdxudot0 ( int routine, double oudot0[23] ) { /* Compute unconstrained equations */ int i; double eps[23],Z1[23][3],Z2[23][3],A1[23][3],A2[23][3],K1[23][3],K2[23][3]; sdlhs(routine); /* Solve equations for udots */ Z1[7][0] = -((.000522305028403106*(fs0[22]*s22))+(2.84758288062061*(fs0[22]* c22))); Z1[7][2] = ((2.84758288062061*(fs0[22]*s22))-(.000522305028403106*(fs0[22]* c22))); Z2[7][0] = ((.0458114594397548*(fs0[22]*s22))-(2.55897956380677e-6*(fs0[22]* c22))); Z2[7][1] = (fs0[22]+((.0008576248566379*(fs0[22]*s22))+(4.67573108997905*( fs0[22]*c22)))); Z2[7][2] = ((2.55897956380677e-6*(fs0[22]*s22))+(.0458114594397548*(fs0[22]* c22))); Z1[6][0] = -((.000521432707401758*(fs0[21]*s21))+(2.84517801421117*(fs0[21]* c21))); Z1[6][2] = ((2.84517801421117*(fs0[21]*s21))-(.000521432707401758*(fs0[21]* c21))); Z2[6][0] = ((3.85809387637547e-6*(fs0[21]*c21))-(.0519143251523376*(fs0[21]* s21))); Z2[6][1] = (fs0[21]+((.000856192505553687*(fs0[21]*s21))+(4.67178229933474*( fs0[21]*c21)))); Z2[6][2] = -((3.85809387637547e-6*(fs0[21]*s21))+(.0519143251523376*(fs0[21] *c21))); Z1[18][0] = ((.973576614358514*(fs0[20]*c20))-(.958615160326027*(fs0[20]*s20 ))); Z1[18][2] = -((.958615160326027*(fs0[20]*c20))+(.973576614358514*(fs0[20]* s20))); Z2[18][0] = (((.0274347997310184*(fs0[20]*c20))+(.240343600676817*(fs0[20]* s20)))-((.0230067638478247*(fs0[20]*c20))+(.0233658387446043*(fs0[20]*s20) ))); Z2[18][1] = (fs0[20]+((.973576614358514*(fs0[20]*rikt[20][1][0]))-( .958615160326027*(fs0[20]*rikt[20][1][2])))); Z2[18][2] = (((.0230067638478247*(fs0[20]*s20))-(.0233658387446043*(fs0[20]* c20)))+((.240343600676817*(fs0[20]*c20))-(.0274347997310184*(fs0[20]*s20)) )); Z1[17][0] = ((.978182151053824*(fs0[19]*c19))-(.959249247688944*(fs0[19]*s19 ))); Z1[17][2] = -((.959249247688944*(fs0[19]*c19))+(.978182151053824*(fs0[19]* s19))); Z2[17][0] = (((.0230219819445347*(fs0[19]*c19))+(.0234763716252918*(fs0[19]* s19)))-((.028064966614006*(fs0[19]*c19))+(.240303874633767*(fs0[19]*s19))) ); Z2[17][1] = (fs0[19]+((.978182151053824*(fs0[19]*rikt[19][1][0]))-( .959249247688944*(fs0[19]*rikt[19][1][2])))); Z2[17][2] = (((.0234763716252918*(fs0[19]*c19))-(.0230219819445347*(fs0[19]* s19)))+((.028064966614006*(fs0[19]*s19))-(.240303874633767*(fs0[19]*c19))) ); eps[18] = (fs0[18]-Z2[18][1]); Z1[18][0] = (Z1[18][0]+(eps[18]*G1[18][0])); Z1[18][2] = (Z1[18][2]+(eps[18]*G1[18][2])); Z2[18][0] = (Z2[18][0]+(eps[18]*G2[18][0])); Z2[18][1] = (eps[18]+Z2[18][1]); Z2[18][2] = (Z2[18][2]+(eps[18]*G2[18][2])); Z1[16][0] = ((Z1[18][0]*c18)+(Z1[18][2]*s18)); Z1[16][2] = ((Z1[18][2]*c18)-(Z1[18][0]*s18)); Z2[16][0] = ((.036*((Z1[18][2]*c18)-(Z1[18][0]*s18)))+((Z2[18][0]*c18)+( Z2[18][2]*s18))); Z2[16][1] = (Z2[18][1]-(.196*((Z1[18][0]*c18)+(Z1[18][2]*s18)))); Z2[16][2] = (((Z2[18][2]*c18)-(Z2[18][0]*s18))-(.036*((Z1[18][0]*c18)+( Z1[18][2]*s18)))); eps[17] = (fs0[17]-Z2[17][1]); Z1[17][0] = (Z1[17][0]+(eps[17]*G1[17][0])); Z1[17][2] = (Z1[17][2]+(eps[17]*G1[17][2])); Z2[17][0] = (Z2[17][0]+(eps[17]*G2[17][0])); Z2[17][1] = (eps[17]+Z2[17][1]); Z2[17][2] = (Z2[17][2]+(eps[17]*G2[17][2])); Z1[15][0] = ((Z1[17][0]*c17)+(Z1[17][2]*s17)); Z1[15][2] = ((Z1[17][2]*c17)-(Z1[17][0]*s17)); Z2[15][0] = ((.036*((Z1[17][0]*s17)-(Z1[17][2]*c17)))+((Z2[17][0]*c17)+( Z2[17][2]*s17))); Z2[15][1] = (Z2[17][1]-(.196*((Z1[17][0]*c17)+(Z1[17][2]*s17)))); Z2[15][2] = ((.036*((Z1[17][0]*c17)+(Z1[17][2]*s17)))+((Z2[17][2]*c17)-( Z2[17][0]*s17))); eps[16] = (fs0[16]-Z2[16][1]); Z1[16][0] = (Z1[16][0]+(eps[16]*G1[16][0])); Z1[16][2] = (Z1[16][2]+(eps[16]*G1[16][2])); Z2[16][0] = (Z2[16][0]+(eps[16]*G2[16][0])); Z2[16][1] = (eps[16]+Z2[16][1]); Z2[16][2] = (Z2[16][2]+(eps[16]*G2[16][2])); Z1[5][0] = ((Z1[16][0]*c16)+(Z1[16][2]*s16)); Z1[5][2] = ((Z1[16][2]*c16)-(Z1[16][0]*s16)); Z2[5][0] = ((.09879136*((Z1[16][2]*c16)-(Z1[16][0]*s16)))+((Z2[16][0]*c16)+( Z2[16][2]*s16))); Z2[5][1] = (Z2[16][1]+((rikt[16][1][0]*Z1[16][0])+(rikt[16][1][2]*Z1[16][2]) )); Z2[5][2] = (((Z2[16][2]*c16)-(Z2[16][0]*s16))-(.09879136*((Z1[16][0]*c16)+( Z1[16][2]*s16)))); eps[15] = (fs0[15]-Z2[15][1]); Z1[15][0] = (Z1[15][0]+(eps[15]*G1[15][0])); Z1[15][2] = (Z1[15][2]+(eps[15]*G1[15][2])); Z2[15][0] = (Z2[15][0]+(eps[15]*G2[15][0])); Z2[15][1] = (eps[15]+Z2[15][1]); Z2[15][2] = (Z2[15][2]+(eps[15]*G2[15][2])); Z1[5][0] = (Z1[5][0]+((Z1[15][0]*c15)+(Z1[15][2]*s15))); Z1[5][2] = (Z1[5][2]+((Z1[15][2]*c15)-(Z1[15][0]*s15))); Z2[5][0] = (Z2[5][0]+((.10120864*((Z1[15][0]*s15)-(Z1[15][2]*c15)))+(( Z2[15][0]*c15)+(Z2[15][2]*s15)))); Z2[5][1] = (Z2[5][1]+(Z2[15][1]+((rikt[15][1][0]*Z1[15][0])+(rikt[15][1][2]* Z1[15][2])))); Z2[5][2] = (Z2[5][2]+((.10120864*((Z1[15][0]*c15)+(Z1[15][2]*s15)))+(( Z2[15][2]*c15)-(Z2[15][0]*s15)))); Z1[12][0] = -((7.13884380751755*(fs0[14]*s14))+(8.04284255327246*(fs0[14]* c14))); Z1[12][2] = ((8.04284255327246*(fs0[14]*s14))-(7.13884380751755*(fs0[14]*c14 ))); Z2[12][0] = (((.0061168913506177*(fs0[14]*s14))-(.000342119388521631*( fs0[14]*c14)))+((.0356942190375877*(fs0[14]*c14))-(.0402142127663623*( fs0[14]*s14)))); Z2[12][1] = (fs0[14]-((7.13884380751755*(fs0[14]*rikt[14][1][2]))+( 8.04284255327246*(fs0[14]*rikt[14][1][0])))); Z2[12][2] = (((.000342119388521631*(fs0[14]*s14))+(.0061168913506177*( fs0[14]*c14)))-((.0356942190375877*(fs0[14]*s14))+(.0402142127663623*( fs0[14]*c14)))); Z1[11][0] = -((7.14670103793689*(fs0[13]*s13))+(8.03501187922594*(fs0[13]* c13))); Z1[11][2] = ((8.03501187922594*(fs0[13]*s13))-(7.14670103793689*(fs0[13]*c13 ))); Z2[11][0] = (((.0171361049110685*(fs0[13]*s13))-(.0209258314399415*(fs0[13]* c13)))+((.0401750593961297*(fs0[13]*s13))-(.0357335051896845*(fs0[13]*c13) ))); Z2[11][1] = (fs0[13]-((7.14670103793689*(fs0[13]*rikt[13][1][2]))+( 8.03501187922594*(fs0[13]*rikt[13][1][0])))); Z2[11][2] = (((.0171361049110685*(fs0[13]*c13))+(.0209258314399415*(fs0[13]* s13)))+((.0357335051896845*(fs0[13]*s13))+(.0401750593961297*(fs0[13]*c13) ))); eps[12] = (fs0[12]-Z2[12][1]); Z1[12][0] = (Z1[12][0]+(eps[12]*G1[12][0])); Z1[12][2] = (Z1[12][2]+(eps[12]*G1[12][2])); Z2[12][0] = (Z2[12][0]+(eps[12]*G2[12][0])); Z2[12][1] = (eps[12]+Z2[12][1]); Z2[12][2] = (Z2[12][2]+(eps[12]*G2[12][2])); Z1[10][0] = ((Z1[12][0]*c12)+(Z1[12][2]*s12)); Z1[10][2] = ((Z1[12][2]*c12)-(Z1[12][0]*s12)); Z2[10][0] = ((.003*((Z1[12][2]*c12)-(Z1[12][0]*s12)))+((Z2[12][0]*c12)+( Z2[12][2]*s12))); Z2[10][1] = (Z2[12][1]+((rikt[12][1][0]*Z1[12][0])+(rikt[12][1][2]*Z1[12][2] ))); Z2[10][2] = (((Z2[12][2]*c12)-(Z2[12][0]*s12))-(.003*((Z1[12][0]*c12)+( Z1[12][2]*s12)))); eps[11] = (fs0[11]-Z2[11][1]); Z1[11][0] = (Z1[11][0]+(eps[11]*G1[11][0])); Z1[11][2] = (Z1[11][2]+(eps[11]*G1[11][2])); Z2[11][0] = (Z2[11][0]+(eps[11]*G2[11][0])); Z2[11][1] = (eps[11]+Z2[11][1]); Z2[11][2] = (Z2[11][2]+(eps[11]*G2[11][2])); Z1[9][0] = ((Z1[11][0]*c11)+(Z1[11][2]*s11)); Z1[9][2] = ((Z1[11][2]*c11)-(Z1[11][0]*s11)); Z2[9][0] = ((.003*((Z1[11][0]*s11)-(Z1[11][2]*c11)))+((Z2[11][0]*c11)+( Z2[11][2]*s11))); Z2[9][1] = (Z2[11][1]+((rikt[11][1][0]*Z1[11][0])+(rikt[11][1][2]*Z1[11][2]) )); Z2[9][2] = ((.003*((Z1[11][0]*c11)+(Z1[11][2]*s11)))+((Z2[11][2]*c11)-( Z2[11][0]*s11))); eps[10] = (fs0[10]-Z2[10][1]); Z1[10][0] = (Z1[10][0]+(eps[10]*G1[10][0])); Z1[10][2] = (Z1[10][2]+(eps[10]*G1[10][2])); Z2[10][0] = (Z2[10][0]+(eps[10]*G2[10][0])); Z2[10][1] = (eps[10]+Z2[10][1]); Z2[10][2] = (Z2[10][2]+(eps[10]*G2[10][2])); Z1[5][0] = (Z1[5][0]+((Z1[10][0]*c10)+(Z1[10][2]*s10))); Z1[5][2] = (Z1[5][2]+((Z1[10][2]*c10)-(Z1[10][0]*s10))); Z2[5][0] = (Z2[5][0]+((.04579136*((Z1[10][2]*c10)-(Z1[10][0]*s10)))+(( Z2[10][0]*c10)+(Z2[10][2]*s10)))); Z2[5][1] = (Z2[5][1]+(Z2[10][1]+((rikt[10][1][0]*Z1[10][0])+(rikt[10][1][2]* Z1[10][2])))); Z2[5][2] = (Z2[5][2]+(((Z2[10][2]*c10)-(Z2[10][0]*s10))-(.04579136*(( Z1[10][0]*c10)+(Z1[10][2]*s10))))); eps[9] = (fs0[9]-Z2[9][1]); Z1[9][0] = (Z1[9][0]+(eps[9]*G1[9][0])); Z1[9][2] = (Z1[9][2]+(eps[9]*G1[9][2])); Z2[9][0] = (Z2[9][0]+(eps[9]*G2[9][0])); Z2[9][1] = (eps[9]+Z2[9][1]); Z2[9][2] = (Z2[9][2]+(eps[9]*G2[9][2])); Z1[5][0] = (Z1[5][0]+((Z1[9][0]*c9)+(Z1[9][2]*s9))); Z1[5][2] = (Z1[5][2]+((Z1[9][2]*c9)-(Z1[9][0]*s9))); Z2[5][0] = (Z2[5][0]+((.04820864*((Z1[9][0]*s9)-(Z1[9][2]*c9)))+((Z2[9][0]* c9)+(Z2[9][2]*s9)))); Z2[5][1] = (Z2[5][1]+(Z2[9][1]+((rikt[9][1][0]*Z1[9][0])+(rikt[9][1][2]* Z1[9][2])))); Z2[5][2] = (Z2[5][2]+((.04820864*((Z1[9][0]*c9)+(Z1[9][2]*s9)))+((Z2[9][2]* c9)-(Z2[9][0]*s9)))); Z1[5][0] = (Z1[5][0]-((.24882040334792*(fs0[8]*s8))+(1.76573460036658*( fs0[8]*c8)))); Z1[5][2] = (Z1[5][2]+((1.76573460036658*(fs0[8]*s8))-(.24882040334792*( fs0[8]*c8)))); Z2[5][0] = (Z2[5][0]+(((.00030073429230243*(fs0[8]*c8))-(.00213413746738706* (fs0[8]*s8)))+((.00073029518364718*(fs0[8]*s8))-(.000153974280839144*( fs0[8]*c8))))); Z2[5][1] = (Z2[5][1]+(fs0[8]-((.24882040334792*(fs0[8]*rikt[8][1][2]))+( 1.76573460036658*(fs0[8]*rikt[8][1][0]))))); Z2[5][2] = (Z2[5][2]+(((.000153974280839144*(fs0[8]*s8))+(.00073029518364718 *(fs0[8]*c8)))-((.00030073429230243*(fs0[8]*s8))+(.00213413746738706*( fs0[8]*c8))))); eps[7] = (fs0[7]-Z2[7][1]); eps[6] = (fs0[6]-Z2[6][1]); eps[5] = (fs0[5]-Z2[5][2]); Z1[5][0] = (Z1[5][0]+(eps[5]*G1[5][0])); Z1[5][1] = (eps[5]*G1[5][1]); Z1[5][2] = (Z1[5][2]+(eps[5]*G1[5][2])); Z2[5][0] = (Z2[5][0]+(eps[5]*G2[5][0])); Z2[5][1] = (Z2[5][1]+(eps[5]*G2[5][1])); Z2[5][2] = (eps[5]+Z2[5][2]); Z1[4][0] = Z1[5][0]; Z1[4][1] = Z1[5][1]; Z1[4][2] = Z1[5][2]; Z2[4][0] = Z2[5][0]; Z2[4][1] = Z2[5][1]; Z2[4][2] = Z2[5][2]; eps[4] = (fs0[4]-Z2[4][1]); Z1[4][0] = (Z1[4][0]+(eps[4]*G1[4][0])); Z1[4][1] = (Z1[4][1]+(eps[4]*G1[4][1])); Z1[4][2] = (Z1[4][2]+(eps[4]*G1[4][2])); Z2[4][0] = (Z2[4][0]+(eps[4]*G2[4][0])); Z2[4][1] = (eps[4]+Z2[4][1]); Z1[3][0] = Z1[4][0]; Z1[3][1] = Z1[4][1]; Z1[3][2] = Z1[4][2]; Z2[3][0] = Z2[4][0]; Z2[3][1] = Z2[4][1]; Z2[3][2] = Z2[4][2]; eps[3] = (fs0[3]-Z2[3][0]); Z1[3][0] = (Z1[3][0]+(eps[3]*G1[3][0])); Z1[3][1] = (Z1[3][1]+(eps[3]*G1[3][1])); Z1[3][2] = (Z1[3][2]+(eps[3]*G1[3][2])); Z2[3][0] = (eps[3]+Z2[3][0]); Z1[2][0] = ((Cik[3][0][2]*Z1[3][2])+((Cik[3][0][0]*Z1[3][0])+(Cik[3][0][1]* Z1[3][1]))); Z1[2][1] = ((Cik[3][1][2]*Z1[3][2])+((Cik[3][1][0]*Z1[3][0])+(Cik[3][1][1]* Z1[3][1]))); Z1[2][2] = ((Cik[3][2][2]*Z1[3][2])+((Cik[3][2][0]*Z1[3][0])+(Cik[3][2][1]* Z1[3][1]))); Z2[2][0] = ((Cik[3][0][2]*Z2[3][2])+((Cik[3][0][0]*Z2[3][0])+(Cik[3][0][1]* Z2[3][1]))); Z2[2][1] = ((Cik[3][1][2]*Z2[3][2])+((Cik[3][1][0]*Z2[3][0])+(Cik[3][1][1]* Z2[3][1]))); Z2[2][2] = ((Cik[3][2][2]*Z2[3][2])+((Cik[3][2][0]*Z2[3][0])+(Cik[3][2][1]* Z2[3][1]))); eps[2] = (fs0[2]-Z1[2][2]); Z1[2][0] = (Z1[2][0]+(eps[2]*G1[2][0])); Z1[2][1] = (Z1[2][1]+(eps[2]*G1[2][1])); Z1[2][2] = (eps[2]+Z1[2][2]); Z1[1][0] = Z1[2][0]; Z1[1][1] = Z1[2][1]; Z1[1][2] = Z1[2][2]; Z2[1][0] = (Z2[2][0]-(q[2]*Z1[2][1])); Z2[1][1] = (Z2[2][1]+(q[2]*Z1[2][0])); Z2[1][2] = Z2[2][2]; eps[1] = (fs0[1]-Z1[1][1]); Z1[1][0] = (Z1[1][0]+(eps[1]*G1[1][0])); Z1[1][1] = (eps[1]+Z1[1][1]); Z2[1][0] = (Z2[1][0]+(eps[1]*G2[1][0])); Z2[1][1] = (Z2[1][1]+(eps[1]*G2[1][1])); Z1[0][0] = Z1[1][0]; Z1[0][1] = Z1[1][1]; Z1[0][2] = Z1[1][2]; Z2[0][0] = (Z2[1][0]+(q[1]*Z1[1][2])); Z2[0][1] = Z2[1][1]; Z2[0][2] = (Z2[1][2]-(q[1]*Z1[1][0])); eps[0] = (fs0[0]-Z1[0][0]); udot[0] = (DD[0]*eps[0]); udot[1] = ((DD[1]*eps[1])-(G1[1][0]*udot[0])); udot[2] = ((DD[2]*eps[2])-((G1[2][0]*udot[0])+(G1[2][1]*udot[1]))); K1[3][0] = ((Cik[3][2][0]*udot[2])+((Cik[3][0][0]*udot[0])+(Cik[3][1][0]* udot[1]))); K1[3][1] = ((Cik[3][2][1]*udot[2])+((Cik[3][0][1]*udot[0])+(Cik[3][1][1]* udot[1]))); K1[3][2] = ((Cik[3][2][2]*udot[2])+((Cik[3][0][2]*udot[0])+(Cik[3][1][2]* udot[1]))); udot[3] = ((DD[3]*eps[3])-((G1[3][2]*K1[3][2])+((G1[3][0]*K1[3][0])+( G1[3][1]*K1[3][1])))); udot[4] = ((DD[4]*eps[4])-((G2[4][0]*udot[3])+((G1[4][2]*K1[3][2])+(( G1[4][0]*K1[3][0])+(G1[4][1]*K1[3][1]))))); udot[5] = ((DD[5]*eps[5])-(((G1[5][2]*K1[3][2])+((G1[5][0]*K1[3][0])+( G1[5][1]*K1[3][1])))+((G2[5][0]*udot[3])+(G2[5][1]*udot[4])))); udot[6] = (DD[6]*eps[6]); udot[7] = (DD[7]*eps[7]); K1[8][0] = (((.00120864*(udot[5]*c8))+((.00120864*(udot[3]*s8))+( rikt[8][1][0]*udot[4])))+((K1[3][0]*c8)-(K1[3][2]*s8))); K1[8][1] = (K1[3][1]-((.00252618*udot[5])+(.3829286*udot[3]))); K1[8][2] = (((.00120864*(udot[5]*s8))+((rikt[8][1][2]*udot[4])-(.00120864*( udot[3]*c8))))+((K1[3][0]*s8)+(K1[3][2]*c8))); K2[8][0] = ((udot[3]*c8)-(udot[5]*s8)); K2[8][2] = ((udot[3]*s8)+(udot[5]*c8)); udot[8] = ((1.49386933528469*fs0[8])-(((.00073029518364718*K2[8][2])+( udot[4]-(.000153974280839144*K2[8][0])))-((.24882040334792*K1[8][2])+( 1.76573460036658*K1[8][0])))); A2[8][1] = (udot[4]+udot[8]); K1[9][0] = (((.04820864*(udot[5]*c9))+((.04820864*(udot[3]*s9))+( rikt[9][1][0]*udot[4])))+((K1[3][0]*c9)-(K1[3][2]*s9))); K1[9][1] = (K1[3][1]+((.0620714*udot[3])-(.01952618*udot[5]))); K1[9][2] = (((.04820864*(udot[5]*s9))+((rikt[9][1][2]*udot[4])-(.04820864*( udot[3]*c9))))+((K1[3][0]*s9)+(K1[3][2]*c9))); K2[9][0] = ((udot[3]*c9)-(udot[5]*s9)); K2[9][2] = ((udot[3]*s9)+(udot[5]*c9)); udot[9] = ((DD[9]*eps[9])-(((G1[9][0]*K1[9][0])+(G1[9][2]*K1[9][2]))+(( G2[9][2]*K2[9][2])+(udot[4]+(G2[9][0]*K2[9][0]))))); A2[9][1] = (udot[4]+udot[9]); K1[10][0] = (((K1[3][0]*c10)-(K1[3][2]*s10))+(((rikt[10][1][0]*udot[4])-( .04579136*(udot[3]*s10)))-(.04579136*(udot[5]*c10)))); K1[10][1] = (K1[3][1]+((.0620714*udot[3])-(.01952618*udot[5]))); K1[10][2] = (((K1[3][0]*s10)+(K1[3][2]*c10))+(((.04579136*(udot[3]*c10))+( rikt[10][1][2]*udot[4]))-(.04579136*(udot[5]*s10)))); K2[10][0] = ((udot[3]*c10)-(udot[5]*s10)); K2[10][2] = ((udot[3]*s10)+(udot[5]*c10)); udot[10] = ((DD[10]*eps[10])-(((G1[10][0]*K1[10][0])+(G1[10][2]*K1[10][2]))+ ((G2[10][2]*K2[10][2])+(udot[4]+(G2[10][0]*K2[10][0]))))); A2[10][1] = (udot[4]+udot[10]); K1[11][0] = (((.003*(K2[9][2]*c11))+((.003*(K2[9][0]*s11))+(A2[9][1]* rikt[11][1][0])))+((K1[9][0]*c11)-(K1[9][2]*s11))); K1[11][1] = (K1[9][1]+((.016*K2[9][2])+(.291*K2[9][0]))); K1[11][2] = (((.003*(K2[9][2]*s11))+((A2[9][1]*rikt[11][1][2])-(.003*( K2[9][0]*c11))))+((K1[9][0]*s11)+(K1[9][2]*c11))); K2[11][0] = ((K2[9][0]*c11)-(K2[9][2]*s11)); K2[11][2] = ((K2[9][0]*s11)+(K2[9][2]*c11)); udot[11] = ((DD[11]*eps[11])-(((A2[9][1]+(G2[11][0]*K2[11][0]))+(G2[11][2]* K2[11][2]))+((G1[11][0]*K1[11][0])+(G1[11][2]*K1[11][2])))); A2[11][1] = (A2[9][1]+udot[11]); K1[12][0] = (((K1[10][0]*c12)-(K1[10][2]*s12))+(((A2[10][1]*rikt[12][1][0])- (.003*(K2[10][0]*s12)))-(.003*(K2[10][2]*c12)))); K1[12][1] = (K1[10][1]+((.016*K2[10][2])+(.291*K2[10][0]))); K1[12][2] = (((K1[10][0]*s12)+(K1[10][2]*c12))+(((.003*(K2[10][0]*c12))+( A2[10][1]*rikt[12][1][2]))-(.003*(K2[10][2]*s12)))); K2[12][0] = ((K2[10][0]*c12)-(K2[10][2]*s12)); K2[12][2] = ((K2[10][0]*s12)+(K2[10][2]*c12)); udot[12] = ((DD[12]*eps[12])-(((A2[10][1]+(G2[12][0]*K2[12][0]))+(G2[12][2]* K2[12][2]))+((G1[12][0]*K1[12][0])+(G1[12][2]*K1[12][2])))); A2[12][1] = (A2[10][1]+udot[12]); K1[13][0] = (((K1[11][0]*c13)-(K1[11][2]*s13))+(((A2[11][1]*rikt[13][1][0])- (.005*(K2[11][0]*s13)))-(.005*(K2[11][2]*c13)))); K1[13][1] = (K1[11][1]+((.297*K2[11][0])-(.015*K2[11][2]))); K1[13][2] = (((K1[11][0]*s13)+(K1[11][2]*c13))+(((.005*(K2[11][0]*c13))+( A2[11][1]*rikt[13][1][2]))-(.005*(K2[11][2]*s13)))); K2[13][0] = ((K2[11][0]*c13)-(K2[11][2]*s13)); K2[13][2] = ((K2[11][0]*s13)+(K2[11][2]*c13)); udot[13] = ((377.831870134533*fs0[13])-(((.0171361049110685*K2[13][2])+( A2[11][1]-(.0209258314399415*K2[13][0])))-((7.14670103793689*K1[13][2])+( 8.03501187922594*K1[13][0])))); A2[13][1] = (A2[11][1]+udot[13]); K1[14][0] = (((.005*(K2[12][2]*c14))+((.005*(K2[12][0]*s14))+(A2[12][1]* rikt[14][1][0])))+((K1[12][0]*c14)-(K1[12][2]*s14))); K1[14][1] = (K1[12][1]+((.297*K2[12][0])-(.015*K2[12][2]))); K1[14][2] = (((.005*(K2[12][2]*s14))+((A2[12][1]*rikt[14][1][2])-(.005*( K2[12][0]*c14))))+((K1[12][0]*s14)+(K1[12][2]*c14))); K2[14][0] = ((K2[12][0]*c14)-(K2[12][2]*s14)); K2[14][2] = ((K2[12][0]*s14)+(K2[12][2]*c14)); udot[14] = ((378.678658433892*fs0[14])-(((.0061168913506177*K2[14][2])+( A2[12][1]-(.000342119388521631*K2[14][0])))-((7.13884380751755*K1[14][2])+ (8.04284255327246*K1[14][0])))); A2[14][1] = (A2[12][1]+udot[14]); K1[15][0] = (((.10120864*(udot[5]*c15))+((.10120864*(udot[3]*s15))+( rikt[15][1][0]*udot[4])))+((K1[3][0]*c15)-(K1[3][2]*s15))); K1[15][1] = (K1[3][1]-((.01052618*udot[5])+(.2639286*udot[3]))); K1[15][2] = (((.10120864*(udot[5]*s15))+((rikt[15][1][2]*udot[4])-(.10120864 *(udot[3]*c15))))+((K1[3][0]*s15)+(K1[3][2]*c15))); K2[15][0] = ((udot[3]*c15)-(udot[5]*s15)); K2[15][2] = ((udot[3]*s15)+(udot[5]*c15)); udot[15] = ((DD[15]*eps[15])-(((G1[15][0]*K1[15][0])+(G1[15][2]*K1[15][2]))+ ((G2[15][2]*K2[15][2])+(udot[4]+(G2[15][0]*K2[15][0]))))); A2[15][1] = (udot[4]+udot[15]); K1[16][0] = (((K1[3][0]*c16)-(K1[3][2]*s16))+(((rikt[16][1][0]*udot[4])-( .09879136*(udot[3]*s16)))-(.09879136*(udot[5]*c16)))); K1[16][1] = (K1[3][1]-((.01052618*udot[5])+(.2639286*udot[3]))); K1[16][2] = (((K1[3][0]*s16)+(K1[3][2]*c16))+(((.09879136*(udot[3]*c16))+( rikt[16][1][2]*udot[4]))-(.09879136*(udot[5]*s16)))); K2[16][0] = ((udot[3]*c16)-(udot[5]*s16)); K2[16][2] = ((udot[3]*s16)+(udot[5]*c16)); udot[16] = ((DD[16]*eps[16])-(((G1[16][0]*K1[16][0])+(G1[16][2]*K1[16][2]))+ ((G2[16][2]*K2[16][2])+(udot[4]+(G2[16][0]*K2[16][0]))))); A2[16][1] = (udot[4]+udot[16]); K1[17][0] = (((.036*(K2[15][2]*c17))+((.036*(K2[15][0]*s17))-(.196*( A2[15][1]*c17))))+((K1[15][0]*c17)-(K1[15][2]*s17))); K1[17][1] = (K1[15][1]+(.196*K2[15][0])); K1[17][2] = (((.036*(K2[15][2]*s17))-((.036*(K2[15][0]*c17))+(.196*( A2[15][1]*s17))))+((K1[15][0]*s17)+(K1[15][2]*c17))); K2[17][0] = ((K2[15][0]*c17)-(K2[15][2]*s17)); K2[17][2] = ((K2[15][0]*s17)+(K2[15][2]*c17)); udot[17] = ((DD[17]*eps[17])-(((A2[15][1]+(G2[17][0]*K2[17][0]))+(G2[17][2]* K2[17][2]))+((G1[17][0]*K1[17][0])+(G1[17][2]*K1[17][2])))); A2[17][1] = (A2[15][1]+udot[17]); K1[18][0] = (((K1[16][0]*c18)-(K1[16][2]*s18))-((.036*(K2[16][2]*c18))+(( .036*(K2[16][0]*s18))+(.196*(A2[16][1]*c18))))); K1[18][1] = (K1[16][1]+(.196*K2[16][0])); K1[18][2] = (((K1[16][0]*s18)+(K1[16][2]*c18))+(((.036*(K2[16][0]*c18))-( .196*(A2[16][1]*s18)))-(.036*(K2[16][2]*s18)))); K2[18][0] = ((K2[16][0]*c18)-(K2[16][2]*s18)); K2[18][2] = ((K2[16][0]*s18)+(K2[16][2]*c18)); udot[18] = ((DD[18]*eps[18])-(((A2[16][1]+(G2[18][0]*K2[18][0]))+(G2[18][2]* K2[18][2]))+((G1[18][0]*K1[18][0])+(G1[18][2]*K1[18][2])))); A2[18][1] = (A2[16][1]+udot[18]); K1[19][0] = (((.024*(K2[17][2]*c19))+((.024*(K2[17][0]*s19))+(A2[17][1]* rikt[19][1][0])))+((K1[17][0]*c19)-(K1[17][2]*s19))); K1[19][1] = (K1[17][1]+((.022*K2[17][2])+(.17*K2[17][0]))); K1[19][2] = (((.024*(K2[17][2]*s19))+((A2[17][1]*rikt[19][1][2])-(.024*( K2[17][0]*c19))))+((K1[17][0]*s19)+(K1[17][2]*c19))); K2[19][0] = ((K2[17][0]*c19)-(K2[17][2]*s19)); K2[19][2] = ((K2[17][0]*s19)+(K2[17][2]*c19)); udot[19] = ((1185.34545417985*fs0[19])-(((.978182151053824*K1[19][0])-( .959249247688944*K1[19][2]))+((A2[17][1]-(.028064966614006*K2[19][0]))-( .240303874633767*K2[19][2])))); A2[19][1] = (A2[17][1]+udot[19]); K1[20][0] = (((K1[18][0]*c20)-(K1[18][2]*s20))+(((A2[18][1]*rikt[20][1][0])- (.024*(K2[18][0]*s20)))-(.024*(K2[18][2]*c20)))); K1[20][1] = (K1[18][1]+((.022*K2[18][2])+(.17*K2[18][0]))); K1[20][2] = (((K1[18][0]*s20)+(K1[18][2]*c20))+(((.024*(K2[18][0]*c20))+( A2[18][1]*rikt[20][1][2]))-(.024*(K2[18][2]*s20)))); K2[20][0] = ((K2[18][0]*c20)-(K2[18][2]*s20)); K2[20][2] = ((K2[18][0]*s20)+(K2[18][2]*c20)); udot[20] = ((1184.34517824184*fs0[20])-(((.240343600676817*K2[20][2])+( A2[18][1]+(.0274347997310184*K2[20][0])))+((.973576614358514*K1[20][0])-( .958615160326027*K1[20][2])))); A2[20][1] = (A2[18][1]+udot[20]); K1[21][0] = -(1.642*(udot[6]*c21)); K1[21][2] = -(1.642*(udot[6]*s21)); udot[21] = ((385.517906754109*fs0[21])-(udot[6]-((.000521432707401758* K1[21][2])+(2.84517801421117*K1[21][0])))); A2[21][1] = (udot[6]+udot[21]); K1[22][0] = -(1.642*(udot[7]*c22)); K1[22][2] = -(1.642*(udot[7]*s22)); udot[22] = ((386.601655908384*fs0[22])-(udot[7]-((.000522305028403106* K1[22][2])+(2.84758288062061*K1[22][0])))); A2[22][1] = (udot[7]+udot[22]); for (i = 0; i <= 22; i++) { oudot0[i] = udot[i]; } /* Used 0.75 seconds CPU time, 0 additional bytes of memory. Equations contain 527 adds/subtracts/negates 754 multiplies 0 divides 316 assignments */ } void BOCfly :: sdudot0 ( double oudot0[23] ) { if ((roustate != 2) && (roustate != 3)) { sdseterr(66,23); return; } sdxudot0(66,oudot0); } void BOCfly :: sdsetudot ( double iudot[23] ) { /* Assign udots and advance to stage Dynamics Ready */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(68,23); return; } for (i = 0; i <= 22; i++) { udot[i] = iudot[i]; } sdrhs(); } void BOCfly :: sdxudotm ( int routine, double imult[1], double oudotm[23] ) { /* Compute udots due only to multipliers */ int i; double eps[23],Z1[23][3],Z2[23][3],A1[23][3],A2[23][3],K1[23][3],K2[23][3]; sdlhs(routine); for (i = 0; i <= 22; i++) { udot[i] = 0.; } for (i = 0; i <= 22; i++) { oudotm[i] = udot[i]; } /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 46 assignments */ } void BOCfly :: sdudotm ( double imult[1], double oudotm[23] ) { if ((roustate != 2) && (roustate != 3)) { sdseterr(67,23); return; } sdxudotm(67,imult,oudotm); } void BOCfly :: sdderiv ( double oqdot[24], double oudot[23] ) { /* This is the derivative section for a 18-body free-sding system with 23 degree(s) of freedom, including 6 degrees of freedom for attitude and translation of the base body. */ int i; double udot0[23],udot1[23]; double eps[23],Z1[23][3],Z2[23][3],A1[23][3],A2[23][3],K1[23][3],K2[23][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(17,23); return; } if (stabvelq == 1) { sdseterr(17,32); } if (stabposq == 1) { sdseterr(17,33); } wsiz = 0; /* Compute unconstrained equations */ sdxudot0(17,udot0); sdrhs(); wrank = 0; for (i = 0; i <= 23; i++) { oqdot[i] = qdot[i]; } for (i = 0; i <= 22; i++) { oudot[i] = udot[i]; } /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 47 assignments */ } /* Compute residuals for use with DAE integrator */ void BOCfly :: sdresid ( double eqdot[24], double eudot[23], double emults[1], double resid[47] ) { int i; double uderrs[23]; if ((roustate != 2) && (roustate != 3)) { sdseterr(16,23); return; } if (stabposq == 1) { sdseterr(16,33); } sdfulltrq(eudot,emults,uderrs); for (i = 0; i < 24; i++) { resid[i] = eqdot[i]-qdot[i]; } for (i = 0; i < 23; i++) { resid[24+i] = uderrs[i]; } for (i = 0; i < 23; i++) { udot[i] = eudot[i]; } sdrhs(); /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 24 adds/subtracts/negates 0 multiplies 0 divides 70 assignments */ } void BOCfly :: sdmult ( double omults[1], int *owrank, int omultmap[1] ) { if (roustate != 3) { sdseterr(34,24); return; } *owrank = wrank; } void BOCfly :: sdreac ( double force[18][3], double torque[18][3] ) { /* Generated 27-Mar-2000 00:59:03 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 69086664 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 */ if (roustate != 3) { sdseterr(31,24); return; } /* Compute reaction forces for non-weld tree joints */ fc[22][0] = ((.02601109*(AnkAtk[22][0]-gk[22][0]))-ufk[17][0]); fc[22][1] = -ufk[17][1]; fc[22][2] = ((.02601109*(AnkAtk[22][2]-gk[22][2]))-ufk[17][2]); tc[22][0] = ((WkIkWk[22][0]-(1e-8*Onkb[22][1]))-(utk[17][0]+((.00250244* fc[22][2])-(.28317448*fc[22][1])))); tc[22][1] = ((.00050087*Onkb[22][1])-(utk[17][1]+((5.194e-5*fc[22][2])+( .28317448*fc[22][0])))); tc[22][2] = ((WkIkWk[22][2]+(.00013693*Onkb[22][1]))-(utk[17][2]-((5.194e-5* fc[22][1])+(.00250244*fc[22][0])))); fccikt[22][0] = ((fc[22][0]*c22)+(fc[22][2]*s22)); fccikt[22][1] = fc[22][1]; fccikt[22][2] = ((fc[22][2]*c22)-(fc[22][0]*s22)); ffk[7][0] = (ufk[2][0]-fccikt[22][0]); ffk[7][1] = (ufk[2][1]-fccikt[22][1]); ffk[7][2] = (ufk[2][2]-fccikt[22][2]); ttk[7][0] = (utk[2][0]-(((.00249756*fccikt[22][2])+(.41830132*fccikt[22][1]) )+((tc[22][0]*c22)+(tc[22][2]*s22)))); ttk[7][1] = (utk[2][1]-(tc[22][1]+((5.197e-5*fccikt[22][2])-(.41830132* fccikt[22][0])))); ttk[7][2] = (utk[2][2]-(((tc[22][2]*c22)-(tc[22][0]*s22))-((5.197e-5* fccikt[22][1])+(.00249756*fccikt[22][0])))); fc[21][0] = ((.0260106*(AnkAtk[21][0]-gk[21][0]))-ufk[16][0]); fc[21][1] = -ufk[16][1]; fc[21][2] = ((.0260106*(AnkAtk[21][2]-gk[21][2]))-ufk[16][2]); tc[21][0] = ((WkIkWk[21][0]+(1e-8*Onkb[21][1]))-(utk[16][0]+((5.59e-6* fc[21][2])-(.28373605*fc[21][1])))); tc[21][1] = ((.0004999*Onkb[21][1])-(utk[16][1]+((5.2e-5*fc[21][2])+( .28373605*fc[21][0])))); tc[21][2] = ((WkIkWk[21][2]-(.00013462*Onkb[21][1]))-(utk[16][2]-((5.59e-6* fc[21][0])+(5.2e-5*fc[21][1])))); fccikt[21][0] = ((fc[21][0]*c21)+(fc[21][2]*s21)); fccikt[21][1] = fc[21][1]; fccikt[21][2] = ((fc[21][2]*c21)-(fc[21][0]*s21)); ffk[6][0] = (ufk[1][0]-fccikt[21][0]); ffk[6][1] = (ufk[1][1]-fccikt[21][1]); ffk[6][2] = (ufk[1][2]-fccikt[21][2]); ttk[6][0] = (utk[1][0]-(((3.3e-7*fccikt[21][2])+(.4177396*fccikt[21][1]))+(( tc[21][0]*c21)+(tc[21][2]*s21)))); ttk[6][1] = (utk[1][1]-(tc[21][1]+((5.201e-5*fccikt[21][2])-(.4177396* fccikt[21][0])))); ttk[6][2] = (utk[1][2]-(((tc[21][2]*c21)-(tc[21][0]*s21))-((3.3e-7* fccikt[21][0])+(5.201e-5*fccikt[21][1])))); fc[20][0] = ((.05944511*(AnkAtk[20][0]-gk[20][0]))-ufk[15][0]); fc[20][1] = ((.05944511*(AnkAtk[20][1]+(9.80665*Cik[3][2][1])))-ufk[15][1]); fc[20][2] = ((.05944511*(AnkAtk[20][2]-gk[20][2]))-ufk[15][2]); tc[20][0] = ((WkIkWk[20][0]+((7.338e-5*onk[20][2])+((2.77e-5*Onkb[20][1])+( .00086575*onk[20][0]))))-(utk[15][0]+((.01382852*fc[20][1])-(.00560346* fc[20][2])))); tc[20][1] = ((WkIkWk[20][1]+((.00020754*onk[20][2])+((2.77e-5*onk[20][0])+( .00082196*Onkb[20][1]))))-(utk[15][1]+((.01361601*fc[20][2])-(.01382852* fc[20][0])))); tc[20][2] = ((WkIkWk[20][2]+((3.093e-5*onk[20][2])+((7.338e-5*onk[20][0])+( .00020754*Onkb[20][1]))))-(utk[15][2]+((.00560346*fc[20][0])-(.01361601* fc[20][1])))); fccikt[20][0] = ((fc[20][0]*c20)+(fc[20][2]*s20)); fccikt[20][1] = fc[20][1]; fccikt[20][2] = ((fc[20][2]*c20)-(fc[20][0]*s20)); ffk[18][0] = (ufk[13][0]-fccikt[20][0]); ffk[18][1] = (ufk[13][1]-fccikt[20][1]); ffk[18][2] = (ufk[13][2]-fccikt[20][2]); ttk[18][0] = (utk[13][0]-(((.01739645*fccikt[20][1])+(.02325425* fccikt[20][2]))+((tc[20][0]*c20)+(tc[20][2]*s20)))); ttk[18][1] = (utk[13][1]-(tc[20][1]-((.00792098*fccikt[20][2])+(.01739645* fccikt[20][0])))); ttk[18][2] = (utk[13][2]-(((.00792098*fccikt[20][1])-(.02325425* fccikt[20][0]))+((tc[20][2]*c20)-(tc[20][0]*s20)))); fc[19][0] = ((.05946345*(AnkAtk[19][0]-gk[19][0]))-ufk[14][0]); fc[19][1] = ((.05946345*(AnkAtk[19][1]+(9.80665*Cik[3][2][1])))-ufk[14][1]); fc[19][2] = ((.05946345*(AnkAtk[19][2]-gk[19][2]))-ufk[14][2]); tc[19][0] = ((WkIkWk[19][0]+((7.331e-5*onk[19][2])+((.00086458*onk[19][0])-( 2.761e-5*Onkb[19][1]))))-(utk[14][0]+((.00486049*fc[19][2])+(.01387793* fc[19][1])))); tc[19][1] = ((WkIkWk[19][1]+(((.00082117*Onkb[19][1])-(2.761e-5*onk[19][0])) -(.00020674*onk[19][2])))-(utk[14][1]+((.01360932*fc[19][2])-(.01387793* fc[19][0])))); tc[19][2] = ((WkIkWk[19][2]+((3.059e-5*onk[19][2])+((7.331e-5*onk[19][0])-( .00020674*Onkb[19][1]))))-(utk[14][2]-((.00486049*fc[19][0])+(.01360932* fc[19][1])))); fccikt[19][0] = ((fc[19][0]*c19)+(fc[19][2]*s19)); fccikt[19][1] = fc[19][1]; fccikt[19][2] = ((fc[19][2]*c19)-(fc[19][0]*s19)); ffk[17][0] = (ufk[12][0]-fccikt[19][0]); ffk[17][1] = (ufk[12][1]-fccikt[19][1]); ffk[17][2] = (ufk[12][2]-fccikt[19][2]); ttk[17][0] = (utk[12][0]-(((.01750499*fccikt[19][1])-(.02455916* fccikt[19][2]))+((tc[19][0]*c19)+(tc[19][2]*s19)))); ttk[17][1] = (utk[12][1]-(tc[19][1]-((.00792736*fccikt[19][2])+(.01750499* fccikt[19][0])))); ttk[17][2] = (utk[12][2]-(((.00792736*fccikt[19][1])+(.02455916* fccikt[19][0]))+((tc[19][2]*c19)-(tc[19][0]*s19)))); fc[18][0] = ((.43358398*(AnkAtk[18][0]-gk[18][0]))-ffk[18][0]); fc[18][1] = ((.43358398*(AnkAtk[18][1]+(9.80665*Cik[3][2][1])))-ffk[18][1]); fc[18][2] = ((.43358398*(AnkAtk[18][2]-gk[18][2]))-ffk[18][2]); tc[18][0] = ((WkIkWk[18][0]+((.00047517*onk[18][2])+((3.024e-5*Onkb[18][1])+ (.00933861*onk[18][0]))))-(ttk[18][0]-((.00074575*fc[18][2])+(.15260355* fc[18][1])))); tc[18][1] = ((WkIkWk[18][1]+(((3.024e-5*onk[18][0])+(.00915219*Onkb[18][1])) -(.00106364*onk[18][2])))-(ttk[18][1]+((.01407902*fc[18][2])+(.15260355* fc[18][0])))); tc[18][2] = ((WkIkWk[18][2]+((.00032466*onk[18][2])+((.00047517*onk[18][0])- (.00106364*Onkb[18][1]))))-(ttk[18][2]+((.00074575*fc[18][0])-(.01407902* fc[18][1])))); fccikt[18][0] = ((fc[18][0]*c18)+(fc[18][2]*s18)); fccikt[18][1] = fc[18][1]; fccikt[18][2] = ((fc[18][2]*c18)-(fc[18][0]*s18)); ffk[16][0] = (ufk[11][0]-fccikt[18][0]); ffk[16][1] = (ufk[11][1]-fccikt[18][1]); ffk[16][2] = (ufk[11][2]-fccikt[18][2]); ttk[16][0] = (utk[11][0]-(((.02604073*fccikt[18][2])-(.01408605* fccikt[18][1]))+((tc[18][0]*c18)+(tc[18][2]*s18)))); ttk[16][1] = (utk[11][1]-(tc[18][1]+((.00305957*fccikt[18][2])+(.01408605* fccikt[18][0])))); ttk[16][2] = (utk[11][2]-(((tc[18][2]*c18)-(tc[18][0]*s18))-((.00305957* fccikt[18][1])+(.02604073*fccikt[18][0])))); fc[17][0] = ((.43362906*(AnkAtk[17][0]-gk[17][0]))-ffk[17][0]); fc[17][1] = ((.43362906*(AnkAtk[17][1]+(9.80665*Cik[3][2][1])))-ffk[17][1]); fc[17][2] = ((.43362906*(AnkAtk[17][2]-gk[17][2]))-ffk[17][2]); tc[17][0] = ((WkIkWk[17][0]+((.00047536*onk[17][2])+((.00933952*onk[17][0])- (2.97e-5*Onkb[17][1]))))-(ttk[17][0]-((.00055916*fc[17][2])+(.15249501* fc[17][1])))); tc[17][1] = ((WkIkWk[17][1]+((.00105064*onk[17][2])+((.00915719*Onkb[17][1]) -(2.97e-5*onk[17][0]))))-(ttk[17][1]+((.01407264*fc[17][2])+(.15249501* fc[17][0])))); tc[17][2] = ((WkIkWk[17][2]+((.00032059*onk[17][2])+((.00047536*onk[17][0])+ (.00105064*Onkb[17][1]))))-(ttk[17][2]+((.00055916*fc[17][0])-(.01407264* fc[17][1])))); fccikt[17][0] = ((fc[17][0]*c17)+(fc[17][2]*s17)); fccikt[17][1] = fc[17][1]; fccikt[17][2] = ((fc[17][2]*c17)-(fc[17][0]*s17)); ffk[15][0] = (ufk[10][0]-fccikt[17][0]); ffk[15][1] = (ufk[10][1]-fccikt[17][1]); ffk[15][2] = (ufk[10][2]-fccikt[17][2]); ttk[15][0] = (utk[10][0]-(((tc[17][0]*c17)+(tc[17][2]*s17))-((.01305614* fccikt[17][1])+(.02714343*fccikt[17][2])))); ttk[15][1] = (utk[10][1]-(tc[17][1]+((.00307461*fccikt[17][2])+(.01305614* fccikt[17][0])))); ttk[15][2] = (utk[10][2]-(((.02714343*fccikt[17][0])-(.00307461* fccikt[17][1]))+((tc[17][2]*c17)-(tc[17][0]*s17)))); fc[16][0] = ((1.01701701*(AnkAtk[16][0]-gk[16][0]))-ffk[16][0]); fc[16][1] = ((1.01701701*(AnkAtk[16][1]+(9.80665*Cik[3][2][1])))-ffk[16][1]); fc[16][2] = ((1.01701701*(AnkAtk[16][2]-gk[16][2]))-ffk[16][2]); tc[16][0] = ((WkIkWk[16][0]+((.00099076*onk[16][2])+((9.583e-5*Onkb[16][1])+ (.03712787*onk[16][0]))))-(ttk[16][0]-((.00995927*fc[16][2])+(.21008605* fc[16][1])))); tc[16][1] = ((WkIkWk[16][1]+(((9.583e-5*onk[16][0])+(.03679631*Onkb[16][1])) -(.00244367*onk[16][2])))-(ttk[16][1]+((.00305957*fc[16][2])+(.21008605* fc[16][0])))); tc[16][2] = ((WkIkWk[16][2]+((.00074417*onk[16][2])+((.00099076*onk[16][0])- (.00244367*Onkb[16][1]))))-(ttk[16][2]+((.00995927*fc[16][0])-(.00305957* fc[16][1])))); fccikt[16][0] = ((fc[16][0]*c16)+(fc[16][2]*s16)); fccikt[16][1] = fc[16][1]; fccikt[16][2] = ((fc[16][2]*c16)-(fc[16][0]*s16)); ffk[5][0] = (ufk[0][0]-fccikt[16][0]); ffk[5][1] = (ufk[0][1]-fccikt[16][1]); ffk[5][2] = (ufk[0][2]-fccikt[16][2]); ttk[5][0] = (utk[0][0]-(((.09879136*fccikt[16][2])-(.2639286*fccikt[16][1])) +((tc[16][0]*c16)+(tc[16][2]*s16)))); ttk[5][1] = (utk[0][1]-(tc[16][1]+((.01052618*fccikt[16][2])+(.2639286* fccikt[16][0])))); ttk[5][2] = (utk[0][2]-(((tc[16][2]*c16)-(tc[16][0]*s16))-((.01052618* fccikt[16][1])+(.09879136*fccikt[16][0])))); fc[15][0] = ((1.00985702*(AnkAtk[15][0]-gk[15][0]))-ffk[15][0]); fc[15][1] = ((1.00985702*(AnkAtk[15][1]+(9.80665*Cik[3][2][1])))-ffk[15][1]); fc[15][2] = ((1.00985702*(AnkAtk[15][2]-gk[15][2]))-ffk[15][2]); tc[15][0] = ((WkIkWk[15][0]+((.00098554*onk[15][2])+((.03667794*onk[15][0])- (9.282e-5*Onkb[15][1]))))-(ttk[15][0]+((.00885657*fc[15][2])-(.20905614* fc[15][1])))); tc[15][1] = ((WkIkWk[15][1]+((.00236551*onk[15][2])+((.03636003*Onkb[15][1]) -(9.282e-5*onk[15][0]))))-(ttk[15][1]+((.00307461*fc[15][2])+(.20905614* fc[15][0])))); tc[15][2] = ((WkIkWk[15][2]+((.00072894*onk[15][2])+((.00098554*onk[15][0])+ (.00236551*Onkb[15][1]))))-(ttk[15][2]-((.00307461*fc[15][1])+(.00885657* fc[15][0])))); fccikt[15][0] = ((fc[15][0]*c15)+(fc[15][2]*s15)); fccikt[15][1] = fc[15][1]; fccikt[15][2] = ((fc[15][2]*c15)-(fc[15][0]*s15)); ffk[5][0] = (ffk[5][0]-fccikt[15][0]); ffk[5][1] = (ffk[5][1]-fccikt[15][1]); ffk[5][2] = (ffk[5][2]-fccikt[15][2]); ttk[5][0] = (ttk[5][0]-(((tc[15][0]*c15)+(tc[15][2]*s15))-((.10120864* fccikt[15][2])+(.2639286*fccikt[15][1])))); ttk[5][1] = (ttk[5][1]-(tc[15][1]+((.01052618*fccikt[15][2])+(.2639286* fccikt[15][0])))); ttk[5][2] = (ttk[5][2]-(((.10120864*fccikt[15][0])-(.01052618*fccikt[15][1]) )+((tc[15][2]*c15)-(tc[15][0]*s15)))); fc[14][0] = ((.5325902*(AnkAtk[14][0]-gk[14][0]))-ufk[9][0]); fc[14][1] = ((.5325902*(AnkAtk[14][1]+(9.80665*Cik[3][2][1])))-ufk[9][1]); fc[14][2] = ((.5325902*(AnkAtk[14][2]-gk[14][2]))-ufk[9][2]); tc[14][0] = ((WkIkWk[14][0]+((.00016063*onk[14][2])+((1.71e-5*Onkb[14][1])+( .00031249*onk[14][0]))))-(utk[9][0]-((.00095499*fc[14][2])+(.03987912* fc[14][1])))); tc[14][1] = ((WkIkWk[14][1]+(((1.71e-5*onk[14][0])+(.00112646*Onkb[14][1]))- (4.13e-6*onk[14][2])))-(utk[9][1]+((.03539679*fc[14][2])+(.03987912* fc[14][0])))); tc[14][2] = ((WkIkWk[14][2]+((.00102891*onk[14][2])+((.00016063*onk[14][0])- (4.13e-6*Onkb[14][1]))))-(utk[9][2]+((.00095499*fc[14][0])-(.03539679* fc[14][1])))); fccikt[14][0] = ((fc[14][0]*c14)+(fc[14][2]*s14)); fccikt[14][1] = fc[14][1]; fccikt[14][2] = ((fc[14][2]*c14)-(fc[14][0]*s14)); ffk[12][0] = (ufk[7][0]-fccikt[14][0]); ffk[12][1] = (ufk[7][1]-fccikt[14][1]); ffk[12][2] = (ufk[7][2]-fccikt[14][2]); ttk[12][0] = (utk[7][0]-(((.17112496*fccikt[14][1])-(.00157104*fccikt[14][2] ))+((tc[14][0]*c14)+(tc[14][2]*s14)))); ttk[12][1] = (utk[7][1]-(tc[14][1]-((.00174373*fccikt[14][2])+(.17112496* fccikt[14][0])))); ttk[12][2] = (utk[7][2]-(((.00157104*fccikt[14][0])+(.00174373*fccikt[14][1] ))+((tc[14][2]*c14)-(tc[14][0]*s14)))); fc[13][0] = ((.53380008*(AnkAtk[13][0]-gk[13][0]))-ufk[8][0]); fc[13][1] = ((.53380008*(AnkAtk[13][1]+(9.80665*Cik[3][2][1])))-ufk[8][1]); fc[13][2] = ((.53380008*(AnkAtk[13][2]-gk[13][2]))-ufk[8][2]); tc[13][0] = ((WkIkWk[13][0]+((.00016083*onk[13][2])+((.00031494*onk[13][0])- (1.886e-5*Onkb[13][1]))))-(utk[8][0]-((.00193095*fc[13][2])+(.03983908* fc[13][1])))); tc[13][1] = ((WkIkWk[13][1]+((4.29e-6*onk[13][2])+((.00112921*Onkb[13][1])-( 1.886e-5*onk[13][0]))))-(utk[8][1]+((.03543467*fc[13][2])+(.03983908* fc[13][0])))); tc[13][2] = ((WkIkWk[13][2]+((.00103272*onk[13][2])+((4.29e-6*Onkb[13][1])+( .00016083*onk[13][0]))))-(utk[8][2]+((.00193095*fc[13][0])-(.03543467* fc[13][1])))); fccikt[13][0] = ((fc[13][0]*c13)+(fc[13][2]*s13)); fccikt[13][1] = fc[13][1]; fccikt[13][2] = ((fc[13][2]*c13)-(fc[13][0]*s13)); ffk[11][0] = (ufk[6][0]-fccikt[13][0]); ffk[11][1] = (ufk[6][1]-fccikt[13][1]); ffk[11][2] = (ufk[6][2]-fccikt[13][2]); ttk[11][0] = (utk[6][0]-(((.17081701*fccikt[13][1])-(.00061135*fccikt[13][2] ))+((tc[13][0]*c13)+(tc[13][2]*s13)))); ttk[11][1] = (utk[6][1]-(tc[13][1]-((.00172166*fccikt[13][2])+(.17081701* fccikt[13][0])))); ttk[11][2] = (utk[6][2]-(((.00061135*fccikt[13][0])+(.00172166*fccikt[13][1] ))+((tc[13][2]*c13)-(tc[13][0]*s13)))); fc[12][0] = ((1.02007998*(AnkAtk[12][0]-gk[12][0]))-ffk[12][0]); fc[12][1] = ((1.02007998*(AnkAtk[12][1]+(9.80665*Cik[3][2][1])))-ffk[12][1]); fc[12][2] = ((1.02007998*(AnkAtk[12][2]-gk[12][2]))-ffk[12][2]); tc[12][0] = ((WkIkWk[12][0]+(((.00821833*onk[12][0])-(1.283e-5*Onkb[12][1])) -(.00026918*onk[12][2])))-(ttk[12][0]+((.00342896*fc[12][2])-(.12587504* fc[12][1])))); tc[12][1] = ((WkIkWk[12][1]+(((.00838445*Onkb[12][1])-(1.283e-5*onk[12][0])) -(.00019836*onk[12][2])))-(ttk[12][1]+((.12587504*fc[12][0])-(.01674373* fc[12][2])))); tc[12][2] = ((WkIkWk[12][2]+((.00057568*onk[12][2])-((.00019836*Onkb[12][1]) +(.00026918*onk[12][0]))))-(ttk[12][2]+((.01674373*fc[12][1])-(.00342896* fc[12][0])))); fccikt[12][0] = ((fc[12][0]*c12)+(fc[12][2]*s12)); fccikt[12][1] = fc[12][1]; fccikt[12][2] = ((fc[12][2]*c12)-(fc[12][0]*s12)); ffk[10][0] = (ufk[5][0]-fccikt[12][0]); ffk[10][1] = (ufk[5][1]-fccikt[12][1]); ffk[10][2] = (ufk[5][2]-fccikt[12][2]); ttk[10][0] = (utk[5][0]-(((.00064476*fccikt[12][2])+(.15909602*fccikt[12][1] ))+((tc[12][0]*c12)+(tc[12][2]*s12)))); ttk[10][1] = (utk[5][1]-(tc[12][1]-((.00656361*fccikt[12][2])+(.15909602* fccikt[12][0])))); ttk[10][2] = (utk[5][2]-(((.00656361*fccikt[12][1])-(.00064476*fccikt[12][0] ))+((tc[12][2]*c12)-(tc[12][0]*s12)))); fc[11][0] = ((1.02133193*(AnkAtk[11][0]-gk[11][0]))-ffk[11][0]); fc[11][1] = ((1.02133193*(AnkAtk[11][1]+(9.80665*Cik[3][2][1])))-ffk[11][1]); fc[11][2] = ((1.02133193*(AnkAtk[11][2]-gk[11][2]))-ffk[11][2]); tc[11][0] = ((WkIkWk[11][0]+(((1.273e-5*Onkb[11][1])+(.00826684*onk[11][0])) -(.00026397*onk[11][2])))-(ttk[11][0]-((.00561135*fc[11][2])+(.12618299* fc[11][1])))); tc[11][1] = ((WkIkWk[11][1]+((.00020104*onk[11][2])+((1.273e-5*onk[11][0])+( .00843352*Onkb[11][1]))))-(ttk[11][1]+((.12618299*fc[11][0])-(.01672166* fc[11][2])))); tc[11][2] = ((WkIkWk[11][2]+((.00057599*onk[11][2])+((.00020104*Onkb[11][1]) -(.00026397*onk[11][0]))))-(ttk[11][2]+((.00561135*fc[11][0])+(.01672166* fc[11][1])))); fccikt[11][0] = ((fc[11][0]*c11)+(fc[11][2]*s11)); fccikt[11][1] = fc[11][1]; fccikt[11][2] = ((fc[11][2]*c11)-(fc[11][0]*s11)); ffk[9][0] = (ufk[4][0]-fccikt[11][0]); ffk[9][1] = (ufk[4][1]-fccikt[11][1]); ffk[9][2] = (ufk[4][2]-fccikt[11][2]); ttk[9][0] = (utk[4][0]-(((.15931143*fccikt[11][1])-(.00284123*fccikt[11][2]) )+((tc[11][0]*c11)+(tc[11][2]*s11)))); ttk[9][1] = (utk[4][1]-(tc[11][1]-((.0065711*fccikt[11][2])+(.15931143* fccikt[11][0])))); ttk[9][2] = (utk[4][2]-(((.00284123*fccikt[11][0])+(.0065711*fccikt[11][1])) +((tc[11][2]*c11)-(tc[11][0]*s11)))); fc[10][0] = ((2.8984419*(AnkAtk[10][0]-gk[10][0]))-ffk[10][0]); fc[10][1] = ((2.8984419*(AnkAtk[10][1]+(9.80665*Cik[3][2][1])))-ffk[10][1]); fc[10][2] = ((2.8984419*(AnkAtk[10][2]-gk[10][2]))-ffk[10][2]); tc[10][0] = ((WkIkWk[10][0]+((.00224427*onk[10][2])+((6.279e-5*Onkb[10][1])+ (.05476817*onk[10][0]))))-(ttk[10][0]-((.00235524*fc[10][2])+(.13190398* fc[10][1])))); tc[10][1] = ((WkIkWk[10][1]+(((6.279e-5*onk[10][0])+(.05493593*Onkb[10][1])) -(.00156061*onk[10][2])))-(ttk[10][1]+((.00943639*fc[10][2])+(.13190398* fc[10][0])))); tc[10][2] = ((WkIkWk[10][2]+((.00344556*onk[10][2])+((.00224427*onk[10][0])- (.00156061*Onkb[10][1]))))-(ttk[10][2]+((.00235524*fc[10][0])-(.00943639* fc[10][1])))); fccikt[10][0] = ((fc[10][0]*c10)+(fc[10][2]*s10)); fccikt[10][1] = fc[10][1]; fccikt[10][2] = ((fc[10][2]*c10)-(fc[10][0]*s10)); ffk[5][0] = (ffk[5][0]-fccikt[10][0]); ffk[5][1] = (ffk[5][1]-fccikt[10][1]); ffk[5][2] = (ffk[5][2]-fccikt[10][2]); ttk[5][0] = (ttk[5][0]-(((.04579136*fccikt[10][2])+(.0620714*fccikt[10][1])) +((tc[10][0]*c10)+(tc[10][2]*s10)))); ttk[5][1] = (ttk[5][1]-(tc[10][1]+((.01952618*fccikt[10][2])-(.0620714* fccikt[10][0])))); ttk[5][2] = (ttk[5][2]-(((tc[10][2]*c10)-(tc[10][0]*s10))-((.01952618* fccikt[10][1])+(.04579136*fccikt[10][0])))); fc[9][0] = ((2.89586298*(AnkAtk[9][0]-gk[9][0]))-ffk[9][0]); fc[9][1] = ((2.89586298*(AnkAtk[9][1]+(9.80665*Cik[3][2][1])))-ffk[9][1]); fc[9][2] = ((2.89586298*(AnkAtk[9][2]-gk[9][2]))-ffk[9][2]); tc[9][0] = ((WkIkWk[9][0]+((.00223918*onk[9][2])+((.05459333*onk[9][0])-( 5.918e-5*Onkb[9][1]))))-(ttk[9][0]+((.00015877*fc[9][2])-(.13168857* fc[9][1])))); tc[9][1] = ((WkIkWk[9][1]+((.00147758*onk[9][2])+((.05477009*Onkb[9][1])-( 5.918e-5*onk[9][0]))))-(ttk[9][1]+((.0094289*fc[9][2])+(.13168857*fc[9][0] )))); tc[9][2] = ((WkIkWk[9][2]+((.00343412*onk[9][2])+((.00147758*Onkb[9][1])+( .00223918*onk[9][0]))))-(ttk[9][2]-((.00015877*fc[9][0])+(.0094289* fc[9][1])))); fccikt[9][0] = ((fc[9][0]*c9)+(fc[9][2]*s9)); fccikt[9][1] = fc[9][1]; fccikt[9][2] = ((fc[9][2]*c9)-(fc[9][0]*s9)); ffk[5][0] = (ffk[5][0]-fccikt[9][0]); ffk[5][1] = (ffk[5][1]-fccikt[9][1]); ffk[5][2] = (ffk[5][2]-fccikt[9][2]); ttk[5][0] = (ttk[5][0]-(((.0620714*fccikt[9][1])-(.04820864*fccikt[9][2]))+( (tc[9][0]*c9)+(tc[9][2]*s9)))); ttk[5][1] = (ttk[5][1]-(tc[9][1]+((.01952618*fccikt[9][2])-(.0620714* fccikt[9][0])))); ttk[5][2] = (ttk[5][2]-(((.04820864*fccikt[9][0])-(.01952618*fccikt[9][1]))+ ((tc[9][2]*c9)-(tc[9][0]*s9)))); fc[8][0] = ((9.84983599*(AnkAtk[8][0]-gk[8][0]))-ufk[3][0]); fc[8][1] = ((9.84983599*(AnkAtk[8][1]+(9.80665*Cik[3][2][1])))-ufk[3][1]); fc[8][2] = ((9.84983599*(AnkAtk[8][2]-gk[8][2]))-ufk[3][2]); tc[8][0] = ((WkIkWk[8][0]+((.00823665*onk[8][2])+((1.657e-5*Onkb[8][1])+( .52257967*onk[8][0]))))-(utk[3][0]-((.0007183*fc[8][2])+(.12000071* fc[8][1])))); tc[8][1] = ((WkIkWk[8][1]+(((1.657e-5*onk[8][0])+(.52474672*Onkb[8][1]))-( .00036016*onk[8][2])))-(utk[3][1]+((.01691003*fc[8][2])+(.12000071* fc[8][0])))); tc[8][2] = ((WkIkWk[8][2]+((.01799022*onk[8][2])+((.00823665*onk[8][0])-( .00036016*Onkb[8][1]))))-(utk[3][2]+((.0007183*fc[8][0])-(.01691003* fc[8][1])))); fccikt[8][0] = ((fc[8][0]*c8)+(fc[8][2]*s8)); fccikt[8][1] = fc[8][1]; fccikt[8][2] = ((fc[8][2]*c8)-(fc[8][0]*s8)); ffk[5][0] = (ffk[5][0]-fccikt[8][0]); ffk[5][1] = (ffk[5][1]-fccikt[8][1]); ffk[5][2] = (ffk[5][2]-fccikt[8][2]); ttk[5][0] = (ttk[5][0]-(((tc[8][0]*c8)+(tc[8][2]*s8))-((.00120864* fccikt[8][2])+(.3829286*fccikt[8][1])))); ttk[5][1] = (ttk[5][1]-(tc[8][1]+((.00252618*fccikt[8][2])+(.3829286* fccikt[8][0])))); ttk[5][2] = (ttk[5][2]-(((.00120864*fccikt[8][0])-(.00252618*fccikt[8][1]))+ ((tc[8][2]*c8)-(tc[8][0]*s8)))); fc[7][0] = ((.1816698*(AnkAtk[7][0]-(9.80665*s7)))-ffk[7][0]); fc[7][1] = -ffk[7][1]; fc[7][2] = ((.1816698*(AnkAtk[7][2]+(9.80665*c7)))-ffk[7][2]); tc[7][0] = ((WkIkWk[7][0]-(8e-8*udot[7]))-(ttk[7][0]+((.00249756*fc[7][2])-( 1.22369868*fc[7][1])))); tc[7][1] = ((.04207299*udot[7])-(ttk[7][1]+((5.197e-5*fc[7][2])+(1.22369868* fc[7][0])))); tc[7][2] = ((WkIkWk[7][2]+(.00230647*udot[7]))-(ttk[7][2]-((5.197e-5* fc[7][1])+(.00249756*fc[7][0])))); fc[6][0] = ((.18166784*(AnkAtk[6][0]-(9.80665*s6)))-ffk[6][0]); fc[6][1] = -ffk[6][1]; fc[6][2] = ((.18166784*(AnkAtk[6][2]+(9.80665*c6)))-ffk[6][2]); tc[6][0] = ((WkIkWk[6][0]+(8e-8*udot[6]))-(ttk[6][0]+((3.3e-7*fc[6][2])-( 1.2242604*fc[6][1])))); tc[6][1] = ((.04205639*udot[6])-(ttk[6][1]+((5.201e-5*fc[6][2])+(1.2242604* fc[6][0])))); tc[6][2] = ((WkIkWk[6][2]-(.00226939*udot[6]))-(ttk[6][2]-((3.3e-7*fc[6][0]) +(5.201e-5*fc[6][1])))); fc[5][0] = ((11.02968359*(Ankb[3][0]+(9.80665*Cik[3][2][0])))-ffk[5][0]); fc[5][1] = ((11.02968359*(Ankb[3][1]+(9.80665*Cik[3][2][1])))-ffk[5][1]); fc[5][2] = ((11.02968359*(Ankb[3][2]+(9.80665*Cik[3][2][2])))-ffk[5][2]); tc[5][0] = ((WkIkWk[5][0]+(((.48506516*udot[3])-(5.312e-5*udot[4]))-( .01450199*udot[5])))-ttk[5][0]); tc[5][1] = ((WkIkWk[5][1]+(((.47268161*udot[4])-(5.312e-5*udot[3]))-( .00031516*udot[5])))-ttk[5][1]); tc[5][2] = ((WkIkWk[5][2]+((.03863225*udot[5])-((.00031516*udot[4])+( .01450199*udot[3]))))-ttk[5][2]); fccikt[5][0] = fc[5][0]; fccikt[5][1] = fc[5][1]; fccikt[5][2] = fc[5][2]; ffk[4][0] = -fccikt[5][0]; ffk[4][1] = -fccikt[5][1]; ffk[4][2] = -fccikt[5][2]; ttk[4][0] = -tc[5][0]; ttk[4][1] = -tc[5][1]; ttk[4][2] = -tc[5][2]; fc[4][0] = -ffk[4][0]; fc[4][1] = -ffk[4][1]; fc[4][2] = -ffk[4][2]; tc[4][0] = -ttk[4][0]; tc[4][1] = -ttk[4][1]; tc[4][2] = -ttk[4][2]; fccikt[4][0] = fc[4][0]; fccikt[4][1] = fc[4][1]; fccikt[4][2] = fc[4][2]; ffk[3][0] = -fccikt[4][0]; ffk[3][1] = -fccikt[4][1]; ffk[3][2] = -fccikt[4][2]; ttk[3][0] = -tc[4][0]; ttk[3][1] = -tc[4][1]; ttk[3][2] = -tc[4][2]; fc[3][0] = -ffk[3][0]; fc[3][1] = -ffk[3][1]; fc[3][2] = -ffk[3][2]; tc[3][0] = -ttk[3][0]; tc[3][1] = -ttk[3][1]; tc[3][2] = -ttk[3][2]; fccikt[3][0] = ((Cik[3][0][2]*fc[3][2])+((Cik[3][0][0]*fc[3][0])+( Cik[3][0][1]*fc[3][1]))); fccikt[3][1] = ((Cik[3][1][2]*fc[3][2])+((Cik[3][1][0]*fc[3][0])+( Cik[3][1][1]*fc[3][1]))); fccikt[3][2] = ((Cik[3][2][2]*fc[3][2])+((Cik[3][2][0]*fc[3][0])+( Cik[3][2][1]*fc[3][1]))); ffk[2][0] = -fccikt[3][0]; ffk[2][1] = -fccikt[3][1]; ffk[2][2] = -fccikt[3][2]; ttk[2][0] = -((Cik[3][0][2]*tc[3][2])+((Cik[3][0][0]*tc[3][0])+(Cik[3][0][1] *tc[3][1]))); ttk[2][1] = -((Cik[3][1][2]*tc[3][2])+((Cik[3][1][0]*tc[3][0])+(Cik[3][1][1] *tc[3][1]))); ttk[2][2] = -((Cik[3][2][2]*tc[3][2])+((Cik[3][2][0]*tc[3][0])+(Cik[3][2][1] *tc[3][1]))); fc[2][0] = -ffk[2][0]; fc[2][1] = -ffk[2][1]; fc[2][2] = -ffk[2][2]; tc[2][0] = -ttk[2][0]; tc[2][1] = -ttk[2][1]; tc[2][2] = -ttk[2][2]; fccikt[2][0] = fc[2][0]; fccikt[2][1] = fc[2][1]; fccikt[2][2] = fc[2][2]; ffk[1][0] = -fccikt[2][0]; ffk[1][1] = -fccikt[2][1]; ffk[1][2] = -fccikt[2][2]; ttk[1][0] = -(tc[2][0]-(fccikt[2][1]*q[2])); ttk[1][1] = -(tc[2][1]+(fccikt[2][0]*q[2])); ttk[1][2] = -tc[2][2]; fc[1][0] = -ffk[1][0]; fc[1][1] = -ffk[1][1]; fc[1][2] = -ffk[1][2]; tc[1][0] = -ttk[1][0]; tc[1][1] = -ttk[1][1]; tc[1][2] = -ttk[1][2]; fccikt[1][0] = fc[1][0]; fccikt[1][1] = fc[1][1]; fccikt[1][2] = fc[1][2]; ffk[0][0] = -fccikt[1][0]; ffk[0][1] = -fccikt[1][1]; ffk[0][2] = -fccikt[1][2]; ttk[0][0] = -(tc[1][0]+(fccikt[1][2]*q[1])); ttk[0][1] = -tc[1][1]; ttk[0][2] = -(tc[1][2]-(fccikt[1][0]*q[1])); fc[0][0] = -ffk[0][0]; fc[0][1] = -ffk[0][1]; fc[0][2] = -ffk[0][2]; tc[0][0] = -ttk[0][0]; tc[0][1] = -ttk[0][1]; tc[0][2] = -ttk[0][2]; force[0][0] = fc[5][0]; torque[0][0] = tc[5][0]; force[0][1] = fc[5][1]; torque[0][1] = tc[5][1]; force[0][2] = fc[5][2]; torque[0][2] = tc[5][2]; force[1][0] = fc[6][0]; torque[1][0] = tc[6][0]; force[1][1] = fc[6][1]; torque[1][1] = tc[6][1]; force[1][2] = fc[6][2]; torque[1][2] = tc[6][2]; force[2][0] = fc[7][0]; torque[2][0] = tc[7][0]; force[2][1] = fc[7][1]; torque[2][1] = tc[7][1]; force[2][2] = fc[7][2]; torque[2][2] = tc[7][2]; force[3][0] = fc[8][0]; torque[3][0] = tc[8][0]; force[3][1] = fc[8][1]; torque[3][1] = tc[8][1]; force[3][2] = fc[8][2]; torque[3][2] = tc[8][2]; force[4][0] = fc[9][0]; torque[4][0] = tc[9][0]; force[4][1] = fc[9][1]; torque[4][1] = tc[9][1]; force[4][2] = fc[9][2]; torque[4][2] = tc[9][2]; force[5][0] = fc[10][0]; torque[5][0] = tc[10][0]; force[5][1] = fc[10][1]; torque[5][1] = tc[10][1]; force[5][2] = fc[10][2]; torque[5][2] = tc[10][2]; force[6][0] = fc[11][0]; torque[6][0] = tc[11][0]; force[6][1] = fc[11][1]; torque[6][1] = tc[11][1]; force[6][2] = fc[11][2]; torque[6][2] = tc[11][2]; force[7][0] = fc[12][0]; torque[7][0] = tc[12][0]; force[7][1] = fc[12][1]; torque[7][1] = tc[12][1]; force[7][2] = fc[12][2]; torque[7][2] = tc[12][2]; force[8][0] = fc[13][0]; torque[8][0] = tc[13][0]; force[8][1] = fc[13][1]; torque[8][1] = tc[13][1]; force[8][2] = fc[13][2]; torque[8][2] = tc[13][2]; force[9][0] = fc[14][0]; torque[9][0] = tc[14][0]; force[9][1] = fc[14][1]; torque[9][1] = tc[14][1]; force[9][2] = fc[14][2]; torque[9][2] = tc[14][2]; force[10][0] = fc[15][0]; torque[10][0] = tc[15][0]; force[10][1] = fc[15][1]; torque[10][1] = tc[15][1]; force[10][2] = fc[15][2]; torque[10][2] = tc[15][2]; force[11][0] = fc[16][0]; torque[11][0] = tc[16][0]; force[11][1] = fc[16][1]; torque[11][1] = tc[16][1]; force[11][2] = fc[16][2]; torque[11][2] = tc[16][2]; force[12][0] = fc[17][0]; torque[12][0] = tc[17][0]; force[12][1] = fc[17][1]; torque[12][1] = tc[17][1]; force[12][2] = fc[17][2]; torque[12][2] = tc[17][2]; force[13][0] = fc[18][0]; torque[13][0] = tc[18][0]; force[13][1] = fc[18][1]; torque[13][1] = tc[18][1]; force[13][2] = fc[18][2]; torque[13][2] = tc[18][2]; force[14][0] = fc[19][0]; torque[14][0] = tc[19][0]; force[14][1] = fc[19][1]; torque[14][1] = tc[19][1]; force[14][2] = fc[19][2]; torque[14][2] = tc[19][2]; force[15][0] = fc[20][0]; torque[15][0] = tc[20][0]; force[15][1] = fc[20][1]; torque[15][1] = tc[20][1]; force[15][2] = fc[20][2]; torque[15][2] = tc[20][2]; force[16][0] = fc[21][0]; torque[16][0] = tc[21][0]; force[16][1] = fc[21][1]; torque[16][1] = tc[21][1]; force[16][2] = fc[21][2]; torque[16][2] = tc[21][2]; force[17][0] = fc[22][0]; torque[17][0] = tc[22][0]; force[17][1] = fc[22][1]; torque[17][1] = tc[22][1]; force[17][2] = fc[22][2]; torque[17][2] = tc[22][2]; /* Compute reaction forces for tree weld joints */ /* Used 0.63 seconds CPU time, 8192 additional bytes of memory. Equations contain 710 adds/subtracts/negates 542 multiplies 0 divides 426 assignments */ } void BOCfly :: sdmom ( double lm[3], double am[3], double *ke ) { /* Compute system linear and angular momentum, and kinetic energy. Generated 27-Mar-2000 00:59:03 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 69086664 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 */ double lk[18][3],hnk[18][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(19,23); return; } hnk[0][0] = (((.48506516*u[3])-(5.312e-5*u[4]))-(.01450199*u[5])); hnk[0][1] = (((.47268161*u[4])-(5.312e-5*u[3]))-(.00031516*u[5])); hnk[0][2] = ((.03863225*u[5])-((.00031516*u[4])+(.01450199*u[3]))); hnk[3][0] = ((.00823665*wk[8][2])+((1.657e-5*wk[8][1])+(.52257967*wk[8][0])) ); hnk[3][1] = (((1.657e-5*wk[8][0])+(.52474672*wk[8][1]))-(.00036016*wk[8][2]) ); hnk[3][2] = ((.01799022*wk[8][2])+((.00823665*wk[8][0])-(.00036016*wk[8][1]) )); hnk[4][0] = ((.00223918*wk[9][2])+((.05459333*wk[9][0])-(5.918e-5*wk[9][1])) ); hnk[4][1] = ((.00147758*wk[9][2])+((.05477009*wk[9][1])-(5.918e-5*wk[9][0])) ); hnk[4][2] = ((.00343412*wk[9][2])+((.00147758*wk[9][1])+(.00223918*wk[9][0]) )); hnk[5][0] = ((.00224427*wk[10][2])+((6.279e-5*wk[10][1])+(.05476817* wk[10][0]))); hnk[5][1] = (((6.279e-5*wk[10][0])+(.05493593*wk[10][1]))-(.00156061* wk[10][2])); hnk[5][2] = ((.00344556*wk[10][2])+((.00224427*wk[10][0])-(.00156061* wk[10][1]))); hnk[6][0] = (((1.273e-5*wk[11][1])+(.00826684*wk[11][0]))-(.00026397* wk[11][2])); hnk[6][1] = ((.00020104*wk[11][2])+((1.273e-5*wk[11][0])+(.00843352* wk[11][1]))); hnk[6][2] = ((.00057599*wk[11][2])+((.00020104*wk[11][1])-(.00026397* wk[11][0]))); hnk[7][0] = (((.00821833*wk[12][0])-(1.283e-5*wk[12][1]))-(.00026918* wk[12][2])); hnk[7][1] = (((.00838445*wk[12][1])-(1.283e-5*wk[12][0]))-(.00019836* wk[12][2])); hnk[7][2] = ((.00057568*wk[12][2])-((.00019836*wk[12][1])+(.00026918* wk[12][0]))); hnk[8][0] = ((.00016083*wk[13][2])+((.00031494*wk[13][0])-(1.886e-5* wk[13][1]))); hnk[8][1] = ((4.29e-6*wk[13][2])+((.00112921*wk[13][1])-(1.886e-5*wk[13][0]) )); hnk[8][2] = ((.00103272*wk[13][2])+((4.29e-6*wk[13][1])+(.00016083*wk[13][0] ))); hnk[9][0] = ((.00016063*wk[14][2])+((1.71e-5*wk[14][1])+(.00031249*wk[14][0] ))); hnk[9][1] = (((1.71e-5*wk[14][0])+(.00112646*wk[14][1]))-(4.13e-6*wk[14][2]) ); hnk[9][2] = ((.00102891*wk[14][2])+((.00016063*wk[14][0])-(4.13e-6*wk[14][1] ))); hnk[10][0] = ((.00098554*wk[15][2])+((.03667794*wk[15][0])-(9.282e-5* wk[15][1]))); hnk[10][1] = ((.00236551*wk[15][2])+((.03636003*wk[15][1])-(9.282e-5* wk[15][0]))); hnk[10][2] = ((.00072894*wk[15][2])+((.00098554*wk[15][0])+(.00236551* wk[15][1]))); hnk[11][0] = ((.00099076*wk[16][2])+((9.583e-5*wk[16][1])+(.03712787* wk[16][0]))); hnk[11][1] = (((9.583e-5*wk[16][0])+(.03679631*wk[16][1]))-(.00244367* wk[16][2])); hnk[11][2] = ((.00074417*wk[16][2])+((.00099076*wk[16][0])-(.00244367* wk[16][1]))); hnk[12][0] = ((.00047536*wk[17][2])+((.00933952*wk[17][0])-(2.97e-5* wk[17][1]))); hnk[12][1] = ((.00105064*wk[17][2])+((.00915719*wk[17][1])-(2.97e-5* wk[17][0]))); hnk[12][2] = ((.00032059*wk[17][2])+((.00047536*wk[17][0])+(.00105064* wk[17][1]))); hnk[13][0] = ((.00047517*wk[18][2])+((3.024e-5*wk[18][1])+(.00933861* wk[18][0]))); hnk[13][1] = (((3.024e-5*wk[18][0])+(.00915219*wk[18][1]))-(.00106364* wk[18][2])); hnk[13][2] = ((.00032466*wk[18][2])+((.00047517*wk[18][0])-(.00106364* wk[18][1]))); hnk[14][0] = ((7.331e-5*wk[19][2])+((.00086458*wk[19][0])-(2.761e-5* wk[19][1]))); hnk[14][1] = (((.00082117*wk[19][1])-(2.761e-5*wk[19][0]))-(.00020674* wk[19][2])); hnk[14][2] = ((3.059e-5*wk[19][2])+((7.331e-5*wk[19][0])-(.00020674* wk[19][1]))); hnk[15][0] = ((7.338e-5*wk[20][2])+((2.77e-5*wk[20][1])+(.00086575*wk[20][0] ))); hnk[15][1] = ((.00020754*wk[20][2])+((2.77e-5*wk[20][0])+(.00082196* wk[20][1]))); hnk[15][2] = ((3.093e-5*wk[20][2])+((7.338e-5*wk[20][0])+(.00020754* wk[20][1]))); lm[0] = ((.02601109*vnk[22][0])+((.0260106*vnk[21][0])+((.05944511* vnk[20][0])+((.05946345*vnk[19][0])+((.43358398*vnk[18][0])+((.43362906* vnk[17][0])+((1.01701701*vnk[16][0])+((1.00985702*vnk[15][0])+((.5325902* vnk[14][0])+((.53380008*vnk[13][0])+((1.02007998*vnk[12][0])+((1.02133193* vnk[11][0])+((2.8984419*vnk[10][0])+((2.89586298*vnk[9][0])+((9.84983599* vnk[8][0])+((.1816698*vnk[7][0])+((.18166784*vnk[6][0])+(11.02968359*u[0]) ))))))))))))))))); lm[1] = ((.05944511*vnk[20][1])+((.05946345*vnk[19][1])+((.43358398* vnk[18][1])+((.43362906*vnk[17][1])+((1.01701701*vnk[16][1])+((1.00985702* vnk[15][1])+((.5325902*vnk[14][1])+((.53380008*vnk[13][1])+((1.02007998* vnk[12][1])+((1.02133193*vnk[11][1])+((2.8984419*vnk[10][1])+((2.89586298* vnk[9][1])+((9.84983599*vnk[8][1])+(11.02968359*u[1])))))))))))))); lm[2] = ((.02601109*vnk[22][2])+((.0260106*vnk[21][2])+((.05944511* vnk[20][2])+((.05946345*vnk[19][2])+((.43358398*vnk[18][2])+((.43362906* vnk[17][2])+((1.01701701*vnk[16][2])+((1.00985702*vnk[15][2])+((.5325902* vnk[14][2])+((.53380008*vnk[13][2])+((1.02007998*vnk[12][2])+((1.02133193* vnk[11][2])+((2.8984419*vnk[10][2])+((2.89586298*vnk[9][2])+((9.84983599* vnk[8][2])+((.1816698*vnk[7][2])+((.18166784*vnk[6][2])+(11.02968359*u[2]) ))))))))))))))))); temp[0] = (((9.84983599*((rnk[8][1]*vnk[8][2])-(rnk[8][2]*vnk[8][1])))+(( cnk[8][0][2]*hnk[3][2])+((Cik[3][0][1]*hnk[3][1])+(cnk[8][0][0]*hnk[3][0]) )))+((((.00230647*(u[7]*s7))-(8e-8*(u[7]*c7)))-(.028873609728288*vnk[7][2] ))+(((.0284195119356928*vnk[6][2])+((8e-8*(u[6]*c6))-(.00226939*(u[6]*s6)) ))+((11.02968359*((q[1]*u[2])-(q[2]*u[1])))+((Cik[3][0][2]*hnk[0][2])+(( Cik[3][0][0]*hnk[0][0])+(Cik[3][0][1]*hnk[0][1]))))))); temp[1] = (((2.8984419*((rnk[10][1]*vnk[10][2])-(rnk[10][2]*vnk[10][1])))+(( cnk[10][0][2]*hnk[5][2])+((Cik[3][0][1]*hnk[5][1])+(cnk[10][0][0]* hnk[5][0]))))+(((2.89586298*((rnk[9][1]*vnk[9][2])-(rnk[9][2]*vnk[9][1]))) +((cnk[9][0][2]*hnk[4][2])+((Cik[3][0][1]*hnk[4][1])+(cnk[9][0][0]* hnk[4][0]))))+temp[0])); temp[2] = (((1.02007998*((rnk[12][1]*vnk[12][2])-(rnk[12][2]*vnk[12][1])))+( (cnk[12][0][2]*hnk[7][2])+((Cik[3][0][1]*hnk[7][1])+(cnk[12][0][0]* hnk[7][0]))))+(((1.02133193*((rnk[11][1]*vnk[11][2])-(rnk[11][2]* vnk[11][1])))+((cnk[11][0][2]*hnk[6][2])+((Cik[3][0][1]*hnk[6][1])+( cnk[11][0][0]*hnk[6][0]))))+temp[1])); temp[3] = (((.5325902*((rnk[14][1]*vnk[14][2])-(rnk[14][2]*vnk[14][1])))+(( cnk[14][0][2]*hnk[9][2])+((Cik[3][0][1]*hnk[9][1])+(cnk[14][0][0]* hnk[9][0]))))+(((.53380008*((rnk[13][1]*vnk[13][2])-(rnk[13][2]*vnk[13][1] )))+((cnk[13][0][2]*hnk[8][2])+((Cik[3][0][1]*hnk[8][1])+(cnk[13][0][0]* hnk[8][0]))))+temp[2])); temp[4] = (((1.01701701*((rnk[16][1]*vnk[16][2])-(rnk[16][2]*vnk[16][1])))+( (cnk[16][0][2]*hnk[11][2])+((Cik[3][0][1]*hnk[11][1])+(cnk[16][0][0]* hnk[11][0]))))+(((1.00985702*((rnk[15][1]*vnk[15][2])-(rnk[15][2]* vnk[15][1])))+((cnk[15][0][2]*hnk[10][2])+((Cik[3][0][1]*hnk[10][1])+( cnk[15][0][0]*hnk[10][0]))))+temp[3])); temp[5] = (((.43358398*((rnk[18][1]*vnk[18][2])-(rnk[18][2]*vnk[18][1])))+(( cnk[18][0][2]*hnk[13][2])+((Cik[3][0][1]*hnk[13][1])+(cnk[18][0][0]* hnk[13][0]))))+(((.43362906*((rnk[17][1]*vnk[17][2])-(rnk[17][2]* vnk[17][1])))+((cnk[17][0][2]*hnk[12][2])+((Cik[3][0][1]*hnk[12][1])+( cnk[17][0][0]*hnk[12][0]))))+temp[4])); temp[6] = (((.004068874832946*vnk[21][2])+((1e-8*(cnk[21][0][0]*wk[21][1]))- (.00013462*(cnk[21][0][2]*wk[21][1]))))+(((.05944511*((rnk[20][1]* vnk[20][2])-(rnk[20][2]*vnk[20][1])))+((cnk[20][0][2]*hnk[15][2])+(( Cik[3][0][1]*hnk[15][1])+(cnk[20][0][0]*hnk[15][0]))))+(((.05946345*(( rnk[19][1]*vnk[19][2])-(rnk[19][2]*vnk[19][1])))+((cnk[19][0][2]* hnk[14][2])+((Cik[3][0][1]*hnk[14][1])+(cnk[19][0][0]*hnk[14][0]))))+ temp[5]))); am[0] = (((((.00013693*(cnk[22][0][2]*wk[22][1]))-(1e-8*(cnk[22][0][0]* wk[22][1])))-(.0041341880783896*vnk[22][2]))+temp[6])-((com[1]*lm[2])-( com[2]*lm[1]))); temp[0] = (((.04207299*u[7])+(.1816698*((rnk[7][2]*vnk[7][0])-(rnk[7][0]* vnk[7][2]))))+(((.04205639*u[6])+(.18166784*((rnk[6][2]*vnk[6][0])-( rnk[6][0]*vnk[6][2]))))+((11.02968359*((q[2]*u[0])-(q[0]*u[2])))+(( Cik[3][1][2]*hnk[0][2])+((Cik[3][1][0]*hnk[0][0])+(Cik[3][1][1]*hnk[0][1]) ))))); temp[1] = (((2.89586298*((rnk[9][2]*vnk[9][0])-(rnk[9][0]*vnk[9][2])))+(( cnk[9][1][2]*hnk[4][2])+((Cik[3][1][1]*hnk[4][1])+(cnk[9][1][0]*hnk[4][0]) )))+(((9.84983599*((rnk[8][2]*vnk[8][0])-(rnk[8][0]*vnk[8][2])))+(( cnk[8][1][2]*hnk[3][2])+((Cik[3][1][1]*hnk[3][1])+(cnk[8][1][0]*hnk[3][0]) )))+temp[0])); temp[2] = (((1.02133193*((rnk[11][2]*vnk[11][0])-(rnk[11][0]*vnk[11][2])))+( (cnk[11][1][2]*hnk[6][2])+((Cik[3][1][1]*hnk[6][1])+(cnk[11][1][0]* hnk[6][0]))))+(((2.8984419*((rnk[10][2]*vnk[10][0])-(rnk[10][0]*vnk[10][2] )))+((cnk[10][1][2]*hnk[5][2])+((Cik[3][1][1]*hnk[5][1])+(cnk[10][1][0]* hnk[5][0]))))+temp[1])); temp[3] = (((.53380008*((rnk[13][2]*vnk[13][0])-(rnk[13][0]*vnk[13][2])))+(( cnk[13][1][2]*hnk[8][2])+((Cik[3][1][1]*hnk[8][1])+(cnk[13][1][0]* hnk[8][0]))))+(((1.02007998*((rnk[12][2]*vnk[12][0])-(rnk[12][0]* vnk[12][2])))+((cnk[12][1][2]*hnk[7][2])+((Cik[3][1][1]*hnk[7][1])+( cnk[12][1][0]*hnk[7][0]))))+temp[2])); temp[4] = (((1.00985702*((rnk[15][2]*vnk[15][0])-(rnk[15][0]*vnk[15][2])))+( (cnk[15][1][2]*hnk[10][2])+((Cik[3][1][1]*hnk[10][1])+(cnk[15][1][0]* hnk[10][0]))))+(((.5325902*((rnk[14][2]*vnk[14][0])-(rnk[14][0]*vnk[14][2] )))+((cnk[14][1][2]*hnk[9][2])+((Cik[3][1][1]*hnk[9][1])+(cnk[14][1][0]* hnk[9][0]))))+temp[3])); temp[5] = (((.43362906*((rnk[17][2]*vnk[17][0])-(rnk[17][0]*vnk[17][2])))+(( cnk[17][1][2]*hnk[12][2])+((Cik[3][1][1]*hnk[12][1])+(cnk[17][1][0]* hnk[12][0]))))+(((1.01701701*((rnk[16][2]*vnk[16][0])-(rnk[16][0]* vnk[16][2])))+((cnk[16][1][2]*hnk[11][2])+((Cik[3][1][1]*hnk[11][1])+( cnk[16][1][0]*hnk[11][0]))))+temp[4])); temp[6] = (((.05946345*((rnk[19][2]*vnk[19][0])-(rnk[19][0]*vnk[19][2])))+(( cnk[19][1][2]*hnk[14][2])+((Cik[3][1][1]*hnk[14][1])+(cnk[19][1][0]* hnk[14][0]))))+(((.43358398*((rnk[18][2]*vnk[18][0])-(rnk[18][0]* vnk[18][2])))+((cnk[18][1][2]*hnk[13][2])+((Cik[3][1][1]*hnk[13][1])+( cnk[18][1][0]*hnk[13][0]))))+temp[5])); am[1] = ((((.00050087*wk[22][1])+(.02601109*((rnk[22][2]*vnk[22][0])-( rnk[22][0]*vnk[22][2]))))+(((.0004999*wk[21][1])+(.0260106*((rnk[21][2]* vnk[21][0])-(rnk[21][0]*vnk[21][2]))))+(((.05944511*((rnk[20][2]* vnk[20][0])-(rnk[20][0]*vnk[20][2])))+((cnk[20][1][2]*hnk[15][2])+(( Cik[3][1][1]*hnk[15][1])+(cnk[20][1][0]*hnk[15][0]))))+temp[6])))-((com[2] *lm[0])-(com[0]*lm[2]))); temp[0] = (((9.84983599*((rnk[8][0]*vnk[8][1])-(rnk[8][1]*vnk[8][0])))+(( cnk[8][2][2]*hnk[3][2])+((Cik[3][2][1]*hnk[3][1])+(cnk[8][2][0]*hnk[3][0]) )))+(((.028873609728288*vnk[7][0])+((8e-8*(u[7]*s7))+(.00230647*(u[7]*c7)) ))+(((11.02968359*((q[0]*u[1])-(q[1]*u[0])))+((Cik[3][2][2]*hnk[0][2])+(( Cik[3][2][0]*hnk[0][0])+(Cik[3][2][1]*hnk[0][1]))))-((.0284195119356928* vnk[6][0])+((8e-8*(u[6]*s6))+(.00226939*(u[6]*c6))))))); temp[1] = (((2.8984419*((rnk[10][0]*vnk[10][1])-(rnk[10][1]*vnk[10][0])))+(( cnk[10][2][2]*hnk[5][2])+((Cik[3][2][1]*hnk[5][1])+(cnk[10][2][0]* hnk[5][0]))))+(((2.89586298*((rnk[9][0]*vnk[9][1])-(rnk[9][1]*vnk[9][0]))) +((cnk[9][2][2]*hnk[4][2])+((Cik[3][2][1]*hnk[4][1])+(cnk[9][2][0]* hnk[4][0]))))+temp[0])); temp[2] = (((1.02007998*((rnk[12][0]*vnk[12][1])-(rnk[12][1]*vnk[12][0])))+( (cnk[12][2][2]*hnk[7][2])+((Cik[3][2][1]*hnk[7][1])+(cnk[12][2][0]* hnk[7][0]))))+(((1.02133193*((rnk[11][0]*vnk[11][1])-(rnk[11][1]* vnk[11][0])))+((cnk[11][2][2]*hnk[6][2])+((Cik[3][2][1]*hnk[6][1])+( cnk[11][2][0]*hnk[6][0]))))+temp[1])); temp[3] = (((.5325902*((rnk[14][0]*vnk[14][1])-(rnk[14][1]*vnk[14][0])))+(( cnk[14][2][2]*hnk[9][2])+((Cik[3][2][1]*hnk[9][1])+(cnk[14][2][0]* hnk[9][0]))))+(((.53380008*((rnk[13][0]*vnk[13][1])-(rnk[13][1]*vnk[13][0] )))+((cnk[13][2][2]*hnk[8][2])+((Cik[3][2][1]*hnk[8][1])+(cnk[13][2][0]* hnk[8][0]))))+temp[2])); temp[4] = (((1.01701701*((rnk[16][0]*vnk[16][1])-(rnk[16][1]*vnk[16][0])))+( (cnk[16][2][2]*hnk[11][2])+((Cik[3][2][1]*hnk[11][1])+(cnk[16][2][0]* hnk[11][0]))))+(((1.00985702*((rnk[15][0]*vnk[15][1])-(rnk[15][1]* vnk[15][0])))+((cnk[15][2][2]*hnk[10][2])+((Cik[3][2][1]*hnk[10][1])+( cnk[15][2][0]*hnk[10][0]))))+temp[3])); temp[5] = (((.43358398*((rnk[18][0]*vnk[18][1])-(rnk[18][1]*vnk[18][0])))+(( cnk[18][2][2]*hnk[13][2])+((Cik[3][2][1]*hnk[13][1])+(cnk[18][2][0]* hnk[13][0]))))+(((.43362906*((rnk[17][0]*vnk[17][1])-(rnk[17][1]* vnk[17][0])))+((cnk[17][2][2]*hnk[12][2])+((Cik[3][2][1]*hnk[12][1])+( cnk[17][2][0]*hnk[12][0]))))+temp[4])); temp[6] = ((((1e-8*(cnk[21][2][0]*wk[21][1]))-(.00013462*(cnk[21][2][2]* wk[21][1])))-(.004068874832946*vnk[21][0]))+(((.05944511*((rnk[20][0]* vnk[20][1])-(rnk[20][1]*vnk[20][0])))+((cnk[20][2][2]*hnk[15][2])+(( Cik[3][2][1]*hnk[15][1])+(cnk[20][2][0]*hnk[15][0]))))+(((.05946345*(( rnk[19][0]*vnk[19][1])-(rnk[19][1]*vnk[19][0])))+((cnk[19][2][2]* hnk[14][2])+((Cik[3][2][1]*hnk[14][1])+(cnk[19][2][0]*hnk[14][0]))))+ temp[5]))); am[2] = ((((.0041341880783896*vnk[22][0])+((.00013693*(cnk[22][2][2]* wk[22][1]))-(1e-8*(cnk[22][2][0]*wk[22][1]))))+temp[6])-((com[0]*lm[1])-( com[1]*lm[0]))); temp[0] = (((.04207299*(u[7]*u[7]))+(.1816698*((vnk[7][0]*vnk[7][0])+( vnk[7][2]*vnk[7][2]))))+(((.04205639*(u[6]*u[6]))+(.18166784*((vnk[6][0]* vnk[6][0])+(vnk[6][2]*vnk[6][2]))))+((11.02968359*((u[2]*u[2])+((u[0]*u[0] )+(u[1]*u[1]))))+((hnk[0][2]*u[5])+((hnk[0][0]*u[3])+(hnk[0][1]*u[4])))))); temp[1] = (((2.89586298*((vnk[9][2]*vnk[9][2])+((vnk[9][0]*vnk[9][0])+( vnk[9][1]*vnk[9][1]))))+((hnk[4][2]*wk[9][2])+((hnk[4][0]*wk[9][0])+( hnk[4][1]*wk[9][1]))))+(((9.84983599*((vnk[8][2]*vnk[8][2])+((vnk[8][0]* vnk[8][0])+(vnk[8][1]*vnk[8][1]))))+((hnk[3][2]*wk[8][2])+((hnk[3][0]* wk[8][0])+(hnk[3][1]*wk[8][1]))))+temp[0])); temp[2] = (((1.02133193*((vnk[11][2]*vnk[11][2])+((vnk[11][0]*vnk[11][0])+( vnk[11][1]*vnk[11][1]))))+((hnk[6][2]*wk[11][2])+((hnk[6][0]*wk[11][0])+( hnk[6][1]*wk[11][1]))))+(((2.8984419*((vnk[10][2]*vnk[10][2])+((vnk[10][0] *vnk[10][0])+(vnk[10][1]*vnk[10][1]))))+((hnk[5][2]*wk[10][2])+((hnk[5][0] *wk[10][0])+(hnk[5][1]*wk[10][1]))))+temp[1])); temp[3] = (((.53380008*((vnk[13][2]*vnk[13][2])+((vnk[13][0]*vnk[13][0])+( vnk[13][1]*vnk[13][1]))))+((hnk[8][2]*wk[13][2])+((hnk[8][0]*wk[13][0])+( hnk[8][1]*wk[13][1]))))+(((1.02007998*((vnk[12][2]*vnk[12][2])+(( vnk[12][0]*vnk[12][0])+(vnk[12][1]*vnk[12][1]))))+((hnk[7][2]*wk[12][2])+( (hnk[7][0]*wk[12][0])+(hnk[7][1]*wk[12][1]))))+temp[2])); temp[4] = (((1.00985702*((vnk[15][2]*vnk[15][2])+((vnk[15][0]*vnk[15][0])+( vnk[15][1]*vnk[15][1]))))+((hnk[10][2]*wk[15][2])+((hnk[10][0]*wk[15][0])+ (hnk[10][1]*wk[15][1]))))+(((.5325902*((vnk[14][2]*vnk[14][2])+(( vnk[14][0]*vnk[14][0])+(vnk[14][1]*vnk[14][1]))))+((hnk[9][2]*wk[14][2])+( (hnk[9][0]*wk[14][0])+(hnk[9][1]*wk[14][1]))))+temp[3])); temp[5] = (((.43362906*((vnk[17][2]*vnk[17][2])+((vnk[17][0]*vnk[17][0])+( vnk[17][1]*vnk[17][1]))))+((hnk[12][2]*wk[17][2])+((hnk[12][0]*wk[17][0])+ (hnk[12][1]*wk[17][1]))))+(((1.01701701*((vnk[16][2]*vnk[16][2])+(( vnk[16][0]*vnk[16][0])+(vnk[16][1]*vnk[16][1]))))+((hnk[11][2]*wk[16][2])+ ((hnk[11][0]*wk[16][0])+(hnk[11][1]*wk[16][1]))))+temp[4])); temp[6] = (((.05946345*((vnk[19][2]*vnk[19][2])+((vnk[19][0]*vnk[19][0])+( vnk[19][1]*vnk[19][1]))))+((hnk[14][2]*wk[19][2])+((hnk[14][0]*wk[19][0])+ (hnk[14][1]*wk[19][1]))))+(((.43358398*((vnk[18][2]*vnk[18][2])+(( vnk[18][0]*vnk[18][0])+(vnk[18][1]*vnk[18][1]))))+((hnk[13][2]*wk[18][2])+ ((hnk[13][0]*wk[18][0])+(hnk[13][1]*wk[18][1]))))+temp[5])); *ke = (.5*(((.00050087*(wk[22][1]*wk[22][1]))+(.02601109*((vnk[22][0]* vnk[22][0])+(vnk[22][2]*vnk[22][2]))))+(((.0004999*(wk[21][1]*wk[21][1]))+ (.0260106*((vnk[21][0]*vnk[21][0])+(vnk[21][2]*vnk[21][2]))))+(((.05944511 *((vnk[20][2]*vnk[20][2])+((vnk[20][0]*vnk[20][0])+(vnk[20][1]*vnk[20][1]) )))+((hnk[15][2]*wk[20][2])+((hnk[15][0]*wk[20][0])+(hnk[15][1]*wk[20][1]) )))+temp[6])))); /* Used 0.58 seconds CPU time, 65536 additional bytes of memory. Equations contain 475 adds/subtracts/negates 609 multiplies 0 divides 77 assignments */ } void BOCfly :: sdsys ( double *mtoto, double cm[3], double icm[3][3] ) { /* Compute system total mass, and instantaneous center of mass and inertia matrix. Generated 27-Mar-2000 00:59:04 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 69086664 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 */ double ikcnkt[23][3][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(20,23); return; } *mtoto = 33.20998161; cm[0] = com[0]; cm[1] = com[1]; cm[2] = com[2]; ikcnkt[5][0][0] = (((.48506516*Cik[3][0][0])-(5.312e-5*Cik[3][0][1]))-( .01450199*Cik[3][0][2])); ikcnkt[5][0][1] = (((.48506516*Cik[3][1][0])-(5.312e-5*Cik[3][1][1]))-( .01450199*Cik[3][1][2])); ikcnkt[5][0][2] = (((.48506516*Cik[3][2][0])-(5.312e-5*Cik[3][2][1]))-( .01450199*Cik[3][2][2])); ikcnkt[5][1][0] = (((.47268161*Cik[3][0][1])-(5.312e-5*Cik[3][0][0]))-( .00031516*Cik[3][0][2])); ikcnkt[5][1][1] = (((.47268161*Cik[3][1][1])-(5.312e-5*Cik[3][1][0]))-( .00031516*Cik[3][1][2])); ikcnkt[5][1][2] = (((.47268161*Cik[3][2][1])-(5.312e-5*Cik[3][2][0]))-( .00031516*Cik[3][2][2])); ikcnkt[5][2][0] = ((.03863225*Cik[3][0][2])-((.00031516*Cik[3][0][1])+( .01450199*Cik[3][0][0]))); ikcnkt[5][2][1] = ((.03863225*Cik[3][1][2])-((.00031516*Cik[3][1][1])+( .01450199*Cik[3][1][0]))); ikcnkt[5][2][2] = ((.03863225*Cik[3][2][2])-((.00031516*Cik[3][2][1])+( .01450199*Cik[3][2][0]))); ikcnkt[6][0][0] = ((.04182592*c6)-(7.5e-7*s6)); ikcnkt[6][0][2] = -((7.5e-7*c6)+(.04182592*s6)); ikcnkt[6][1][0] = ((8e-8*c6)-(.00226939*s6)); ikcnkt[6][1][2] = -((8e-8*s6)+(.00226939*c6)); ikcnkt[6][2][0] = ((.00022475*s6)-(7.5e-7*c6)); ikcnkt[6][2][2] = ((7.5e-7*s6)+(.00022475*c6)); ikcnkt[7][0][0] = ((.04183501*c7)-(7.5e-7*s7)); ikcnkt[7][0][2] = -((7.5e-7*c7)+(.04183501*s7)); ikcnkt[7][1][0] = ((.00230647*s7)-(8e-8*c7)); ikcnkt[7][1][2] = ((8e-8*s7)+(.00230647*c7)); ikcnkt[7][2][0] = ((.00023224*s7)-(7.5e-7*c7)); ikcnkt[7][2][2] = ((7.5e-7*s7)+(.00023224*c7)); ikcnkt[8][0][0] = ((.00823665*cnk[8][0][2])+((1.657e-5*Cik[3][0][1])+( .52257967*cnk[8][0][0]))); ikcnkt[8][0][1] = ((.00823665*cnk[8][1][2])+((1.657e-5*Cik[3][1][1])+( .52257967*cnk[8][1][0]))); ikcnkt[8][0][2] = ((.00823665*cnk[8][2][2])+((1.657e-5*Cik[3][2][1])+( .52257967*cnk[8][2][0]))); ikcnkt[8][1][0] = (((1.657e-5*cnk[8][0][0])+(.52474672*Cik[3][0][1]))-( .00036016*cnk[8][0][2])); ikcnkt[8][1][1] = (((1.657e-5*cnk[8][1][0])+(.52474672*Cik[3][1][1]))-( .00036016*cnk[8][1][2])); ikcnkt[8][1][2] = (((1.657e-5*cnk[8][2][0])+(.52474672*Cik[3][2][1]))-( .00036016*cnk[8][2][2])); ikcnkt[8][2][0] = ((.01799022*cnk[8][0][2])+((.00823665*cnk[8][0][0])-( .00036016*Cik[3][0][1]))); ikcnkt[8][2][1] = ((.01799022*cnk[8][1][2])+((.00823665*cnk[8][1][0])-( .00036016*Cik[3][1][1]))); ikcnkt[8][2][2] = ((.01799022*cnk[8][2][2])+((.00823665*cnk[8][2][0])-( .00036016*Cik[3][2][1]))); ikcnkt[9][0][0] = ((.00223918*cnk[9][0][2])+((.05459333*cnk[9][0][0])-( 5.918e-5*Cik[3][0][1]))); ikcnkt[9][0][1] = ((.00223918*cnk[9][1][2])+((.05459333*cnk[9][1][0])-( 5.918e-5*Cik[3][1][1]))); ikcnkt[9][0][2] = ((.00223918*cnk[9][2][2])+((.05459333*cnk[9][2][0])-( 5.918e-5*Cik[3][2][1]))); ikcnkt[9][1][0] = ((.00147758*cnk[9][0][2])+((.05477009*Cik[3][0][1])-( 5.918e-5*cnk[9][0][0]))); ikcnkt[9][1][1] = ((.00147758*cnk[9][1][2])+((.05477009*Cik[3][1][1])-( 5.918e-5*cnk[9][1][0]))); ikcnkt[9][1][2] = ((.00147758*cnk[9][2][2])+((.05477009*Cik[3][2][1])-( 5.918e-5*cnk[9][2][0]))); ikcnkt[9][2][0] = ((.00343412*cnk[9][0][2])+((.00147758*Cik[3][0][1])+( .00223918*cnk[9][0][0]))); ikcnkt[9][2][1] = ((.00343412*cnk[9][1][2])+((.00147758*Cik[3][1][1])+( .00223918*cnk[9][1][0]))); ikcnkt[9][2][2] = ((.00343412*cnk[9][2][2])+((.00147758*Cik[3][2][1])+( .00223918*cnk[9][2][0]))); ikcnkt[10][0][0] = ((.00224427*cnk[10][0][2])+((6.279e-5*Cik[3][0][1])+( .05476817*cnk[10][0][0]))); ikcnkt[10][0][1] = ((.00224427*cnk[10][1][2])+((6.279e-5*Cik[3][1][1])+( .05476817*cnk[10][1][0]))); ikcnkt[10][0][2] = ((.00224427*cnk[10][2][2])+((6.279e-5*Cik[3][2][1])+( .05476817*cnk[10][2][0]))); ikcnkt[10][1][0] = (((6.279e-5*cnk[10][0][0])+(.05493593*Cik[3][0][1]))-( .00156061*cnk[10][0][2])); ikcnkt[10][1][1] = (((6.279e-5*cnk[10][1][0])+(.05493593*Cik[3][1][1]))-( .00156061*cnk[10][1][2])); ikcnkt[10][1][2] = (((6.279e-5*cnk[10][2][0])+(.05493593*Cik[3][2][1]))-( .00156061*cnk[10][2][2])); ikcnkt[10][2][0] = ((.00344556*cnk[10][0][2])+((.00224427*cnk[10][0][0])-( .00156061*Cik[3][0][1]))); ikcnkt[10][2][1] = ((.00344556*cnk[10][1][2])+((.00224427*cnk[10][1][0])-( .00156061*Cik[3][1][1]))); ikcnkt[10][2][2] = ((.00344556*cnk[10][2][2])+((.00224427*cnk[10][2][0])-( .00156061*Cik[3][2][1]))); ikcnkt[11][0][0] = (((1.273e-5*Cik[3][0][1])+(.00826684*cnk[11][0][0]))-( .00026397*cnk[11][0][2])); ikcnkt[11][0][1] = (((1.273e-5*Cik[3][1][1])+(.00826684*cnk[11][1][0]))-( .00026397*cnk[11][1][2])); ikcnkt[11][0][2] = (((1.273e-5*Cik[3][2][1])+(.00826684*cnk[11][2][0]))-( .00026397*cnk[11][2][2])); ikcnkt[11][1][0] = ((.00020104*cnk[11][0][2])+((1.273e-5*cnk[11][0][0])+( .00843352*Cik[3][0][1]))); ikcnkt[11][1][1] = ((.00020104*cnk[11][1][2])+((1.273e-5*cnk[11][1][0])+( .00843352*Cik[3][1][1]))); ikcnkt[11][1][2] = ((.00020104*cnk[11][2][2])+((1.273e-5*cnk[11][2][0])+( .00843352*Cik[3][2][1]))); ikcnkt[11][2][0] = ((.00057599*cnk[11][0][2])+((.00020104*Cik[3][0][1])-( .00026397*cnk[11][0][0]))); ikcnkt[11][2][1] = ((.00057599*cnk[11][1][2])+((.00020104*Cik[3][1][1])-( .00026397*cnk[11][1][0]))); ikcnkt[11][2][2] = ((.00057599*cnk[11][2][2])+((.00020104*Cik[3][2][1])-( .00026397*cnk[11][2][0]))); ikcnkt[12][0][0] = (((.00821833*cnk[12][0][0])-(1.283e-5*Cik[3][0][1]))-( .00026918*cnk[12][0][2])); ikcnkt[12][0][1] = (((.00821833*cnk[12][1][0])-(1.283e-5*Cik[3][1][1]))-( .00026918*cnk[12][1][2])); ikcnkt[12][0][2] = (((.00821833*cnk[12][2][0])-(1.283e-5*Cik[3][2][1]))-( .00026918*cnk[12][2][2])); ikcnkt[12][1][0] = (((.00838445*Cik[3][0][1])-(1.283e-5*cnk[12][0][0]))-( .00019836*cnk[12][0][2])); ikcnkt[12][1][1] = (((.00838445*Cik[3][1][1])-(1.283e-5*cnk[12][1][0]))-( .00019836*cnk[12][1][2])); ikcnkt[12][1][2] = (((.00838445*Cik[3][2][1])-(1.283e-5*cnk[12][2][0]))-( .00019836*cnk[12][2][2])); ikcnkt[12][2][0] = ((.00057568*cnk[12][0][2])-((.00019836*Cik[3][0][1])+( .00026918*cnk[12][0][0]))); ikcnkt[12][2][1] = ((.00057568*cnk[12][1][2])-((.00019836*Cik[3][1][1])+( .00026918*cnk[12][1][0]))); ikcnkt[12][2][2] = ((.00057568*cnk[12][2][2])-((.00019836*Cik[3][2][1])+( .00026918*cnk[12][2][0]))); ikcnkt[13][0][0] = ((.00016083*cnk[13][0][2])+((.00031494*cnk[13][0][0])-( 1.886e-5*Cik[3][0][1]))); ikcnkt[13][0][1] = ((.00016083*cnk[13][1][2])+((.00031494*cnk[13][1][0])-( 1.886e-5*Cik[3][1][1]))); ikcnkt[13][0][2] = ((.00016083*cnk[13][2][2])+((.00031494*cnk[13][2][0])-( 1.886e-5*Cik[3][2][1]))); ikcnkt[13][1][0] = ((4.29e-6*cnk[13][0][2])+((.00112921*Cik[3][0][1])-( 1.886e-5*cnk[13][0][0]))); ikcnkt[13][1][1] = ((4.29e-6*cnk[13][1][2])+((.00112921*Cik[3][1][1])-( 1.886e-5*cnk[13][1][0]))); ikcnkt[13][1][2] = ((4.29e-6*cnk[13][2][2])+((.00112921*Cik[3][2][1])-( 1.886e-5*cnk[13][2][0]))); ikcnkt[13][2][0] = ((.00103272*cnk[13][0][2])+((4.29e-6*Cik[3][0][1])+( .00016083*cnk[13][0][0]))); ikcnkt[13][2][1] = ((.00103272*cnk[13][1][2])+((4.29e-6*Cik[3][1][1])+( .00016083*cnk[13][1][0]))); ikcnkt[13][2][2] = ((.00103272*cnk[13][2][2])+((4.29e-6*Cik[3][2][1])+( .00016083*cnk[13][2][0]))); ikcnkt[14][0][0] = ((.00016063*cnk[14][0][2])+((1.71e-5*Cik[3][0][1])+( .00031249*cnk[14][0][0]))); ikcnkt[14][0][1] = ((.00016063*cnk[14][1][2])+((1.71e-5*Cik[3][1][1])+( .00031249*cnk[14][1][0]))); ikcnkt[14][0][2] = ((.00016063*cnk[14][2][2])+((1.71e-5*Cik[3][2][1])+( .00031249*cnk[14][2][0]))); ikcnkt[14][1][0] = (((1.71e-5*cnk[14][0][0])+(.00112646*Cik[3][0][1]))-( 4.13e-6*cnk[14][0][2])); ikcnkt[14][1][1] = (((1.71e-5*cnk[14][1][0])+(.00112646*Cik[3][1][1]))-( 4.13e-6*cnk[14][1][2])); ikcnkt[14][1][2] = (((1.71e-5*cnk[14][2][0])+(.00112646*Cik[3][2][1]))-( 4.13e-6*cnk[14][2][2])); ikcnkt[14][2][0] = ((.00102891*cnk[14][0][2])+((.00016063*cnk[14][0][0])-( 4.13e-6*Cik[3][0][1]))); ikcnkt[14][2][1] = ((.00102891*cnk[14][1][2])+((.00016063*cnk[14][1][0])-( 4.13e-6*Cik[3][1][1]))); ikcnkt[14][2][2] = ((.00102891*cnk[14][2][2])+((.00016063*cnk[14][2][0])-( 4.13e-6*Cik[3][2][1]))); ikcnkt[15][0][0] = ((.00098554*cnk[15][0][2])+((.03667794*cnk[15][0][0])-( 9.282e-5*Cik[3][0][1]))); ikcnkt[15][0][1] = ((.00098554*cnk[15][1][2])+((.03667794*cnk[15][1][0])-( 9.282e-5*Cik[3][1][1]))); ikcnkt[15][0][2] = ((.00098554*cnk[15][2][2])+((.03667794*cnk[15][2][0])-( 9.282e-5*Cik[3][2][1]))); ikcnkt[15][1][0] = ((.00236551*cnk[15][0][2])+((.03636003*Cik[3][0][1])-( 9.282e-5*cnk[15][0][0]))); ikcnkt[15][1][1] = ((.00236551*cnk[15][1][2])+((.03636003*Cik[3][1][1])-( 9.282e-5*cnk[15][1][0]))); ikcnkt[15][1][2] = ((.00236551*cnk[15][2][2])+((.03636003*Cik[3][2][1])-( 9.282e-5*cnk[15][2][0]))); ikcnkt[15][2][0] = ((.00072894*cnk[15][0][2])+((.00098554*cnk[15][0][0])+( .00236551*Cik[3][0][1]))); ikcnkt[15][2][1] = ((.00072894*cnk[15][1][2])+((.00098554*cnk[15][1][0])+( .00236551*Cik[3][1][1]))); ikcnkt[15][2][2] = ((.00072894*cnk[15][2][2])+((.00098554*cnk[15][2][0])+( .00236551*Cik[3][2][1]))); ikcnkt[16][0][0] = ((.00099076*cnk[16][0][2])+((9.583e-5*Cik[3][0][1])+( .03712787*cnk[16][0][0]))); ikcnkt[16][0][1] = ((.00099076*cnk[16][1][2])+((9.583e-5*Cik[3][1][1])+( .03712787*cnk[16][1][0]))); ikcnkt[16][0][2] = ((.00099076*cnk[16][2][2])+((9.583e-5*Cik[3][2][1])+( .03712787*cnk[16][2][0]))); ikcnkt[16][1][0] = (((9.583e-5*cnk[16][0][0])+(.03679631*Cik[3][0][1]))-( .00244367*cnk[16][0][2])); ikcnkt[16][1][1] = (((9.583e-5*cnk[16][1][0])+(.03679631*Cik[3][1][1]))-( .00244367*cnk[16][1][2])); ikcnkt[16][1][2] = (((9.583e-5*cnk[16][2][0])+(.03679631*Cik[3][2][1]))-( .00244367*cnk[16][2][2])); ikcnkt[16][2][0] = ((.00074417*cnk[16][0][2])+((.00099076*cnk[16][0][0])-( .00244367*Cik[3][0][1]))); ikcnkt[16][2][1] = ((.00074417*cnk[16][1][2])+((.00099076*cnk[16][1][0])-( .00244367*Cik[3][1][1]))); ikcnkt[16][2][2] = ((.00074417*cnk[16][2][2])+((.00099076*cnk[16][2][0])-( .00244367*Cik[3][2][1]))); ikcnkt[17][0][0] = ((.00047536*cnk[17][0][2])+((.00933952*cnk[17][0][0])-( 2.97e-5*Cik[3][0][1]))); ikcnkt[17][0][1] = ((.00047536*cnk[17][1][2])+((.00933952*cnk[17][1][0])-( 2.97e-5*Cik[3][1][1]))); ikcnkt[17][0][2] = ((.00047536*cnk[17][2][2])+((.00933952*cnk[17][2][0])-( 2.97e-5*Cik[3][2][1]))); ikcnkt[17][1][0] = ((.00105064*cnk[17][0][2])+((.00915719*Cik[3][0][1])-( 2.97e-5*cnk[17][0][0]))); ikcnkt[17][1][1] = ((.00105064*cnk[17][1][2])+((.00915719*Cik[3][1][1])-( 2.97e-5*cnk[17][1][0]))); ikcnkt[17][1][2] = ((.00105064*cnk[17][2][2])+((.00915719*Cik[3][2][1])-( 2.97e-5*cnk[17][2][0]))); ikcnkt[17][2][0] = ((.00032059*cnk[17][0][2])+((.00047536*cnk[17][0][0])+( .00105064*Cik[3][0][1]))); ikcnkt[17][2][1] = ((.00032059*cnk[17][1][2])+((.00047536*cnk[17][1][0])+( .00105064*Cik[3][1][1]))); ikcnkt[17][2][2] = ((.00032059*cnk[17][2][2])+((.00047536*cnk[17][2][0])+( .00105064*Cik[3][2][1]))); ikcnkt[18][0][0] = ((.00047517*cnk[18][0][2])+((3.024e-5*Cik[3][0][1])+( .00933861*cnk[18][0][0]))); ikcnkt[18][0][1] = ((.00047517*cnk[18][1][2])+((3.024e-5*Cik[3][1][1])+( .00933861*cnk[18][1][0]))); ikcnkt[18][0][2] = ((.00047517*cnk[18][2][2])+((3.024e-5*Cik[3][2][1])+( .00933861*cnk[18][2][0]))); ikcnkt[18][1][0] = (((3.024e-5*cnk[18][0][0])+(.00915219*Cik[3][0][1]))-( .00106364*cnk[18][0][2])); ikcnkt[18][1][1] = (((3.024e-5*cnk[18][1][0])+(.00915219*Cik[3][1][1]))-( .00106364*cnk[18][1][2])); ikcnkt[18][1][2] = (((3.024e-5*cnk[18][2][0])+(.00915219*Cik[3][2][1]))-( .00106364*cnk[18][2][2])); ikcnkt[18][2][0] = ((.00032466*cnk[18][0][2])+((.00047517*cnk[18][0][0])-( .00106364*Cik[3][0][1]))); ikcnkt[18][2][1] = ((.00032466*cnk[18][1][2])+((.00047517*cnk[18][1][0])-( .00106364*Cik[3][1][1]))); ikcnkt[18][2][2] = ((.00032466*cnk[18][2][2])+((.00047517*cnk[18][2][0])-( .00106364*Cik[3][2][1]))); ikcnkt[19][0][0] = ((7.331e-5*cnk[19][0][2])+((.00086458*cnk[19][0][0])-( 2.761e-5*Cik[3][0][1]))); ikcnkt[19][0][1] = ((7.331e-5*cnk[19][1][2])+((.00086458*cnk[19][1][0])-( 2.761e-5*Cik[3][1][1]))); ikcnkt[19][0][2] = ((7.331e-5*cnk[19][2][2])+((.00086458*cnk[19][2][0])-( 2.761e-5*Cik[3][2][1]))); ikcnkt[19][1][0] = (((.00082117*Cik[3][0][1])-(2.761e-5*cnk[19][0][0]))-( .00020674*cnk[19][0][2])); ikcnkt[19][1][1] = (((.00082117*Cik[3][1][1])-(2.761e-5*cnk[19][1][0]))-( .00020674*cnk[19][1][2])); ikcnkt[19][1][2] = (((.00082117*Cik[3][2][1])-(2.761e-5*cnk[19][2][0]))-( .00020674*cnk[19][2][2])); ikcnkt[19][2][0] = ((3.059e-5*cnk[19][0][2])+((7.331e-5*cnk[19][0][0])-( .00020674*Cik[3][0][1]))); ikcnkt[19][2][1] = ((3.059e-5*cnk[19][1][2])+((7.331e-5*cnk[19][1][0])-( .00020674*Cik[3][1][1]))); ikcnkt[19][2][2] = ((3.059e-5*cnk[19][2][2])+((7.331e-5*cnk[19][2][0])-( .00020674*Cik[3][2][1]))); ikcnkt[20][0][0] = ((7.338e-5*cnk[20][0][2])+((2.77e-5*Cik[3][0][1])+( .00086575*cnk[20][0][0]))); ikcnkt[20][0][1] = ((7.338e-5*cnk[20][1][2])+((2.77e-5*Cik[3][1][1])+( .00086575*cnk[20][1][0]))); ikcnkt[20][0][2] = ((7.338e-5*cnk[20][2][2])+((2.77e-5*Cik[3][2][1])+( .00086575*cnk[20][2][0]))); ikcnkt[20][1][0] = ((.00020754*cnk[20][0][2])+((2.77e-5*cnk[20][0][0])+( .00082196*Cik[3][0][1]))); ikcnkt[20][1][1] = ((.00020754*cnk[20][1][2])+((2.77e-5*cnk[20][1][0])+( .00082196*Cik[3][1][1]))); ikcnkt[20][1][2] = ((.00020754*cnk[20][2][2])+((2.77e-5*cnk[20][2][0])+( .00082196*Cik[3][2][1]))); ikcnkt[20][2][0] = ((3.093e-5*cnk[20][0][2])+((7.338e-5*cnk[20][0][0])+( .00020754*Cik[3][0][1]))); ikcnkt[20][2][1] = ((3.093e-5*cnk[20][1][2])+((7.338e-5*cnk[20][1][0])+( .00020754*Cik[3][1][1]))); ikcnkt[20][2][2] = ((3.093e-5*cnk[20][2][2])+((7.338e-5*cnk[20][2][0])+( .00020754*Cik[3][2][1]))); ikcnkt[21][0][0] = ((.00046692*cnk[21][0][0])-(4e-8*cnk[21][0][2])); ikcnkt[21][0][2] = ((.00046692*cnk[21][2][0])-(4e-8*cnk[21][2][2])); ikcnkt[21][1][0] = ((1e-8*cnk[21][0][0])-(.00013462*cnk[21][0][2])); ikcnkt[21][1][2] = ((1e-8*cnk[21][2][0])-(.00013462*cnk[21][2][2])); ikcnkt[21][2][0] = ((3.217e-5*cnk[21][0][2])-(4e-8*cnk[21][0][0])); ikcnkt[21][2][2] = ((3.217e-5*cnk[21][2][2])-(4e-8*cnk[21][2][0])); ikcnkt[22][0][0] = ((.00046679*cnk[22][0][0])-(4e-8*cnk[22][0][2])); ikcnkt[22][0][2] = ((.00046679*cnk[22][2][0])-(4e-8*cnk[22][2][2])); ikcnkt[22][1][0] = ((.00013693*cnk[22][0][2])-(1e-8*cnk[22][0][0])); ikcnkt[22][1][2] = ((.00013693*cnk[22][2][2])-(1e-8*cnk[22][2][0])); ikcnkt[22][2][0] = ((3.326e-5*cnk[22][0][2])-(4e-8*cnk[22][0][0])); ikcnkt[22][2][2] = ((3.326e-5*cnk[22][2][2])-(4e-8*cnk[22][2][0])); temp[0] = (((.1816698*(.0252601943623936+(rnk[7][2]*rnk[7][2])))+(( ikcnkt[7][0][0]*c7)+(ikcnkt[7][2][0]*s7)))+(((.18166784*(.0244724317206889 +(rnk[6][2]*rnk[6][2])))+((ikcnkt[6][0][0]*c6)+(ikcnkt[6][2][0]*s6)))+(( 11.02968359*((q[1]*q[1])+(q[2]*q[2])))+((Cik[3][0][2]*ikcnkt[5][2][0])+(( Cik[3][0][0]*ikcnkt[5][0][0])+(Cik[3][0][1]*ikcnkt[5][1][0])))))); temp[1] = (((2.89586298*((rnk[9][1]*rnk[9][1])+(rnk[9][2]*rnk[9][2])))+(( cnk[9][0][2]*ikcnkt[9][2][0])+((Cik[3][0][1]*ikcnkt[9][1][0])+( cnk[9][0][0]*ikcnkt[9][0][0]))))+(((9.84983599*((rnk[8][1]*rnk[8][1])+( rnk[8][2]*rnk[8][2])))+((cnk[8][0][2]*ikcnkt[8][2][0])+((Cik[3][0][1]* ikcnkt[8][1][0])+(cnk[8][0][0]*ikcnkt[8][0][0]))))+temp[0])); temp[2] = (((1.02133193*((rnk[11][1]*rnk[11][1])+(rnk[11][2]*rnk[11][2])))+( (cnk[11][0][2]*ikcnkt[11][2][0])+((Cik[3][0][1]*ikcnkt[11][1][0])+( cnk[11][0][0]*ikcnkt[11][0][0]))))+(((2.8984419*((rnk[10][1]*rnk[10][1])+( rnk[10][2]*rnk[10][2])))+((cnk[10][0][2]*ikcnkt[10][2][0])+((Cik[3][0][1]* ikcnkt[10][1][0])+(cnk[10][0][0]*ikcnkt[10][0][0]))))+temp[1])); temp[3] = (((.53380008*((rnk[13][1]*rnk[13][1])+(rnk[13][2]*rnk[13][2])))+(( cnk[13][0][2]*ikcnkt[13][2][0])+((Cik[3][0][1]*ikcnkt[13][1][0])+( cnk[13][0][0]*ikcnkt[13][0][0]))))+(((1.02007998*((rnk[12][1]*rnk[12][1])+ (rnk[12][2]*rnk[12][2])))+((cnk[12][0][2]*ikcnkt[12][2][0])+((Cik[3][0][1] *ikcnkt[12][1][0])+(cnk[12][0][0]*ikcnkt[12][0][0]))))+temp[2])); temp[4] = (((1.00985702*((rnk[15][1]*rnk[15][1])+(rnk[15][2]*rnk[15][2])))+( (cnk[15][0][2]*ikcnkt[15][2][0])+((Cik[3][0][1]*ikcnkt[15][1][0])+( cnk[15][0][0]*ikcnkt[15][0][0]))))+(((.5325902*((rnk[14][1]*rnk[14][1])+( rnk[14][2]*rnk[14][2])))+((cnk[14][0][2]*ikcnkt[14][2][0])+((Cik[3][0][1]* ikcnkt[14][1][0])+(cnk[14][0][0]*ikcnkt[14][0][0]))))+temp[3])); temp[5] = (((.43362906*((rnk[17][1]*rnk[17][1])+(rnk[17][2]*rnk[17][2])))+(( cnk[17][0][2]*ikcnkt[17][2][0])+((Cik[3][0][1]*ikcnkt[17][1][0])+( cnk[17][0][0]*ikcnkt[17][0][0]))))+(((1.01701701*((rnk[16][1]*rnk[16][1])+ (rnk[16][2]*rnk[16][2])))+((cnk[16][0][2]*ikcnkt[16][2][0])+((Cik[3][0][1] *ikcnkt[16][1][0])+(cnk[16][0][0]*ikcnkt[16][0][0]))))+temp[4])); temp[6] = (((.05946345*((rnk[19][1]*rnk[19][1])+(rnk[19][2]*rnk[19][2])))+(( cnk[19][0][2]*ikcnkt[19][2][0])+((Cik[3][0][1]*ikcnkt[19][1][0])+( cnk[19][0][0]*ikcnkt[19][0][0]))))+(((.43358398*((rnk[18][1]*rnk[18][1])+( rnk[18][2]*rnk[18][2])))+((cnk[18][0][2]*ikcnkt[18][2][0])+((Cik[3][0][1]* ikcnkt[18][1][0])+(cnk[18][0][0]*ikcnkt[18][0][0]))))+temp[5])); temp[7] = (((.02601109*(.0252617455875136+(rnk[22][2]*rnk[22][2])))+(( cnk[22][0][0]*ikcnkt[22][0][0])+(cnk[22][0][2]*ikcnkt[22][2][0])))+((( .0260106*(.0244707860345881+(rnk[21][2]*rnk[21][2])))+((cnk[21][0][0]* ikcnkt[21][0][0])+(cnk[21][0][2]*ikcnkt[21][2][0])))+(((.05944511*(( rnk[20][1]*rnk[20][1])+(rnk[20][2]*rnk[20][2])))+((cnk[20][0][2]* ikcnkt[20][2][0])+((Cik[3][0][1]*ikcnkt[20][1][0])+(cnk[20][0][0]* ikcnkt[20][0][0]))))+temp[6]))); icm[0][0] = (temp[7]-(33.20998161*((com[1]*com[1])+(com[2]*com[2])))); temp[0] = ((((.028873609728288*rnk[7][0])+((.00230647*s7)-(8e-8*c7)))+(((( 8e-8*c6)-(.00226939*s6))-(.0284195119356928*rnk[6][0]))+(((Cik[3][0][2]* ikcnkt[5][2][1])+((Cik[3][0][0]*ikcnkt[5][0][1])+(Cik[3][0][1]* ikcnkt[5][1][1])))-(11.02968359*(q[0]*q[1])))))+(((cnk[8][0][2]* ikcnkt[8][2][1])+((Cik[3][0][1]*ikcnkt[8][1][1])+(cnk[8][0][0]* ikcnkt[8][0][1])))-(9.84983599*(rnk[8][0]*rnk[8][1])))); temp[1] = ((((cnk[11][0][2]*ikcnkt[11][2][1])+((Cik[3][0][1]* ikcnkt[11][1][1])+(cnk[11][0][0]*ikcnkt[11][0][1])))-(1.02133193*( rnk[11][0]*rnk[11][1])))+((((cnk[10][0][2]*ikcnkt[10][2][1])+(( Cik[3][0][1]*ikcnkt[10][1][1])+(cnk[10][0][0]*ikcnkt[10][0][1])))-( 2.8984419*(rnk[10][0]*rnk[10][1])))+((((cnk[9][0][2]*ikcnkt[9][2][1])+(( Cik[3][0][1]*ikcnkt[9][1][1])+(cnk[9][0][0]*ikcnkt[9][0][1])))-(2.89586298 *(rnk[9][0]*rnk[9][1])))+temp[0]))); temp[2] = ((((cnk[14][0][2]*ikcnkt[14][2][1])+((Cik[3][0][1]* ikcnkt[14][1][1])+(cnk[14][0][0]*ikcnkt[14][0][1])))-(.5325902*(rnk[14][0] *rnk[14][1])))+((((cnk[13][0][2]*ikcnkt[13][2][1])+((Cik[3][0][1]* ikcnkt[13][1][1])+(cnk[13][0][0]*ikcnkt[13][0][1])))-(.53380008*( rnk[13][0]*rnk[13][1])))+((((cnk[12][0][2]*ikcnkt[12][2][1])+(( Cik[3][0][1]*ikcnkt[12][1][1])+(cnk[12][0][0]*ikcnkt[12][0][1])))-( 1.02007998*(rnk[12][0]*rnk[12][1])))+temp[1]))); temp[3] = ((((cnk[17][0][2]*ikcnkt[17][2][1])+((Cik[3][0][1]* ikcnkt[17][1][1])+(cnk[17][0][0]*ikcnkt[17][0][1])))-(.43362906*( rnk[17][0]*rnk[17][1])))+((((cnk[16][0][2]*ikcnkt[16][2][1])+(( Cik[3][0][1]*ikcnkt[16][1][1])+(cnk[16][0][0]*ikcnkt[16][0][1])))-( 1.01701701*(rnk[16][0]*rnk[16][1])))+((((cnk[15][0][2]*ikcnkt[15][2][1])+( (Cik[3][0][1]*ikcnkt[15][1][1])+(cnk[15][0][0]*ikcnkt[15][0][1])))-( 1.00985702*(rnk[15][0]*rnk[15][1])))+temp[2]))); temp[4] = ((((cnk[20][0][2]*ikcnkt[20][2][1])+((Cik[3][0][1]* ikcnkt[20][1][1])+(cnk[20][0][0]*ikcnkt[20][0][1])))-(.05944511*( rnk[20][0]*rnk[20][1])))+((((cnk[19][0][2]*ikcnkt[19][2][1])+(( Cik[3][0][1]*ikcnkt[19][1][1])+(cnk[19][0][0]*ikcnkt[19][0][1])))-( .05946345*(rnk[19][0]*rnk[19][1])))+((((cnk[18][0][2]*ikcnkt[18][2][1])+(( Cik[3][0][1]*ikcnkt[18][1][1])+(cnk[18][0][0]*ikcnkt[18][0][1])))-( .43358398*(rnk[18][0]*rnk[18][1])))+temp[3]))); icm[0][1] = ((33.20998161*(com[0]*com[1]))+(((.0041341880783896*rnk[22][0])+ ((.00013693*cnk[22][0][2])-(1e-8*cnk[22][0][0])))+((((1e-8*cnk[21][0][0])- (.00013462*cnk[21][0][2]))-(.004068874832946*rnk[21][0]))+temp[4]))); temp[0] = ((((cnk[8][0][2]*ikcnkt[8][2][2])+((Cik[3][0][1]*ikcnkt[8][1][2])+ (cnk[8][0][0]*ikcnkt[8][0][2])))-(9.84983599*(rnk[8][0]*rnk[8][2])))+(((( ikcnkt[7][0][2]*c7)+(ikcnkt[7][2][2]*s7))-(.1816698*(rnk[7][0]*rnk[7][2])) )+((((Cik[3][0][2]*ikcnkt[5][2][2])+((Cik[3][0][0]*ikcnkt[5][0][2])+( Cik[3][0][1]*ikcnkt[5][1][2])))-(11.02968359*(q[0]*q[2])))+((( ikcnkt[6][0][2]*c6)+(ikcnkt[6][2][2]*s6))-(.18166784*(rnk[6][0]*rnk[6][2]) ))))); temp[1] = ((((cnk[11][0][2]*ikcnkt[11][2][2])+((Cik[3][0][1]* ikcnkt[11][1][2])+(cnk[11][0][0]*ikcnkt[11][0][2])))-(1.02133193*( rnk[11][0]*rnk[11][2])))+((((cnk[10][0][2]*ikcnkt[10][2][2])+(( Cik[3][0][1]*ikcnkt[10][1][2])+(cnk[10][0][0]*ikcnkt[10][0][2])))-( 2.8984419*(rnk[10][0]*rnk[10][2])))+((((cnk[9][0][2]*ikcnkt[9][2][2])+(( Cik[3][0][1]*ikcnkt[9][1][2])+(cnk[9][0][0]*ikcnkt[9][0][2])))-(2.89586298 *(rnk[9][0]*rnk[9][2])))+temp[0]))); temp[2] = ((((cnk[14][0][2]*ikcnkt[14][2][2])+((Cik[3][0][1]* ikcnkt[14][1][2])+(cnk[14][0][0]*ikcnkt[14][0][2])))-(.5325902*(rnk[14][0] *rnk[14][2])))+((((cnk[13][0][2]*ikcnkt[13][2][2])+((Cik[3][0][1]* ikcnkt[13][1][2])+(cnk[13][0][0]*ikcnkt[13][0][2])))-(.53380008*( rnk[13][0]*rnk[13][2])))+((((cnk[12][0][2]*ikcnkt[12][2][2])+(( Cik[3][0][1]*ikcnkt[12][1][2])+(cnk[12][0][0]*ikcnkt[12][0][2])))-( 1.02007998*(rnk[12][0]*rnk[12][2])))+temp[1]))); temp[3] = ((((cnk[17][0][2]*ikcnkt[17][2][2])+((Cik[3][0][1]* ikcnkt[17][1][2])+(cnk[17][0][0]*ikcnkt[17][0][2])))-(.43362906*( rnk[17][0]*rnk[17][2])))+((((cnk[16][0][2]*ikcnkt[16][2][2])+(( Cik[3][0][1]*ikcnkt[16][1][2])+(cnk[16][0][0]*ikcnkt[16][0][2])))-( 1.01701701*(rnk[16][0]*rnk[16][2])))+((((cnk[15][0][2]*ikcnkt[15][2][2])+( (Cik[3][0][1]*ikcnkt[15][1][2])+(cnk[15][0][0]*ikcnkt[15][0][2])))-( 1.00985702*(rnk[15][0]*rnk[15][2])))+temp[2]))); temp[4] = ((((cnk[20][0][2]*ikcnkt[20][2][2])+((Cik[3][0][1]* ikcnkt[20][1][2])+(cnk[20][0][0]*ikcnkt[20][0][2])))-(.05944511*( rnk[20][0]*rnk[20][2])))+((((cnk[19][0][2]*ikcnkt[19][2][2])+(( Cik[3][0][1]*ikcnkt[19][1][2])+(cnk[19][0][0]*ikcnkt[19][0][2])))-( .05946345*(rnk[19][0]*rnk[19][2])))+((((cnk[18][0][2]*ikcnkt[18][2][2])+(( Cik[3][0][1]*ikcnkt[18][1][2])+(cnk[18][0][0]*ikcnkt[18][0][2])))-( .43358398*(rnk[18][0]*rnk[18][2])))+temp[3]))); icm[0][2] = ((33.20998161*(com[0]*com[2]))+((((cnk[22][0][0]* ikcnkt[22][0][2])+(cnk[22][0][2]*ikcnkt[22][2][2]))-(.02601109*(rnk[22][0] *rnk[22][2])))+((((cnk[21][0][0]*ikcnkt[21][0][2])+(cnk[21][0][2]* ikcnkt[21][2][2]))-(.0260106*(rnk[21][0]*rnk[21][2])))+temp[4]))); icm[1][0] = icm[0][1]; temp[0] = (((.1816698*((rnk[7][0]*rnk[7][0])+(rnk[7][2]*rnk[7][2])))+(( .18166784*((rnk[6][0]*rnk[6][0])+(rnk[6][2]*rnk[6][2])))+((11.02968359*(( q[0]*q[0])+(q[2]*q[2])))+((Cik[3][1][2]*ikcnkt[5][2][1])+((Cik[3][1][0]* ikcnkt[5][0][1])+(Cik[3][1][1]*ikcnkt[5][1][1]))))))+((9.84983599*(( rnk[8][0]*rnk[8][0])+(rnk[8][2]*rnk[8][2])))+((cnk[8][1][2]* ikcnkt[8][2][1])+((Cik[3][1][1]*ikcnkt[8][1][1])+(cnk[8][1][0]* ikcnkt[8][0][1]))))); temp[1] = (((2.8984419*((rnk[10][0]*rnk[10][0])+(rnk[10][2]*rnk[10][2])))+(( cnk[10][1][2]*ikcnkt[10][2][1])+((Cik[3][1][1]*ikcnkt[10][1][1])+( cnk[10][1][0]*ikcnkt[10][0][1]))))+(((2.89586298*((rnk[9][0]*rnk[9][0])+( rnk[9][2]*rnk[9][2])))+((cnk[9][1][2]*ikcnkt[9][2][1])+((Cik[3][1][1]* ikcnkt[9][1][1])+(cnk[9][1][0]*ikcnkt[9][0][1]))))+temp[0])); temp[2] = (((1.02007998*((rnk[12][0]*rnk[12][0])+(rnk[12][2]*rnk[12][2])))+( (cnk[12][1][2]*ikcnkt[12][2][1])+((Cik[3][1][1]*ikcnkt[12][1][1])+( cnk[12][1][0]*ikcnkt[12][0][1]))))+(((1.02133193*((rnk[11][0]*rnk[11][0])+ (rnk[11][2]*rnk[11][2])))+((cnk[11][1][2]*ikcnkt[11][2][1])+((Cik[3][1][1] *ikcnkt[11][1][1])+(cnk[11][1][0]*ikcnkt[11][0][1]))))+temp[1])); temp[3] = (((.5325902*((rnk[14][0]*rnk[14][0])+(rnk[14][2]*rnk[14][2])))+(( cnk[14][1][2]*ikcnkt[14][2][1])+((Cik[3][1][1]*ikcnkt[14][1][1])+( cnk[14][1][0]*ikcnkt[14][0][1]))))+(((.53380008*((rnk[13][0]*rnk[13][0])+( rnk[13][2]*rnk[13][2])))+((cnk[13][1][2]*ikcnkt[13][2][1])+((Cik[3][1][1]* ikcnkt[13][1][1])+(cnk[13][1][0]*ikcnkt[13][0][1]))))+temp[2])); temp[4] = (((1.01701701*((rnk[16][0]*rnk[16][0])+(rnk[16][2]*rnk[16][2])))+( (cnk[16][1][2]*ikcnkt[16][2][1])+((Cik[3][1][1]*ikcnkt[16][1][1])+( cnk[16][1][0]*ikcnkt[16][0][1]))))+(((1.00985702*((rnk[15][0]*rnk[15][0])+ (rnk[15][2]*rnk[15][2])))+((cnk[15][1][2]*ikcnkt[15][2][1])+((Cik[3][1][1] *ikcnkt[15][1][1])+(cnk[15][1][0]*ikcnkt[15][0][1]))))+temp[3])); temp[5] = (((.43358398*((rnk[18][0]*rnk[18][0])+(rnk[18][2]*rnk[18][2])))+(( cnk[18][1][2]*ikcnkt[18][2][1])+((Cik[3][1][1]*ikcnkt[18][1][1])+( cnk[18][1][0]*ikcnkt[18][0][1]))))+(((.43362906*((rnk[17][0]*rnk[17][0])+( rnk[17][2]*rnk[17][2])))+((cnk[17][1][2]*ikcnkt[17][2][1])+((Cik[3][1][1]* ikcnkt[17][1][1])+(cnk[17][1][0]*ikcnkt[17][0][1]))))+temp[4])); temp[6] = ((.0260106*((rnk[21][0]*rnk[21][0])+(rnk[21][2]*rnk[21][2])))+((( .05944511*((rnk[20][0]*rnk[20][0])+(rnk[20][2]*rnk[20][2])))+(( cnk[20][1][2]*ikcnkt[20][2][1])+((Cik[3][1][1]*ikcnkt[20][1][1])+( cnk[20][1][0]*ikcnkt[20][0][1]))))+(((.05946345*((rnk[19][0]*rnk[19][0])+( rnk[19][2]*rnk[19][2])))+((cnk[19][1][2]*ikcnkt[19][2][1])+((Cik[3][1][1]* ikcnkt[19][1][1])+(cnk[19][1][0]*ikcnkt[19][0][1]))))+temp[5]))); icm[1][1] = (.08513015+(((.02601109*((rnk[22][0]*rnk[22][0])+(rnk[22][2]* rnk[22][2])))+temp[6])-(33.20998161*((com[0]*com[0])+(com[2]*com[2]))))); temp[0] = (((ikcnkt[7][1][2]+(.028873609728288*rnk[7][2]))+((ikcnkt[6][1][2] -(.0284195119356928*rnk[6][2]))+(((Cik[3][1][2]*ikcnkt[5][2][2])+(( Cik[3][1][0]*ikcnkt[5][0][2])+(Cik[3][1][1]*ikcnkt[5][1][2])))-( 11.02968359*(q[1]*q[2])))))+(((cnk[8][1][2]*ikcnkt[8][2][2])+(( Cik[3][1][1]*ikcnkt[8][1][2])+(cnk[8][1][0]*ikcnkt[8][0][2])))-(9.84983599 *(rnk[8][1]*rnk[8][2])))); temp[1] = ((((cnk[11][1][2]*ikcnkt[11][2][2])+((Cik[3][1][1]* ikcnkt[11][1][2])+(cnk[11][1][0]*ikcnkt[11][0][2])))-(1.02133193*( rnk[11][1]*rnk[11][2])))+((((cnk[10][1][2]*ikcnkt[10][2][2])+(( Cik[3][1][1]*ikcnkt[10][1][2])+(cnk[10][1][0]*ikcnkt[10][0][2])))-( 2.8984419*(rnk[10][1]*rnk[10][2])))+((((cnk[9][1][2]*ikcnkt[9][2][2])+(( Cik[3][1][1]*ikcnkt[9][1][2])+(cnk[9][1][0]*ikcnkt[9][0][2])))-(2.89586298 *(rnk[9][1]*rnk[9][2])))+temp[0]))); temp[2] = ((((cnk[14][1][2]*ikcnkt[14][2][2])+((Cik[3][1][1]* ikcnkt[14][1][2])+(cnk[14][1][0]*ikcnkt[14][0][2])))-(.5325902*(rnk[14][1] *rnk[14][2])))+((((cnk[13][1][2]*ikcnkt[13][2][2])+((Cik[3][1][1]* ikcnkt[13][1][2])+(cnk[13][1][0]*ikcnkt[13][0][2])))-(.53380008*( rnk[13][1]*rnk[13][2])))+((((cnk[12][1][2]*ikcnkt[12][2][2])+(( Cik[3][1][1]*ikcnkt[12][1][2])+(cnk[12][1][0]*ikcnkt[12][0][2])))-( 1.02007998*(rnk[12][1]*rnk[12][2])))+temp[1]))); temp[3] = ((((cnk[17][1][2]*ikcnkt[17][2][2])+((Cik[3][1][1]* ikcnkt[17][1][2])+(cnk[17][1][0]*ikcnkt[17][0][2])))-(.43362906*( rnk[17][1]*rnk[17][2])))+((((cnk[16][1][2]*ikcnkt[16][2][2])+(( Cik[3][1][1]*ikcnkt[16][1][2])+(cnk[16][1][0]*ikcnkt[16][0][2])))-( 1.01701701*(rnk[16][1]*rnk[16][2])))+((((cnk[15][1][2]*ikcnkt[15][2][2])+( (Cik[3][1][1]*ikcnkt[15][1][2])+(cnk[15][1][0]*ikcnkt[15][0][2])))-( 1.00985702*(rnk[15][1]*rnk[15][2])))+temp[2]))); temp[4] = ((((cnk[20][1][2]*ikcnkt[20][2][2])+((Cik[3][1][1]* ikcnkt[20][1][2])+(cnk[20][1][0]*ikcnkt[20][0][2])))-(.05944511*( rnk[20][1]*rnk[20][2])))+((((cnk[19][1][2]*ikcnkt[19][2][2])+(( Cik[3][1][1]*ikcnkt[19][1][2])+(cnk[19][1][0]*ikcnkt[19][0][2])))-( .05946345*(rnk[19][1]*rnk[19][2])))+((((cnk[18][1][2]*ikcnkt[18][2][2])+(( Cik[3][1][1]*ikcnkt[18][1][2])+(cnk[18][1][0]*ikcnkt[18][0][2])))-( .43358398*(rnk[18][1]*rnk[18][2])))+temp[3]))); icm[1][2] = ((33.20998161*(com[1]*com[2]))+((ikcnkt[22][1][2]+( .0041341880783896*rnk[22][2]))+((ikcnkt[21][1][2]-(.004068874832946* rnk[21][2]))+temp[4]))); icm[2][0] = icm[0][2]; icm[2][1] = icm[1][2]; temp[0] = (((.1816698*(.0252601943623936+(rnk[7][0]*rnk[7][0])))+(( ikcnkt[7][2][2]*c7)-(ikcnkt[7][0][2]*s7)))+(((.18166784*(.0244724317206889 +(rnk[6][0]*rnk[6][0])))+((ikcnkt[6][2][2]*c6)-(ikcnkt[6][0][2]*s6)))+(( 11.02968359*((q[0]*q[0])+(q[1]*q[1])))+((Cik[3][2][2]*ikcnkt[5][2][2])+(( Cik[3][2][0]*ikcnkt[5][0][2])+(Cik[3][2][1]*ikcnkt[5][1][2])))))); temp[1] = (((2.89586298*((rnk[9][0]*rnk[9][0])+(rnk[9][1]*rnk[9][1])))+(( cnk[9][2][2]*ikcnkt[9][2][2])+((Cik[3][2][1]*ikcnkt[9][1][2])+( cnk[9][2][0]*ikcnkt[9][0][2]))))+(((9.84983599*((rnk[8][0]*rnk[8][0])+( rnk[8][1]*rnk[8][1])))+((cnk[8][2][2]*ikcnkt[8][2][2])+((Cik[3][2][1]* ikcnkt[8][1][2])+(cnk[8][2][0]*ikcnkt[8][0][2]))))+temp[0])); temp[2] = (((1.02133193*((rnk[11][0]*rnk[11][0])+(rnk[11][1]*rnk[11][1])))+( (cnk[11][2][2]*ikcnkt[11][2][2])+((Cik[3][2][1]*ikcnkt[11][1][2])+( cnk[11][2][0]*ikcnkt[11][0][2]))))+(((2.8984419*((rnk[10][0]*rnk[10][0])+( rnk[10][1]*rnk[10][1])))+((cnk[10][2][2]*ikcnkt[10][2][2])+((Cik[3][2][1]* ikcnkt[10][1][2])+(cnk[10][2][0]*ikcnkt[10][0][2]))))+temp[1])); temp[3] = (((.53380008*((rnk[13][0]*rnk[13][0])+(rnk[13][1]*rnk[13][1])))+(( cnk[13][2][2]*ikcnkt[13][2][2])+((Cik[3][2][1]*ikcnkt[13][1][2])+( cnk[13][2][0]*ikcnkt[13][0][2]))))+(((1.02007998*((rnk[12][0]*rnk[12][0])+ (rnk[12][1]*rnk[12][1])))+((cnk[12][2][2]*ikcnkt[12][2][2])+((Cik[3][2][1] *ikcnkt[12][1][2])+(cnk[12][2][0]*ikcnkt[12][0][2]))))+temp[2])); temp[4] = (((1.00985702*((rnk[15][0]*rnk[15][0])+(rnk[15][1]*rnk[15][1])))+( (cnk[15][2][2]*ikcnkt[15][2][2])+((Cik[3][2][1]*ikcnkt[15][1][2])+( cnk[15][2][0]*ikcnkt[15][0][2]))))+(((.5325902*((rnk[14][0]*rnk[14][0])+( rnk[14][1]*rnk[14][1])))+((cnk[14][2][2]*ikcnkt[14][2][2])+((Cik[3][2][1]* ikcnkt[14][1][2])+(cnk[14][2][0]*ikcnkt[14][0][2]))))+temp[3])); temp[5] = (((.43362906*((rnk[17][0]*rnk[17][0])+(rnk[17][1]*rnk[17][1])))+(( cnk[17][2][2]*ikcnkt[17][2][2])+((Cik[3][2][1]*ikcnkt[17][1][2])+( cnk[17][2][0]*ikcnkt[17][0][2]))))+(((1.01701701*((rnk[16][0]*rnk[16][0])+ (rnk[16][1]*rnk[16][1])))+((cnk[16][2][2]*ikcnkt[16][2][2])+((Cik[3][2][1] *ikcnkt[16][1][2])+(cnk[16][2][0]*ikcnkt[16][0][2]))))+temp[4])); temp[6] = (((.05946345*((rnk[19][0]*rnk[19][0])+(rnk[19][1]*rnk[19][1])))+(( cnk[19][2][2]*ikcnkt[19][2][2])+((Cik[3][2][1]*ikcnkt[19][1][2])+( cnk[19][2][0]*ikcnkt[19][0][2]))))+(((.43358398*((rnk[18][0]*rnk[18][0])+( rnk[18][1]*rnk[18][1])))+((cnk[18][2][2]*ikcnkt[18][2][2])+((Cik[3][2][1]* ikcnkt[18][1][2])+(cnk[18][2][0]*ikcnkt[18][0][2]))))+temp[5])); temp[7] = (((.02601109*(.0252617455875136+(rnk[22][0]*rnk[22][0])))+(( cnk[22][2][0]*ikcnkt[22][0][2])+(cnk[22][2][2]*ikcnkt[22][2][2])))+((( .0260106*(.0244707860345881+(rnk[21][0]*rnk[21][0])))+((cnk[21][2][0]* ikcnkt[21][0][2])+(cnk[21][2][2]*ikcnkt[21][2][2])))+(((.05944511*(( rnk[20][0]*rnk[20][0])+(rnk[20][1]*rnk[20][1])))+((cnk[20][2][2]* ikcnkt[20][2][2])+((Cik[3][2][1]*ikcnkt[20][1][2])+(cnk[20][2][0]* ikcnkt[20][0][2]))))+temp[6]))); icm[2][2] = (temp[7]-(33.20998161*((com[0]*com[0])+(com[1]*com[1])))); /* Used 1.72 seconds CPU time, 57344 additional bytes of memory. Equations contain 733 adds/subtracts/negates 979 multiplies 0 divides 201 assignments */ } void BOCfly :: sdpos ( int body, double pt[3], double loc[3] ) { /* Return inertial frame location of a point on a body. */ double pv[3]; if (sdchkbnum(21,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(21,23); return; } if (body == -1) { loc[0] = pt[0]; loc[1] = pt[1]; loc[2] = pt[2]; } else { pv[0] = rnb[body][0]+pt[0]*cnb[body][0][0]+pt[1]*cnb[body][0][1]+pt[2]* cnb[body][0][2]; pv[1] = rnb[body][1]+pt[0]*cnb[body][1][0]+pt[1]*cnb[body][1][1]+pt[2]* cnb[body][1][2]; pv[2] = rnb[body][2]+pt[0]*cnb[body][2][0]+pt[1]*cnb[body][2][1]+pt[2]* cnb[body][2][2]; loc[0] = pv[0]; loc[1] = pv[1]; loc[2] = pv[2]; } } void BOCfly :: sdvel ( int body, double pt[3], double velo[3] ) { /* Return inertial frame velocity of a point on a body. */ double pv[3]; if (sdchkbnum(22,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(22,23); return; } if (body == -1) { velo[0] = 0.; velo[1] = 0.; velo[2] = 0.; } else { pv[0] = wb[body][1]*pt[2]-wb[body][2]*pt[1]; pv[1] = wb[body][2]*pt[0]-wb[body][0]*pt[2]; pv[2] = wb[body][0]*pt[1]-wb[body][1]*pt[0]; velo[0] = vnb[body][0]+pv[0]*cnb[body][0][0]+pv[1]*cnb[body][0][1]+pv[2] *cnb[body][0][2]; velo[1] = vnb[body][1]+pv[0]*cnb[body][1][0]+pv[1]*cnb[body][1][1]+pv[2] *cnb[body][1][2]; velo[2] = vnb[body][2]+pv[0]*cnb[body][2][0]+pv[1]*cnb[body][2][1]+pv[2] *cnb[body][2][2]; } } void BOCfly :: sdorient ( int body, double dircos[3][3] ) { /* Return orientation of body w.r.t. ground frame. */ if (sdchkbnum(23,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(23,23); return; } if (body == -1) { dircos[0][0] = 1.; dircos[0][1] = 0.; dircos[0][2] = 0.; dircos[1][0] = 0.; dircos[1][1] = 1.; dircos[1][2] = 0.; dircos[2][0] = 0.; dircos[2][1] = 0.; dircos[2][2] = 1.; } else { dircos[0][0] = cnb[body][0][0]; dircos[0][1] = cnb[body][0][1]; dircos[0][2] = cnb[body][0][2]; dircos[1][0] = cnb[body][1][0]; dircos[1][1] = cnb[body][1][1]; dircos[1][2] = cnb[body][1][2]; dircos[2][0] = cnb[body][2][0]; dircos[2][1] = cnb[body][2][1]; dircos[2][2] = cnb[body][2][2]; } } void BOCfly :: sdangvel ( int body, double avel[3] ) { /* Return angular velocity of the body. */ if (sdchkbnum(24,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(24,23); return; } if (body == -1) { avel[0] = 0.; avel[1] = 0.; avel[2] = 0.; } else { avel[0] = wb[body][0]; avel[1] = wb[body][1]; avel[2] = wb[body][2]; } } void BOCfly :: sdtrans ( int frbod, double ivec[3], int tobod, double ovec[3] ) { /* Transform ivec from frbod frame to tobod frame. */ double pv[3]; if (sdchkbnum(25,frbod) != 0) { return; } if (sdchkbnum(25,tobod) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(25,23); return; } if (frbod == tobod) { sdvcopy(ivec,ovec); return; } if (frbod == -1) { sdvcopy(ivec,pv); ovec[0] = pv[0]*cnb[tobod][0][0]+pv[1]*cnb[tobod][1][0]+pv[2]*cnb[tobod ][2][0]; ovec[1] = pv[0]*cnb[tobod][0][1]+pv[1]*cnb[tobod][1][1]+pv[2]*cnb[tobod ][2][1]; ovec[2] = pv[0]*cnb[tobod][0][2]+pv[1]*cnb[tobod][1][2]+pv[2]*cnb[tobod ][2][2]; return; } if (tobod == -1) { sdvcopy(ivec,pv); ovec[0] = pv[0]*cnb[frbod][0][0]+pv[1]*cnb[frbod][0][1]+pv[2]*cnb[frbod ][0][2]; ovec[1] = pv[0]*cnb[frbod][1][0]+pv[1]*cnb[frbod][1][1]+pv[2]*cnb[frbod ][1][2]; ovec[2] = pv[0]*cnb[frbod][2][0]+pv[1]*cnb[frbod][2][1]+pv[2]*cnb[frbod ][2][2]; return; } pv[0] = ivec[0]*cnb[frbod][0][0]+ivec[1]*cnb[frbod][0][1]+ivec[2]*cnb[frbod ][0][2]; pv[1] = ivec[0]*cnb[frbod][1][0]+ivec[1]*cnb[frbod][1][1]+ivec[2]*cnb[frbod ][1][2]; pv[2] = ivec[0]*cnb[frbod][2][0]+ivec[1]*cnb[frbod][2][1]+ivec[2]*cnb[frbod ][2][2]; ovec[0] = pv[0]*cnb[tobod][0][0]+pv[1]*cnb[tobod][1][0]+pv[2]*cnb[tobod][2][ 0]; ovec[1] = pv[0]*cnb[tobod][0][1]+pv[1]*cnb[tobod][1][1]+pv[2]*cnb[tobod][2][ 1]; ovec[2] = pv[0]*cnb[tobod][0][2]+pv[1]*cnb[tobod][1][2]+pv[2]*cnb[tobod][2][ 2]; } void BOCfly :: sdrel2cart ( int coord, int body, double point[3], double linchg[3], double rotchg[3] ) { /* Return derivative of pt loc and body orient w.r.t. hinge rate */ int x,i,gnd; double lin[3],pv[3]; double pink[3],ptvec[3]; if ((coord < 0) || (coord > 22)) { sdseterr(59,45); return; } if (sdchkbnum(59,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(59,23); return; } gnd = -1; sdvset(0.,0.,0.,linchg); sdvset(0.,0.,0.,rotchg); i = body; for (;;) { if (i == gnd) { return; } x = firstq[i]; if (x <= coord) { if (coord >= x+njntdof[i]) { return; } break; } i = inb[i]; } sddoping(); for (i = 0; i < 3; i++) { pink[i] = ping[coord][i]; lin[i] = hngpt[coord][i]; } sdtrans(gnd,pink,body,pink); if (trans[coord] != 0) { sdvcopy(pink,linchg); } else { sdvcopy(pink,rotchg); sdpos(body,point,ptvec); sdvsub(ptvec,lin,ptvec); sdtrans(gnd,ptvec,body,ptvec); sdvcross(pink,ptvec,linchg); } } void BOCfly :: sdacc ( int body, double pt[3], double accel[3] ) { /* Return linear acceleration a point of the specified body. */ double pv[3]; if (sdchkbnum(32,body) != 0) { return; } if (roustate != 3) { sdseterr(32,24); return; } if (body == -1) { accel[0] = 0.; accel[1] = 0.; accel[2] = 0.; } else { pv[0] = pt[0]*dyad[body][0][0]+pt[1]*dyad[body][0][1]+pt[2]*dyad[body][0 ][2]; pv[1] = pt[0]*dyad[body][1][0]+pt[1]*dyad[body][1][1]+pt[2]*dyad[body][1 ][2]; pv[2] = pt[0]*dyad[body][2][0]+pt[1]*dyad[body][2][1]+pt[2]*dyad[body][2 ][2]; accel[0] = anb[body][0]+pv[0]*cnb[body][0][0]+pv[1]*cnb[body][0][1]+pv[2 ]*cnb[body][0][2]; accel[1] = anb[body][1]+pv[0]*cnb[body][1][0]+pv[1]*cnb[body][1][1]+pv[2 ]*cnb[body][1][2]; accel[2] = anb[body][2]+pv[0]*cnb[body][2][0]+pv[1]*cnb[body][2][1]+pv[2 ]*cnb[body][2][2]; } } void BOCfly :: sdangacc ( int body, double aacc[3] ) { /* Return angular acceleration of the body. */ if (sdchkbnum(33,body) != 0) { return; } if (roustate != 3) { sdseterr(33,24); return; } if (body == -1) { aacc[0] = 0.; aacc[1] = 0.; aacc[2] = 0.; } else { aacc[0] = onb[body][0]; aacc[1] = onb[body][1]; aacc[2] = onb[body][2]; } } void BOCfly :: sdgrav ( double gravin[3] ) { sdseterr(1,19); roustate = 0; } void BOCfly :: sdmass ( int body, double massin ) { if (sdchkbnum(2,body) != 0) { return; } if (body == -1) { sdseterr(2,15); return; } if (mkq[body] != 0) { mk[body] = massin; mkq[body] = 3; } else { sdseterr(2,19); } roustate = 0; } void BOCfly :: sdiner ( int body, double inerin[3][3] ) { int anyques; if (sdchkbnum(3,body) != 0) { return; } if (body == -1) { sdseterr(3,15); return; } anyques = 0; if (ikq[body][0][0] != 0) { ik[body][0][0] = inerin[0][0]; ikq[body][0][0] = 3; anyques = 1; } if (ikq[body][0][1] != 0) { ik[body][0][1] = inerin[0][1]; ikq[body][0][1] = 3; ik[body][1][0] = inerin[0][1]; ikq[body][1][0] = 3; anyques = 1; } if (ikq[body][0][2] != 0) { ik[body][0][2] = inerin[0][2]; ikq[body][0][2] = 3; ik[body][2][0] = inerin[0][2]; ikq[body][2][0] = 3; anyques = 1; } if (ikq[body][1][1] != 0) { ik[body][1][1] = inerin[1][1]; ikq[body][1][1] = 3; anyques = 1; } if (ikq[body][1][2] != 0) { ik[body][1][2] = inerin[1][2]; ikq[body][1][2] = 3; ik[body][2][1] = inerin[1][2]; ikq[body][2][1] = 3; anyques = 1; } if (ikq[body][2][2] != 0) { ik[body][2][2] = inerin[2][2]; ikq[body][2][2] = 3; anyques = 1; } if (anyques == 0) { sdseterr(3,19); } roustate = 0; } void BOCfly :: sdbtj ( int joint, double btjin[3] ) { int anyques; if (sdchkjnum(4,joint) != 0) { return; } anyques = 0; if (rkq[joint][0] != 0) { rk[joint][0] = btjin[0]; rkq[joint][0] = 3; anyques = 1; } if (rkq[joint][1] != 0) { rk[joint][1] = btjin[1]; rkq[joint][1] = 3; anyques = 1; } if (rkq[joint][2] != 0) { rk[joint][2] = btjin[2]; rkq[joint][2] = 3; anyques = 1; } if (anyques == 0) { sdseterr(4,19); } roustate = 0; } void BOCfly :: sditj ( int joint, double itjin[3] ) { int anyques; if (sdchkjnum(5,joint) != 0) { return; } anyques = 0; if (riq[joint][0] != 0) { ri[joint][0] = itjin[0]; riq[joint][0] = 3; anyques = 1; } if (riq[joint][1] != 0) { ri[joint][1] = itjin[1]; riq[joint][1] = 3; anyques = 1; } if (riq[joint][2] != 0) { ri[joint][2] = itjin[2]; riq[joint][2] = 3; anyques = 1; } if (anyques == 0) { sdseterr(5,19); } roustate = 0; } void BOCfly :: sdpin ( int joint, int pinno, double pinin[3] ) { int anyques,offs; if (sdchkjpin(6,joint,pinno) != 0) { return; } anyques = 0; offs = firstq[joint]+pinno; if (jtype[joint] == 21) { offs = offs+3; } if (jtype[joint] == 11) { offs = offs+1; } if (pinq[offs][0] != 0) { pin[offs][0] = pinin[0]; pinq[offs][0] = 3; anyques = 1; } if (pinq[offs][1] != 0) { pin[offs][1] = pinin[1]; pinq[offs][1] = 3; anyques = 1; } if (pinq[offs][2] != 0) { pin[offs][2] = pinin[2]; pinq[offs][2] = 3; anyques = 1; } if (anyques == 0) { sdseterr(6,19); } roustate = 0; } void BOCfly :: sdpres ( int joint, int axis, int presin ) { int anyques; if (sdchkjaxis(37,joint,axis) != 0) { return; } if ((presin != 0) && (presin != 1)) { sdseterr(37,20); } anyques = 0; if (presq[sdindx(joint,axis)] != 0) { if (presin != 0) { pres[sdindx(joint,axis)] = 1.; } else { pres[sdindx(joint,axis)] = 0.; } presq[sdindx(joint,axis)] = 3; anyques = 1; } if (anyques == 0) { sdseterr(37,19); } wwflg = 0; } void BOCfly :: sdstab ( double velin, double posin ) { stabvel = velin; stabvelq = 3; stabpos = posin; stabposq = 3; } void BOCfly :: sdgetgrav ( double gravout[3] ) { gravout[0] = grav[0]; gravout[1] = grav[1]; gravout[2] = grav[2]; } void BOCfly :: sdgetmass ( int body, double *massout ) { if (sdchkbnum(40,body) != 0) { return; } if (body == -1) { sdseterr(40,15); return; } *massout = mk[body]; } void BOCfly :: sdgetiner ( int body, double inerout[3][3] ) { if (sdchkbnum(41,body) != 0) { return; } if (body == -1) { sdseterr(41,15); return; } inerout[0][0] = ik[body][0][0]; inerout[0][1] = ik[body][0][1]; inerout[0][2] = ik[body][0][2]; inerout[1][0] = ik[body][1][0]; inerout[1][1] = ik[body][1][1]; inerout[1][2] = ik[body][1][2]; inerout[2][0] = ik[body][2][0]; inerout[2][1] = ik[body][2][1]; inerout[2][2] = ik[body][2][2]; } void BOCfly :: sdgetbtj ( int joint, double btjout[3] ) { if (sdchkjnum(42,joint) != 0) { return; } btjout[0] = rk[joint][0]; btjout[1] = rk[joint][1]; btjout[2] = rk[joint][2]; } void BOCfly :: sdgetitj ( int joint, double itjout[3] ) { if (sdchkjnum(43,joint) != 0) { return; } itjout[0] = ri[joint][0]; itjout[1] = ri[joint][1]; itjout[2] = ri[joint][2]; } void BOCfly :: sdgetpin ( int joint, int pinno, double pinout[3] ) { int offs; if (sdchkjpin(44,joint,pinno) != 0) { return; } offs = firstq[joint]+pinno; if (jtype[joint] == 21) { offs = offs+3; } if (jtype[joint] == 11) { offs = offs+1; } pinout[0] = pin[offs][0]; pinout[1] = pin[offs][1]; pinout[2] = pin[offs][2]; } void BOCfly :: sdgetpres ( int joint, int axis, int *presout ) { if (sdchkjaxis(45,joint,axis) != 0) { return; } if (pres[sdindx(joint,axis)] != 0.) { *presout = 1; } else { *presout = 0; } } void BOCfly :: sdgetstab ( double *velout, double *posout ) { *velout = stabvel; *posout = stabpos; } void BOCfly :: sdinfo ( int info[50] ) { info[0] = ground; info[1] = nbod; info[2] = ndof; info[3] = ncons; info[4] = nloop; info[5] = nldof; info[6] = nloopc; info[7] = nball; info[8] = nlball; info[9] = npres; info[10] = nuser; info[11] = 0; /* info entries from 12-49 are reserved */ } void BOCfly :: sdjnt ( int joint, int info[50], int tran[6] ) { int i,offs; if (sdchkjnum(48,joint) != 0) { return; } info[0] = jtype[joint]; info[1] = 0; offs = 0; info[2] = inb[joint]; info[3] = outb[joint]; info[4] = njntdof[joint]; info[5] = njntc[joint]; info[6] = njntp[joint]; info[7] = firstq[joint]; info[8] = ballq[joint]; info[9] = firstm[joint]; info[10] = firstp[joint]; /* info entries from 11-49 are reserved */ for (i = 0; i <= 5; i++) { if (i < njntdof[joint]) { tran[i] = trans[offs+firstq[joint]+i]; } else { tran[i] = -1; } } } void BOCfly :: sdcons ( int consno, int info[50] ) { if (sdchkucnum(49,consno) != 0) { return; } /* There are no user constraints in this problem. */ } int BOCfly :: sdchkbnum ( int routine, int bnum ) { if ((bnum < -1) || (bnum > 17)) { sdseterr(routine,15); return 1; } return 0; } int BOCfly :: sdchkjnum ( int routine, int jnum ) { if ((jnum < 0) || (jnum > 17)) { sdseterr(routine,16); return 1; } return 0; } int BOCfly :: sdchkucnum ( int routine, int ucnum ) { if ((ucnum < 0) || (ucnum > -1)) { sdseterr(routine,21); return 1; } return 0; } int BOCfly :: sdchkjaxis ( int routine, int jnum, int axnum ) { int maxax; if (sdchkjnum(routine,jnum) != 0) { return 1; } if ((axnum < 0) || (axnum > 6)) { sdseterr(routine,17); return 1; } maxax = njntdof[jnum]-1; if ((jtype[jnum] == 4) || (jtype[jnum] == 6) || (jtype[jnum] == 21)) { maxax = maxax+1; } if (axnum > maxax) { sdseterr(routine,18); return 1; } return 0; } int BOCfly :: sdchkjpin ( int routine, int jnum, int pinno ) { int maxax,pinok; if (sdchkjnum(routine,jnum) != 0) { return 1; } if ((pinno < 0) || (pinno > 5)) { sdseterr(routine,17); return 1; } if (njntdof[jnum] >= 3) { maxax = 2; } else { maxax = njntdof[jnum]-1; } if (jtype[jnum] == 4) { maxax = -1; } if (jtype[jnum] == 7) { maxax = 0; } pinok = 0; if (pinno <= maxax) { pinok = 1; } if (pinok == 0) { sdseterr(routine,18); return 1; } return 0; } /* Convert state to form using 1-2-3 Euler angles for ball joints. */ void BOCfly :: sdst2ang ( double st[24], double stang[23] ) { int i; double dc[3][3]; for (i = 0; i < 23; i++) { stang[i] = st[i]; } sdquat2dc(st[3],st[4],st[5],st[23],dc); sddc2ang(dc,&stang[3],&stang[4],&stang[5]); } /* Convert 1-2-3 form of state back to Euler parameters for ball joints. */ void BOCfly :: sdang2st ( double stang[23], double st[24] ) { int i; double dc[3][3]; for (i = 0; i < 23; i++) { st[i] = stang[i]; } sdang2dc(stang[3],stang[4],stang[5],dc); sddc2quat(dc,&st[3],&st[4],&st[5],&st[23]); } /* Normalize Euler parameters in state. */ void BOCfly :: sdnrmsterr ( double st[24], double normst[24], int routine ) { int i; double norm; for (i = 0; i < 24; i++) { normst[i] = st[i]; } norm = sqrt(st[3]*st[3]+st[4]*st[4]+st[5]*st[5]+st[23]*st[23]); if (routine != 0) { if ((norm < .9) || (norm > 1.1)) { sdseterr(routine,14); } } if (norm == 0.) { normst[23] = 1.; norm = 1.; } norm = 1./norm; normst[3] = normst[3]*norm; normst[4] = normst[4]*norm; normst[5] = normst[5]*norm; normst[23] = normst[23]*norm; } void BOCfly :: sdnormst ( double st[24], double normst[24] ) { sdnrmsterr(st,normst,0); } void BOCfly :: sdgentime ( int *gentm ) { *gentm = 5853; } /* Done. CPU seconds used: 12.03 Memory used: 1310720 bytes. Equation complexity: sdstate: 1169 adds 1521 multiplies 1 divides 1151 assignments sdderiv: 3063 adds 3897 multiplies 57 divides 2448 assignments sdresid: 1583 adds 1396 multiplies 0 divides 1042 assignments sdreac: 710 adds 542 multiplies 0 divides 426 assignments sdmom: 475 adds 609 multiplies 0 divides 77 assignments sdsys: 733 adds 979 multiplies 0 divides 201 assignments */ // END source: fly_dyn.c : //============================================== //============================================== //============================================== //============================================== //============================================== //============================================== BOCfly :: BOCfly() : BocSimBase("fly") { error_ = BOCflyerrorCONST; topo_ = BOCflytopoCONST; input_ = BOCflyinputCONST; }; BOCfly :: ~BOCfly() { }; //============================================== //============================================== //============================================== //==============================================