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
00038
00039
00040
00041
00042
00043
00044
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
00062
00063
00064
00065
00066
00067
00068
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
00133 return 0;
00134 }
00135 char formatstring[64];
00136 snprintf(formatstring,64,"%%%dc %%f %%f\n%%n",outputNameLen);
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
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
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
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
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237