libcm is a C development framework with an emphasis on audio signal processing applications.
Nevar pievienot vairāk kā 25 tēmas Tēmai ir jāsākas ar burtu vai ciparu, tā var saturēt domu zīmes ('-') un var būt līdz 35 simboliem gara.

cmVectOpsTemplateCode.h 78KB

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