libcm is a C development framework with an emphasis on audio signal processing applications.
選択できるのは25トピックまでです。 トピックは、先頭が英数字で、英数字とダッシュ('-')を使用した35文字以内のものにしてください。

cmVectOpsTemplateCode.h 79KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778277927802781278227832784278527862787278827892790279127922793279427952796279727982799280028012802280328042805280628072808280928102811281228132814281528162817281828192820282128222823282428252826282728282829283028312832283328342835283628372838283928402841284228432844284528462847284828492850285128522853285428552856285728582859286028612862286328642865286628672868286928702871287228732874287528762877287828792880288128822883288428852886288728882889289028912892289328942895289628972898289929002901290229032904290529062907290829092910291129122913291429152916291729182919292029212922292329242925292629272928292929302931293229332934293529362937293829392940294129422943294429452946294729482949295029512952295329542955295629572958295929602961296229632964296529662967296829692970297129722973297429752976297729782979298029812982298329842985298629872988298929902991299229932994299529962997299829993000300130023003300430053006300730083009301030113012301330143015301630173018301930203021302230233024302530263027302830293030303130323033303430353036303730383039304030413042304330443045304630473048304930503051305230533054305530563057305830593060306130623063306430653066306730683069307030713072307330743075307630773078307930803081308230833084308530863087308830893090309130923093309430953096309730983099310031013102310331043105310631073108310931103111311231133114311531163117311831193120312131223123312431253126312731283129313031313132313331343135313631373138313931403141314231433144314531463147314831493150315131523153315431553156315731583159316031613162316331643165316631673168316931703171317231733174317531763177317831793180318131823183318431853186318731883189319031913192319331943195319631973198319932003201320232033204320532063207320832093210321132123213321432153216321732183219322032213222322332243225322632273228322932303231323232333234323532363237323832393240324132423243324432453246
  1. #ifdef cmVectOpsTemplateCode_h
  2. void VECT_OP_FUNC(VPrint)( cmRpt_t* rpt, const char* fmt, ... )
  3. {
  4. va_list vl;
  5. va_start(vl,fmt);
  6. if( rpt != NULL )
  7. cmRptVPrintf(rpt,fmt,vl);
  8. else
  9. vprintf(fmt,vl);
  10. va_end(vl);
  11. }
  12. void VECT_OP_FUNC(Printf)( cmRpt_t* rpt, unsigned rowCnt, unsigned colCnt, const VECT_OP_TYPE* sbp, unsigned fieldWidth, unsigned decPlCnt, const char* fmt, unsigned flags )
  13. {
  14. unsigned cci;
  15. unsigned outColCnt = 10;
  16. if( fieldWidth < 0 )
  17. fieldWidth = 10;
  18. if( decPlCnt < 0 )
  19. decPlCnt = 4;
  20. if( outColCnt == -1 )
  21. outColCnt = colCnt;
  22. for(cci=0; cci<colCnt; cci+=outColCnt)
  23. {
  24. unsigned ci0 = cci;
  25. unsigned cn = cci + outColCnt;
  26. unsigned ri;
  27. if(cn > colCnt)
  28. cn = colCnt;
  29. if( colCnt > outColCnt )
  30. {
  31. if( cmIsFlag(flags,cmPrintMatlabLabelsFl) )
  32. VECT_OP_FUNC(VPrint)(rpt,"Columns:%i to %i\n",ci0,cn-1);
  33. else
  34. if( cmIsFlag(flags,cmPrintShortLabelsFl) )
  35. VECT_OP_FUNC(VPrint)(rpt,"%3i: ",ci0);
  36. }
  37. if( rowCnt > 1 )
  38. VECT_OP_FUNC(VPrint)(rpt,"\n");
  39. for(ri=0; ri<rowCnt; ++ri)
  40. {
  41. unsigned ci;
  42. for(ci=ci0; ci<cn; ++ci )
  43. VECT_OP_FUNC(VPrint)(rpt,fmt,fieldWidth,decPlCnt,sbp[ (ci*rowCnt) + ri ]);
  44. if( cn > 0 )
  45. VECT_OP_FUNC(VPrint)(rpt,"\n");
  46. }
  47. }
  48. }
  49. void VECT_OP_FUNC(Print)( cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* sbp )
  50. { VECT_OP_FUNC(Printf)(rpt,rn,cn,sbp,cmDefaultFieldWidth,cmDefaultDecPlCnt,"%*.*f ",cmPrintShortLabelsFl); }
  51. void VECT_OP_FUNC(PrintE)( cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* sbp )
  52. { VECT_OP_FUNC(Printf)(rpt,rn,cn,sbp,cmDefaultFieldWidth,cmDefaultDecPlCnt,"%*.*e ",cmPrintShortLabelsFl); }
  53. void VECT_OP_FUNC(PrintLf)( const char* label, cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* dbp, unsigned fieldWidth, unsigned decPlCnt, const char* fmt )
  54. {
  55. VECT_OP_FUNC(VPrint)( rpt, "%s\n", label );
  56. VECT_OP_FUNC(Printf)( rpt, rn, cn, dbp, fieldWidth, decPlCnt,fmt,cmPrintShortLabelsFl );
  57. }
  58. void VECT_OP_FUNC(PrintL)( const char* label, cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* dbp )
  59. {
  60. VECT_OP_FUNC(VPrint)( rpt, "%s\n", label );
  61. VECT_OP_FUNC(Printf)( rpt, rn, cn, dbp, cmDefaultFieldWidth,cmDefaultDecPlCnt,"%*.*f ",cmPrintShortLabelsFl );
  62. }
  63. void VECT_OP_FUNC(PrintLE)( const char* label, cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* dbp )
  64. {
  65. VECT_OP_FUNC(VPrint)( rpt, "%s\n", label );
  66. VECT_OP_FUNC(Printf)( rpt, rn, cn, dbp, cmDefaultFieldWidth,cmDefaultDecPlCnt,"%*.*e ",cmPrintShortLabelsFl );
  67. }
  68. VECT_OP_TYPE* VECT_OP_FUNC(NormalizeProbabilityVV)(VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp)
  69. {
  70. VECT_OP_TYPE sum = VECT_OP_FUNC(Sum)(sbp,dn);
  71. if( sum == 0 )
  72. sum = 1;
  73. return VECT_OP_FUNC(DivVVS)(dbp,dn,sbp,sum);
  74. }
  75. VECT_OP_TYPE* VECT_OP_FUNC(NormalizeProbability)(VECT_OP_TYPE* dbp, unsigned dn)
  76. { return VECT_OP_FUNC(NormalizeProbabilityVV)(dbp,dn,dbp); }
  77. VECT_OP_TYPE* VECT_OP_FUNC(NormalizeProbabilityN)(VECT_OP_TYPE* dbp, unsigned dn, unsigned stride)
  78. {
  79. VECT_OP_TYPE sum = VECT_OP_FUNC(SumN)(dbp,dn,stride);
  80. if( sum == 0 )
  81. return dbp;
  82. VECT_OP_TYPE* dp = dbp;
  83. VECT_OP_TYPE* ep = dp + (dn*stride);
  84. for(; dp < ep; dp+=stride )
  85. *dp /= sum;
  86. return dbp;
  87. }
  88. VECT_OP_TYPE* VECT_OP_FUNC(StandardizeRows)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, VECT_OP_TYPE* uV, VECT_OP_TYPE* sdV )
  89. {
  90. bool uFl = false;
  91. bool sFl = false;
  92. unsigned i;
  93. if( uV == NULL )
  94. {
  95. uV = cmMemAllocZ(VECT_OP_TYPE,drn);
  96. uFl = true;
  97. }
  98. if( sdV == NULL )
  99. {
  100. sdV = cmMemAllocZ(VECT_OP_TYPE,drn);
  101. sFl = true;
  102. }
  103. VECT_OP_FUNC(MeanM)(uV, dbp, drn, dcn, 1 );
  104. VECT_OP_FUNC(VarianceM)(sdV, dbp, drn, dcn, uV, 1 );
  105. for(i=0; i<dcn; ++i)
  106. {
  107. VECT_OP_FUNC(SubVV)(dbp + i * drn, drn, uV );
  108. VECT_OP_FUNC(DivVV)(dbp + i * drn, drn, sdV );
  109. }
  110. if(uFl)
  111. cmMemFree(uV);
  112. if(sFl)
  113. cmMemFree(sdV);
  114. return dbp;
  115. }
  116. VECT_OP_TYPE* VECT_OP_FUNC(StandardizeCols)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, VECT_OP_TYPE* uV, VECT_OP_TYPE* sdV )
  117. {
  118. bool uFl = false;
  119. bool sFl = false;
  120. unsigned i;
  121. if( uV == NULL )
  122. {
  123. uV = cmMemAllocZ(VECT_OP_TYPE,dcn);
  124. uFl = true;
  125. }
  126. if( sdV == NULL )
  127. {
  128. sdV = cmMemAllocZ(VECT_OP_TYPE,dcn);
  129. sFl = true;
  130. }
  131. VECT_OP_FUNC(MeanM)(uV, dbp, drn, dcn, 0 );
  132. VECT_OP_FUNC(VarianceM)(sdV, dbp, drn, dcn, uV, 0 );
  133. for(i=0; i<drn; ++i)
  134. {
  135. VECT_OP_FUNC(SubVVNN)(dbp + i, dcn, drn, uV, 1 );
  136. VECT_OP_FUNC(DivVVNN)(dbp + i, dcn, drn, sdV, 1 );
  137. }
  138. if(uFl)
  139. cmMemFree(uV);
  140. if(sFl)
  141. cmMemFree(sdV);
  142. return dbp;
  143. }
  144. VECT_OP_TYPE* VECT_OP_FUNC(HalfWaveRectify)(VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
  145. {
  146. VECT_OP_TYPE* dp = dbp;
  147. VECT_OP_TYPE* ep = dbp + dn;
  148. for(; dp < ep; ++dp,++sp )
  149. *dp = *sp < 0 ? 0 : *sp;
  150. return dbp;
  151. }
  152. VECT_OP_TYPE* VECT_OP_FUNC(CumSum)(VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp)
  153. {
  154. VECT_OP_TYPE* dep = dbp + dn;
  155. VECT_OP_TYPE* rp = dbp;
  156. VECT_OP_TYPE sum = 0;
  157. while( dbp < dep )
  158. {
  159. sum += *sbp++;
  160. *dbp++ = sum;
  161. }
  162. return rp;
  163. }
  164. VECT_OP_TYPE VECT_OP_FUNC(Mean)( const VECT_OP_TYPE* bp, unsigned n )
  165. { return VECT_OP_FUNC(Sum)(bp,n)/n; }
  166. VECT_OP_TYPE VECT_OP_FUNC(MeanN)( const VECT_OP_TYPE* bp, unsigned n, unsigned stride )
  167. { return VECT_OP_FUNC(SumN)(bp,n,stride)/n; }
  168. VECT_OP_TYPE* VECT_OP_FUNC(MeanM)( VECT_OP_TYPE* dp, const VECT_OP_TYPE* sp, unsigned srn, unsigned scn, unsigned dim )
  169. {
  170. unsigned i;
  171. unsigned cn = dim == 0 ? scn : srn;
  172. unsigned rn = dim == 0 ? srn : scn;
  173. unsigned inc = dim == 0 ? srn : 1;
  174. unsigned stride = dim == 0 ? 1 : srn;
  175. unsigned d0 = 0;
  176. for(i=0; i<cn; ++i, d0+=inc)
  177. dp[i] = VECT_OP_FUNC(MeanN)(sp + d0, rn, stride );
  178. return dp;
  179. }
  180. VECT_OP_TYPE* VECT_OP_FUNC(Mean2)( VECT_OP_TYPE* dp, const VECT_OP_TYPE* (*srcFuncPtr)(void* arg, unsigned idx ), unsigned D, unsigned N, void* argPtr )
  181. {
  182. unsigned i,n;
  183. const VECT_OP_TYPE* sp;
  184. VECT_OP_FUNC(Zero)(dp,D);
  185. if( N > 1 )
  186. {
  187. n = 0;
  188. for(i=0; i<N; ++i)
  189. if((sp = srcFuncPtr(argPtr,i)) != NULL )
  190. {
  191. VECT_OP_FUNC(AddVV)(dp,D,sp);
  192. ++n;
  193. }
  194. VECT_OP_FUNC(DivVS)(dp,D,n);
  195. }
  196. return dp;
  197. }
  198. VECT_OP_TYPE VECT_OP_FUNC(Variance)( const VECT_OP_TYPE* sp, unsigned sn, const VECT_OP_TYPE* avgPtr )
  199. { return VECT_OP_FUNC(VarianceN)(sp,sn,1,avgPtr); }
  200. VECT_OP_TYPE VECT_OP_FUNC(VarianceN)( const VECT_OP_TYPE* sp, unsigned sn, unsigned stride, const VECT_OP_TYPE* meanPtr )
  201. {
  202. VECT_OP_TYPE mean = 0;
  203. if( sn <= 1 )
  204. return 0;
  205. if( meanPtr == NULL )
  206. mean = VECT_OP_FUNC(MeanN)( sp, sn, stride );
  207. else
  208. mean = *meanPtr;
  209. const VECT_OP_TYPE* ep = sp + (sn*stride);
  210. VECT_OP_TYPE sum = 0;
  211. for(; sp < ep; sp += stride )
  212. sum += (*sp-mean) * (*sp-mean);
  213. return sum / (sn-1);
  214. }
  215. VECT_OP_TYPE* VECT_OP_FUNC(VarianceM)(VECT_OP_TYPE* dp, const VECT_OP_TYPE* sp, unsigned srn, unsigned scn, const VECT_OP_TYPE* avgPtr, unsigned dim )
  216. {
  217. unsigned i;
  218. unsigned cn = dim == 0 ? scn : srn;
  219. unsigned rn = dim == 0 ? srn : scn;
  220. unsigned inc = dim == 0 ? srn : 1;
  221. unsigned stride = dim == 0 ? 1 : srn;
  222. unsigned d0 = 0;
  223. for(i=0; i<cn; ++i, d0+=inc)
  224. dp[i] = VECT_OP_FUNC(VarianceN)(sp + d0, rn, stride, avgPtr==NULL ? NULL : avgPtr+i );
  225. return dp;
  226. }
  227. unsigned VECT_OP_FUNC(NormToMax)( VECT_OP_TYPE* dp, unsigned dn )
  228. {
  229. unsigned i = VECT_OP_FUNC(MaxIndex)(dp,dn,1);
  230. if( i != cmInvalidIdx )
  231. {
  232. VECT_OP_TYPE v = dp[i];
  233. VECT_OP_FUNC(DivVS)(dp,dn,v);
  234. }
  235. return i;
  236. }
  237. unsigned VECT_OP_FUNC(NormToAbsMax)( VECT_OP_TYPE* dp, unsigned dn, VECT_OP_TYPE fact )
  238. {
  239. if( dn == 0 )
  240. return cmInvalidIdx;
  241. unsigned i = 0;
  242. unsigned mi = 0;
  243. VECT_OP_TYPE mx = fabs(dp[0]);
  244. for(i=1; i<dn; ++i)
  245. if( fabs(dp[i])>mx )
  246. {
  247. mi = i;
  248. mx = fabs(dp[i]);
  249. }
  250. VECT_OP_FUNC(MultVS)(dp,dn,fact/mx);
  251. return mi;
  252. }
  253. VECT_OP_TYPE VECT_OP_FUNC(AlphaNorm)(const VECT_OP_TYPE* sp, unsigned sn, VECT_OP_TYPE alpha )
  254. {
  255. double sum = 0;
  256. const VECT_OP_TYPE* bp = sp;
  257. const VECT_OP_TYPE* ep = sp + sn;
  258. while( bp < ep )
  259. sum += pow(fabs(*bp++),alpha);
  260. return (VECT_OP_TYPE)pow(sum/sn,1.0/alpha);
  261. }
  262. void VECT_OP_FUNC(GaussCovariance)(VECT_OP_TYPE* yM, unsigned D, const VECT_OP_TYPE* xM, unsigned xN, const VECT_OP_TYPE* uV, const unsigned* selIdxV, unsigned selKey )
  263. {
  264. unsigned i,j,k,n = 0;
  265. VECT_OP_TYPE tV[ D ];
  266. VECT_OP_FUNC(Fill)(yM,D*D,0);
  267. // if the mean was not given - then calculate it
  268. if( uV == NULL )
  269. {
  270. VECT_OP_FUNC(Fill)(tV,D,0);
  271. // sum each row of xM[] into uM[]
  272. for(i=0; i<D; ++i)
  273. {
  274. n = 0;
  275. for(j=0; j<xN; ++j)
  276. if( selIdxV==NULL || selIdxV[j]==selKey )
  277. {
  278. tV[i] += xM[ (j*D) + i ];
  279. ++n;
  280. }
  281. }
  282. // form an average from the sum in tV[]
  283. VECT_OP_FUNC(DivVS)(tV,D,n);
  284. uV = tV;
  285. }
  286. for(i=0; i<D; ++i)
  287. for(j=i; j<D; ++j)
  288. {
  289. n = 0;
  290. for(k=0; k<xN; ++k)
  291. if( selIdxV==NULL || selIdxV[k]==selKey)
  292. {
  293. unsigned yi = (i*D)+j;
  294. yM[ yi ] += ((xM[ (k*D)+j ]-uV[j]) * (xM[ (k*D) + i ]-uV[i]));
  295. if( i != j )
  296. yM[ (j*D)+i ] = yM[ yi ];
  297. ++n;
  298. }
  299. }
  300. if( n>1 )
  301. VECT_OP_FUNC(DivVS)( yM, D*D, n-1 );
  302. }
  303. void VECT_OP_FUNC(GaussCovariance2)(VECT_OP_TYPE* yM, unsigned D, const VECT_OP_TYPE* (*srcFunc)(void* userPtr, unsigned idx), unsigned xN, void* userPtr, const VECT_OP_TYPE* uV, const unsigned* selIdxV, unsigned selKey )
  304. {
  305. unsigned i,j,k = 0,n;
  306. VECT_OP_TYPE tV[ D ];
  307. const VECT_OP_TYPE* sp;
  308. VECT_OP_FUNC(Fill)(yM,D*D,0);
  309. // if the mean was not given - then calculate it
  310. if( uV == NULL )
  311. {
  312. VECT_OP_FUNC(Fill)(tV,D,0);
  313. n = 0;
  314. // sum each row of xM[] into uM[]
  315. for(i=0; i<xN; ++i)
  316. if( (selIdxV==NULL || selIdxV[i]==selKey) && ((sp=srcFunc(userPtr,i))!=NULL) )
  317. {
  318. VECT_OP_FUNC(AddVV)(tV,D,sp);
  319. ++n;
  320. }
  321. // form an average from the sum in tV[]
  322. VECT_OP_FUNC(DivVS)(tV,D,n);
  323. uV = tV;
  324. }
  325. for(i=0; i<xN; ++i)
  326. if( selIdxV==NULL || selIdxV[i]==selKey )
  327. {
  328. // get a pointer to the ith data point
  329. const VECT_OP_TYPE* sV = srcFunc(userPtr,i);
  330. // note: this algorithm works because when a data point element (scalar)
  331. // is multiplied by another data point element those two elements
  332. // are always part of the same data point (vector). Two elements
  333. // from different data points are never multiplied.
  334. if( sV != NULL )
  335. for(j=0; j<D; ++j)
  336. for(k=j; k<D; ++k)
  337. yM[j + k*D] += (sV[j]-uV[j]) * (sV[k]-uV[k]);
  338. }
  339. if( n > 1 )
  340. VECT_OP_FUNC(DivVS)( yM, D*D, n-1 );
  341. // fill in the lower triangle
  342. for(j=0; j<D; ++j)
  343. for(k=j; k<D; ++k)
  344. yM[k + j*D] = yM[j + k*D];
  345. }
  346. bool VECT_OP_FUNC(Equal)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
  347. {
  348. const VECT_OP_TYPE* ep = s0p + sn;
  349. while( s0p < ep )
  350. if( *s0p++ != *s1p++ )
  351. return false;
  352. return true;
  353. }
  354. bool VECT_OP_FUNC(IsNormal)( const VECT_OP_TYPE* sp, unsigned sn )
  355. {
  356. const VECT_OP_TYPE* ep = sp + sn;
  357. for(; sp<ep; ++sp)
  358. if( !isnormal(*sp) )
  359. return false;
  360. return true;
  361. }
  362. bool VECT_OP_FUNC(IsNormalZ)(const VECT_OP_TYPE* sp, unsigned sn )
  363. {
  364. const VECT_OP_TYPE* ep = sp + sn;
  365. for(; sp<ep; ++sp)
  366. if( (*sp != 0) && (!isnormal(*sp)) )
  367. return false;
  368. return true;
  369. }
  370. unsigned VECT_OP_FUNC(FindNonNormal)( unsigned* dp, unsigned dn, const VECT_OP_TYPE* sbp )
  371. {
  372. const VECT_OP_TYPE* sp = sbp;
  373. const VECT_OP_TYPE* ep = sp + dn;
  374. unsigned n = 0;
  375. for(; sp<ep; ++sp)
  376. if( !isnormal(*sp) )
  377. dp[n++] = sp - sbp;
  378. return n;
  379. }
  380. unsigned VECT_OP_FUNC(FindNonNormalZ)( unsigned* dp, unsigned dn, const VECT_OP_TYPE* sbp )
  381. {
  382. const VECT_OP_TYPE* sp = sbp;
  383. const VECT_OP_TYPE* ep = sp + dn;
  384. unsigned n = 0;
  385. for(; sp<ep; ++sp)
  386. if( (*sp!=0) && (!isnormal(*sp)) )
  387. dp[n++] = sp - sbp;
  388. return n;
  389. }
  390. unsigned VECT_OP_FUNC(ZeroCrossCount)( const VECT_OP_TYPE* bp, unsigned bn, VECT_OP_TYPE* delaySmpPtr)
  391. {
  392. unsigned n = delaySmpPtr != NULL ? ((*delaySmpPtr >= 0) != (*bp >= 0)) : 0 ;
  393. const VECT_OP_TYPE* ep = bp + bn;
  394. for(; bp<ep-1; ++bp)
  395. if( (*bp >= 0) != (*(bp+1) >= 0) )
  396. ++n;
  397. if( delaySmpPtr != NULL )
  398. *delaySmpPtr = *bp;
  399. return n;
  400. }
  401. VECT_OP_TYPE VECT_OP_FUNC(SquaredSum)( const VECT_OP_TYPE* bp, unsigned bn )
  402. {
  403. VECT_OP_TYPE sum = 0;
  404. const VECT_OP_TYPE* ep = bp + bn;
  405. for(; bp < ep; ++bp )
  406. sum += *bp * *bp;
  407. return sum;
  408. }
  409. VECT_OP_TYPE VECT_OP_FUNC(RMS)( const VECT_OP_TYPE* bp, unsigned bn, unsigned wndSmpCnt )
  410. {
  411. const VECT_OP_TYPE* ep = bp + bn;
  412. if( bn==0 )
  413. return 0;
  414. assert( bn <= wndSmpCnt );
  415. double sum = 0;
  416. for(; bp < ep; ++bp )
  417. sum += *bp * *bp;
  418. return (VECT_OP_TYPE)sqrt(sum/wndSmpCnt);
  419. }
  420. VECT_OP_TYPE* VECT_OP_FUNC(RmsV)( VECT_OP_TYPE* dp, unsigned dn, const VECT_OP_TYPE* sp, unsigned sn, unsigned wndSmpCnt, unsigned hopSmpCnt )
  421. {
  422. const VECT_OP_TYPE* dep = dp + dn;
  423. const VECT_OP_TYPE* sep = sp + sn;
  424. VECT_OP_TYPE* rp = dp;
  425. for(; dp<dep && sp<sep; sp+=hopSmpCnt)
  426. *dp++ = VECT_OP_FUNC(RMS)( sp, cmMin(wndSmpCnt,sep-sp), wndSmpCnt );
  427. VECT_OP_FUNC(Zero)(dp,dep-dp);
  428. return rp;
  429. }
  430. VECT_OP_TYPE VECT_OP_FUNC(EuclidNorm)( const VECT_OP_TYPE* sp, unsigned sn )
  431. { return (VECT_OP_TYPE)sqrt( VECT_OP_FUNC(MultSumVV)(sp,sp,sn)); }
  432. /*
  433. From:http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/doc/voicebox/distitpf.html
  434. [nf1,p2]=size(pf1);
  435. p1=p2-1;
  436. nf2=size(pf2,1);
  437. nx= min(nf1,nf2);
  438. r = pf1(1:nx,:)./pf2(1:nx,:);
  439. q = r-log(r);
  440. s = sum( q(:,2:p1),2) + 0.5 * (q(:,1)+q(:,p2))
  441. d= s/p1-1;
  442. */
  443. VECT_OP_TYPE VECT_OP_FUNC(ItakuraDistance)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
  444. {
  445. VECT_OP_TYPE d = 0;
  446. VECT_OP_TYPE r[ sn ];
  447. VECT_OP_TYPE q[ sn ];
  448. // r = pf1(1:nx,:)./pf2(1:nx,:);
  449. VECT_OP_FUNC(DivVVV)(r,sn,s0p,s1p);
  450. //q=log(r);
  451. VECT_OP_FUNC(LogV)(q,sn,r);
  452. //r = r - q = r - log(r)
  453. VECT_OP_FUNC(SubVV)(r,sn,q);
  454. //r = r - sn = r - log(r) - 1
  455. VECT_OP_FUNC(SubVS)(r,sn,sn);
  456. // d = sum(r);
  457. d = VECT_OP_FUNC(Sum)(r,sn);
  458. return (VECT_OP_TYPE)(d / sn);
  459. //d = log( VECT_OP_FUNC(Sum)(r,sn) /sn );
  460. //d -= VECT_OP_FUNC(Sum)(q,sn)/sn;
  461. return d;
  462. }
  463. VECT_OP_TYPE VECT_OP_FUNC(CosineDistance)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
  464. {
  465. VECT_OP_TYPE d0 = VECT_OP_FUNC(EuclidNorm)(s0p,sn);
  466. VECT_OP_TYPE d1 = VECT_OP_FUNC(EuclidNorm)(s1p,sn);
  467. if( d0 == 0 )
  468. d0 = cmReal_MIN;
  469. if( d1 == 0 )
  470. d1 = cmReal_MIN;
  471. return (VECT_OP_TYPE)(VECT_OP_FUNC(MultSumVV)(s0p,s1p,sn) / (d0 * d1));
  472. }
  473. VECT_OP_TYPE VECT_OP_FUNC(EuclidDistance)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
  474. {
  475. double d = 0;
  476. const VECT_OP_TYPE* sep = s0p + sn;
  477. for(; s0p<sep; ++s0p,++s1p)
  478. d += (*s0p - *s1p) * (*s0p - *s1p);
  479. return (VECT_OP_TYPE)(sqrt(d));
  480. }
  481. VECT_OP_TYPE VECT_OP_FUNC(L1Distance)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
  482. {
  483. double d = 0;
  484. const VECT_OP_TYPE* sep = s0p + sn;
  485. for(; s0p<sep; ++s0p,++s1p)
  486. d += (VECT_OP_TYPE)fabs(*s0p - *s1p);
  487. return d;
  488. }
  489. VECT_OP_TYPE VECT_OP_FUNC(MahalanobisDistance)( const VECT_OP_TYPE* x, unsigned D, const VECT_OP_TYPE* u, const VECT_OP_TYPE* invCovM )
  490. {
  491. VECT_OP_TYPE t[ D ];
  492. VECT_OP_TYPE d[ D ];
  493. // t[] = x[] - u[];
  494. VECT_OP_FUNC(SubVVV)(t,D,x,u);
  495. // d[1,D] = t[1,D] * covM[D,D]
  496. VECT_OP_FUNC(MultVVM)( d, D, t, D, invCovM );
  497. // d = sum(d[].*t[])
  498. VECT_OP_TYPE dist = VECT_OP_FUNC(MultSumVV)(d,t,D);
  499. return (VECT_OP_TYPE)sqrt(dist);
  500. }
  501. VECT_OP_TYPE VECT_OP_FUNC(KL_Distance)( const VECT_OP_TYPE* up, const VECT_OP_TYPE* sp, unsigned sn )
  502. {
  503. VECT_OP_TYPE v[ sn ];
  504. VECT_OP_FUNC(DivVVV)(v,sn,up,sp); // v = up ./ sp
  505. VECT_OP_FUNC(LogV)(v,sn,v); // v = log(v)
  506. VECT_OP_FUNC(MultVV)(v,sn,up); // v *= up;
  507. return VECT_OP_FUNC(Sum)(v,sn); // sum(v)
  508. }
  509. VECT_OP_TYPE VECT_OP_FUNC(KL_Distance2)( const VECT_OP_TYPE* up, const VECT_OP_TYPE* sp, unsigned sn )
  510. {
  511. VECT_OP_TYPE v0[ sn ];
  512. VECT_OP_TYPE v1[ sn ];
  513. VECT_OP_FUNC(NormalizeProbabilityVV)(v0,sn,up);
  514. VECT_OP_FUNC(NormalizeProbabilityVV)(v1,sn,sp);
  515. return VECT_OP_FUNC(KL_Distance)(v0,v1,sn);
  516. }
  517. /// If dv[scn] is non NULL then return the Euclidean distance from sv[scn] to each column of sm[srn,scn].
  518. /// The function returns the index of the closest data point (column) in sm[].
  519. unsigned VECT_OP_FUNC(EuclidDistanceVM)( VECT_OP_TYPE* dv, const VECT_OP_TYPE* sv, const VECT_OP_TYPE* sm, unsigned srn, unsigned scn )
  520. {
  521. unsigned minIdx = cmInvalidIdx;
  522. VECT_OP_TYPE minDist = 0;
  523. unsigned i = 0;
  524. for(; i<scn; ++i )
  525. {
  526. VECT_OP_TYPE dist = VECT_OP_FUNC(EuclidDistance)(sv, sm + (i*srn), srn );
  527. if( dv != NULL )
  528. *dv++ = dist;
  529. if( dist < minDist || minIdx == cmInvalidIdx )
  530. {
  531. minIdx = i;
  532. minDist = dist;
  533. }
  534. }
  535. return minIdx;
  536. }
  537. void VECT_OP_FUNC(DistVMM)( VECT_OP_TYPE* dM, VECT_OP_TYPE* mvV, unsigned* miV, unsigned rn, const VECT_OP_TYPE* s0M, unsigned s0cn, const VECT_OP_TYPE* s1M, unsigned s1cn, VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ), void* userPtr )
  538. {
  539. unsigned i,j,k;
  540. // for each col in s0M[];
  541. for(i=0,k=0; i<s0cn; ++i)
  542. {
  543. VECT_OP_TYPE min_val = VECT_OP_MAX;
  544. unsigned min_idx = cmInvalidIdx;
  545. // for each col in s1M[]
  546. for(j=0; j<s1cn; ++j,++k)
  547. {
  548. // v = distance(s0M[:,i],s1M[:,j]
  549. VECT_OP_TYPE v = distFunc( userPtr, s1M + (j*rn), s0M + (i*rn), rn );
  550. if( dM != NULL )
  551. dM[k] = v; // store distance
  552. // track closest col in s1M[]
  553. if( v < min_val || min_idx==cmInvalidIdx )
  554. {
  555. min_val = v;
  556. min_idx = j;
  557. }
  558. }
  559. if( mvV != NULL )
  560. mvV[i] = min_val;
  561. if( miV != NULL )
  562. miV[i] = min_idx;
  563. }
  564. }
  565. void VECT_OP_FUNC(SelectRandom) ( VECT_OP_TYPE *dM, unsigned* selIdxV, unsigned selIdxN, const VECT_OP_TYPE* sM, unsigned srn, unsigned scn )
  566. {
  567. bool freeFl = false;
  568. unsigned i;
  569. assert( selIdxN != 0 );
  570. // if no selIdxV[] was given then create one
  571. if( selIdxV == NULL )
  572. {
  573. selIdxV = cmMemAlloc( unsigned, selIdxN );
  574. freeFl = true;
  575. }
  576. // select datapoints at random
  577. cmVOU_UniqueRandom(selIdxV,selIdxN,scn);
  578. // copy the data points into the output matrix
  579. if( dM != NULL )
  580. for(i=0; i<selIdxN; ++i)
  581. {
  582. assert( selIdxV[i] < scn );
  583. VECT_OP_FUNC(Copy)( dM + (i*srn), srn, sM + selIdxV[i]*srn );
  584. }
  585. if( freeFl )
  586. cmMemPtrFree(&selIdxV);
  587. }
  588. void VECT_OP_FUNC(_SelectDist)( VECT_OP_TYPE *dM, unsigned* selIdxV, unsigned selIdxN, const VECT_OP_TYPE* sM, unsigned srn, unsigned scn, VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ), void* userPtr, bool avgFl )
  589. {
  590. unsigned i;
  591. unsigned dcn = 0;
  592. bool freeFl = false;
  593. assert( selIdxN > 0 );
  594. if( dM == NULL )
  595. {
  596. dM = cmMemAllocZ( VECT_OP_TYPE, srn*selIdxN );
  597. freeFl = true;
  598. }
  599. // allocate distM[scn,selIdxN] to hold the distances from each selected column to all columns in sM[]
  600. VECT_OP_TYPE* distM = cmMemAllocZ( VECT_OP_TYPE, scn*selIdxN );
  601. // sumV[] is a temp vector to hold the summed distances to from the selected columns to each column in sM[]
  602. VECT_OP_TYPE* sumV = cmMemAllocZ( VECT_OP_TYPE, scn );
  603. // select a random point from sM[] and copy it to the first column of dM[]
  604. cmVOU_Random(&i,1,scn);
  605. VECT_OP_FUNC(Copy)(dM, srn, sM + (i*srn));
  606. if( selIdxV != NULL )
  607. selIdxV[0] = i;
  608. for(dcn=1; dcn<selIdxN; ++dcn)
  609. {
  610. // set distM[scn,dcn] with the dist from dM[dcn,srn] to each column in sM[]
  611. VECT_OP_FUNC(DistVMM)( distM, NULL, NULL, srn, dM, dcn, sM, scn, distFunc, userPtr );
  612. // sum the rows of distM[ scn, dcn ] into sumV[scn]
  613. VECT_OP_FUNC(SumMN)( distM, scn, dcn, sumV );
  614. if( avgFl )
  615. VECT_OP_FUNC(DivVS)( sumV, scn, dcn );
  616. // find the point in sM[] which has the greatest combined distance to all previously selected points.
  617. unsigned maxIdx = VECT_OP_FUNC(MaxIndex)(sumV, scn, 1 );
  618. // copy the point into dM[]
  619. VECT_OP_FUNC(Copy)(dM + (dcn*srn), srn, sM + (maxIdx*srn));
  620. if( selIdxV != NULL )
  621. selIdxV[dcn] = maxIdx;
  622. }
  623. cmMemPtrFree(&distM);
  624. cmMemPtrFree(&sumV);
  625. if( freeFl )
  626. cmMemPtrFree(&dM);
  627. }
  628. void VECT_OP_FUNC(SelectMaxDist)( VECT_OP_TYPE *dM, unsigned* selIdxV, unsigned selIdxN, const VECT_OP_TYPE* sM, unsigned srn, unsigned scn, VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ), void* userPtr )
  629. { VECT_OP_FUNC(_SelectDist)(dM,selIdxV,selIdxN,sM,srn,scn,distFunc,userPtr,false); }
  630. void VECT_OP_FUNC(SelectMaxAvgDist)( VECT_OP_TYPE *dM, unsigned* selIdxV, unsigned selIdxN, const VECT_OP_TYPE* sM, unsigned srn, unsigned scn, VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ), void* userPtr )
  631. { VECT_OP_FUNC(_SelectDist)(dM,selIdxV,selIdxN,sM,srn,scn,distFunc,userPtr,true); }
  632. #ifdef CM_VECTOP
  633. VECT_OP_TYPE VECT_OP_FUNC(MultSumVV)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
  634. { return VECT_OP_BLAS_FUNC(dot)(sn, s0p, 1, s1p, 1); }
  635. #else
  636. VECT_OP_TYPE VECT_OP_FUNC(MultSumVV)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
  637. {
  638. VECT_OP_TYPE sum = 0;
  639. const VECT_OP_TYPE* sep = s0p + sn;
  640. while(s0p<sep)
  641. sum += *s0p++ * *s1p++;
  642. return sum;
  643. }
  644. #endif
  645. VECT_OP_TYPE VECT_OP_FUNC(MultSumVS)( const VECT_OP_TYPE* s0p, unsigned sn, VECT_OP_TYPE s1 )
  646. {
  647. VECT_OP_TYPE sum = 0;
  648. const VECT_OP_TYPE* sep = s0p + sn;
  649. while(s0p<sep)
  650. sum += *s0p++ * s1;
  651. return sum;
  652. }
  653. #ifdef CM_VECTOP
  654. VECT_OP_TYPE* VECT_OP_FUNC(MultVMV)( VECT_OP_TYPE* dbp, unsigned mrn, const VECT_OP_TYPE* mp, unsigned mcn, const VECT_OP_TYPE* vp )
  655. {
  656. VECT_OP_BLAS_FUNC(gemv)( CblasColMajor, CblasNoTrans, mrn, mcn, 1.0, mp, mrn, vp, 1, 0.0, dbp, 1 );
  657. return dbp;
  658. }
  659. #else
  660. VECT_OP_TYPE* VECT_OP_FUNC(MultVMV)( VECT_OP_TYPE* dbp, unsigned mrn, const VECT_OP_TYPE* mp, unsigned mcn, const VECT_OP_TYPE* vp )
  661. {
  662. const VECT_OP_TYPE* dep = dbp + mrn;
  663. VECT_OP_TYPE* dp = dbp;
  664. const VECT_OP_TYPE* vep = vp + mcn;
  665. // for each dest element
  666. for(; dbp < dep; ++dbp )
  667. {
  668. const VECT_OP_TYPE* vbp = vp;
  669. const VECT_OP_TYPE* mbp = mp++;
  670. *dbp = 0;
  671. // for each source vector row and src mtx col
  672. while( vbp < vep )
  673. {
  674. *dbp += *mbp * *vbp++;
  675. mbp += mrn;
  676. }
  677. }
  678. return dp;
  679. }
  680. #endif
  681. #ifdef CM_VECTOP
  682. VECT_OP_TYPE* VECT_OP_FUNC(MultVVM)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* vp, unsigned vn, const VECT_OP_TYPE* mp )
  683. {
  684. VECT_OP_BLAS_FUNC(gemv)( CblasColMajor, CblasTrans, vn, dn, 1.0, mp, vn, vp, 1, 0.0, dbp, 1 );
  685. return dbp;
  686. }
  687. #else
  688. VECT_OP_TYPE* VECT_OP_FUNC(MultVVM)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* vp, unsigned vn, const VECT_OP_TYPE* mp )
  689. {
  690. unsigned i;
  691. for(i=0; i<dn; ++i)
  692. dbp[i] = VECT_OP_FUNC(MultSumVV)(vp,mp + (i*vn),vn);
  693. return dbp;
  694. }
  695. #endif
  696. #ifdef CM_VECTOP
  697. VECT_OP_TYPE* VECT_OP_FUNC(MultVMtV)( VECT_OP_TYPE* dbp, unsigned mcn, const VECT_OP_TYPE* mp, unsigned mrn, const VECT_OP_TYPE* vp )
  698. {
  699. VECT_OP_BLAS_FUNC(gemv)( CblasColMajor, CblasTrans, mrn, mcn, 1.0, mp, mrn, vp, 1, 0.0, dbp, 1 );
  700. return dbp;
  701. }
  702. #else
  703. VECT_OP_TYPE* VECT_OP_FUNC(MultVMtV)( VECT_OP_TYPE* dbp, unsigned mcn, const VECT_OP_TYPE* mp, unsigned mrn, const VECT_OP_TYPE* vp )
  704. {
  705. const VECT_OP_TYPE* dep = dbp + mcn;
  706. VECT_OP_TYPE* dp = dbp;
  707. const VECT_OP_TYPE* vep = vp + mrn;
  708. // for each dest element
  709. for(; dbp < dep; ++dbp )
  710. {
  711. const VECT_OP_TYPE* vbp = vp;
  712. *dbp = 0;
  713. // for each source vector row and src mtx col
  714. while( vbp < vep )
  715. *dbp += *mp++ * *vbp++;
  716. }
  717. return dp;
  718. }
  719. #endif
  720. VECT_OP_TYPE* VECT_OP_FUNC(MultDiagVMV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* mp, unsigned mcn, const VECT_OP_TYPE* vp )
  721. {
  722. VECT_OP_TYPE* rp = dbp;
  723. const VECT_OP_TYPE* mep = mp + (dn*mcn);
  724. // for each dest element
  725. for(; mp < mep; mp += dn+1 )
  726. *dbp++ = *vp++ * *mp;
  727. return rp;
  728. }
  729. /*
  730. Fortran Doc: http://www.netlib.org/blas/cgemm.f
  731. C Doc: http://techpubs.sgi.com/library/tpl/cgi-bin/getdoc.cgi?cmd=getdoc&coll=0650&db=man&fname=3%20INTRO_CBLAS
  732. C = alpha * op(A) * op(B) + beta * C
  733. cblas_Xgemm(
  734. order, enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102};
  735. transposeA, enum CBLAS_TRANSPOSE { CblasNoTrans, CblasTrans, CBlasConjTrans }
  736. transposeB,
  737. M, row op(A) and rows C (i.e. rows of A 'after' optional transpose)
  738. N, col op(B) and cols C (i.e. rows of B 'after' optional transpose)
  739. K, col op(A) and rows op(B)
  740. alpha, A scalar
  741. A, pointer to source matrix A
  742. lda, number of rows in A as it is stored in memory (assuming col major order)
  743. B, pointer to source matrix B
  744. ldb, number of rows in B as it is stored in memory (assuming col major order)
  745. beta C scalar
  746. C, pointer to destination matrix C
  747. ldc number of rows in C as it is stored in memory (assuming col major order)
  748. )
  749. */
  750. #ifdef CM_VECTOP
  751. VECT_OP_TYPE* VECT_OP_FUNC(MultMMM1)(VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, VECT_OP_TYPE alpha, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned n, VECT_OP_TYPE beta, unsigned flags )
  752. {
  753. bool t0fl = cmIsFlag(flags,kTransposeM0Fl);
  754. bool t1fl = cmIsFlag(flags,kTransposeM1Fl);
  755. VECT_OP_BLAS_FUNC(gemm)(
  756. CblasColMajor,
  757. t0fl ? CblasTrans : CblasNoTrans,
  758. t1fl ? CblasTrans : CblasNoTrans,
  759. drn, dcn, n,
  760. alpha,
  761. m0, t0fl ? n : drn,
  762. m1, t1fl ? dcn : n,
  763. beta,
  764. dbp, drn );
  765. return dbp;
  766. }
  767. #else
  768. // Not implemented.
  769. #endif
  770. #ifdef CM_VECTOP
  771. VECT_OP_TYPE* VECT_OP_FUNC(MultMMM2)(VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, VECT_OP_TYPE alpha, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned n, VECT_OP_TYPE beta, unsigned flags, unsigned dprn, unsigned m0prn, unsigned m1prn )
  772. {
  773. VECT_OP_BLAS_FUNC(gemm)(
  774. CblasColMajor,
  775. cmIsFlag(flags,kTransposeM0Fl) ? CblasTrans : CblasNoTrans,
  776. cmIsFlag(flags,kTransposeM1Fl) ? CblasTrans : CblasNoTrans,
  777. drn, dcn, n,
  778. alpha,
  779. m0, m0prn,
  780. m1, m1prn,
  781. beta,
  782. dbp, dprn );
  783. return dbp;
  784. }
  785. #else
  786. // Not implemented.
  787. #endif
  788. #ifdef CM_VECTOP
  789. VECT_OP_TYPE* VECT_OP_FUNC(MultMMM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned n )
  790. {
  791. VECT_OP_BLAS_FUNC(gemm)(
  792. CblasColMajor,
  793. CblasNoTrans, CblasNoTrans,
  794. drn, dcn, n,
  795. 1.0, m0, drn,
  796. m1, n,
  797. 0.0, dbp, drn );
  798. return dbp;
  799. }
  800. #else
  801. VECT_OP_TYPE* VECT_OP_FUNC(MultMMM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned m0cn_m1rn )
  802. {
  803. unsigned i;
  804. for(i=0; i<dcn; ++i)
  805. VECT_OP_FUNC(MultVMV)(dbp+(i*drn),drn,m0,m0cn_m1rn,m1+(i*m0cn_m1rn));
  806. return dbp;
  807. }
  808. #endif
  809. #ifdef CM_VECTOP
  810. VECT_OP_TYPE* VECT_OP_FUNC(MultMMMt)(VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned m0cn_m1rn )
  811. {
  812. VECT_OP_BLAS_FUNC(gemm)( CblasColMajor, CblasNoTrans, CblasTrans,
  813. drn, dcn, m0cn_m1rn,
  814. 1.0, m0, drn,
  815. m1, dcn,
  816. 0.0, dbp, drn );
  817. return dbp;
  818. }
  819. #else
  820. VECT_OP_TYPE* VECT_OP_FUNC(MultMMMt)(VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned m0cn_m1rn )
  821. {
  822. unsigned i,j,k;
  823. VECT_OP_FUNC(Zero)(dbp,drn*dcn);
  824. for(i=0; i<dcn; ++i)
  825. for(j=0; j<drn; ++j)
  826. for(k=0; k<m0cn_m1rn; ++k)
  827. dbp[ i*drn + j ] += m0[ k*drn + j ] * m1[ k*dcn + i ];
  828. return dbp;
  829. }
  830. #endif
  831. VECT_OP_TYPE* VECT_OP_FUNC(PowVS)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE expo )
  832. {
  833. VECT_OP_TYPE* dp = dbp;
  834. VECT_OP_TYPE* ep = dp + dn;
  835. for(; dp < ep; ++dp )
  836. *dp = (VECT_OP_TYPE)pow(*dp,expo);
  837. return dbp;
  838. }
  839. VECT_OP_TYPE* VECT_OP_FUNC(PowVVS)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp, VECT_OP_TYPE expo )
  840. {
  841. VECT_OP_TYPE* dp = dbp;
  842. VECT_OP_TYPE* ep = dp + dn;
  843. for(; dp < ep; ++dp,++sp )
  844. *dp = (VECT_OP_TYPE)pow(*sp,expo);
  845. return dbp;
  846. }
  847. VECT_OP_TYPE* VECT_OP_FUNC(LogV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp )
  848. {
  849. VECT_OP_TYPE* dp = dbp;
  850. VECT_OP_TYPE* ep = dp + dn;
  851. for(; dp <ep; ++dp,++sbp)
  852. *dp = (VECT_OP_TYPE)log(*sbp);
  853. return dbp;
  854. }
  855. VECT_OP_TYPE* VECT_OP_FUNC(AmplToDbVV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, VECT_OP_TYPE minDb )
  856. {
  857. VECT_OP_TYPE minVal = pow(10.0,minDb/20.0);
  858. VECT_OP_TYPE* dp = dbp;
  859. VECT_OP_TYPE* ep = dp + dn;
  860. for(; dp<ep; ++dp,++sbp)
  861. *dp = *sbp<minVal ? minDb : 20.0 * log10(*sbp);
  862. return dbp;
  863. }
  864. VECT_OP_TYPE* VECT_OP_FUNC(DbToAmplVV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp)
  865. {
  866. VECT_OP_TYPE* dp = dbp;
  867. VECT_OP_TYPE* ep = dp + dn;
  868. for(; dp<ep; ++dp,++sbp)
  869. *dp = pow(10.0,*sbp/20.0);
  870. return dbp;
  871. }
  872. VECT_OP_TYPE* VECT_OP_FUNC(PowToDbVV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, VECT_OP_TYPE minDb )
  873. {
  874. VECT_OP_TYPE minVal = pow(10.0,minDb/10.0);
  875. VECT_OP_TYPE* dp = dbp;
  876. VECT_OP_TYPE* ep = dp + dn;
  877. for(; dp<ep; ++dp,++sbp)
  878. *dp = *sbp<minVal ? minDb : 10.0 * log10(*sbp);
  879. return dbp;
  880. }
  881. VECT_OP_TYPE* VECT_OP_FUNC(DbToPowVV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp)
  882. {
  883. VECT_OP_TYPE* dp = dbp;
  884. VECT_OP_TYPE* ep = dp + dn;
  885. for(; dp<ep; ++dp,++sbp)
  886. *dp = pow(10.0,*sbp/10.0);
  887. return dbp;
  888. }
  889. VECT_OP_TYPE* VECT_OP_FUNC(RandSymPosDef)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE* t )
  890. {
  891. unsigned i,j;
  892. bool fl = t == NULL;
  893. if( fl )
  894. t = cmMemAlloc( VECT_OP_TYPE , dn*dn );
  895. do
  896. {
  897. // intialize t[] as a square symetric matrix with random values
  898. for(i=0; i<dn; ++i)
  899. for(j=i; j<dn; ++j)
  900. {
  901. VECT_OP_TYPE v = (VECT_OP_TYPE)rand()/RAND_MAX;
  902. t[ (i*dn) + j ] = v;
  903. if( i != j )
  904. t[ (j*dn) + i ] = v;
  905. }
  906. // square t[] to force the eigenvalues to be positive
  907. VECT_OP_FUNC(MultMMM)(dbp,dn,dn,t,t,dn);
  908. VECT_OP_FUNC(Copy)(t,dn*dn,dbp);
  909. // test that func is positive definite
  910. }while( VECT_OP_FUNC(Chol)(t,dn)==NULL );
  911. if( fl )
  912. cmMemFree(t);
  913. return dbp;
  914. }
  915. // Calculate the determinant of a matrix previously factored by
  916. // the lapack function dgetrf_()
  917. VECT_OP_TYPE VECT_OP_FUNC(LUDet)( const VECT_OP_TYPE* lu, const int_lap_t* ipiv, int rn )
  918. {
  919. VECT_OP_TYPE det1 = 1;
  920. int det2 = 0;
  921. int i;
  922. for(i=0; i<rn; ++i)
  923. {
  924. if( ipiv != NULL && ipiv[i] != (i+1) )
  925. det1 = -det1;
  926. det1 = lu[ (i*rn) + i ] * det1;
  927. if( det1 == 0 )
  928. break;
  929. while( fabs(det1) <= 1 )
  930. {
  931. det1 *= 10;
  932. det2 -= 1;
  933. }
  934. //continue;
  935. while( fabs(det1) >= 10 )
  936. {
  937. det1 /= 10;
  938. det2 += 1;
  939. }
  940. }
  941. // Here's where underflow or overflow might happen.
  942. // Enable floating point exception handling to trap.
  943. det1 *= pow(10.0,det2);
  944. return det1;
  945. }
  946. // take the inverse of a matrix factored via lapack dgetrf_()
  947. VECT_OP_TYPE* VECT_OP_FUNC(LUInverse)(VECT_OP_TYPE* dp, int_lap_t* ipiv, int drn )
  948. {
  949. int_lap_t ispec = 1;
  950. int_lap_t rn = drn;
  951. int_lap_t n1 = drn;
  952. int_lap_t n2 = drn;
  953. int_lap_t n3 = drn;
  954. int_lap_t n4 = drn;
  955. char funcNameStr[] = {"DGETRI"};
  956. // Calculate the NB factor for LWORK -
  957. // The two args are length of string args 'funcNameStr' and ' '.
  958. // It is not clear how many 'n' args are requred so all are passed set to 'drn'
  959. int nb = ilaenv_(&ispec, funcNameStr, " ", &n1,&n2,&n3,&n4, strlen(funcNameStr), 1 );
  960. VECT_OP_TYPE w[drn * nb]; // allocate working memory
  961. int_lap_t info;
  962. // calculate inv(A) base on LU factorization
  963. VECT_OP_LAP_FUNC(getri_)(&rn,dp,&rn,ipiv,w,&rn,&info);
  964. assert(info==0);
  965. return info ==0 ? dp : NULL;
  966. }
  967. VECT_OP_TYPE VECT_OP_FUNC(DetM)( const VECT_OP_TYPE* sp, unsigned srn )
  968. {
  969. int_lap_t arn = srn;
  970. VECT_OP_TYPE A[ arn * arn ];
  971. int_lap_t ipiv[ arn ];
  972. int_lap_t info;
  973. VECT_OP_FUNC(Copy)(A,arn*arn,sp);
  974. // PLU factor
  975. VECT_OP_LAP_FUNC(getrf_)(&arn,&arn,A,&arn,ipiv,&info);
  976. if( info == 0 )
  977. return VECT_OP_FUNC(LUDet)(A,ipiv,arn);
  978. return 0;
  979. }
  980. VECT_OP_TYPE VECT_OP_FUNC(DetDiagM)( const VECT_OP_TYPE* sp, unsigned srn )
  981. { return VECT_OP_FUNC(LUDet)(sp,NULL,srn); }
  982. VECT_OP_TYPE VECT_OP_FUNC(LogDetM)( const VECT_OP_TYPE* sp, unsigned srn )
  983. {
  984. cmReal_t det = 0;
  985. unsigned ne2 = srn * srn;
  986. VECT_OP_TYPE U[ne2];
  987. const VECT_OP_TYPE* up = U;
  988. const VECT_OP_TYPE* ep = up + ne2;
  989. VECT_OP_FUNC(Copy)(U,ne2,sp);
  990. VECT_OP_FUNC(Chol)(U,srn);
  991. for(; up<ep; up += (srn+1) )
  992. det += log(*up);
  993. return 2*det;
  994. }
  995. VECT_OP_TYPE VECT_OP_FUNC(LogDetDiagM)( const VECT_OP_TYPE* sp, unsigned srn )
  996. { return log(VECT_OP_FUNC(DetDiagM)(sp,srn)); }
  997. VECT_OP_TYPE* VECT_OP_FUNC(InvM)( VECT_OP_TYPE* dp, unsigned drn )
  998. {
  999. int_lap_t rn = drn;
  1000. int_lap_t ipiv[ rn ];
  1001. int_lap_t info;
  1002. // PLU factor
  1003. VECT_OP_LAP_FUNC(getrf_)(&rn,&rn,dp,&rn,ipiv,&info);
  1004. if( info == 0 )
  1005. return VECT_OP_FUNC(LUInverse)(dp,ipiv,rn );
  1006. return NULL;
  1007. }
  1008. VECT_OP_TYPE* VECT_OP_FUNC(InvDiagM)( VECT_OP_TYPE* dp, unsigned drn )
  1009. {
  1010. const VECT_OP_TYPE* dep = dp + (drn*drn);
  1011. VECT_OP_TYPE* rp = dp;
  1012. for(; dp < dep; dp += drn+1 )
  1013. {
  1014. *dp = 1.0 / *dp;
  1015. // if any element on the diagonal is zero then the
  1016. // determinant is zero and the matrix is not invertable
  1017. if( *dp == 0 )
  1018. break;
  1019. }
  1020. return dp < dep ? NULL : rp;
  1021. }
  1022. VECT_OP_TYPE* VECT_OP_FUNC(SolveLS)( VECT_OP_TYPE* A, unsigned an, VECT_OP_TYPE* B, unsigned bcn )
  1023. {
  1024. int_lap_t aN = an;
  1025. int_lap_t bcN = bcn;
  1026. int_lap_t ipiv[ an ];
  1027. int_lap_t info = 0;
  1028. VECT_OP_LAP_FUNC(gesv_)(&aN,&bcN,(VECT_OP_TYPE*)A,&aN,ipiv,B,&aN,&info);
  1029. return info == 0 ? B : NULL;
  1030. }
  1031. VECT_OP_TYPE* VECT_OP_FUNC(Chol)(VECT_OP_TYPE* A, unsigned an )
  1032. {
  1033. char uplo = 'U';
  1034. int_lap_t N = an;
  1035. int_lap_t lda = an;
  1036. int_lap_t info = 0;
  1037. VECT_OP_LAP_FUNC(potrf_(&uplo,&N,(VECT_OP_TYPE*)A,&lda,&info));
  1038. return info == 0 ? A : NULL;
  1039. }
  1040. VECT_OP_TYPE* VECT_OP_FUNC(CholZ)(VECT_OP_TYPE* A, unsigned an )
  1041. {
  1042. unsigned i,j;
  1043. VECT_OP_FUNC(Chol)(A,an);
  1044. // zero the lower triangle of A
  1045. for(i=0; i<an; ++i)
  1046. for(j=i+1; j<an; ++j)
  1047. A[ (i*an) + j ] = 0;
  1048. return A;
  1049. }
  1050. VECT_OP_TYPE VECT_OP_FUNC(FracAvg)( double bi, double ei, const VECT_OP_TYPE* sbp, unsigned sn )
  1051. {
  1052. unsigned bii = cmMax(0,cmMin(sn-1,(unsigned)ceil(bi)));
  1053. unsigned eii = cmMax(0,cmMin(sn,(unsigned)floor(ei)+1));
  1054. double begW = bii - bi;
  1055. double endW = eii - floor(ei);
  1056. double cnt = eii - bii;
  1057. double sum = (double)VECT_OP_FUNC(Sum)(sbp+bii,eii-bii);
  1058. if( begW>0 && bii > 0 )
  1059. {
  1060. cnt += begW;
  1061. sum += begW * sbp[ bii-1 ];
  1062. }
  1063. if( endW>0 && eii+1 < sn )
  1064. {
  1065. cnt += endW;
  1066. sum += endW * sbp[ eii+1 ];
  1067. }
  1068. return (VECT_OP_TYPE)(sum / cnt);
  1069. }
  1070. VECT_OP_TYPE* VECT_OP_FUNC(DownSampleAvg)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, unsigned sn )
  1071. {
  1072. const VECT_OP_TYPE* dep = dbp + dn;
  1073. VECT_OP_TYPE* rp = dbp;
  1074. unsigned i = 0;
  1075. double fact = (double)sn / dn;
  1076. assert( sn >= dn );
  1077. for(i=0; dbp < dep; ++i )
  1078. *dbp++ = VECT_OP_FUNC(FracAvg)( fact*i, fact*(i+1), sbp, sn );
  1079. return rp;
  1080. }
  1081. VECT_OP_TYPE* VECT_OP_FUNC(UpSampleInterp)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, unsigned sn )
  1082. {
  1083. const VECT_OP_TYPE* dep = dbp + dn;
  1084. const VECT_OP_TYPE* sep = sbp + sn;
  1085. VECT_OP_TYPE* rp = dbp;
  1086. double fact = (double)sn / dn;
  1087. double phs = 0;
  1088. assert( sn <= dn );
  1089. while( dbp<dep )
  1090. {
  1091. if( sbp < sep )
  1092. *dbp++ = (VECT_OP_TYPE)((*sbp) + (phs * ((*(sbp+1)) - (*sbp))));
  1093. else
  1094. *dbp++ = (*(sep-1));
  1095. phs += fact;
  1096. while( phs > 1.0 )
  1097. {
  1098. phs -= 1.0;
  1099. sbp++;
  1100. }
  1101. }
  1102. return rp;
  1103. }
  1104. VECT_OP_TYPE* VECT_OP_FUNC(FitToSize)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, unsigned sn )
  1105. {
  1106. if( dn == sn )
  1107. return VECT_OP_FUNC(Copy)(dbp,dn,sbp);
  1108. if( dn < sn )
  1109. return VECT_OP_FUNC(DownSampleAvg)(dbp,dn,sbp,sn);
  1110. return VECT_OP_FUNC(UpSampleInterp)(dbp,dn,sbp,sn);
  1111. }
  1112. VECT_OP_TYPE* VECT_OP_FUNC(LinearMap)(VECT_OP_TYPE* dV, unsigned dn, VECT_OP_TYPE* sV, unsigned sn )
  1113. {
  1114. if( dn == sn )
  1115. {
  1116. memcpy(dV,sV,dn*sizeof(VECT_OP_TYPE));
  1117. return dV;
  1118. }
  1119. unsigned i,j,k;
  1120. // if stretching
  1121. if( dn > sn )
  1122. {
  1123. VECT_OP_TYPE f_n = (VECT_OP_TYPE)dn/sn;
  1124. VECT_OP_TYPE f_nn = f_n;
  1125. unsigned i_n = floor(f_n);
  1126. k = 0;
  1127. i = 0;
  1128. // for each set of ceiling(dn/sn) dst values
  1129. while(1)
  1130. {
  1131. // repeat floor(dn/sn) src val into dst
  1132. for(j=0; j<i_n; ++j,++i)
  1133. dV[i] = sV[k];
  1134. if( k + 1 == sn )
  1135. break;
  1136. // interpolate between the cur and nxt source value
  1137. VECT_OP_TYPE w = f_nn - floor(f_nn);
  1138. dV[i] = sV[k] + w * (sV[k+1]-sV[k]);
  1139. ++i;
  1140. ++k;
  1141. i_n = floor(f_n - (1.0-w));
  1142. f_nn += f_n;
  1143. }
  1144. }
  1145. else // if shrinking
  1146. {
  1147. VECT_OP_TYPE f_n = (VECT_OP_TYPE)sn/dn;
  1148. VECT_OP_TYPE f_nn = f_n;
  1149. unsigned i_n = floor(f_n);
  1150. k = 0;
  1151. i = 0;
  1152. VECT_OP_TYPE acc = 0;
  1153. // for each seq of ceil(sn/dn) src values
  1154. while(1)
  1155. {
  1156. // accum first floor(sn/dn) src values
  1157. for(j=0; j<i_n; ++j,++i)
  1158. acc += sV[i];
  1159. if( k == dn-1 )
  1160. {
  1161. dV[k] = acc/f_n;
  1162. break;
  1163. }
  1164. // interpolate frac of last src value
  1165. VECT_OP_TYPE w = f_nn - floor(f_nn);
  1166. // form avg
  1167. dV[k] = (acc + (w*sV[i]))/f_n;
  1168. // reload acc with inverse frac of src value
  1169. acc = (1.0-w) * sV[i];
  1170. ++i;
  1171. ++k;
  1172. i_n = floor(f_n-(1.0-w));
  1173. f_nn += f_n;
  1174. }
  1175. }
  1176. return dV;
  1177. }
  1178. VECT_OP_TYPE* VECT_OP_FUNC(Random)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE minVal, VECT_OP_TYPE maxVal )
  1179. {
  1180. const VECT_OP_TYPE* dep = dbp + dn;
  1181. VECT_OP_TYPE* dp =dbp;
  1182. double fact = (maxVal - minVal)/RAND_MAX;
  1183. while( dbp < dep )
  1184. *dbp++ = fact * rand() + minVal;
  1185. return dp;
  1186. }
  1187. unsigned* VECT_OP_FUNC(WeightedRandInt)( unsigned *dbp, unsigned dn, const VECT_OP_TYPE* wp, unsigned wn )
  1188. {
  1189. unsigned i,j;
  1190. VECT_OP_TYPE a[ wn ];
  1191. // form bin boundaries by taking a cum. sum of the weight values.
  1192. VECT_OP_FUNC(CumSum)(a,wn,wp);
  1193. for(j=0; j<dn; ++j)
  1194. {
  1195. // gen a random number from a uniform distribution betwen 0 and the max value from the cumsum.
  1196. VECT_OP_TYPE rv = (VECT_OP_TYPE)rand() * a[wn-1] / RAND_MAX;
  1197. // find the bin the rv falls into
  1198. for(i=0; i<wn-1; ++i)
  1199. if( rv <= a[i] )
  1200. {
  1201. dbp[j] = i;
  1202. break;
  1203. }
  1204. if(i==wn-1)
  1205. dbp[j]= wn-1;
  1206. }
  1207. return dbp;
  1208. }
  1209. VECT_OP_TYPE* VECT_OP_FUNC(RandomGauss)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE mean, VECT_OP_TYPE var )
  1210. {
  1211. const VECT_OP_TYPE* dep = dbp + dn;
  1212. VECT_OP_TYPE* rp = dbp;
  1213. // The code below implements the Box-Muller uniform to
  1214. // Gaussian distribution transformation. In rectangular
  1215. // coordinates this transform is defined as:
  1216. // y1 = sqrt( - 2.0 * log(x1) ) * cos( 2.0*M_PI*x2 )
  1217. // y2 = sqrt( - 2.0 * log(x1) ) * sin( 2.0*M_PI*x2 )
  1218. //
  1219. while( dbp < dep )
  1220. *dbp++ = sqrt( -2.0 * log((VECT_OP_TYPE)rand()/RAND_MAX)) * cos(2.0*M_PI*((VECT_OP_TYPE)rand()/RAND_MAX)) * var + mean;
  1221. return rp;
  1222. }
  1223. VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* varV )
  1224. {
  1225. VECT_OP_TYPE* rp = dbp;
  1226. const VECT_OP_TYPE* dep = dbp + dn;
  1227. while( dbp < dep )
  1228. VECT_OP_FUNC(RandomGauss)( dbp++, 1, *meanV++, *varV++ );
  1229. return rp;
  1230. }
  1231. VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussM)( VECT_OP_TYPE* dbp, unsigned rn, unsigned cn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* varV )
  1232. {
  1233. unsigned i;
  1234. for(i=0; i<cn; ++i)
  1235. VECT_OP_FUNC(RandomGaussV)( dbp+(i*rn), rn, meanV, varV );
  1236. return dbp;
  1237. }
  1238. VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussDiagM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* covarM )
  1239. {
  1240. unsigned i,j;
  1241. for(i=0; i<dcn; ++i)
  1242. for(j=0; j<drn; ++j)
  1243. VECT_OP_FUNC(RandomGauss)(dbp + (i*drn)+j, 1, meanV[j], covarM[ (j*drn) + j]);
  1244. return dbp;
  1245. }
  1246. VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussNonDiagM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* covarM, VECT_OP_TYPE* t )
  1247. {
  1248. bool fl = t == NULL;
  1249. if( fl )
  1250. t = cmMemAlloc(VECT_OP_TYPE, drn * drn );
  1251. VECT_OP_FUNC(Copy)(t,drn*drn,covarM);
  1252. if( VECT_OP_FUNC(CholZ)(t,drn) == NULL )
  1253. {
  1254. // Cholesky decomposition failed - should try eigen analysis next
  1255. // From octave mvnrnd.m
  1256. // [E,Lambda]=eig(Sigma);
  1257. // if (min(diag(Lambda))<0),error('Sigma must be positive semi-definite.'),end
  1258. // U = sqrt(Lambda)*E';
  1259. assert(0);
  1260. }
  1261. /*
  1262. unsigned i,j;
  1263. for(i=0; i<drn; ++i)
  1264. {
  1265. for(j=0; j<drn; ++j)
  1266. printf("%f ",t[ (j*drn) + i]);
  1267. printf("\n");
  1268. }
  1269. */
  1270. VECT_OP_FUNC(RandomGaussNonDiagM2)(dbp,drn,dcn,meanV,t);
  1271. if(fl)
  1272. cmMemFree(t);
  1273. return dbp;
  1274. }
  1275. VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussNonDiagM2)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* uM )
  1276. {
  1277. unsigned i;
  1278. for(i=0; i<dcn; ++i)
  1279. {
  1280. VECT_OP_TYPE r[ drn ];
  1281. VECT_OP_FUNC(RandomGauss)(r,drn,0,1); // r = randn(drn,1);
  1282. VECT_OP_FUNC(MultVVM)( dbp+(i*drn),drn,r,drn,uM); // dbp[:i] = r * uM;
  1283. VECT_OP_FUNC(AddVV)( dbp+(i*drn),drn,meanV); // dbp[:,i] += meanV;
  1284. }
  1285. return dbp;
  1286. }
  1287. VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussMM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* meanM, const VECT_OP_TYPE* varM, unsigned K )
  1288. {
  1289. unsigned k;
  1290. unsigned D = drn;
  1291. unsigned N = dcn/K;
  1292. for(k=0; k<K; ++k)
  1293. VECT_OP_FUNC(RandomGaussM)( dbp + (k*N*D), drn, N, meanM + (k*D), varM + (k*D) );
  1294. return dbp;
  1295. }
  1296. VECT_OP_TYPE* VECT_OP_FUNC(CircleCoords)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE x, VECT_OP_TYPE y, VECT_OP_TYPE varX, VECT_OP_TYPE varY )
  1297. {
  1298. unsigned i;
  1299. for(i=0; i<dn; ++i)
  1300. {
  1301. double a = 2.0*M_PI*i/(dn-1);
  1302. dbp[ i ] = (VECT_OP_TYPE)(varX * cos(a) + x);
  1303. dbp[ i+dn ] = (VECT_OP_TYPE)(varY * sin(a) + y);
  1304. }
  1305. return dbp;
  1306. }
  1307. unsigned VECT_OP_FUNC(SynthSine)( VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz )
  1308. {
  1309. const VECT_OP_TYPE* dep = dbp + dn;
  1310. double rps = 2.0*M_PI*hz/srate;
  1311. while( dbp < dep )
  1312. *dbp++ = (VECT_OP_TYPE)sin( rps * phase++ );
  1313. return phase;
  1314. }
  1315. unsigned VECT_OP_FUNC(SynthCosine)( VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz )
  1316. {
  1317. const VECT_OP_TYPE* dep = dbp + dn;
  1318. double rps = 2.0*M_PI*hz/srate;
  1319. while( dbp < dep )
  1320. *dbp++ = (VECT_OP_TYPE)cos( rps * phase++ );
  1321. return phase;
  1322. }
  1323. unsigned VECT_OP_FUNC(SynthSquare)( VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz, unsigned otCnt )
  1324. {
  1325. const VECT_OP_TYPE* dep = dbp + dn;
  1326. if( otCnt > 0 )
  1327. {
  1328. unsigned i;
  1329. // initialize the buffer with the fundamental
  1330. VECT_OP_FUNC(SynthSine)( dbp, dn, phase, srate, hz );
  1331. otCnt *= 2;
  1332. // sum in each additional harmonic
  1333. for(i=3; i<otCnt; i+=2)
  1334. {
  1335. VECT_OP_TYPE* dp = dbp;
  1336. double rps = 2.0 * M_PI * i * hz / srate;
  1337. unsigned phs = phase;
  1338. double g = 1.0/i;
  1339. while( dp < dep )
  1340. *dp++ += (VECT_OP_TYPE)(g * sin( rps * phs++ ));
  1341. }
  1342. }
  1343. return phase + (dep - dbp);
  1344. }
  1345. unsigned VECT_OP_FUNC(SynthTriangle)( VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz, unsigned otCnt )
  1346. {
  1347. const VECT_OP_TYPE* dep = dbp + dn;
  1348. if( otCnt > 0 )
  1349. {
  1350. unsigned i;
  1351. // initialize the buffer with the fundamental
  1352. VECT_OP_FUNC(SynthCosine)( dbp, dn, phase, srate, hz );
  1353. otCnt *= 2;
  1354. // sum in each additional harmonic
  1355. for(i=3; i<otCnt; i+=2)
  1356. {
  1357. VECT_OP_TYPE* dp = dbp;
  1358. double rps = 2.0 * M_PI * i * hz / srate;
  1359. unsigned phs = phase;
  1360. double g = 1.0/(i*i);
  1361. while( dp < dep )
  1362. *dp++ += (VECT_OP_TYPE)(g * cos( rps * phs++ ));
  1363. }
  1364. }
  1365. return phase + (dep - dbp);
  1366. }
  1367. unsigned VECT_OP_FUNC(SynthSawtooth)( VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz, unsigned otCnt )
  1368. {
  1369. const VECT_OP_TYPE* dep = dbp + dn;
  1370. if( otCnt > 0 )
  1371. {
  1372. unsigned i;
  1373. // initialize the buffer with the fundamental
  1374. VECT_OP_FUNC(SynthSine)( dbp, dn, phase, srate, hz );
  1375. // sum in each additional harmonic
  1376. for(i=2; i<otCnt; ++i)
  1377. {
  1378. VECT_OP_TYPE* dp = dbp;
  1379. double rps = 2.0 * M_PI * i * hz / srate;
  1380. unsigned phs = phase;
  1381. double g = 1.0/i;
  1382. while( dp < dep )
  1383. *dp++ += (VECT_OP_TYPE)(g * sin( rps * phs++ ));
  1384. }
  1385. VECT_OP_FUNC(MultVS)(dbp,dn,2.0/M_PI);
  1386. }
  1387. return phase + (dep - dbp);
  1388. }
  1389. unsigned VECT_OP_FUNC(SynthPulseCos)( VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz, unsigned otCnt )
  1390. {
  1391. const VECT_OP_TYPE* dep = dbp + dn;
  1392. if( otCnt > 0 )
  1393. {
  1394. unsigned i;
  1395. // initialize the buffer with the fundamental
  1396. VECT_OP_FUNC(SynthCosine)( dbp, dn, phase, srate, hz );
  1397. // sum in each additional harmonic
  1398. for(i=1; i<otCnt; ++i)
  1399. {
  1400. VECT_OP_TYPE* dp = dbp;
  1401. double rps = 2.0 * M_PI * i * hz / srate;
  1402. unsigned phs = phase;
  1403. while( dp < dep )
  1404. *dp++ += (VECT_OP_TYPE)cos( rps * phs++ );
  1405. }
  1406. VECT_OP_FUNC(MultVS)(dbp,dn,1.0/otCnt);
  1407. }
  1408. return phase + (dep - dbp);
  1409. }
  1410. unsigned VECT_OP_FUNC(SynthImpulse)( VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz )
  1411. {
  1412. const VECT_OP_TYPE* dep = dbp + dn;
  1413. double pi2 = 2.0*M_PI;
  1414. double rps = pi2*hz/srate;
  1415. double v0,v1 = fmod( rps * phase, pi2);
  1416. if( dbp == dep )
  1417. return phase;
  1418. // the phase is set to zero when the first output should be a 1
  1419. if( phase == 0 )
  1420. {
  1421. *dbp++ = 1;
  1422. ++phase;
  1423. }
  1424. while( dbp < dep )
  1425. {
  1426. // the phase vector will always be increasing
  1427. // the modulus of the phase vector will wrap with frequency 'hz'
  1428. v0 = fmod( rps * phase++, pi2 );
  1429. // notice when wrapping occurs
  1430. *dbp++ = (VECT_OP_TYPE)(v0 < v1);
  1431. v1 = v0;
  1432. }
  1433. // check if the next output should be a 1
  1434. // (this eliminates the problem of not having access to v1 on the next call to this function
  1435. if( fmod( rps * phase, pi2 ) < v1 )
  1436. phase = 0;
  1437. return phase;
  1438. }
  1439. VECT_OP_TYPE VECT_OP_FUNC(SynthPinkNoise)( VECT_OP_TYPE* dbp, unsigned n, VECT_OP_TYPE delaySmp )
  1440. {
  1441. const VECT_OP_TYPE* dep = dbp + n;
  1442. VECT_OP_TYPE tmp[ n ];
  1443. VECT_OP_FUNC(Random)(tmp,n,-1.0,1.0);
  1444. VECT_OP_TYPE* sp = tmp;
  1445. VECT_OP_TYPE reg = delaySmp;
  1446. for(; dbp < dep; ++sp)
  1447. {
  1448. *dbp++ = (*sp + reg)/2.0;
  1449. reg = *sp;
  1450. }
  1451. return *sp;
  1452. }
  1453. VECT_OP_TYPE* VECT_OP_FUNC(LinSpace)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE base, VECT_OP_TYPE limit )
  1454. {
  1455. unsigned i = 0;
  1456. for(; i<dn; ++i)
  1457. dbp[i] = base + i*(limit-base)/(dn-1);
  1458. return dbp;
  1459. }
  1460. VECT_OP_TYPE* VECT_OP_FUNC(LinearToDb)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp, VECT_OP_TYPE mult )
  1461. {
  1462. const VECT_OP_TYPE* dep = dbp + dn;
  1463. VECT_OP_TYPE* rp = dbp;
  1464. while( dbp < dep )
  1465. *dbp++ = (VECT_OP_TYPE)(mult * log10( VECT_OP_EPSILON + *sp++ ));
  1466. return rp;
  1467. }
  1468. VECT_OP_TYPE* VECT_OP_FUNC(dBToLinear)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp, VECT_OP_TYPE mult )
  1469. {
  1470. const VECT_OP_TYPE* dep = dbp + dn;
  1471. VECT_OP_TYPE* rp = dbp;
  1472. while( dbp < dep )
  1473. *dbp++ = (VECT_OP_TYPE)pow(10.0, *sp++ / mult );
  1474. return rp;
  1475. }
  1476. VECT_OP_TYPE* VECT_OP_FUNC(AmplitudeToDb)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
  1477. { return VECT_OP_FUNC(LinearToDb)(dbp,dn,sp,20.0); }
  1478. VECT_OP_TYPE* VECT_OP_FUNC(PowerToDb)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
  1479. { return VECT_OP_FUNC(LinearToDb)(dbp,dn,sp,10.0); }
  1480. VECT_OP_TYPE* VECT_OP_FUNC(dBToAmplitude)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
  1481. { return VECT_OP_FUNC(dBToLinear)( dbp,dn,sp,20); }
  1482. VECT_OP_TYPE* VECT_OP_FUNC(dBToPower)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
  1483. { return VECT_OP_FUNC(dBToLinear)( dbp,dn,sp,10); }
  1484. unsigned VECT_OP_FUNC(SynthPhasor)(VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz )
  1485. {
  1486. const VECT_OP_TYPE* dep = dbp + dn;
  1487. while( dbp < dep )
  1488. *dbp++ = (VECT_OP_TYPE)fmod( (hz * phase++)/srate, 1.0 );
  1489. return phase;
  1490. }
  1491. VECT_OP_TYPE VECT_OP_FUNC(KaiserBetaFromSidelobeReject)( double sidelobeRejectDb )
  1492. {
  1493. double beta;
  1494. if( sidelobeRejectDb < 13.26 )
  1495. sidelobeRejectDb = 13.26;
  1496. else
  1497. if( sidelobeRejectDb > 120.0)
  1498. sidelobeRejectDb = 120.0;
  1499. if( sidelobeRejectDb < 60.0 )
  1500. beta = (0.76609 * pow(sidelobeRejectDb - 13.26,0.4)) + (0.09834*(sidelobeRejectDb-13.26));
  1501. else
  1502. beta = 0.12438 * (sidelobeRejectDb + 6.3);
  1503. return (VECT_OP_TYPE)beta;
  1504. }
  1505. VECT_OP_TYPE VECT_OP_FUNC(KaiserFreqResolutionFactor)( double sidelobeRejectDb )
  1506. { return (6.0 * (sidelobeRejectDb + 12.0))/155.0; }
  1507. VECT_OP_TYPE* VECT_OP_FUNC(Kaiser)( VECT_OP_TYPE* dbp, unsigned n, double beta )
  1508. {
  1509. bool zeroFl = false;
  1510. int M = 0;
  1511. double den = cmBessel0(beta); // wnd func denominator
  1512. int cnt = n;
  1513. int i;
  1514. assert( n >= 3 );
  1515. // force ele cnt to be odd
  1516. if( cmIsEvenU(cnt) )
  1517. {
  1518. cnt--;
  1519. zeroFl = true;
  1520. }
  1521. // at this point cnt is odd and >= 3
  1522. // calc half the window length
  1523. M = (int)((cnt - 1.0)/2.0);
  1524. double Msqrd = M*M;
  1525. for(i=0; i<cnt; i++)
  1526. {
  1527. double v0 = (double)(i - M);
  1528. double num = cmBessel0(beta * sqrt(1.0 - ((v0*v0)/Msqrd)));
  1529. dbp[i] = (VECT_OP_TYPE)(num/den);
  1530. }
  1531. if( zeroFl )
  1532. dbp[cnt] = 0.0; // zero the extra element in the output array
  1533. return dbp;
  1534. }
  1535. VECT_OP_TYPE* VECT_OP_FUNC(Gaussian)( VECT_OP_TYPE* dbp, unsigned dn, double mean, double variance )
  1536. {
  1537. int M = dn-1;
  1538. double sqrt2pi = sqrt(2.0*M_PI);
  1539. unsigned i;
  1540. for(i=0; i<dn; i++)
  1541. {
  1542. double arg = ((((double)i/M) - 0.5) * M);
  1543. arg = pow( (double)(arg-mean), 2.0);
  1544. arg = exp( -arg / (2.0*variance));
  1545. dbp[i] = (VECT_OP_TYPE)(arg / (sqrt(variance) * sqrt2pi));
  1546. }
  1547. return dbp;
  1548. }
  1549. VECT_OP_TYPE* VECT_OP_FUNC(Hamming)( VECT_OP_TYPE* dbp, unsigned dn )
  1550. {
  1551. const VECT_OP_TYPE* dep = dbp + dn;
  1552. VECT_OP_TYPE* dp = dbp;
  1553. double fact = 2.0 * M_PI / (dn-1);
  1554. unsigned i;
  1555. for(i=0; dbp < dep; ++i )
  1556. *dbp++ = (VECT_OP_TYPE)(.54 - (.46 * cos(fact*i)));
  1557. return dp;
  1558. }
  1559. VECT_OP_TYPE* VECT_OP_FUNC(Hann)( VECT_OP_TYPE* dbp, unsigned dn )
  1560. {
  1561. const VECT_OP_TYPE* dep = dbp + dn;
  1562. VECT_OP_TYPE* dp = dbp;
  1563. double fact = 2.0 * M_PI / (dn-1);
  1564. unsigned i;
  1565. for(i=0; dbp < dep; ++i )
  1566. *dbp++ = (VECT_OP_TYPE)(.5 - (.5 * cos(fact*i)));
  1567. return dp;
  1568. }
  1569. VECT_OP_TYPE* VECT_OP_FUNC(HannMatlab)( VECT_OP_TYPE* dbp, unsigned dn )
  1570. {
  1571. const VECT_OP_TYPE* dep = dbp + dn;
  1572. VECT_OP_TYPE* dp = dbp;
  1573. double fact = 2.0 * M_PI / (dn+1);
  1574. unsigned i;
  1575. for(i=0; dbp < dep; ++i )
  1576. *dbp++ = (VECT_OP_TYPE)(0.5*(1.0-cos(fact*(i+1))));
  1577. return dp;
  1578. }
  1579. VECT_OP_TYPE* VECT_OP_FUNC(Triangle)( VECT_OP_TYPE* dbp, unsigned dn )
  1580. {
  1581. unsigned n = dn/2;
  1582. VECT_OP_TYPE incr = 1.0/n;
  1583. VECT_OP_FUNC(Seq)(dbp,n,0,incr);
  1584. VECT_OP_FUNC(Seq)(dbp+n,dn-n,1,-incr);
  1585. return dbp;
  1586. }
  1587. VECT_OP_TYPE* VECT_OP_FUNC(GaussWin)( VECT_OP_TYPE* dbp, unsigned dn, double arg )
  1588. {
  1589. const VECT_OP_TYPE* dep = dbp + dn;
  1590. VECT_OP_TYPE* rp = dbp;
  1591. int N = (dep - dbp) - 1;
  1592. int n = -N/2;
  1593. if( N == 0 )
  1594. *dbp = 1.0;
  1595. else
  1596. {
  1597. while( dbp < dep )
  1598. {
  1599. double a = (arg * n++) / (N/2);
  1600. *dbp++ = (VECT_OP_TYPE)exp( -(a*a)/2 );
  1601. }
  1602. }
  1603. return rp;
  1604. }
  1605. VECT_OP_TYPE* VECT_OP_FUNC(Filter)(
  1606. VECT_OP_TYPE* y,
  1607. unsigned yn,
  1608. const VECT_OP_TYPE* x,
  1609. unsigned xn,
  1610. cmReal_t b0,
  1611. const cmReal_t* b,
  1612. const cmReal_t* a,
  1613. cmReal_t* d,
  1614. unsigned dn )
  1615. {
  1616. int i,j;
  1617. VECT_OP_TYPE y0 = 0;
  1618. unsigned n = cmMin( yn, xn );
  1619. // This is a direct form II algorithm based on the MATLAB implmentation
  1620. // http://www.mathworks.com/access/helpdesk/help/techdoc/ref/filter.html#f83-1015962
  1621. for(i=0; i<n; ++i)
  1622. {
  1623. y[i] = (x[i] * b0) + d[0];
  1624. y0 = y[i];
  1625. for(j=0; j<dn; ++j)
  1626. d[j] = (b[j] * x[i]) - (a[j] * y0) + d[j+1];
  1627. }
  1628. // if fewer input samples than output samples - zero the end of the output buffer
  1629. if( yn > xn )
  1630. VECT_OP_FUNC(Fill)(y+i,yn-i,0);
  1631. return y;
  1632. }
  1633. VECT_OP_TYPE* VECT_OP_FUNC(FilterFilter)(struct cmFilter_str* f, cmRC_t (*func)( struct cmFilter_str* f, const VECT_OP_TYPE* x, unsigned xn, VECT_OP_TYPE* y, unsigned yn ), const cmReal_t bb[], unsigned bn, const cmReal_t aa[], unsigned an, const VECT_OP_TYPE* x, unsigned xn, VECT_OP_TYPE* y, unsigned yn )
  1634. {
  1635. int i,j;
  1636. int nfilt = cmMax(bn,an);
  1637. int nfact = 3*(nfilt-1);
  1638. const cmReal_t* a = aa;
  1639. const cmReal_t* b = bb;
  1640. cmReal_t* m = NULL;
  1641. cmReal_t* p;
  1642. unsigned zn = (nfilt-1)*(nfilt-1);
  1643. unsigned mn = 2*zn; // space for mtx z0 and z1
  1644. mn += nfilt; // space for zero padded coeff vector
  1645. mn += 2*nfact; // space for begin/end sequences
  1646. if( nfact >= xn )
  1647. {
  1648. return cmOkRC;
  1649. }
  1650. m = cmMemAllocZ( cmReal_t, mn );
  1651. p = m;
  1652. cmReal_t* z0 = p;
  1653. p += zn;
  1654. cmReal_t* z1 = p;
  1655. p += zn;
  1656. cmReal_t* s0 = p;
  1657. p += nfact;
  1658. cmReal_t* s1 = p;
  1659. p += nfact;
  1660. // zero pad the shorter coeff vect
  1661. if( bn < nfilt )
  1662. {
  1663. cmVOR_Copy(p,bn,bb);
  1664. b = p;
  1665. p += nfilt;
  1666. }
  1667. else
  1668. if( an < nfilt )
  1669. {
  1670. cmVOR_Copy(p,an,aa);
  1671. a = p;
  1672. p += nfilt;
  1673. }
  1674. // z0=eye(nfilt-1)
  1675. cmVOR_Identity(z0,nfilt-1,nfilt-1);
  1676. // z1=[eye(nfilt-1,nfilt-2); zeros(1,nfilt-1)];
  1677. cmVOR_Identity(z1,nfilt-1,nfilt-2);
  1678. // z0(:,1) -= a(:)
  1679. for(i=0; i<nfilt-1; ++i)
  1680. z0[i] -= -a[i+1];
  1681. // z0(:,2:end) -= z1;
  1682. for(i=1; i<nfilt-1; ++i)
  1683. for(j=0; j<nfilt-1; ++j)
  1684. z0[ (i*(nfilt-1)) + j ] -= z1[ ((i-1)*(nfilt-1)) + j ];
  1685. // z1 = b - (a * b[0])
  1686. for(i=1; i<nfilt; ++i)
  1687. z1[i-1] = b[i] - (a[i] * b[0]);
  1688. // z1 = z0\z1
  1689. cmVOR_SolveLS(z0,nfilt-1,z1,1);
  1690. // if yn<xn then truncate x.
  1691. xn = cmMin(xn,yn);
  1692. yn = xn;
  1693. // fill in the beginning sequence
  1694. for(i=0; i<nfact; ++i)
  1695. s0[i] = 2*x[0] - x[ nfact-i ];
  1696. // fill in the ending sequence
  1697. for(i=0; i<nfact; ++i)
  1698. s1[i] = 2*x[xn-1] - x[ xn-2-i ];
  1699. cmVOR_MultVVS( z0, nfact, z1, s0[0]);
  1700. unsigned pn = cmMin(1024,xn);
  1701. //acFilter* f = cmFilterAlloc(c,NULL,b,bn,a,an,pn,z0);
  1702. cmFilterInit(f,b,bn,a,an,pn,z0);
  1703. const VECT_OP_TYPE* xx = x;
  1704. for(j=0; j<2; ++j)
  1705. {
  1706. unsigned n = pn;
  1707. // filter begining sequence
  1708. cmFilterExecR(f,s0,nfact,s0,nfact);
  1709. // filter middle sequence
  1710. for(i=0; i<xn; i+=n)
  1711. {
  1712. n = cmMin(pn,xn-i);
  1713. func(f,xx+i,n,y+i,n);
  1714. }
  1715. // filter ending sequence
  1716. cmFilterExecR(f,s1,nfact,s1,nfact);
  1717. // flip all the sequences
  1718. cmVOR_Flip(s0,nfact);
  1719. cmVOR_Flip(s1,nfact);
  1720. VECT_OP_FUNC(Flip)(y,yn);
  1721. if( j==0)
  1722. {
  1723. // swap the begin and end sequences
  1724. cmReal_t* t = s0;
  1725. s0 = s1;
  1726. s1 = t;
  1727. xx = y;
  1728. cmVOR_MultVVS( z0, nfact, z1, s0[0]);
  1729. cmFilterInit(f,b,bn,a,an,pn,z0);
  1730. }
  1731. }
  1732. //cmFilterFree(&f);
  1733. cmMemPtrFree(&m);
  1734. return y;
  1735. }
  1736. VECT_OP_TYPE* VECT_OP_FUNC(LP_Sinc)(VECT_OP_TYPE* dp, unsigned dn, double srate, double fcHz, unsigned flags )
  1737. {
  1738. VECT_OP_TYPE* rp = dp;
  1739. int dM = dn % 2; // dM is used to handle odd length windows
  1740. int M = (dn - dM)/2;
  1741. int Mi = -M;
  1742. double signFact = cmIsFlag(flags, kHighPass_LPSincFl) ? -0.5 : 0.5;
  1743. double phsFact = 2.0 * M_PI * fcHz / srate;
  1744. double sum = 0;
  1745. M += dM;
  1746. //printf("M=%i Mi=%i sign:%f phs:%f\n",M,Mi,signFact,phsFact);
  1747. for(; Mi<M; ++Mi,++dp)
  1748. {
  1749. double phs = phsFact * Mi;
  1750. *dp = Mi == 0 ? 0.5 : signFact * sin(phs)/phs;
  1751. sum += *dp;
  1752. }
  1753. if( cmIsFlag(flags,kNormalize_LPSincFl) )
  1754. VECT_OP_FUNC(DivVS)(rp,dn,sum);
  1755. return rp;
  1756. }
  1757. VECT_OP_TYPE VECT_OP_FUNC(ComplexDetect)(const VECT_OP_TYPE* mag0V, const VECT_OP_TYPE* mag1V, const VECT_OP_TYPE* phs0V, const VECT_OP_TYPE* phs1V, const VECT_OP_TYPE* phs2V, unsigned binCnt )
  1758. {
  1759. double sum = 0;
  1760. const VECT_OP_TYPE* ep = mag0V + binCnt;
  1761. unsigned i = 0;
  1762. for(; mag0V < ep; ++i )
  1763. {
  1764. // calc phase deviation from expected
  1765. double dev_rads = *phs0V++ - (2 * *phs1V++) + *phs2V++;
  1766. // map deviation into range: -pi to pi
  1767. //double dev_rads1 = mod(dev_rads0 + M_PI, -2*M_PI ) + M_PI;
  1768. while( dev_rads > M_PI)
  1769. dev_rads -= 2*M_PI;
  1770. while( dev_rads < -M_PI)
  1771. dev_rads += 2*M_PI;
  1772. // convert into rect coord's
  1773. double m1r = *mag1V++;
  1774. double m0r = *mag0V * cos(dev_rads);
  1775. double m0i = *mag0V++ * sin(dev_rads);
  1776. // calc the combined amplitude and phase deviation
  1777. // sum += hypot( m1 - (m0 * e^(-1*dev_rads)));
  1778. sum += hypot( m1r-m0r, -m0i );
  1779. }
  1780. return (VECT_OP_TYPE)sum;
  1781. }
  1782. VECT_OP_TYPE* VECT_OP_FUNC(MelMask)( VECT_OP_TYPE* maskMtx, unsigned filterCnt, unsigned binCnt, double srate, unsigned flags )
  1783. {
  1784. unsigned fi,bi;
  1785. double mxh = srate/2.0; // nyquist
  1786. double dh = mxh/(binCnt-1) ; // binHz
  1787. double mxm = 1127.0 * log( 1.0 + mxh/700.0); // max mel value in Hz
  1788. double dm = mxm / (filterCnt+1); // avg mel band hz
  1789. double sum = 0;
  1790. for(fi=0; fi<filterCnt; ++fi)
  1791. {
  1792. double m = (fi+1) * dm;
  1793. // calc min/center/max frequencies for this band
  1794. double minHz = 700.0 * (exp((m-dm)/1127.01048)-1.0);
  1795. double ctrHz = 700.0 * (exp( m /1127.01048)-1.0);
  1796. double maxHz = 700.0 * (exp((m+dm)/1127.01048)-1.0);
  1797. // shift the band min/ctr/max to the nearest bin ctr frequency
  1798. if( cmIsFlag(flags,kShiftMelFl) )
  1799. {
  1800. unsigned i;
  1801. i = (unsigned)floor(minHz/dh);
  1802. minHz = minHz - (dh*i) < dh*(i+1) - minHz ? dh*i : dh*(i+1);
  1803. i = (unsigned)floor(ctrHz/dh);
  1804. ctrHz = ctrHz - (dh*i) < dh*(i+1) - ctrHz ? dh*i : dh*(i+1);
  1805. i = (unsigned)floor(maxHz/dh);
  1806. maxHz = maxHz - (dh*i) < dh*(i+1) - maxHz ? dh*i : dh*(i+1);
  1807. }
  1808. // calc the height of the triangle - such that all bands have equal area
  1809. double a = 2.0/(maxHz - minHz);
  1810. for(bi=0; bi<binCnt; ++bi)
  1811. {
  1812. double h = bi*dh;
  1813. unsigned mi = bi*filterCnt + fi;
  1814. if( h < minHz || h > maxHz )
  1815. maskMtx[mi] = 0;
  1816. else
  1817. {
  1818. if( h <= ctrHz )
  1819. maskMtx[mi] = a * (h - minHz)/(ctrHz-minHz);
  1820. else
  1821. maskMtx[mi] = a * (maxHz - h)/(maxHz-ctrHz);
  1822. sum += maskMtx[mi];
  1823. }
  1824. }
  1825. }
  1826. if( cmIsFlag(flags,kNormalizeMelFl) )
  1827. VECT_OP_FUNC(DivVS)( maskMtx, (filterCnt*binCnt), sum );
  1828. return maskMtx;
  1829. }
  1830. unsigned VECT_OP_FUNC(BarkMap)(unsigned* binIdxV, unsigned* cntV, unsigned bandCnt, unsigned binCnt, double srate )
  1831. {
  1832. if( bandCnt == 0 )
  1833. return 0;
  1834. //zwicker & fastl: psychoacoustics 1999, page 159
  1835. double bandUprHz[] = { 100, 200, 300, 400, 510, 630, 770, 920, 1080, 1270, 1480, 1720, 2000, 2320, 2700, 3150, 3700, 4400, 5300, 6400, 7700, 9500, 12000, 15500 };
  1836. unsigned hn = sizeof(bandUprHz)/sizeof(double);
  1837. unsigned i, bi = 0;
  1838. bandCnt = cmMin(hn,bandCnt);
  1839. binIdxV[0] = 0;
  1840. cntV[0] = 1;
  1841. for(i=1; bi < bandCnt && i<binCnt; ++i)
  1842. {
  1843. double hz = srate * i / (2 * (binCnt-1));
  1844. if( hz <= bandUprHz[bi] )
  1845. cntV[bi]++;
  1846. else
  1847. {
  1848. //printf("%i %i %i %f\n",bi,binIdxV[bi],cntV[bi],bandUprHz[bi]);
  1849. ++bi;
  1850. if( bi < bandCnt )
  1851. {
  1852. binIdxV[bi] = i;
  1853. cntV[bi] = 1;
  1854. }
  1855. }
  1856. }
  1857. return bi;
  1858. }
  1859. VECT_OP_TYPE* VECT_OP_FUNC(TriangleMask)(VECT_OP_TYPE* maskMtx, unsigned bandCnt, unsigned binCnt, const VECT_OP_TYPE* ctrHzV, VECT_OP_TYPE binHz, VECT_OP_TYPE stSpread, const VECT_OP_TYPE* lfV, const VECT_OP_TYPE* hfV )
  1860. {
  1861. unsigned i,j;
  1862. VECT_OP_TYPE v0[ bandCnt ];
  1863. VECT_OP_TYPE v1[ bandCnt ];
  1864. // if no lower/upper band limits were give use a fixed semitone band width
  1865. if( lfV==NULL || hfV==NULL)
  1866. {
  1867. for(i=0; i<bandCnt; ++i)
  1868. {
  1869. v0[i] = ctrHzV[i] * pow(2.0,-stSpread/12.0);
  1870. v1[i] = ctrHzV[i] * pow(2.0, stSpread/12.0);
  1871. }
  1872. lfV = v0;
  1873. hfV = v1;
  1874. }
  1875. VECT_OP_FUNC(Zero)(maskMtx,bandCnt*binCnt);
  1876. // for each band
  1877. for(i=0; i<bandCnt; ++i)
  1878. {
  1879. // calc bin index of first possible bin in this band
  1880. // j = (unsigned)floor(lfV[i] / binHz);
  1881. double binHz_j = 0;
  1882. // for each bin whose ctr frq is <= the band upper limit
  1883. for(j=0; j<binCnt; ++j)
  1884. {
  1885. double v;
  1886. // if bin[j] is inside the lower leg of the triangle
  1887. if( lfV[i] <= binHz_j && binHz_j <= ctrHzV[i] )
  1888. v = (binHz_j - lfV[i]) / cmMax(VECT_OP_MIN, ctrHzV[i] - lfV[i] );
  1889. else
  1890. // if bin[j] is inside the upper leg of the triangle
  1891. if( ctrHzV[i] < binHz_j && binHz_j <= hfV[i] )
  1892. v = (hfV[i] - binHz_j) / cmMax(VECT_OP_MIN, hfV[i] - ctrHzV[i] );
  1893. else
  1894. v = 0;
  1895. maskMtx[ (j*bandCnt)+i ] = v;
  1896. binHz_j = binHz * (j+1);
  1897. }
  1898. }
  1899. return maskMtx;
  1900. }
  1901. VECT_OP_TYPE* VECT_OP_FUNC(BarkMask)(VECT_OP_TYPE* maskMtx, unsigned bandCnt, unsigned binCnt, double binHz )
  1902. {
  1903. // -1 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 (23+1)
  1904. VECT_OP_TYPE b[]= {0, 50,150,250,350,450,570,700,840,1000,1170,1370,1600,1850,2150,2500,2900,3400,4000,4800,5800,7000,8500,10500,13500, 15500 };
  1905. bandCnt = cmMin(bandCnt,kDefaultBarkBandCnt);
  1906. VECT_OP_FUNC(TriangleMask)(maskMtx, bandCnt, binCnt, b+1, binHz, 0, b+0, b+2 );
  1907. return maskMtx;
  1908. }
  1909. VECT_OP_TYPE* VECT_OP_FUNC(TerhardtThresholdMask)(VECT_OP_TYPE* maskV, unsigned binCnt, double srate, unsigned flags )
  1910. {
  1911. unsigned i;
  1912. double c0 = cmIsFlag(flags,kModifiedTtmFl) ? 0.6 : 1.0;
  1913. double c1 = cmIsFlag(flags,kModifiedTtmFl) ? 0.5 : 6.5;
  1914. maskV[0]=0;
  1915. for(i=0; i<binCnt; ++i)
  1916. {
  1917. double hz = srate * i / (2 * (binCnt-1));
  1918. maskV[i] = pow(pow(10,(c0 * -3.64* pow(hz/1000,-0.8) + c1 * exp(-0.6 * pow(hz/1000 - 3.3,2)) - 0.001* pow(hz/1000,4))/20),2);
  1919. }
  1920. return maskV;
  1921. }
  1922. VECT_OP_TYPE* VECT_OP_FUNC(ShroederSpreadingFunc)(VECT_OP_TYPE* m, unsigned bandCnt, double srate)
  1923. {
  1924. int fi,bi;
  1925. for(fi=0; fi<bandCnt; ++fi)
  1926. for(bi=0; bi<bandCnt; ++bi )
  1927. m[ fi + (bi*bandCnt) ] = pow(10,(15.81 + 7.5 * ((fi-bi)+0.474)-17.5*pow(1+pow((fi-bi)+0.474,2),0.5))/10);
  1928. return m;
  1929. }
  1930. VECT_OP_TYPE* VECT_OP_FUNC(DctMatrix)( VECT_OP_TYPE* dp, unsigned coeffCnt, unsigned filtCnt )
  1931. {
  1932. VECT_OP_TYPE* dbp = dp;
  1933. double c0 = 1.0/sqrt(filtCnt/2); // row 1-coeffCnt factor
  1934. double c1 = c0 * sqrt(2)/2; // row 0 factor
  1935. unsigned i,j;
  1936. // for each column
  1937. for(i=0; i<filtCnt; ++i)
  1938. // for each row
  1939. for(j=0; j<coeffCnt; ++j)
  1940. *dp++ = (j==0 ? c1 : c0) * cos( (0.5 + i) * M_PI * j / filtCnt);
  1941. return dbp;
  1942. }
  1943. unsigned VECT_OP_FUNC(PeakIndexes)( unsigned* dbp, unsigned dn, const VECT_OP_TYPE* sbp, unsigned sn, VECT_OP_TYPE threshold )
  1944. {
  1945. unsigned pkCnt = 0;
  1946. const unsigned* dep = dbp + dn;
  1947. const VECT_OP_TYPE* sep = sbp + sn;
  1948. const VECT_OP_TYPE* s2p = sbp;
  1949. const VECT_OP_TYPE* s0p = s2p++;
  1950. const VECT_OP_TYPE* s1p = s2p++;
  1951. while( dbp < dep && s2p < sep )
  1952. {
  1953. if( (*s0p < *s1p) && (*s1p > *s2p) && (*s1p >= threshold) )
  1954. {
  1955. *dbp++ = s1p - sbp;
  1956. s0p = s2p++;
  1957. s1p = s2p++;
  1958. ++pkCnt;
  1959. }
  1960. else
  1961. {
  1962. s0p = s1p;
  1963. s1p = s2p++;
  1964. }
  1965. }
  1966. return pkCnt;
  1967. }
  1968. unsigned VECT_OP_FUNC(BinIndex)( const VECT_OP_TYPE* sbp, unsigned sn, VECT_OP_TYPE v )
  1969. {
  1970. const VECT_OP_TYPE* sep = sbp + sn;
  1971. const VECT_OP_TYPE* bp = sbp;
  1972. sep--;
  1973. for(; sbp < sep; ++sbp )
  1974. if( *sbp <= v && v < *(sbp+1) )
  1975. return sbp - bp;
  1976. return cmInvalidIdx;
  1977. }
  1978. unsigned VECT_OP_FUNC(Kmeans)(
  1979. unsigned* classIdxV, // classIdxV[scn] - data point class assignments
  1980. VECT_OP_TYPE* centroidM, // centroidM[srn,K] - cluster centroids
  1981. unsigned K, // count of clusters
  1982. const VECT_OP_TYPE* sM, // sM[srn,scn] source data matrix
  1983. unsigned srn, // dimensionality of each data point
  1984. unsigned scn, // count of data points
  1985. const unsigned* selIdxV, // data subset selection id vector (optional)
  1986. unsigned selKey, // data subset selection key (optional)
  1987. bool initFromCentroidFl,// true if the starting centroids are in centroidM[]
  1988. VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ),
  1989. void* userDistPtr
  1990. )
  1991. {
  1992. unsigned D = srn; // data dimensionality
  1993. unsigned N = scn; // count of data points to cluster
  1994. unsigned iterCnt = 0;
  1995. unsigned ki;
  1996. unsigned i = 0;
  1997. unsigned selN = N;
  1998. // if a data point selection vector was given
  1999. if( selIdxV != NULL )
  2000. {
  2001. selN = 0;
  2002. for(i=0; i<N; ++i)
  2003. {
  2004. selN += selIdxV[i]==selKey;
  2005. classIdxV[i] = K;
  2006. }
  2007. }
  2008. assert(K<=selN);
  2009. // if the numer of datapoints and the number of clusters is the same
  2010. // make the datapoints the centroids and return
  2011. if( K == selN )
  2012. {
  2013. ki = 0;
  2014. for(i=0; i<N; ++i)
  2015. if( selIdxV==NULL || selIdxV[i]==selKey )
  2016. {
  2017. VECT_OP_FUNC(Copy)(centroidM+(ki*D),D,sM+(i*D));
  2018. classIdxV[ki] = ki;
  2019. ++ki;
  2020. }
  2021. return 0;
  2022. }
  2023. // if centroidM[] has not been initialized with the starting centroid vectors.
  2024. if( initFromCentroidFl == false )
  2025. {
  2026. unsigned* kiV = cmMemAlloc( unsigned, N );
  2027. // select K unique datapoints at random as the initial centroids
  2028. cmVOU_RandomSeq(kiV,N);
  2029. for(i=0,ki=0; i<N && ki<K; ++i)
  2030. {
  2031. if( selIdxV==NULL || selIdxV[ kiV[i] ]==selKey )
  2032. {
  2033. VECT_OP_FUNC(Copy)( centroidM + (ki*D), D, sM + (kiV[i]*D) );
  2034. ++ki;
  2035. }
  2036. }
  2037. cmMemPtrFree(&kiV);
  2038. }
  2039. unsigned* nV = cmMemAllocZ( unsigned,K);
  2040. while(1)
  2041. {
  2042. unsigned changeCnt = 0;
  2043. cmVOU_Zero(nV,K);
  2044. // for each data point - assign data point to a cluster
  2045. for(i=0; i<N; ++i)
  2046. if( selIdxV==NULL || selIdxV[i] == selKey )
  2047. {
  2048. // set ki with the index of the centroid closest to sM[:,i]
  2049. VECT_OP_FUNC(DistVMM)( NULL, NULL, &ki, D, sM + (i*srn), 1, centroidM, K, distFunc, userDistPtr );
  2050. assert(ki<K);
  2051. nV[ki]++;
  2052. changeCnt += ( ki != classIdxV[i] );
  2053. classIdxV[i] = ki;
  2054. }
  2055. // if no data points change classes then the centroids have converged
  2056. if( changeCnt == 0 )
  2057. break;
  2058. ++iterCnt;
  2059. // zero the centroid matrix
  2060. VECT_OP_FUNC(Fill)(centroidM, D*K, 0 );
  2061. // update the centroids
  2062. for(ki=0; ki<K; ++ki)
  2063. {
  2064. unsigned n = 0;
  2065. // sum the all datapoints belonging to class ki
  2066. for(i=0; i<N; ++i)
  2067. if( classIdxV[i] == ki )
  2068. {
  2069. VECT_OP_FUNC(AddVV)(centroidM + (ki*D), D, sM + (i*srn) );
  2070. ++n;
  2071. }
  2072. // convert the sum to a mean to form the centroid
  2073. if( n > 0 )
  2074. VECT_OP_FUNC(DivVS)(centroidM + (ki*D), D, n );
  2075. }
  2076. }
  2077. cmVOU_PrintL("class cnt:",NULL,1,K,nV);
  2078. cmMemPtrFree(&nV);
  2079. return iterCnt;
  2080. }
  2081. unsigned VECT_OP_FUNC(Kmeans2)(
  2082. unsigned* classIdxV, // classIdxV[scn] - data point class assignments
  2083. VECT_OP_TYPE* centroidM, // centroidM[srn,K] - cluster centroids
  2084. unsigned K, // count of clusters
  2085. const VECT_OP_TYPE* (*srcFunc)(void* userPtr, unsigned frmIdx ),
  2086. unsigned srn, // dimensionality of each data point
  2087. unsigned scn, // count of data points
  2088. void* userSrcPtr, // callback data for srcFunc
  2089. VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ),
  2090. void* distUserPtr,
  2091. int maxIterCnt,
  2092. int deltaStopCnt
  2093. )
  2094. {
  2095. unsigned D = srn; // data dimensionality
  2096. unsigned N = scn; // count of data points to cluster
  2097. unsigned iterCnt = 0;
  2098. unsigned ki;
  2099. unsigned i = 0;
  2100. const VECT_OP_TYPE* sp;
  2101. assert(K<N);
  2102. deltaStopCnt = cmMax(0,deltaStopCnt);
  2103. // nV[K] - class assignment vector
  2104. unsigned* nV = cmMemAllocZ( unsigned,2*K);
  2105. // roV[K] - read-only flag centroid
  2106. // centroids flagged as read-only will not be updated by the clustering routine
  2107. unsigned* roV = nV + K;
  2108. // copy the read-only flags into roV[K]
  2109. for(i=0; i<K; ++i)
  2110. roV[i] = classIdxV[i];
  2111. while(1)
  2112. {
  2113. unsigned changeCnt = 0;
  2114. cmVOU_Zero(nV,K);
  2115. // for each data point - assign data point to a cluster
  2116. for(i=0; i<N; ++i)
  2117. if((sp = srcFunc(userSrcPtr,i)) != NULL)
  2118. {
  2119. // set ki with the index of the centroid closest to sM[:,i]
  2120. VECT_OP_FUNC(DistVMM)( NULL, NULL, &ki, D, sp, 1, centroidM, K, distFunc, distUserPtr );
  2121. assert(ki<K);
  2122. // track the number of data points assigned to each centroid
  2123. nV[ki]++;
  2124. // track the number of data points which change classes
  2125. changeCnt += ( ki != classIdxV[i] );
  2126. // update the class that this data point belongs to
  2127. classIdxV[i] = ki;
  2128. }
  2129. // if the count of data points which changed classes is less than deltaStopCnt
  2130. // then the centroids have converged
  2131. if( changeCnt <= deltaStopCnt )
  2132. break;
  2133. if( maxIterCnt!=-1 && iterCnt>=maxIterCnt )
  2134. break;
  2135. // track the number of interations required to converge
  2136. ++iterCnt;
  2137. fprintf(stderr,"%i:%i (", iterCnt,changeCnt );
  2138. for(i=0; i<K; ++i)
  2139. fprintf(stderr,"%i ",nV[i]);
  2140. fprintf(stderr,") ");
  2141. fflush(stderr);
  2142. // update the centroids
  2143. for(ki=0; ki<K; ++ki)
  2144. if( roV[ki]==0 )
  2145. {
  2146. unsigned n = 0;
  2147. VECT_OP_FUNC(Zero)(centroidM + (ki*D), D );
  2148. // sum the all datapoints belonging to class ki
  2149. for(i=0; i<N; ++i)
  2150. if( classIdxV[i] == ki && ((sp=srcFunc(userSrcPtr,i))!=NULL))
  2151. {
  2152. VECT_OP_FUNC(AddVV)(centroidM + (ki*D), D, sp );
  2153. ++n;
  2154. }
  2155. // convert the sum to a mean to form the centroid
  2156. if( n > 0 )
  2157. VECT_OP_FUNC(DivVS)(centroidM + (ki*D), D, n );
  2158. }
  2159. }
  2160. cmMemPtrFree(&nV);
  2161. return iterCnt;
  2162. }
  2163. VECT_OP_TYPE* VECT_OP_FUNC(GaussPDF)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, VECT_OP_TYPE mean, VECT_OP_TYPE stdDev )
  2164. {
  2165. VECT_OP_TYPE* rp = dbp;
  2166. const VECT_OP_TYPE* dep = dbp + dn;
  2167. VECT_OP_TYPE var = stdDev * stdDev;
  2168. VECT_OP_TYPE fact0 = 1.0/sqrt(2*M_PI*var);
  2169. VECT_OP_TYPE fact1 = 2.0 * var;
  2170. for(; dbp < dep; ++sbp )
  2171. *dbp++ = fact0 * exp( -((*sbp-mean)*(*sbp-mean))/ fact1 );
  2172. return rp;
  2173. }
  2174. /// Evaluate a multivariate normal distribution defined by meanV[D] and covarM[D,D]
  2175. /// at the data points held in the columns of xM[D,N]. Return the evaluation
  2176. /// results in the vector yV[N].
  2177. bool VECT_OP_FUNC(MultVarGaussPDF)( VECT_OP_TYPE* yV, const VECT_OP_TYPE* xM, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* covarM, unsigned D, unsigned N, bool diagFl )
  2178. {
  2179. VECT_OP_TYPE det0;
  2180. // calc the determinant of the covariance matrix
  2181. if( diagFl )
  2182. // kpl 1/16/11 det0 = VECT_OP_FUNC(LogDetDiagM)(covarM,D);
  2183. det0 = VECT_OP_FUNC(DetDiagM)(covarM,D);
  2184. else
  2185. // kpl 1/16/11 det0 = VECT_OP_FUNC(LogDetM)(covarM,D);
  2186. det0 = VECT_OP_FUNC(DetM)(covarM,D);
  2187. assert(det0 != 0 );
  2188. if( det0 == 0 )
  2189. return false;
  2190. // calc the inverse of the covariance matrix
  2191. VECT_OP_TYPE icM[D*D];
  2192. VECT_OP_FUNC(Copy)(icM,D*D,covarM);
  2193. VECT_OP_TYPE* r;
  2194. if( diagFl )
  2195. r = VECT_OP_FUNC(InvDiagM)(icM,D);
  2196. else
  2197. r = VECT_OP_FUNC(InvM)(icM,D);
  2198. if( r == NULL )
  2199. return false;
  2200. VECT_OP_FUNC(MultVarGaussPDF2)( yV, xM, meanV, icM, det0, D, N, diagFl );
  2201. return true;
  2202. }
  2203. VECT_OP_TYPE* VECT_OP_FUNC(MultVarGaussPDF2)( VECT_OP_TYPE* yV, const VECT_OP_TYPE* xM, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* icM, VECT_OP_TYPE logDet, unsigned D, unsigned N, bool diagFl )
  2204. {
  2205. unsigned i;
  2206. double fact = (-(cmReal_t)D/2) * log(2.0*M_PI) - 0.5*logDet;
  2207. for(i=0; i<N; ++i)
  2208. {
  2209. VECT_OP_TYPE dx[D];
  2210. VECT_OP_TYPE t[D];
  2211. // dx[] difference between mean and ith data point
  2212. VECT_OP_FUNC(SubVVV)(dx,D, xM + (i*D), meanV);
  2213. // t[] = dx[] * inv(covarM);
  2214. if( diagFl )
  2215. VECT_OP_FUNC(MultDiagVMV)(t,D,icM,D,dx);
  2216. else
  2217. VECT_OP_FUNC(MultVMV)(t,D,icM,D,dx);
  2218. // dist = sum(dx[] * t[])
  2219. cmReal_t dist = VECT_OP_FUNC(MultSumVV)(t,dx,D);
  2220. yV[i] = exp( fact - (0.5*dist) );
  2221. }
  2222. return yV;
  2223. }
  2224. VECT_OP_TYPE* VECT_OP_FUNC(MultVarGaussPDF3)(
  2225. VECT_OP_TYPE* yV,
  2226. const VECT_OP_TYPE* (*srcFunc)(void* funcDataPtr, unsigned frmIdx ),
  2227. void* funcDataPtr,
  2228. const VECT_OP_TYPE* meanV,
  2229. const VECT_OP_TYPE* icM,
  2230. VECT_OP_TYPE logDet,
  2231. unsigned D,
  2232. unsigned N,
  2233. bool diagFl )
  2234. {
  2235. unsigned i;
  2236. double fact = (-(cmReal_t)D/2) * log(2.0*M_PI) - 0.5*logDet;
  2237. for(i=0; i<N; ++i)
  2238. {
  2239. VECT_OP_TYPE dx[D];
  2240. VECT_OP_TYPE t[D];
  2241. const VECT_OP_TYPE* xV = srcFunc( funcDataPtr, i );
  2242. if( xV == NULL )
  2243. yV[i] = 0;
  2244. else
  2245. {
  2246. // dx[] difference between mean and ith data point
  2247. VECT_OP_FUNC(SubVVV)(dx, D, xV, meanV);
  2248. // t[] = dx[] * inv(covarM);
  2249. if( diagFl )
  2250. VECT_OP_FUNC(MultDiagVMV)(t,D,icM,D,dx);
  2251. else
  2252. VECT_OP_FUNC(MultVMV)(t,D,icM,D,dx);
  2253. // dist = sum(dx[] * t[])
  2254. cmReal_t dist = VECT_OP_FUNC(MultSumVV)(t,dx,D);
  2255. yV[i] = exp( fact - (0.5*dist) );
  2256. }
  2257. }
  2258. return yV;
  2259. }
  2260. /// stateV[timeN]
  2261. /// a[stateN,stateN],
  2262. /// b[stateN,timeN]
  2263. /// phi[stateN].
  2264. void VECT_OP_FUNC(DiscreteViterbi)(unsigned* stateV, unsigned tN, unsigned sN, const VECT_OP_TYPE* phi, const VECT_OP_TYPE* a, const VECT_OP_TYPE* b )
  2265. {
  2266. unsigned* psiM = cmMemAlloc( unsigned, sN*tN ); // psi[sN,tN]
  2267. VECT_OP_TYPE* dV = cmMemAlloc( VECT_OP_TYPE, 2*sN );
  2268. VECT_OP_TYPE* d0V = dV;
  2269. VECT_OP_TYPE* d1V = dV + sN;
  2270. int t,i,j;
  2271. // calc the prob of starting in each state given the observations
  2272. VECT_OP_FUNC(MultVVV)( d0V, sN, phi, b );
  2273. VECT_OP_FUNC(NormalizeProbability)( d0V, sN ); // scale to prevent underflow
  2274. // for each time step
  2275. for(t=1; t<tN; ++t)
  2276. {
  2277. // for each possible next state
  2278. for(j=0; j<sN; ++j)
  2279. {
  2280. VECT_OP_TYPE mv = 0;
  2281. unsigned mi = 0;
  2282. // The following loop could be replaced with these vector op's:
  2283. // VECT_OP_TYPE tV[ sN ];
  2284. // VECT_OP_TYPE(MultVVV)(tV,sN,d0V,a + (j*sN));
  2285. // mi = VECT_OP_TYPE(MaxIndex)(tV,sN);
  2286. // mv = tV[mi];
  2287. // for each possible prev state
  2288. for(i=0; i<sN; ++i)
  2289. {
  2290. // calc prob of having ended in state i and transitioning to state j
  2291. VECT_OP_TYPE v = d0V[i] * a[ i + (j*sN) ];
  2292. // track the most likely transition ending in state j
  2293. if( v > mv )
  2294. {
  2295. mv = v;
  2296. mi = i;
  2297. }
  2298. }
  2299. // scale the prob of the most likely state by the prob of the obs given that state
  2300. d1V[j] = mv * b[ (t*sN) + j ];
  2301. // store the most likely previous state given that the current state is j
  2302. // (this is the key to understanding the backtracking step below)
  2303. psiM[ (t*sN) + j ] = mi;
  2304. }
  2305. VECT_OP_FUNC(NormalizeProbability)( d1V, sN ); // scale to prevent underflow
  2306. // swap d0V and d1V
  2307. VECT_OP_TYPE* tmp = d0V;
  2308. d0V = d1V;
  2309. d1V = tmp;
  2310. }
  2311. // store the most likely ending state
  2312. stateV[tN-1] = VECT_OP_FUNC(MaxIndex)( d0V, sN, 1 );
  2313. // given the most likely next step select the most likely previous step
  2314. for(t=tN-2; t>=0; --t)
  2315. stateV[t] = psiM[ ((t+1)*sN) + stateV[t+1] ];
  2316. cmMemPtrFree( &psiM );
  2317. cmMemPtrFree( &dV );
  2318. }
  2319. bool VECT_OP_FUNC(ClipLine2)( VECT_OP_TYPE x0, VECT_OP_TYPE y0, VECT_OP_TYPE x1, VECT_OP_TYPE y1, VECT_OP_TYPE xMin, VECT_OP_TYPE yMin, VECT_OP_TYPE xMax, VECT_OP_TYPE yMax, VECT_OP_TYPE* t0, VECT_OP_TYPE* t1 )
  2320. {
  2321. VECT_OP_TYPE dx = x1 - x0;
  2322. VECT_OP_TYPE dy = y1 - y0;
  2323. VECT_OP_TYPE p=0,q=0,r=0;
  2324. *t0 = 0.0;
  2325. *t1 = 1.0;
  2326. unsigned i;
  2327. for(i=0; i<4; ++i)
  2328. {
  2329. switch(i)
  2330. {
  2331. case 0: p=-dx; q=-(xMin - x0); break; // left
  2332. case 1: p= dx; q= (xMax - x0); break; // right
  2333. case 2: p=-dy; q=-(yMin - y0); break; // bottom
  2334. case 3: p= dy; q= (yMax - y0); break; // top
  2335. }
  2336. // if parallel to edge i
  2337. if( p == 0 )
  2338. {
  2339. // if entirely outside of window
  2340. if( q < 0 )
  2341. return false;
  2342. continue;
  2343. }
  2344. r = p/q;
  2345. // if travelling right/up
  2346. if( p < 0 )
  2347. {
  2348. // travelling away from x1,y1
  2349. if( r > *t1 )
  2350. return false;
  2351. // update distance on line to point of intersection
  2352. if( r > *t0 )
  2353. *t0 = r;
  2354. }
  2355. else // if travelling left/down
  2356. {
  2357. // travelling away from x1,y1
  2358. if( r < *t0 )
  2359. return false;
  2360. // update distance on line to point of intersection
  2361. if( r < *t1 )
  2362. *t1 = r;
  2363. }
  2364. }
  2365. return true;
  2366. }
  2367. /// (Uses the Laing-Barsky clipping algorithm)
  2368. /// From: http://www.skytopia.com/project/articles/compsci/clipping.html
  2369. bool VECT_OP_FUNC(ClipLine)( VECT_OP_TYPE* x0, VECT_OP_TYPE* y0, VECT_OP_TYPE* x1, VECT_OP_TYPE* y1, VECT_OP_TYPE xMin, VECT_OP_TYPE yMin, VECT_OP_TYPE xMax, VECT_OP_TYPE yMax )
  2370. {
  2371. VECT_OP_TYPE t0;
  2372. VECT_OP_TYPE t1;
  2373. if( VECT_OP_FUNC(ClipLine2)(*x0,*y0,*x1,*y1,xMin,yMin,xMax,yMax,&t0,&t1) )
  2374. {
  2375. VECT_OP_TYPE dx = *x1 - *x0;
  2376. VECT_OP_TYPE dy = *y1 - *y0;
  2377. *x0 = *x0 + t0*dx;
  2378. *x1 = *x0 + t1*dx;
  2379. *y0 = *y0 + t0*dy;
  2380. *y1 = *y0 + t1*dy;
  2381. return true;
  2382. }
  2383. return false;
  2384. }
  2385. bool VECT_OP_FUNC(IsLineInRect)( VECT_OP_TYPE x0, VECT_OP_TYPE y0, VECT_OP_TYPE x1, VECT_OP_TYPE y1, VECT_OP_TYPE xMin, VECT_OP_TYPE yMin, VECT_OP_TYPE xMax, VECT_OP_TYPE yMax )
  2386. {
  2387. VECT_OP_TYPE t0;
  2388. VECT_OP_TYPE t1;
  2389. return VECT_OP_FUNC(ClipLine2)(x0,y0,x1,y1,xMin,yMin,xMax,yMax,&t0,&t1);
  2390. }
  2391. VECT_OP_TYPE VECT_OP_FUNC(PtToLineDistance)( VECT_OP_TYPE x0, VECT_OP_TYPE y0, VECT_OP_TYPE x1, VECT_OP_TYPE y1, VECT_OP_TYPE px, VECT_OP_TYPE py)
  2392. {
  2393. // from:http://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
  2394. double normalLength = sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0));
  2395. if( normalLength <= 0 )
  2396. return 0;
  2397. return (VECT_OP_TYPE)fabs((px - x0) * (y1 - y0) - (py - y0) * (x1 - x0)) / normalLength;
  2398. }
  2399. void VECT_OP_FUNC(Lsq1)(const VECT_OP_TYPE* x, const VECT_OP_TYPE* y, unsigned n, VECT_OP_TYPE* b0, VECT_OP_TYPE* b1 )
  2400. {
  2401. VECT_OP_TYPE sx = 0;
  2402. VECT_OP_TYPE sy = 0;
  2403. VECT_OP_TYPE sx_2 = 0;
  2404. VECT_OP_TYPE sxy = 0;
  2405. unsigned i;
  2406. if( x == NULL )
  2407. {
  2408. for(i=0; i<n; ++i)
  2409. {
  2410. VECT_OP_TYPE xx = i;
  2411. sx += xx;
  2412. sx_2 += xx * xx;
  2413. sxy += xx * y[i];
  2414. sy += y[i];
  2415. }
  2416. }
  2417. else
  2418. {
  2419. for(i=0; i<n; ++i)
  2420. {
  2421. sx += x[i];
  2422. sx_2 += x[i] * x[i];
  2423. sxy += x[i] * y[i];
  2424. sy += y[i];
  2425. }
  2426. }
  2427. *b1 = (sxy * n - sx * sy) / (sx_2 * n - sx*sx);
  2428. *b0 = (sy - (*b1) * sx) / n;
  2429. }
  2430. #endif