ADMB Documentation  11.1.2364
 All Classes Files Functions Variables Typedefs Friends Defines
f1b2v10.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: f1b2v10.cpp 2345 2014-09-15 21:57:03Z johnoel $
00003  *
00004  * Author: David Fournier and Mollie Brooks
00005  * Copyright (c) 2008-2012 Regents of the University of California
00006  */
00011 #include <df1b2fun.h>
00012 
00013 #ifndef OPT_LIB
00014   #include <cassert>
00015   #include <climits>
00016 #endif
00017 
00022 df1b2vector pow(const df1b2vector& _v,double x)
00023 {
00024   ADUNCONST(df1b2vector,v);
00025   //check_shape(x,M,"operator *");
00026   int mmin=v.indexmin();
00027   int mmax=v.indexmax();
00028   df1b2vector tmp;
00029   tmp.noallocate(mmin,mmax);
00030   for (int i=mmin;i<=mmax;i++) tmp(i)=pow(v(i),x);
00031   return tmp;
00032 }
00033 
00038 df1b2vector pow(const  df1b2vector& _x,const df1b2vector& _v)
00039 {
00040   ADUNCONST(df1b2vector,x);
00041   ADUNCONST(df1b2vector,v);
00042   //check_shape(x,M,"operator *");
00043   //check_shape(x,M,"operator *");
00044   int mmin=v.indexmin();
00045   int mmax=v.indexmax();
00046   df1b2vector tmp;
00047   tmp.noallocate(mmin,mmax);
00048   for (int i=mmin;i<=mmax;i++) tmp(i)=pow(x(i),v(i));
00049   return tmp;
00050 }
00051 
00056 df1b2vector pow(const df1b2vector& _v,const df1b2variable & _x)
00057 {
00058   ADUNCONST(df1b2vector,v);
00059   ADUNCONST(df1b2variable,x);
00060   int mmin=v.indexmin();
00061   int mmax=v.indexmax();
00062   df1b2vector tmp;
00063   tmp.noallocate(mmin,mmax);
00064   for (int i=mmin;i<=mmax;i++)
00065   {
00066     tmp(i)=pow(v(i),x);
00067   }
00068   return tmp;
00069 }
00070 
00075 df1b2vector pow(const df1b2variable & _v,const df1b2vector& _x)
00076 {
00077   ADUNCONST(df1b2variable,v);
00078   ADUNCONST(df1b2vector,x);
00079   int mmin=x.indexmin();
00080   int mmax=x.indexmax();
00081   df1b2vector tmp;
00082   tmp.noallocate(mmin,mmax);
00083   for (int i=mmin;i<=mmax;i++)
00084   {
00085     tmp(i)=pow(v,x(i));
00086   }
00087   return tmp;
00088 }
00089 
00094 df1b2vector pow(const double v,const df1b2vector& _x)
00095 {
00096   ADUNCONST(df1b2vector,x);
00097   int mmin=x.indexmin();
00098   int mmax=x.indexmax();
00099   df1b2vector tmp;
00100   tmp.noallocate(mmin,mmax);
00101   for (int i=mmin;i<=mmax;i++)
00102   {
00103     tmp(i)=pow(v,x(i));
00104   }
00105   return tmp;
00106 }
00112 df1b2vector pow(const dvector& x,  const df1b2variable& a)
00113 {
00114   RETURN_ARRAYS_INCREMENT();
00115   df1b2vector y(x.indexmin(), x.indexmax());
00116   for(int i=x.indexmin(); i<=x.indexmax(); i++)
00117   {
00118     y(i)=pow(x(i),a);
00119   }
00120 
00121   RETURN_ARRAYS_DECREMENT();
00122   return(y);
00123 }
00129 df1b2vector pow(const dvector& x,  const df1b2vector& a)
00130 {
00131   RETURN_ARRAYS_INCREMENT();
00132   df1b2vector y(x.indexmin(), x.indexmax());
00133 
00134   for(int i=x.indexmin(); i<=x.indexmax(); i++)
00135   {
00136     y(i)=pow(x(i),a(i));
00137   }
00138 
00139   RETURN_ARRAYS_DECREMENT();
00140   return(y);
00141 }
00147   dvar_vector pow(const dvar_vector& v1, const dvector& v2)
00148   {
00149     RETURN_ARRAYS_INCREMENT();
00150     dvar_vector tmp(v1.indexmin(),v1.indexmax());
00151     for (int i=v1.indexmin();i<=v1.indexmax();i++)
00152     {
00153       tmp.elem(i)=pow(v1.elem(i),v2.elem(i));
00154     }
00155     RETURN_ARRAYS_DECREMENT();
00156     return(tmp);
00157   }
00163   df1b2vector pow(df1b2vector const& _x,dvector const& v)
00164   {
00165    ADUNCONST(df1b2vector,x);
00166    int mmin=v.indexmin();
00167    int mmax=v.indexmax();
00168    df1b2vector tmp;
00169    tmp.noallocate(mmin,mmax);
00170    for (int i=mmin;i<=mmax;i++) tmp(i)=pow(x(i),v(i));
00171    return tmp;
00172   }
00173 
00178 df1b2variable operator * (const df1b2vector& _x,const df1b2vector& _y)
00179 {
00180   ADUNCONST(df1b2vector,x)
00181   ADUNCONST(df1b2vector,y)
00182   df1b2variable tmp=0.0;
00183   int mmin=x.indexmin();
00184   int mmax=x.indexmax();
00185   for (int i=mmin;i<=mmax;i++)
00186   {
00187     *tmp.get_u() += *x(i).get_u() *  *y(i).get_u();
00188   }
00189   double * zd=tmp.get_u_dot();
00190   for (int j=0;j<df1b2variable::nvar;j++)
00191   {
00192     *zd++ = 0;
00193   }
00194   for (int i=mmin;i<=mmax;i++)
00195   {
00196     double * zd=tmp.get_u_dot();
00197     double * xd=x(i).get_u_dot();
00198     double * yd=y(i).get_u_dot();
00199     double xu= *x(i).get_u();
00200     double yu= *y(i).get_u();
00201 
00202     for (int j=0;j<df1b2variable::nvar;j++)
00203     {
00204       *zd++ += yu * *xd++ + xu * *yd++;
00205     }
00206   }
00207   // WRITE WHATEVER ON TAPE
00208   if (!df1b2_gradlist::no_derivatives)
00209     f1b2gradlist->write_pass1_prod(&x,&y,&tmp);
00210 
00211   return tmp;
00212 }
00213 
00218 void ad_read_pass2_prod_vector(void);
00219 
00220  int df1b2_gradlist::write_pass1_prod(const df1b2vector * _px,
00221    const df1b2vector * _py,df1b2variable * pz)
00222  {
00223    ADUNCONST(df1b2vector*,px)
00224    ADUNCONST(df1b2vector*,py)
00225    ncount++;
00226 #if defined(CHECK_COUNT)
00227   if (ncount >= ncount_check)
00228     cout << ncount << endl;
00229 #endif
00230    int nvar=df1b2variable::nvar;
00231 
00232    int mmin=px->indexmin();
00233    int mmax=px->indexmax();
00234 
00235    //int total_bytes=3*sizeof(df1b2_header)
00236     // +2*(nvar+1)*sizeof(double);
00237   size_t total_bytes= 2*sizeof(int) + 2*(mmax-mmin+1)*sizeof(df1b2_header)
00238     + sizeof(df1b2_header) + 2*(mmax-mmin+1)*sizeof(double)
00239     + 2*(mmax-mmin+1)*nvar*sizeof(double);
00240 // string identifier debug stuff
00241 #if defined(SAFE_ALL)
00242   char ids[]="DL";
00243   size_t slen=strlen(ids);
00244   total_bytes+=slen;
00245 #endif
00246 
00247 #ifndef OPT_LIB
00248   assert(total_bytes <= INT_MAX);
00249 #endif
00250 
00251   list.check_buffer_size((int)total_bytes);
00252 
00253   void * tmpptr=list.bptr;
00254 #if defined(SAFE_ALL)
00255   memcpy(list,ids,slen);
00256 #endif
00257 // end of string identifier debug stuff
00258 
00259    memcpy(list,&mmin,sizeof(int));
00260    memcpy(list,&mmax,sizeof(int));
00261    for (int i=mmin;i<=mmax;i++)
00262    {
00263      memcpy(list,(df1b2_header*)( &(*px)(i)  ),sizeof(df1b2_header));
00264      memcpy(list,(df1b2_header*)( &(*py)(i)  ),sizeof(df1b2_header));
00265    }
00266    memcpy(list,(df1b2_header*)(pz),sizeof(df1b2_header));
00267 
00268 
00269    const int sizeofdouble = sizeof(double);
00270    for (int i=mmin;i<=mmax;i++)
00271    {
00272      memcpy(list,(*px)(i).get_u(),sizeofdouble);
00273      memcpy(list,(*py)(i).get_u(),sizeofdouble);
00274    }
00275    for (int i=mmin;i<=mmax;i++)
00276    {
00277      memcpy(list,(*px)(i).get_u_dot(),nvar*sizeofdouble);
00278      memcpy(list,(*py)(i).get_u_dot(),nvar*sizeofdouble);
00279    }
00280    // ***** write  record size
00281    nlist.bptr->numbytes=adptr_diff(list.bptr,tmpptr);
00282    //cout << "XX " << nlist.bptr->numbytes << " " << total_bytes << endl;
00283    nlist.bptr->pf=(ADrfptr)(&ad_read_pass2_prod_vector);
00284       ++nlist;
00285    return 0;
00286  }
00287 
00288 
00289 
00290 void read_pass2_1_prod_vector(void);
00291 void read_pass2_2_prod_vector(void);
00292 void read_pass2_3_prod_vector(void);
00293 
00298 void ad_read_pass2_prod_vector(void)
00299 {
00300   switch(df1b2variable::passnumber)
00301   {
00302   case 1:
00303     read_pass2_1_prod_vector();
00304     break;
00305   case 2:
00306     read_pass2_2_prod_vector();
00307     break;
00308   case 3:
00309     read_pass2_3_prod_vector();
00310     break;
00311   default:
00312     cerr << "illegal value for df1b2variable::pass = "
00313          << df1b2variable::passnumber << endl;
00314     exit(1);
00315   }
00316 }
00317 
00322 void read_pass2_1_prod_vector(void)
00323 {
00324   int nvar=df1b2variable::nvar;
00325   test_smartlist& list=f1b2gradlist->list;
00326   int num_bytes=f1b2gradlist->nlist.bptr->numbytes;
00327   list-=num_bytes;
00328   list.saveposition(); // save pointer to beginning of record;
00329 
00330 #if defined(SAFE_ALL)
00331   checkidentiferstring("DL",f1b2gradlist->list);
00332 #endif
00333   char * bptr=f1b2gradlist->list.bptr;
00334   int& mmin = *(int *) bptr;
00335   bptr+=sizeof(int);
00336   int& mmax = *(int *) bptr;
00337   bptr+=sizeof(int);
00338   df1b2_header_ptr_vector px(mmin,mmax);
00339   df1b2_header_ptr_vector py(mmin,mmax);
00340   double_ptr_vector xdot(mmin,mmax);
00341   double_ptr_vector ydot(mmin,mmax);
00342   dvector xu(mmin,mmax);
00343   dvector yu(mmin,mmax);
00344   for (int i=mmin;i<=mmax;i++)
00345   {
00346     // df1b2_header *
00347     px(i)=(df1b2_header *) bptr;
00348     bptr+=sizeof(df1b2_header);
00349     // df1b2_header *
00350     py(i)=(df1b2_header *) bptr;
00351     bptr+=sizeof(df1b2_header);
00352   }
00353   df1b2_header * pz=(df1b2_header *) bptr;
00354   bptr+=sizeof(df1b2_header);
00355   for (int i=mmin;i<=mmax;i++)
00356   {
00357     memcpy(&(xu(i)),bptr,sizeof(double));
00358     bptr+=sizeof(double);
00359     memcpy(&(yu(i)),bptr,sizeof(double));
00360     bptr+=sizeof(double);
00361   }
00362   for (int i=mmin;i<=mmax;i++)
00363   {
00364     // double *
00365     xdot(i)=(double*)bptr;
00366     bptr+=nvar*sizeof(double);
00367     // double *
00368     ydot(i)=(double*)bptr;
00369     bptr+=nvar*sizeof(double);
00370   }
00371 
00372   list.restoreposition(); // save pointer to beginning of record;
00373 
00374   // ****************************************************************
00375   // turn this off if no third derivatives are calculated
00376   // if (!no_third_derivatives)
00377   // {
00378   // save for second reverse pass
00379   // save identifier 1
00380      test_smartlist & list2 = f1b2gradlist->list2;
00381 
00382   size_t total_bytes=2*nvar*sizeof(double);
00383 // string identifier debug stuff
00384 #if defined(SAFE_ALL)
00385   char ids[]="QK";
00386   size_t slen=strlen(ids);
00387   total_bytes+=slen;
00388 #endif
00389 
00390 #ifndef OPT_LIB
00391   assert(total_bytes <= INT_MAX);
00392 #endif
00393 
00394   list2.check_buffer_size((int)total_bytes);
00395 
00396   void * tmpptr=list2.bptr;
00397 #if defined(SAFE_ALL)
00398   memcpy(list2,ids,slen);
00399 #endif
00400 
00401   fixed_smartlist2 & nlist2 = f1b2gradlist->nlist2;
00402 
00403   const int sizeofdouble = sizeof(double);
00404   memcpy(list2,pz->get_u_bar(),nvar*sizeofdouble);
00405   memcpy(list2,pz->get_u_dot_bar(),nvar*sizeofdouble);
00406   *nlist2.bptr=adptr_diff(list2.bptr,tmpptr);
00407   ++nlist2;
00408 
00409   // Do first reverse pass calculations
00410   for (int i=mmin;i<=mmax;i++)
00411   {
00412     for (int j=0;j<nvar;j++)
00413     {
00414       px(i)->u_bar[j]+=yu(i)*pz->u_bar[j];
00415     }
00416     for (int j=0;j<nvar;j++)
00417     {
00418       py(i)->u_bar[j]+=xu(i)*pz->u_bar[j];
00419     }
00420   }
00421 
00422   for (int i=mmin;i<=mmax;i++)
00423   {
00424     for (int j=0;j<nvar;j++)
00425     {
00426       px(i)->u_bar[j]+=ydot(i)[j]*pz->u_dot_bar[j];
00427     }
00428 
00429     for (int j=0;j<nvar;j++)
00430     {
00431       py(i)->u_bar[j]+=xdot(i)[j]*pz->u_dot_bar[j];
00432     }
00433     for (int j=0;j<nvar;j++)
00434     {
00435       px(i)->u_dot_bar[j]+=yu(i)*pz->u_dot_bar[j];
00436     }
00437     for (int j=0;j<nvar;j++)
00438     {
00439       py(i)->u_dot_bar[j]+=xu(i)*pz->u_dot_bar[j];
00440     }
00441   }
00442 
00443   // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00444   for (int j=0;j<nvar;j++)
00445   {
00446     pz->u_bar[j]=0;
00447   }
00448   for (int j=0;j<nvar;j++)
00449   {
00450     pz->u_dot_bar[j]=0;
00451   }
00452 }
00453 
00458 void read_pass2_2_prod_vector(void)
00459 {
00460   //const int nlist_record_size=sizeof(int)+sizeof(char*);
00461   // We are going forward for bptr and backword for bptr2
00462   //
00463   // list 1
00464   //
00465   int nvar=df1b2variable::nvar;
00466   test_smartlist & list=f1b2gradlist->list;
00467 
00468   //int total_bytes=3*sizeof(df1b2_header)+sizeof(char*)
00469   //  +2*(nvar+1)*sizeof(double);
00470   //int total_bytes=3*sizeof(df1b2_header)
00471   //  +2*(nvar+1)*sizeof(double);
00472 // string identifier debug stuff
00473 #if defined(SAFE_ALL)
00474   //char ids[]="DL";
00475   //int slen=strlen(ids);
00476   //total_bytes+=slen;
00477 #endif
00478 // end of string identifier debug stuff
00479 
00480   fixed_smartlist & nlist=f1b2gradlist->nlist;
00481   int total_bytes=nlist.bptr->numbytes;
00482   list.check_buffer_size(total_bytes);
00483   list.saveposition(); // save pointer to beginning of record;
00484   //cout << "YY " << total_bytes <<  endl;
00485   //
00486   // list 2
00487   //
00488   test_smartlist & list2=f1b2gradlist->list2;
00489   fixed_smartlist2 & nlist2=f1b2gradlist->nlist2;
00490   // get record size
00491   int num_bytes2=*nlist2.bptr;
00492   --nlist2;
00493   // backup the size of the record
00494   list2-=num_bytes2;
00495   list2.saveposition(); // save pointer to beginning of record;
00496   // save the pointer to the beginning of the record
00497   // bptr and bptr2 now both point to the beginning of their records
00498 
00499   //df1b2_header x,z;
00500   //df1b2function2 * pf;
00501 
00502   // get info from tape1
00503   // get info from tape1
00504 #if defined(SAFE_ALL)
00505   checkidentiferstring("DL",list);
00506   checkidentiferstring("QK",list2);
00507 #endif
00508   int mmin = *(int *) list.bptr;
00509   list.bptr+=sizeof(int);
00510   int mmax = *(int *) list.bptr;
00511   list.bptr+=sizeof(int);
00512 
00513   df1b2_header_ptr_vector px(mmin,mmax);
00514   df1b2_header_ptr_vector py(mmin,mmax);
00515   double_ptr_vector xdot(mmin,mmax);
00516   double_ptr_vector ydot(mmin,mmax);
00517   dvector xu(mmin,mmax);
00518   dvector yu(mmin,mmax);
00519   int i;
00520   int j;
00521 
00522   for (i=mmin;i<=mmax;i++)
00523   {
00524     // df1b2_header * //
00525     px(i)=(df1b2_header *) list.bptr;
00526     list.bptr+=sizeof(df1b2_header);
00527     // df1b2_header *
00528     py(i)=(df1b2_header *) list.bptr;
00529     list.bptr+=sizeof(df1b2_header);
00530   }
00531 
00532   df1b2_header * pz=(df1b2_header *) list.bptr;
00533   list.bptr+=sizeof(df1b2_header);
00534 
00535   for (i=mmin;i<=mmax;i++)
00536   {
00537     memcpy(&(xu(i)),list.bptr,sizeof(double));
00538     list.bptr+=sizeof(double);
00539     memcpy(&(yu(i)),list.bptr,sizeof(double));
00540     list.bptr+=sizeof(double);
00541   }
00542   for (i=mmin;i<=mmax;i++)
00543   {
00544     xdot(i)=(double*)list.bptr;
00545     list.bptr+=nvar*sizeof(double);
00546     ydot(i)=(double*)list.bptr;
00547     list.bptr+=nvar*sizeof(double);
00548   }
00549   list.restoreposition(total_bytes); // save pointer to beginning of record;
00550 
00551   double * zbar;
00552   double * zdotbar;
00553 
00554 
00555   zbar=(double*)list2.bptr;
00556   zdotbar=(double*)(list2.bptr+nvar*sizeof(double));
00557   list2.restoreposition(); // save pointer to beginning of record;
00558 
00559   double * z_bar_tilde=pz->get_u_bar_tilde();
00560   double * z_dot_bar_tilde=pz->get_u_dot_bar_tilde();
00561 
00562   for (j=0;j<nvar;j++)
00563   {
00564     z_bar_tilde[j]=0;
00565     z_dot_bar_tilde[j]=0;
00566   }
00567 
00568   for (i=mmin;i<=mmax;i++)
00569   {
00570     double * x_tilde=px(i)->get_u_tilde();
00571     double * x_dot_tilde=px(i)->get_u_dot_tilde();
00572     double * x_bar_tilde=px(i)->get_u_bar_tilde();
00573     double * x_dot_bar_tilde=px(i)->get_u_dot_bar_tilde();
00574     double * y_tilde=py(i)->get_u_tilde();
00575     double * y_dot_tilde=py(i)->get_u_dot_tilde();
00576     double * y_bar_tilde=py(i)->get_u_bar_tilde();
00577     double * y_dot_bar_tilde=py(i)->get_u_dot_bar_tilde();
00578 
00579     int j;
00580 
00581     // start wjth x and add y
00582     for (j=0;j<nvar;j++)
00583     {
00584       z_bar_tilde[j]+=yu(i)*x_bar_tilde[j];
00585       *y_tilde+=zbar[j]*x_bar_tilde[j];
00586     }
00587 
00588     for (j=0;j<nvar;j++)
00589     {
00590       *y_tilde+=zdotbar[j]*x_dot_bar_tilde[j];
00591       z_dot_bar_tilde[j]+=yu(i)*x_dot_bar_tilde[j];
00592     }
00593 
00594     // start wjth y and add x
00595     for (j=0;j<nvar;j++)
00596     {
00597       *x_tilde+=zbar[j]*y_bar_tilde[j];
00598       z_bar_tilde[j]+=xu(i)*y_bar_tilde[j];
00599     }
00600 
00601     for (j=0;j<nvar;j++)
00602     {
00603       *x_tilde+=zdotbar[j]*y_dot_bar_tilde[j];
00604       z_dot_bar_tilde[j]+=xu(i)*y_dot_bar_tilde[j];
00605     }
00606 
00607     for (j=0;j<nvar;j++)
00608     {
00609       y_dot_tilde[j]+=zdotbar[j]*x_bar_tilde[j];
00610       z_dot_bar_tilde[j]+=ydot(i)[j]*x_bar_tilde[j];
00611     }
00612     for (j=0;j<nvar;j++)
00613     {
00614       x_dot_tilde[j]+=zdotbar[j]*y_bar_tilde[j];
00615       z_dot_bar_tilde[j]+=xdot(i)[j]*y_bar_tilde[j];
00616     }
00617   }
00618 }
00619 
00624 void read_pass2_3_prod_vector(void)
00625 {
00626   // We are going backword for bptr and forward for bptr2
00627   // the current entry+2 in bptr is the size of the record i.e
00628   // points to the next record
00629   int nvar=df1b2variable::nvar;
00630   fixed_smartlist & nlist=f1b2gradlist->nlist;
00631   test_smartlist& list=f1b2gradlist->list;
00632    // nlist-=sizeof(int);
00633   // get record size
00634   int num_bytes=nlist.bptr->numbytes;
00635   // backup the size of the record
00636   list-=num_bytes;
00637   list.saveposition(); // save pointer to beginning of record;
00638   // save the pointer to the beginning of the record
00639 
00640   //df1b2_header x,z;
00641   //df1b2function2 * pf;
00642 
00643   // get info from tape1
00644   // get info from tape1
00645 #if defined(SAFE_ALL)
00646   checkidentiferstring("DL",list);
00647 #endif
00648   int& mmin = *(int *) list.bptr;
00649   list.bptr+=sizeof(int);
00650   int& mmax = *(int *) list.bptr;
00651   list.bptr+=sizeof(int);
00652   df1b2_header_ptr_vector px(mmin,mmax);
00653   df1b2_header_ptr_vector py(mmin,mmax);
00654   double_ptr_vector xdot(mmin,mmax);
00655   double_ptr_vector ydot(mmin,mmax);
00656   dvector xu(mmin,mmax);
00657   dvector yu(mmin,mmax);
00658   int i;
00659   int j;
00660   for (i=mmin;i<=mmax;i++)
00661   {
00662     // df1b2_header *
00663     px(i)=(df1b2_header *) list.bptr;
00664     list.bptr+=sizeof(df1b2_header);
00665     // df1b2_header *
00666     py(i)=(df1b2_header *) list.bptr;
00667     list.bptr+=sizeof(df1b2_header);
00668   }
00669   df1b2_header * pz=(df1b2_header *) list.bptr;
00670   list.bptr+=sizeof(df1b2_header);
00671   for (i=mmin;i<=mmax;i++)
00672   {
00673     memcpy(&(xu(i)),list.bptr,sizeof(double));
00674     list.bptr+=sizeof(double);
00675     memcpy(&(yu(i)),list.bptr,sizeof(double));
00676     list.bptr+=sizeof(double);
00677   }
00678   for (i=mmin;i<=mmax;i++)
00679   {
00680     // double *
00681     xdot(i)=(double*)list.bptr;
00682     list.bptr+=nvar*sizeof(double);
00683     // double *
00684     ydot(i)=(double*)list.bptr;
00685     list.bptr+=nvar*sizeof(double);
00686   }
00687 
00688   list.restoreposition(); // save pointer to beginning of record;
00689 
00690   for (i=mmin;i<=mmax;i++)
00691   {
00692     *(px(i)->u_tilde)+=yu(i) * *(pz->u_tilde);
00693     *(py(i)->u_tilde)+=xu(i) * *(pz->u_tilde);
00694     for (j=0;j<nvar;j++)
00695     {
00696       *(py(i)->u_tilde)+=xdot(i)[j]*pz->u_dot_tilde[j];
00697       *(px(i)->u_tilde)+=ydot(i)[j]*pz->u_dot_tilde[j];
00698     }
00699     for (j=0;j<nvar;j++)
00700     {
00701       px(i)->u_dot_tilde[j]+=yu(i)*pz->u_dot_tilde[j];
00702       py(i)->u_dot_tilde[j]+=xu(i)*pz->u_dot_tilde[j];
00703     }
00704   }
00705   *(pz->u_tilde)=0;
00706   for (j=0;j<nvar;j++)
00707   {
00708     pz->u_dot_tilde[j]=0;
00709   }
00710 }