ADMB Documentation  11.1x.2738
 All Classes Files Functions Variables Typedefs Friends Defines
f1b2v10.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: f1b2v10.cpp 2669 2014-11-17 19:39:58Z 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   {
00190     double* zd=tmp.get_u_dot();
00191     for (unsigned int j=0;j<df1b2variable::nvar;j++)
00192     {
00193       *zd++ = 0;
00194     }
00195   }
00196   for (int i=mmin;i<=mmax;i++)
00197   {
00198     double * zd=tmp.get_u_dot();
00199     double * xd=x(i).get_u_dot();
00200     double * yd=y(i).get_u_dot();
00201     double xu= *x(i).get_u();
00202     double yu= *y(i).get_u();
00203 
00204     for (unsigned int j=0;j<df1b2variable::nvar;j++)
00205     {
00206       *zd++ += yu * *xd++ + xu * *yd++;
00207     }
00208   }
00209   // WRITE WHATEVER ON TAPE
00210   if (!df1b2_gradlist::no_derivatives)
00211     f1b2gradlist->write_pass1_prod(&x,&y,&tmp);
00212 
00213   return tmp;
00214 }
00215 
00220 void ad_read_pass2_prod_vector(void);
00221 
00222  int df1b2_gradlist::write_pass1_prod(const df1b2vector * _px,
00223    const df1b2vector * _py,df1b2variable * pz)
00224  {
00225    ADUNCONST(df1b2vector*,px)
00226    ADUNCONST(df1b2vector*,py)
00227    ncount++;
00228 #if defined(CHECK_COUNT)
00229   if (ncount >= ncount_check)
00230     cout << ncount << endl;
00231 #endif
00232   unsigned int nvar=df1b2variable::nvar;
00233 
00234   int mmin=px->indexmin();
00235   int mmax=px->indexmax();
00236 #ifndef OPT_LIB
00237   assert(mmax >= mmin);
00238 #endif
00239   size_t size = (size_t)(mmax - mmin + 1);
00240   const size_t sizeofdouble = sizeof(double);
00241 
00242   //int total_bytes=3*sizeof(df1b2_header)+2*(nvar+1)*sizeof(double);
00243   size_t total_bytes= 2*sizeof(int) + 2 * size * sizeof(df1b2_header)
00244     + sizeof(df1b2_header) + 2 * size * sizeofdouble
00245     + 2 * size * nvar * sizeofdouble;
00246 // string identifier debug stuff
00247 #if defined(SAFE_ALL)
00248   char ids[]="DL";
00249   size_t slen=strlen(ids);
00250   total_bytes+=slen;
00251 #endif
00252 
00253   list.check_buffer_size(total_bytes);
00254 
00255   void * tmpptr=list.bptr;
00256 #if defined(SAFE_ALL)
00257   memcpy(list,ids,slen);
00258 #endif
00259 // end of string identifier debug stuff
00260 
00261    memcpy(list,&mmin,sizeof(int));
00262    memcpy(list,&mmax,sizeof(int));
00263    for (int i=mmin;i<=mmax;i++)
00264    {
00265      memcpy(list,(df1b2_header*)( &(*px)(i)  ),sizeof(df1b2_header));
00266      memcpy(list,(df1b2_header*)( &(*py)(i)  ),sizeof(df1b2_header));
00267    }
00268    memcpy(list,(df1b2_header*)(pz),sizeof(df1b2_header));
00269 
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   unsigned 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   list2.check_buffer_size(total_bytes);
00391 
00392   void * tmpptr=list2.bptr;
00393 #if defined(SAFE_ALL)
00394   memcpy(list2,ids,slen);
00395 #endif
00396 
00397   fixed_smartlist2 & nlist2 = f1b2gradlist->nlist2;
00398 
00399   const int sizeofdouble = sizeof(double);
00400   memcpy(list2,pz->get_u_bar(),nvar*sizeofdouble);
00401   memcpy(list2,pz->get_u_dot_bar(),nvar*sizeofdouble);
00402   *nlist2.bptr=adptr_diff(list2.bptr,tmpptr);
00403   ++nlist2;
00404 
00405   // Do first reverse pass calculations
00406   for (int i=mmin;i<=mmax;i++)
00407   {
00408     for (size_t j=0;j<nvar;j++)
00409     {
00410       px(i)->u_bar[j]+=yu(i)*pz->u_bar[j];
00411     }
00412     for (size_t j=0;j<nvar;j++)
00413     {
00414       py(i)->u_bar[j]+=xu(i)*pz->u_bar[j];
00415     }
00416   }
00417 
00418   for (int i=mmin;i<=mmax;i++)
00419   {
00420     for (size_t j=0;j<nvar;j++)
00421     {
00422       px(i)->u_bar[j]+=ydot(i)[j]*pz->u_dot_bar[j];
00423     }
00424 
00425     for (size_t j=0;j<nvar;j++)
00426     {
00427       py(i)->u_bar[j]+=xdot(i)[j]*pz->u_dot_bar[j];
00428     }
00429     for (size_t j=0;j<nvar;j++)
00430     {
00431       px(i)->u_dot_bar[j]+=yu(i)*pz->u_dot_bar[j];
00432     }
00433     for (size_t j=0;j<nvar;j++)
00434     {
00435       py(i)->u_dot_bar[j]+=xu(i)*pz->u_dot_bar[j];
00436     }
00437   }
00438 
00439   // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00440   for (size_t j=0;j<nvar;j++)
00441   {
00442     pz->u_bar[j]=0;
00443   }
00444   for (size_t j=0;j<nvar;j++)
00445   {
00446     pz->u_dot_bar[j]=0;
00447   }
00448 }
00449 
00454 void read_pass2_2_prod_vector(void)
00455 {
00456   //const int nlist_record_size=sizeof(int)+sizeof(char*);
00457   // We are going forward for bptr and backword for bptr2
00458   //
00459   // list 1
00460   //
00461   unsigned int nvar=df1b2variable::nvar;
00462   test_smartlist & list=f1b2gradlist->list;
00463 
00464   //int total_bytes=3*sizeof(df1b2_header)+sizeof(char*)
00465   //  +2*(nvar+1)*sizeof(double);
00466   //int total_bytes=3*sizeof(df1b2_header)
00467   //  +2*(nvar+1)*sizeof(double);
00468 // string identifier debug stuff
00469 #if defined(SAFE_ALL)
00470   //char ids[]="DL";
00471   //int slen=strlen(ids);
00472   //total_bytes+=slen;
00473 #endif
00474 // end of string identifier debug stuff
00475 
00476   fixed_smartlist & nlist=f1b2gradlist->nlist;
00477   int total_bytes = nlist.bptr->numbytes;
00478   if (total_bytes > 0)
00479   {
00480     list.check_buffer_size((size_t)total_bytes);
00481   }
00482   list.saveposition(); // save pointer to beginning of record;
00483   //cout << "YY " << total_bytes <<  endl;
00484   //
00485   // list 2
00486   //
00487   test_smartlist & list2=f1b2gradlist->list2;
00488   fixed_smartlist2 & nlist2=f1b2gradlist->nlist2;
00489   // get record size
00490   int num_bytes2=*nlist2.bptr;
00491   --nlist2;
00492   // backup the size of the record
00493   list2-=num_bytes2;
00494   list2.saveposition(); // save pointer to beginning of record;
00495   // save the pointer to the beginning of the record
00496   // bptr and bptr2 now both point to the beginning of their records
00497 
00498   //df1b2_header x,z;
00499   //df1b2function2 * pf;
00500 
00501   // get info from tape1
00502   // get info from tape1
00503 #if defined(SAFE_ALL)
00504   checkidentiferstring("DL",list);
00505   checkidentiferstring("QK",list2);
00506 #endif
00507   int mmin = *(int *) list.bptr;
00508   list.bptr+=sizeof(int);
00509   int mmax = *(int *) list.bptr;
00510   list.bptr+=sizeof(int);
00511 
00512   df1b2_header_ptr_vector px(mmin,mmax);
00513   df1b2_header_ptr_vector py(mmin,mmax);
00514   double_ptr_vector xdot(mmin,mmax);
00515   double_ptr_vector ydot(mmin,mmax);
00516   dvector xu(mmin,mmax);
00517   dvector yu(mmin,mmax);
00518   int i;
00519 
00520   for (i=mmin;i<=mmax;i++)
00521   {
00522     // df1b2_header * //
00523     px(i)=(df1b2_header *) list.bptr;
00524     list.bptr+=sizeof(df1b2_header);
00525     // df1b2_header *
00526     py(i)=(df1b2_header *) list.bptr;
00527     list.bptr+=sizeof(df1b2_header);
00528   }
00529 
00530   df1b2_header * pz=(df1b2_header *) list.bptr;
00531   list.bptr+=sizeof(df1b2_header);
00532 
00533   for (i=mmin;i<=mmax;i++)
00534   {
00535     memcpy(&(xu(i)),list.bptr,sizeof(double));
00536     list.bptr+=sizeof(double);
00537     memcpy(&(yu(i)),list.bptr,sizeof(double));
00538     list.bptr+=sizeof(double);
00539   }
00540   for (i=mmin;i<=mmax;i++)
00541   {
00542     xdot(i)=(double*)list.bptr;
00543     list.bptr+=nvar*sizeof(double);
00544     ydot(i)=(double*)list.bptr;
00545     list.bptr+=nvar*sizeof(double);
00546   }
00547   list.restoreposition(total_bytes); // save pointer to beginning of record;
00548 
00549   double * zbar;
00550   double * zdotbar;
00551 
00552 
00553   zbar=(double*)list2.bptr;
00554   zdotbar=(double*)(list2.bptr+nvar*sizeof(double));
00555   list2.restoreposition(); // save pointer to beginning of record;
00556 
00557   double * z_bar_tilde=pz->get_u_bar_tilde();
00558   double * z_dot_bar_tilde=pz->get_u_dot_bar_tilde();
00559 
00560   for (size_t j=0;j<nvar;j++)
00561   {
00562     z_bar_tilde[j]=0;
00563     z_dot_bar_tilde[j]=0;
00564   }
00565 
00566   for (i=mmin;i<=mmax;i++)
00567   {
00568     double * x_tilde=px(i)->get_u_tilde();
00569     double * x_dot_tilde=px(i)->get_u_dot_tilde();
00570     double * x_bar_tilde=px(i)->get_u_bar_tilde();
00571     double * x_dot_bar_tilde=px(i)->get_u_dot_bar_tilde();
00572     double * y_tilde=py(i)->get_u_tilde();
00573     double * y_dot_tilde=py(i)->get_u_dot_tilde();
00574     double * y_bar_tilde=py(i)->get_u_bar_tilde();
00575     double * y_dot_bar_tilde=py(i)->get_u_dot_bar_tilde();
00576 
00577     // start wjth x and add y
00578     for (size_t j=0;j<nvar;j++)
00579     {
00580       z_bar_tilde[j]+=yu(i)*x_bar_tilde[j];
00581       *y_tilde+=zbar[j]*x_bar_tilde[j];
00582     }
00583 
00584     for (size_t j=0;j<nvar;j++)
00585     {
00586       *y_tilde+=zdotbar[j]*x_dot_bar_tilde[j];
00587       z_dot_bar_tilde[j]+=yu(i)*x_dot_bar_tilde[j];
00588     }
00589 
00590     // start wjth y and add x
00591     for (size_t j=0;j<nvar;j++)
00592     {
00593       *x_tilde+=zbar[j]*y_bar_tilde[j];
00594       z_bar_tilde[j]+=xu(i)*y_bar_tilde[j];
00595     }
00596 
00597     for (size_t j=0;j<nvar;j++)
00598     {
00599       *x_tilde+=zdotbar[j]*y_dot_bar_tilde[j];
00600       z_dot_bar_tilde[j]+=xu(i)*y_dot_bar_tilde[j];
00601     }
00602 
00603     for (size_t j=0;j<nvar;j++)
00604     {
00605       y_dot_tilde[j]+=zdotbar[j]*x_bar_tilde[j];
00606       z_dot_bar_tilde[j]+=ydot(i)[j]*x_bar_tilde[j];
00607     }
00608     for (size_t j=0;j<nvar;j++)
00609     {
00610       x_dot_tilde[j]+=zdotbar[j]*y_bar_tilde[j];
00611       z_dot_bar_tilde[j]+=xdot(i)[j]*y_bar_tilde[j];
00612     }
00613   }
00614 }
00615 
00620 void read_pass2_3_prod_vector(void)
00621 {
00622   // We are going backword for bptr and forward for bptr2
00623   // the current entry+2 in bptr is the size of the record i.e
00624   // points to the next record
00625   unsigned int nvar=df1b2variable::nvar;
00626   fixed_smartlist & nlist=f1b2gradlist->nlist;
00627   test_smartlist& list=f1b2gradlist->list;
00628    // nlist-=sizeof(int);
00629   // get record size
00630   int num_bytes=nlist.bptr->numbytes;
00631   // backup the size of the record
00632   list-=num_bytes;
00633   list.saveposition(); // save pointer to beginning of record;
00634   // save the pointer to the beginning of the record
00635 
00636   //df1b2_header x,z;
00637   //df1b2function2 * pf;
00638 
00639   // get info from tape1
00640   // get info from tape1
00641 #if defined(SAFE_ALL)
00642   checkidentiferstring("DL",list);
00643 #endif
00644   int& mmin = *(int *) list.bptr;
00645   list.bptr+=sizeof(int);
00646   int& mmax = *(int *) list.bptr;
00647   list.bptr+=sizeof(int);
00648   df1b2_header_ptr_vector px(mmin,mmax);
00649   df1b2_header_ptr_vector py(mmin,mmax);
00650   double_ptr_vector xdot(mmin,mmax);
00651   double_ptr_vector ydot(mmin,mmax);
00652   dvector xu(mmin,mmax);
00653   dvector yu(mmin,mmax);
00654   int i;
00655   for (i=mmin;i<=mmax;i++)
00656   {
00657     // df1b2_header *
00658     px(i)=(df1b2_header *) list.bptr;
00659     list.bptr+=sizeof(df1b2_header);
00660     // df1b2_header *
00661     py(i)=(df1b2_header *) list.bptr;
00662     list.bptr+=sizeof(df1b2_header);
00663   }
00664   df1b2_header * pz=(df1b2_header *) list.bptr;
00665   list.bptr+=sizeof(df1b2_header);
00666   for (i=mmin;i<=mmax;i++)
00667   {
00668     memcpy(&(xu(i)),list.bptr,sizeof(double));
00669     list.bptr+=sizeof(double);
00670     memcpy(&(yu(i)),list.bptr,sizeof(double));
00671     list.bptr+=sizeof(double);
00672   }
00673   for (i=mmin;i<=mmax;i++)
00674   {
00675     // double *
00676     xdot(i)=(double*)list.bptr;
00677     list.bptr+=nvar*sizeof(double);
00678     // double *
00679     ydot(i)=(double*)list.bptr;
00680     list.bptr+=nvar*sizeof(double);
00681   }
00682 
00683   list.restoreposition(); // save pointer to beginning of record;
00684 
00685   for (i=mmin;i<=mmax;i++)
00686   {
00687     *(px(i)->u_tilde)+=yu(i) * *(pz->u_tilde);
00688     *(py(i)->u_tilde)+=xu(i) * *(pz->u_tilde);
00689     for (size_t j=0;j<nvar;j++)
00690     {
00691       *(py(i)->u_tilde)+=xdot(i)[j]*pz->u_dot_tilde[j];
00692       *(px(i)->u_tilde)+=ydot(i)[j]*pz->u_dot_tilde[j];
00693     }
00694     for (size_t j=0;j<nvar;j++)
00695     {
00696       px(i)->u_dot_tilde[j]+=yu(i)*pz->u_dot_tilde[j];
00697       py(i)->u_dot_tilde[j]+=xu(i)*pz->u_dot_tilde[j];
00698     }
00699   }
00700   *(pz->u_tilde)=0;
00701   for (size_t j=0;j<nvar;j++)
00702   {
00703     pz->u_dot_tilde[j]=0;
00704   }
00705 }