Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

PostureEngine.cc

Go to the documentation of this file.
00001 #include "PostureEngine.h"
00002 #include "Shared/WorldState.h"
00003 #include <stdio.h>
00004 
00005 PostureEngine::~PostureEngine() {}
00006 
00007 void PostureEngine::takeSnapshot() {
00008   for(unsigned int i=0; i<NumOutputs; i++)
00009     cmds[i].set(state->outputs[i],1);
00010 }
00011 
00012 void PostureEngine::clear() {
00013   for(unsigned int i=0; i<NumOutputs; i++)
00014     cmds[i].unset();
00015 }
00016 
00017 PostureEngine& PostureEngine::setOverlay(const PostureEngine& pe) {
00018   for(unsigned int i=0; i<NumOutputs; i++)
00019     if(pe.cmds[i].weight>0)
00020       cmds[i]=pe.cmds[i];
00021   return *this;
00022 }
00023 PostureEngine PostureEngine::createOverlay(const PostureEngine& pe) const {
00024   PostureEngine tmp(*this);
00025   return tmp.setOverlay(pe);
00026 }
00027 PostureEngine& PostureEngine::setUnderlay(const PostureEngine& pe) {
00028   for(unsigned int i=0; i<NumOutputs; i++)
00029     if(cmds[i].weight<=0)
00030       cmds[i]=pe.cmds[i];
00031   return *this;
00032 }
00033 PostureEngine PostureEngine::createUnderlay(const PostureEngine& pe) const {
00034   PostureEngine tmp(*this);
00035   return tmp.setUnderlay(pe);
00036 }
00037 /*! joints being averaged with ::unusedJoint have their weights averaged, but not their values (so an output can crossfade properly)\n
00038  *  @param pe the other PostureEngine
00039  *  @param w amount to weight towards @a pe
00040  *  - if @a w < .001, nothing is done
00041  *  - if @a w > .999, a straight copy of @a pe occurs (sets joints to unused properly at end of fade)
00042  *  - .001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if
00043  *    using repeated additions of a delta value instead of repeated divisions)
00044  *  @return @c *this, stores results into this */
00045 PostureEngine& PostureEngine::setAverage(const PostureEngine& pe, float w) {
00046   if(w<0.001)
00047     return *this;
00048   if(w>0.999)
00049     return (*this=pe);
00050   float wp=1-w;
00051   for(unsigned int i=0; i<NumOutputs; i++)
00052     if(cmds[i].weight>0) {
00053       if(pe.cmds[i].weight>0)
00054         cmds[i].set(cmds[i].value*wp+pe.cmds[i].value*w,cmds[i].weight*wp+pe.cmds[i].weight*w);
00055       else
00056         cmds[i].weight*=wp;
00057     } else
00058       cmds[i].set(pe.cmds[i].value,pe.cmds[i].weight*w);
00059   return *this;
00060 }
00061 /*! joints being averaged with weight<=0 have their weights averaged, but not their values (so an output can crossfade properly)\n
00062  *  @param pe the other PostureEngine
00063  *  @param w amount to weight towards @a pe
00064  *  - if @a w < .001, nothing is done
00065  *  - if @a w > .999, a straight copy of @a pe occurs (sets joints to unused properly at end of fade)
00066  *  - .001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if
00067  *    using repeated additions of a delta value instead of repeated divisions)
00068  *  @return a new posture containing the results */
00069 PostureEngine PostureEngine::createAverage(const PostureEngine& pe, float w) const {
00070   PostureEngine tmp(*this);
00071   return tmp.setAverage(pe,w);
00072 }
00073 PostureEngine& PostureEngine::setCombine(const PostureEngine& pe) {
00074   for(unsigned int i=0; i<NumOutputs; i++) {
00075     float total=cmds[i].weight+pe.cmds[i].weight;
00076     cmds[i].set((cmds[i].value*cmds[i].weight+pe.cmds[i].value*pe.cmds[i].weight)/total,total);
00077   }
00078   return *this;
00079 }
00080 PostureEngine PostureEngine::createCombine(const PostureEngine& pe) const {
00081   PostureEngine tmp(*this);
00082   return tmp.setCombine(pe);
00083 }
00084 
00085 float PostureEngine::diff(const PostureEngine& pe) const {
00086   float ans=0;
00087   for(unsigned int i=0; i<NumOutputs; i++)
00088     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00089       float dif=cmds[i].value-pe.cmds[i].value;
00090       ans+=dif*dif;
00091     }
00092   return ans;
00093 }
00094 
00095 float PostureEngine::avgdiff(const PostureEngine& pe) const {
00096   float ans=0;
00097   unsigned int cnt=0;
00098   for(unsigned int i=0; i<NumOutputs; i++)
00099     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00100       float dif=cmds[i].value-pe.cmds[i].value;
00101       ans+=dif*dif;
00102       cnt++;
00103     }
00104   return ans/cnt;
00105 }
00106 
00107 float PostureEngine::maxdiff(const PostureEngine& pe) const {
00108   float max=0;
00109   for(unsigned int i=0; i<NumOutputs; i++)
00110     if(cmds[i].weight>0 && pe.cmds[i].weight>0) {
00111       float dif=cmds[i].value-pe.cmds[i].value;
00112       dif*=dif;
00113       if(dif>max)
00114         max=dif;
00115     }
00116   return max;
00117 }
00118 
00119 unsigned int PostureEngine::getBinSize() const {
00120   unsigned int len=11;
00121   char buf[255];
00122   for(unsigned int i=0; i<NumOutputs; i++)
00123     if(cmds[i].weight>0)
00124       len+=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight);
00125   return len;
00126 }
00127 
00128 unsigned int PostureEngine::LoadBuffer(const char buf[], unsigned int len) {
00129   unsigned int origlen=len;
00130   clear();
00131   if(strncmp("#POS\n",buf,5)!=0) {
00132     //    std::cout << "ERROR PostureEngine load corrupted - expected #POS header" << std::endl;
00133     return 0;
00134   }
00135   char formatstring[64];
00136   snprintf(formatstring,64,"%%%dc %%f %%f\n%%n",outputNameLen); //std::cout << "Format: " << formatstring << std::endl;
00137   unsigned int idx=0;
00138   unsigned int linenum=2;
00139   char jname[outputNameLen+1];
00140   jname[outputNameLen]='\0';
00141   while(len<=origlen && len>0) {
00142     float fval, fwht;
00143     int written;
00144     //    printf("%d %.9s\n",linenum+1,buf);
00145     if(buf[0]=='#' || buf[0]=='\n') {
00146       if(strncmp("#END\n",buf,5)==0)
00147         return origlen-len;
00148       else {
00149         while(*buf++!='\n') {}
00150         continue;
00151       }
00152     }
00153     written=-1;
00154     sscanf(buf,formatstring,jname,&fval,&fwht,&written);
00155     if(!ChkAdvance(written,&buf,&len,"*** ERROR PostureEngine load corrupted - line %d\n",linenum)) return 0;
00156     linenum++;
00157     //std::cout << '"' << jname << "\"\t" << (float)fval << '\t' << (float)fwht << std::endl;
00158     unsigned int startidx=idx+1;
00159     for(;idx<NumOutputs;idx++)
00160       if(strcmp(jname,outputNames[idx])==0) {
00161         cmds[idx].set(fval,fwht);
00162         break;
00163       }
00164     if(idx==NumOutputs) {
00165       for(idx=0;idx<startidx;idx++)
00166         if(strcmp(jname,outputNames[idx])==0) {
00167           cmds[idx].set(fval,fwht);
00168           break;
00169       }
00170       if(idx==startidx && strcmp(jname,outputNames[idx])!=0)
00171         std::cout << "*** WARNING " << jname << " is not a valid joint on this model." << endl;
00172     }
00173   }
00174   std::cout << "*** WARNING PostureEngine load missing #END" << std::endl;
00175   return origlen-len;
00176 }
00177 
00178 //Why do i need a cast to const char**??? It doesn't work without, should be automatic... grrrr
00179 unsigned int PostureEngine::SaveBuffer(char buf[], unsigned int len) const {
00180   unsigned int origlen=len;
00181   int written=snprintf(buf,len,"#POS\n");
00182   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed on header\n")) return 0;
00183   if(len==0 || len>origlen) {
00184     std::cout << "*** ERROR PostureEngine save overflow on header" << std::endl;
00185     return 0;
00186   }
00187   for(unsigned int i=0; i<NumOutputs; i++)
00188     if(cmds[i].weight>0) {
00189       written=snprintf(buf,len,"%s\t% .4f\t% .4f\n",outputNames[i],cmds[i].value,cmds[i].weight);
00190       if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed\n")) return 0;
00191       if(len==0 || len>origlen) {
00192         std::cout << "*** ERROR PostureEngine save overflow" << std::endl;
00193         return 0;
00194       }
00195     }
00196   written=snprintf(buf,len,"#END\n");
00197   if(!ChkAdvance(written,(const char**)&buf,&len,"*** ERROR PostureEngine save failed on #END\n")) return 0;
00198   if(len==0 || len>origlen) {
00199     std::cout << "*** ERROR PostureEngine save overflow on #END" << std::endl;
00200     return 0;
00201   }
00202   return origlen-len;
00203 }
00204 
00205 bool PostureEngine::ChkAdvance(int res, const char** buf, unsigned int* len, const char* msg) {
00206   if(res>0) {
00207     *buf+=res;
00208     *len-=res;
00209     return true;
00210   } else {
00211     printf("%s",msg);
00212     return false;
00213   }
00214 }
00215 
00216 bool PostureEngine::ChkAdvance(int res, const char** buf, unsigned int* len, const char* msg, int arg1) {
00217   if(res>0) {
00218     *buf+=res;
00219     *len-=res;
00220     return true;
00221   } else {
00222     printf(msg,arg1);
00223     return false;
00224   }
00225 }
00226 
00227 /*! @file
00228  * @brief Implements PostureEngine, a base class for managing the values and weights of all the outputs
00229  * @author ejt (Creator)
00230  *
00231  * $Author: ejt $
00232  * $Name: tekkotsu-1_4_1 $
00233  * $Revision: 1.8 $
00234  * $State: Exp $
00235  * $Date: 2003/06/12 04:16:43 $
00236  */
00237 

Tekkotsu v1.4
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2