libcm is a C development framework with an emphasis on audio signal processing applications.
Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.

cmVectOpsTemplateCode.h 80KB

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