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

cmProc2.c 155KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663266426652666266726682669267026712672267326742675267626772678267926802681268226832684268526862687268826892690269126922693269426952696269726982699270027012702270327042705270627072708270927102711271227132714271527162717271827192720272127222723272427252726272727282729273027312732273327342735273627372738273927402741274227432744274527462747274827492750275127522753275427552756275727582759276027612762276327642765276627672768276927702771277227732774277527762777277827792780278127822783278427852786278727882789279027912792279327942795279627972798279928002801280228032804280528062807280828092810281128122813281428152816281728182819282028212822282328242825282628272828282928302831283228332834283528362837283828392840284128422843284428452846284728482849285028512852285328542855285628572858285928602861286228632864286528662867286828692870287128722873287428752876287728782879288028812882288328842885288628872888288928902891289228932894289528962897289828992900290129022903290429052906290729082909291029112912291329142915291629172918291929202921292229232924292529262927292829292930293129322933293429352936293729382939294029412942294329442945294629472948294929502951295229532954295529562957295829592960296129622963296429652966296729682969297029712972297329742975297629772978297929802981298229832984298529862987298829892990299129922993299429952996299729982999300030013002300330043005300630073008300930103011301230133014301530163017301830193020302130223023302430253026302730283029303030313032303330343035303630373038303930403041304230433044304530463047304830493050305130523053305430553056305730583059306030613062306330643065306630673068306930703071307230733074307530763077307830793080308130823083308430853086308730883089309030913092309330943095309630973098309931003101310231033104310531063107310831093110311131123113311431153116311731183119312031213122312331243125312631273128312931303131313231333134313531363137313831393140314131423143314431453146314731483149315031513152315331543155315631573158315931603161316231633164316531663167316831693170317131723173317431753176317731783179318031813182318331843185318631873188318931903191319231933194319531963197319831993200320132023203320432053206320732083209321032113212321332143215321632173218321932203221322232233224322532263227322832293230323132323233323432353236323732383239324032413242324332443245324632473248324932503251325232533254325532563257325832593260326132623263326432653266326732683269327032713272327332743275327632773278327932803281328232833284328532863287328832893290329132923293329432953296329732983299330033013302330333043305330633073308330933103311331233133314331533163317331833193320332133223323332433253326332733283329333033313332333333343335333633373338333933403341334233433344334533463347334833493350335133523353335433553356335733583359336033613362336333643365336633673368336933703371337233733374337533763377337833793380338133823383338433853386338733883389339033913392339333943395339633973398339934003401340234033404340534063407340834093410341134123413341434153416341734183419342034213422342334243425342634273428342934303431343234333434343534363437343834393440344134423443344434453446344734483449345034513452345334543455345634573458345934603461346234633464346534663467346834693470347134723473347434753476347734783479348034813482348334843485348634873488348934903491349234933494349534963497349834993500350135023503350435053506350735083509351035113512351335143515351635173518351935203521352235233524352535263527352835293530353135323533353435353536353735383539354035413542354335443545354635473548354935503551355235533554355535563557355835593560356135623563356435653566356735683569357035713572357335743575357635773578357935803581358235833584358535863587358835893590359135923593359435953596359735983599360036013602360336043605360636073608360936103611361236133614361536163617361836193620362136223623362436253626362736283629363036313632363336343635363636373638363936403641364236433644364536463647364836493650365136523653365436553656365736583659366036613662366336643665366636673668366936703671367236733674367536763677367836793680368136823683368436853686368736883689369036913692369336943695369636973698369937003701370237033704370537063707370837093710371137123713371437153716371737183719372037213722372337243725372637273728372937303731373237333734373537363737373837393740374137423743374437453746374737483749375037513752375337543755375637573758375937603761376237633764376537663767376837693770377137723773377437753776377737783779378037813782378337843785378637873788378937903791379237933794379537963797379837993800380138023803380438053806380738083809381038113812381338143815381638173818381938203821382238233824382538263827382838293830383138323833383438353836383738383839384038413842384338443845384638473848384938503851385238533854385538563857385838593860386138623863386438653866386738683869387038713872387338743875387638773878387938803881388238833884388538863887388838893890389138923893389438953896389738983899390039013902390339043905390639073908390939103911391239133914391539163917391839193920392139223923392439253926392739283929393039313932393339343935393639373938393939403941394239433944394539463947394839493950395139523953395439553956395739583959396039613962396339643965396639673968396939703971397239733974397539763977397839793980398139823983398439853986398739883989399039913992399339943995399639973998399940004001400240034004400540064007400840094010401140124013401440154016401740184019402040214022402340244025402640274028402940304031403240334034403540364037403840394040404140424043404440454046404740484049405040514052405340544055405640574058405940604061406240634064406540664067406840694070407140724073407440754076407740784079408040814082408340844085408640874088408940904091409240934094409540964097409840994100410141024103410441054106410741084109411041114112411341144115411641174118411941204121412241234124412541264127412841294130413141324133413441354136413741384139414041414142414341444145414641474148414941504151415241534154415541564157415841594160416141624163416441654166416741684169417041714172417341744175417641774178417941804181418241834184418541864187418841894190419141924193419441954196419741984199420042014202420342044205420642074208420942104211421242134214421542164217421842194220422142224223422442254226422742284229423042314232423342344235423642374238423942404241424242434244424542464247424842494250425142524253425442554256425742584259426042614262426342644265426642674268426942704271427242734274427542764277427842794280428142824283428442854286428742884289429042914292429342944295429642974298429943004301430243034304430543064307430843094310431143124313431443154316431743184319432043214322432343244325432643274328432943304331433243334334433543364337433843394340434143424343434443454346434743484349435043514352435343544355435643574358435943604361436243634364436543664367436843694370437143724373437443754376437743784379438043814382438343844385438643874388438943904391439243934394439543964397439843994400440144024403440444054406440744084409441044114412441344144415441644174418441944204421442244234424442544264427442844294430443144324433443444354436443744384439444044414442444344444445444644474448444944504451445244534454445544564457445844594460446144624463446444654466446744684469447044714472447344744475447644774478447944804481448244834484448544864487448844894490449144924493449444954496449744984499450045014502450345044505450645074508450945104511451245134514451545164517451845194520452145224523452445254526452745284529453045314532453345344535453645374538453945404541454245434544454545464547454845494550455145524553455445554556455745584559456045614562456345644565456645674568456945704571457245734574457545764577457845794580458145824583458445854586458745884589459045914592459345944595459645974598459946004601460246034604460546064607460846094610461146124613461446154616461746184619462046214622462346244625462646274628462946304631463246334634463546364637463846394640464146424643464446454646464746484649465046514652465346544655465646574658465946604661466246634664466546664667466846694670467146724673467446754676467746784679468046814682468346844685468646874688468946904691469246934694469546964697469846994700470147024703470447054706470747084709471047114712471347144715471647174718471947204721472247234724472547264727472847294730473147324733473447354736473747384739474047414742474347444745474647474748474947504751475247534754475547564757475847594760476147624763476447654766476747684769477047714772477347744775477647774778477947804781478247834784478547864787478847894790479147924793479447954796479747984799480048014802480348044805480648074808480948104811481248134814481548164817481848194820482148224823482448254826482748284829483048314832483348344835483648374838483948404841484248434844484548464847484848494850485148524853485448554856485748584859486048614862486348644865486648674868486948704871487248734874487548764877487848794880488148824883488448854886488748884889489048914892489348944895489648974898489949004901490249034904490549064907490849094910491149124913491449154916491749184919492049214922492349244925492649274928492949304931493249334934493549364937493849394940494149424943494449454946494749484949495049514952495349544955495649574958495949604961496249634964496549664967496849694970497149724973497449754976497749784979498049814982498349844985498649874988498949904991499249934994499549964997499849995000500150025003500450055006500750085009501050115012501350145015501650175018501950205021502250235024502550265027502850295030503150325033503450355036503750385039504050415042504350445045504650475048504950505051505250535054505550565057505850595060506150625063506450655066506750685069507050715072507350745075507650775078507950805081508250835084508550865087508850895090509150925093509450955096509750985099510051015102510351045105510651075108510951105111511251135114511551165117511851195120512151225123512451255126512751285129513051315132513351345135513651375138513951405141514251435144514551465147514851495150515151525153515451555156515751585159516051615162516351645165516651675168516951705171517251735174517551765177517851795180518151825183518451855186518751885189519051915192519351945195519651975198519952005201520252035204520552065207520852095210521152125213521452155216521752185219522052215222522352245225522652275228522952305231523252335234523552365237523852395240524152425243524452455246524752485249525052515252525352545255525652575258525952605261526252635264526552665267526852695270527152725273527452755276527752785279528052815282528352845285528652875288528952905291529252935294529552965297529852995300530153025303530453055306530753085309531053115312531353145315531653175318531953205321532253235324532553265327532853295330533153325333533453355336533753385339534053415342534353445345534653475348534953505351535253535354535553565357535853595360536153625363536453655366536753685369537053715372537353745375537653775378537953805381538253835384538553865387538853895390539153925393539453955396539753985399540054015402540354045405540654075408540954105411541254135414541554165417541854195420542154225423542454255426542754285429543054315432543354345435543654375438543954405441544254435444544554465447544854495450545154525453545454555456545754585459546054615462546354645465546654675468546954705471547254735474547554765477547854795480548154825483548454855486548754885489549054915492549354945495549654975498549955005501550255035504550555065507550855095510551155125513551455155516551755185519552055215522552355245525552655275528552955305531553255335534553555365537553855395540554155425543554455455546554755485549555055515552555355545555555655575558555955605561556255635564556555665567556855695570557155725573557455755576557755785579558055815582558355845585558655875588558955905591559255935594559555965597559855995600560156025603560456055606560756085609561056115612561356145615561656175618561956205621562256235624562556265627562856295630563156325633563456355636563756385639564056415642564356445645564656475648564956505651565256535654565556565657565856595660566156625663566456655666566756685669567056715672567356745675567656775678567956805681568256835684568556865687568856895690569156925693569456955696569756985699570057015702570357045705570657075708570957105711571257135714571557165717571857195720572157225723572457255726572757285729573057315732573357345735573657375738573957405741574257435744574557465747574857495750575157525753575457555756575757585759576057615762576357645765576657675768576957705771577257735774577557765777577857795780578157825783578457855786578757885789579057915792579357945795579657975798579958005801580258035804580558065807580858095810581158125813581458155816581758185819582058215822582358245825582658275828582958305831583258335834583558365837583858395840584158425843584458455846584758485849585058515852585358545855585658575858585958605861586258635864586558665867586858695870587158725873587458755876587758785879588058815882588358845885588658875888588958905891589258935894589558965897589858995900590159025903590459055906590759085909591059115912591359145915591659175918591959205921592259235924592559265927592859295930593159325933593459355936593759385939594059415942594359445945594659475948594959505951595259535954595559565957595859595960596159625963596459655966596759685969597059715972597359745975597659775978597959805981598259835984598559865987598859895990599159925993599459955996599759985999600060016002600360046005600660076008600960106011601260136014601560166017601860196020602160226023602460256026602760286029603060316032603360346035603660376038603960406041604260436044604560466047604860496050605160526053605460556056605760586059606060616062606360646065606660676068606960706071607260736074607560766077607860796080608160826083608460856086608760886089609060916092609360946095609660976098609961006101610261036104610561066107610861096110611161126113611461156116611761186119612061216122612361246125612661276128612961306131613261336134613561366137613861396140614161426143614461456146614761486149615061516152615361546155615661576158615961606161616261636164616561666167616861696170617161726173617461756176617761786179618061816182618361846185618661876188618961906191619261936194619561966197619861996200620162026203
  1. #include "cmPrefix.h"
  2. #include "cmGlobal.h"
  3. #include "cmRpt.h"
  4. #include "cmErr.h"
  5. #include "cmCtx.h"
  6. #include "cmMem.h"
  7. #include "cmMallocDebug.h"
  8. #include "cmLinkedHeap.h"
  9. #include "cmSymTbl.h"
  10. #include "cmFloatTypes.h"
  11. #include "cmComplexTypes.h"
  12. #include "cmFile.h"
  13. #include "cmFileSys.h"
  14. #include "cmProcObj.h"
  15. #include "cmProcTemplate.h"
  16. #include "cmAudioFile.h"
  17. #include "cmMath.h"
  18. #include "cmProc.h"
  19. #include "cmVectOps.h"
  20. #include "cmKeyboard.h"
  21. #include "cmGnuPlot.h"
  22. #include "cmTime.h"
  23. #include "cmMidi.h"
  24. #include "cmProc2.h"
  25. //------------------------------------------------------------------------------------------------------------
  26. cmArray* cmArrayAllocate( cmCtx* c, cmArray* ap, unsigned eleCnt, unsigned eleByteCnt, unsigned flags )
  27. {
  28. cmArray* p = cmObjAlloc( cmArray, c, ap );
  29. if( eleCnt > 0 && eleByteCnt > 0 )
  30. if( cmArrayInit(p, eleCnt, eleByteCnt, flags ) != cmOkRC )
  31. cmArrayFree(&p);
  32. return cmOkRC;
  33. }
  34. cmRC_t cmArrayFree( cmArray** pp )
  35. {
  36. cmRC_t rc = cmOkRC;
  37. cmArray* p = *pp;
  38. if( pp == NULL || *pp == NULL )
  39. return cmOkRC;
  40. if((rc = cmArrayFinal(p)) != cmOkRC )
  41. return rc;
  42. cmMemPtrFree(&p->ptr);
  43. cmObjFree(pp);
  44. return rc;
  45. }
  46. cmRC_t cmArrayInit( cmArray* p, unsigned eleCnt, unsigned eleByteCnt, unsigned flags )
  47. {
  48. cmRC_t rc = cmOkRC;
  49. if((rc = cmArrayFinal(p)) != cmOkRC )
  50. return rc;
  51. p->allocByteCnt = eleCnt * eleByteCnt;
  52. p->ptr = cmIsFlag(flags,kZeroArrayFl) ? cmMemResizeZ( char, p->ptr, p->allocByteCnt ) : cmMemResize( char, p->ptr, p->allocByteCnt );
  53. p->eleCnt = eleCnt;
  54. p->eleByteCnt = eleByteCnt;
  55. return rc;
  56. }
  57. cmRC_t cmArrayFinal( cmArray* p )
  58. { return cmOkRC; }
  59. char* cmArrayReallocDestroy(cmArray* p, unsigned newEleCnt, unsigned newEleByteCnt, unsigned flags )
  60. {
  61. // if memory is expanding
  62. if( newEleCnt * newEleByteCnt > p->allocByteCnt )
  63. cmArrayInit( p, newEleCnt, newEleByteCnt, flags );
  64. else
  65. {
  66. // ... otherwise memory is contrcmting
  67. p->eleCnt = newEleCnt;
  68. p->eleByteCnt = newEleByteCnt;
  69. if( cmIsFlag( flags, kZeroArrayFl ))
  70. memset(p->ptr,0,p->eleByteCnt);
  71. }
  72. return p->ptr;
  73. }
  74. void cmArrayReallocDestroyV(cmArray* p, int eleByteCnt,unsigned flags, ... )
  75. {
  76. unsigned i;
  77. unsigned eleCnt = 0;
  78. unsigned argCnt = 0;
  79. va_list vl0,vl1;
  80. assert(eleByteCnt>0);
  81. va_start(vl0,flags);
  82. va_copy(vl1,vl0);
  83. while( va_arg(vl0,void**) != NULL )
  84. {
  85. int argEleCnt = va_arg(vl0,int);
  86. assert(argEleCnt>0);
  87. eleCnt += argEleCnt;
  88. ++argCnt;
  89. }
  90. va_end(vl0);
  91. char* a = cmArrayReallocDestroy(p,eleCnt,eleByteCnt,flags);
  92. for(i=0; i<argCnt; ++i)
  93. {
  94. void** vp = va_arg(vl1,void**);
  95. unsigned n = va_arg(vl1,unsigned);
  96. *vp = a;
  97. a += n*eleByteCnt;
  98. }
  99. va_end(vl1);
  100. }
  101. char* cmArrayReallocPreserve(cmArray* p, unsigned newEleCnt, unsigned newEleByteCnt, unsigned flags )
  102. {
  103. unsigned cn = p->eleCnt * p->eleByteCnt;
  104. unsigned dn = newEleCnt * newEleByteCnt;
  105. if( dn > p->allocByteCnt )
  106. p->allocByteCnt = dn;
  107. p->ptr = cmIsFlag(flags,kZeroArrayFl ) ? cmMemResizePZ( char, p->ptr, cn ) : cmMemResizeP( char, p->ptr, cn);
  108. p->eleCnt = newEleCnt;
  109. p->eleByteCnt= newEleByteCnt;
  110. return p->ptr;
  111. }
  112. //------------------------------------------------------------------------------------------------------------
  113. cmAudioFileWr* cmAudioFileWrAlloc( cmCtx* c, cmAudioFileWr* ap, unsigned procSmpCnt, const char* fn, double srate, unsigned chCnt, unsigned bitsPerSample )
  114. {
  115. cmAudioFileWr* p = cmObjAlloc( cmAudioFileWr, c, ap );
  116. if( cmAudioFileWrInit( p, procSmpCnt, fn, srate, chCnt, bitsPerSample ) != cmOkRC )
  117. cmObjFree(&p);
  118. return p;
  119. }
  120. cmRC_t cmAudioFileWrFree( cmAudioFileWr** pp )
  121. {
  122. cmRC_t rc = cmOkRC;
  123. if( pp != NULL && *pp != NULL )
  124. {
  125. cmAudioFileWr* p = *pp;
  126. if((rc = cmAudioFileWrFinal(p)) == cmOkRC )
  127. {
  128. cmMemPtrFree(&p->bufV);
  129. cmMemPtrFree(&p->fn );
  130. cmObjFree(pp);
  131. }
  132. }
  133. return rc;
  134. }
  135. cmRC_t cmAudioFileWrInit( cmAudioFileWr* p, unsigned procSmpCnt, const char* fn, double srate, unsigned chCnt, unsigned bitsPerSample )
  136. {
  137. cmRC_t rc;
  138. cmRC_t afRC;
  139. if((rc = cmAudioFileWrFinal( p)) != cmOkRC )
  140. return rc;
  141. p->h = cmAudioFileNewCreate( fn, srate, bitsPerSample, chCnt, &afRC, p->obj.err.rpt );
  142. if( afRC != kOkAfRC )
  143. return cmCtxRtCondition( &p->obj, afRC, "Unable to open the audio file:'%s'", fn );
  144. p->bufV = cmMemResize( cmSample_t, p->bufV, procSmpCnt * chCnt );
  145. p->procSmpCnt = procSmpCnt;
  146. p->chCnt = chCnt;
  147. p->curChCnt = 0;
  148. p->fn = cmMemResizeZ( cmChar_t, p->fn, strlen(fn)+1 );
  149. strcpy(p->fn,fn);
  150. return rc;
  151. }
  152. cmRC_t cmAudioFileWrFinal( cmAudioFileWr* p )
  153. {
  154. cmRC_t afRC;
  155. if( p != NULL )
  156. {
  157. if( cmAudioFileIsValid( p->h ) )
  158. if(( afRC = cmAudioFileDelete( &p->h )) != kOkAfRC )
  159. return cmCtxRtCondition( &p->obj, afRC, "Unable to close the audio file:'%s'", p->fn );
  160. }
  161. return cmOkRC;
  162. }
  163. cmRC_t cmAudioFileWrExec( cmAudioFileWr* p, unsigned chIdx, const cmSample_t* sp, unsigned sn )
  164. {
  165. cmRC_t afRC;
  166. assert( sn <= p->procSmpCnt && chIdx < p->chCnt );
  167. cmSample_t* buf = p->bufV + (chIdx * p->procSmpCnt);
  168. cmVOS_Copy( buf, sn, sp);
  169. if( sn < p->procSmpCnt )
  170. cmVOS_Fill( buf+sn, p->procSmpCnt-sn, 0 );
  171. p->curChCnt++;
  172. if( p->curChCnt == p->chCnt )
  173. {
  174. p->curChCnt = 0;
  175. cmSample_t* bufPtrPtr[ p->chCnt ];
  176. unsigned i = 0;
  177. for(i=0; i<p->chCnt; ++i)
  178. bufPtrPtr[i] = p->bufV + (i*p->procSmpCnt);
  179. if((afRC = cmAudioFileWriteSample( p->h, p->procSmpCnt, p->chCnt, bufPtrPtr )) != kOkAfRC )
  180. return cmCtxRtCondition( &p->obj, afRC, "Write failed on audio file:'%s'", p->fn );
  181. }
  182. return cmOkRC;
  183. }
  184. void cmAudioFileWrTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  185. {
  186. const char* fn = "/home/kevin/src/cm/test0.aif";
  187. double durSecs = 10;
  188. double srate = 44100;
  189. unsigned chCnt = 2;
  190. unsigned bitsPerSmp = 16;
  191. unsigned procSmpCnt = 64;
  192. double hz = 1000;
  193. unsigned overToneCnt= 1;
  194. unsigned smpCnt = durSecs * srate;
  195. unsigned i;
  196. cmCtx* c = cmCtxAlloc( NULL, rpt, lhH, stH );
  197. cmSigGen* sgp = cmSigGenAlloc( c, NULL, procSmpCnt, srate, kWhiteWfId, hz, overToneCnt );
  198. cmAudioFileWr* awp = cmAudioFileWrAlloc( c, NULL, procSmpCnt, fn, srate, chCnt, bitsPerSmp );
  199. for(i=0; i<smpCnt; ++i)
  200. {
  201. cmSigGenExec( sgp );
  202. cmAudioFileWrExec( awp, 0, sgp->outV, sgp->outN );
  203. cmAudioFileWrExec( awp, 1, sgp->outV, sgp->outN );
  204. i += sgp->outN;
  205. }
  206. printf("Frames:%i\n",smpCnt);
  207. cmAudioFileWrFree(&awp);
  208. cmSigGenFree( &sgp );
  209. cmCtxFree(&c);
  210. cmAudioFileReportFn( fn, 0, 20, rpt );
  211. }
  212. //------------------------------------------------------------------------------------------------------------
  213. cmMatrixBuf* cmMatrixBufAllocFile( cmCtx* c, cmMatrixBuf* p, const char* fn )
  214. {
  215. cmRC_t rc;
  216. cmMatrixBuf* op = cmObjAlloc( cmMatrixBuf, c, p );
  217. if( fn != NULL )
  218. if((rc = cmMatrixBufInitFile(op,fn)) != cmOkRC )
  219. cmObjFree(&op);
  220. return op;
  221. }
  222. cmMatrixBuf* cmMatrixBufAllocCopy(cmCtx* c, cmMatrixBuf* p, unsigned rn, unsigned cn, const cmSample_t* sp )
  223. {
  224. cmRC_t rc;
  225. cmMatrixBuf* op = cmObjAlloc( cmMatrixBuf, c, p );
  226. if( sp != NULL && rn > 0 && cn > 0 )
  227. if((rc = cmMatrixBufInitCopy(op,rn,cn,sp)) != cmOkRC )
  228. cmObjFree(&op);
  229. return op;
  230. }
  231. cmMatrixBuf* cmMatrixBufAlloc( cmCtx* c, cmMatrixBuf* p, unsigned rn, unsigned cn )
  232. {
  233. cmRC_t rc;
  234. cmMatrixBuf* op = cmObjAlloc( cmMatrixBuf, c, p );
  235. if( rn > 0 && cn > 0 )
  236. if((rc = cmMatrixBufInit(op,rn,cn)) != cmOkRC )
  237. cmObjFree(&op);
  238. return op;
  239. }
  240. cmRC_t cmMatrixBufFree( cmMatrixBuf** pp )
  241. {
  242. cmRC_t rc = cmOkRC;
  243. if( pp != NULL && *pp != NULL )
  244. {
  245. cmMatrixBuf* p = *pp;
  246. if((rc = cmMatrixBufFinal(p)) == cmOkRC )
  247. {
  248. cmMemPtrFree(&p->bufPtr);
  249. cmObjFree(pp);
  250. }
  251. }
  252. return rc;
  253. }
  254. void _cmMatrixBufGetFileSize( FILE* fp, unsigned* lineCharCntPtr, unsigned* lineCntPtr )
  255. {
  256. *lineCharCntPtr = 0;
  257. *lineCntPtr = 0;
  258. while( !feof(fp) )
  259. {
  260. char ch;
  261. unsigned charCnt = 0;
  262. while( (ch = getc(fp)) != EOF )
  263. {
  264. charCnt++;
  265. if( ch == '\n' )
  266. break;
  267. }
  268. *lineCntPtr += 1;
  269. if(charCnt > *lineCharCntPtr )
  270. *lineCharCntPtr = charCnt;
  271. }
  272. *lineCharCntPtr += 5; // add a safety margin
  273. }
  274. cmRC_t _cmMatrixBufGetMatrixSize( cmObj* op, FILE* fp, unsigned lineCharCnt, unsigned lineCnt, unsigned* rowCntPtr, unsigned * colCntPtr, const char* fn )
  275. {
  276. unsigned i;
  277. char lineBuf[ lineCharCnt + 1 ];
  278. *rowCntPtr = 0;
  279. *colCntPtr = 0;
  280. for(i=0; i<lineCnt; ++i)
  281. {
  282. if(fgets(lineBuf,lineCharCnt,fp)==NULL)
  283. {
  284. // if the last line is blank then return from here
  285. if( feof(fp) )
  286. return cmOkRC;
  287. return cmCtxRtCondition( op, cmSystemErrorRC, "A read error occured on the matrix file:'%s'.",fn);
  288. }
  289. assert( strlen(lineBuf) < lineCharCnt );
  290. char* lp = lineBuf;
  291. char* tp;
  292. // eat any leading white space
  293. while( (*lp) && isspace(*lp) )
  294. ++lp;
  295. // if the line was blank then skip it
  296. if( strlen(lp) == 0 || *lp == '#' )
  297. continue;
  298. (*rowCntPtr) += 1;
  299. unsigned colCnt;
  300. for(colCnt=0; (tp = strtok(lp," ")) != NULL; ++colCnt )
  301. lp = NULL;
  302. if( colCnt > *colCntPtr )
  303. *colCntPtr = colCnt;
  304. }
  305. return cmOkRC;
  306. }
  307. double _cmMatrixBufStrToNum( cmObj* op, const char* cp )
  308. {
  309. double v;
  310. if( sscanf(cp,"%le ",&v) != 1 )
  311. cmCtxRtCondition( op, cmArgAssertRC, "Parse error reading matrix file.");
  312. return v;
  313. }
  314. cmRC_t _cmMatrixBufReadFile(cmObj* op, FILE* fp, cmSample_t* p, unsigned lineCharCnt, unsigned rn, unsigned cn)
  315. {
  316. char lineBuf[ lineCharCnt+1 ];
  317. unsigned ci = 0;
  318. unsigned ri = 0;
  319. while( fgets(lineBuf,lineCharCnt,fp) != NULL )
  320. {
  321. char* lp = lineBuf;
  322. char* tp;
  323. while( (*lp) && isspace(*lp) )
  324. lp++;
  325. if( strlen(lp) == 0 || *lp == '#' )
  326. continue;
  327. for(ci=0; (tp = strtok(lp," ")) != NULL; ++ci )
  328. {
  329. p[ (ci*rn) + ri ] = _cmMatrixBufStrToNum(op,tp); //atof(tp);
  330. lp = NULL;
  331. }
  332. ++ri;
  333. }
  334. return cmOkRC;
  335. }
  336. cmRC_t cmMatrixBufInitFile( cmMatrixBuf* p, const char* fn )
  337. {
  338. cmRC_t rc;
  339. FILE* fp;
  340. unsigned lineCharCnt;
  341. unsigned lineCnt;
  342. unsigned rn;
  343. unsigned cn;
  344. if((fp = fopen(fn,"rt")) == NULL )
  345. return cmCtxRtCondition( &p->obj, cmSystemErrorRC, "Unable to open the matrix file:'%s'", fn );
  346. // get the length of the longest line in the file
  347. _cmMatrixBufGetFileSize(fp,&lineCharCnt,&lineCnt);
  348. rewind(fp);
  349. // get the count of matrix rows and columns
  350. if((rc=_cmMatrixBufGetMatrixSize( &p->obj, fp, lineCharCnt, lineCnt, &rn, &cn, fn )) != cmOkRC )
  351. goto errLabel;
  352. rewind(fp);
  353. // allocate the matrix memory
  354. cmMatrixBufInit(p,rn,cn);
  355. // fill the matrix from the file
  356. rc = _cmMatrixBufReadFile(&p->obj,fp,p->bufPtr,lineCharCnt,rn,cn);
  357. errLabel:
  358. if( rc != cmOkRC )
  359. cmMatrixBufFinal(p);
  360. fclose(fp);
  361. return rc;
  362. }
  363. cmRC_t cmMatrixBufInitCopy( cmMatrixBuf* p, unsigned rn, unsigned cn, const cmSample_t* sp )
  364. {
  365. cmRC_t rc;
  366. if((rc = cmMatrixBufInit(p,rn,cn)) != cmOkRC )
  367. return rc;
  368. cmVOS_Copy(p->bufPtr,(rn*cn),sp);
  369. return rc;
  370. }
  371. cmRC_t cmMatrixBufInit( cmMatrixBuf* p, unsigned rn, unsigned cn )
  372. {
  373. cmRC_t rc;
  374. if((rc = cmMatrixBufFinal(p)) != cmOkRC )
  375. return rc;
  376. p->rn = rn;
  377. p->cn = cn;
  378. p->bufPtr = cmMemResize( cmSample_t, p->bufPtr, rn*cn );
  379. return cmOkRC;
  380. }
  381. cmRC_t cmMatrixBufFinal( cmMatrixBuf* p )
  382. { return cmOkRC; }
  383. cmSample_t* cmMatrixBufColPtr( cmMatrixBuf* p, unsigned ci )
  384. { assert(ci<p->cn); return p->bufPtr + (ci * p->rn); }
  385. cmSample_t* cmMatrixBufRowPtr( cmMatrixBuf* p, unsigned ri )
  386. { assert(ri<p->rn); return p->bufPtr + ri; }
  387. void cmMatrixBufTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  388. {
  389. cmSample_t v[] = {1,2,2,3};
  390. cmCtx* c = cmCtxAlloc(NULL,rpt,lhH,stH);
  391. cmMatrixBuf* mbp = cmMatrixBufAllocFile(c, NULL, "temp.mat" );
  392. cmMatrixBuf* vbp = cmMatrixBufAllocCopy(c, NULL, 4,1,v);
  393. unsigned i;
  394. printf("rn:%i cn:%i\n",mbp->rn,mbp->cn);
  395. //cmVOS_Print( stdout, 10, 10, mbp->bufPtr );
  396. printf("%.1f\n ",cmVOS_Median( cmMatrixBufColPtr(vbp,0),vbp->rn));
  397. for(i=0; i<mbp->cn; ++i)
  398. {
  399. //cmVOS_Print( stdout, 1, mbp->cn, cmMatrixBufColPtr(c,mbp,i) );
  400. printf("%.1f, ",cmVOS_Median( cmMatrixBufColPtr(mbp,i),mbp->rn));
  401. }
  402. printf("\n");
  403. cmMatrixBufFree(&mbp);
  404. cmMatrixBufFree(&vbp);
  405. cmCtxFree(&c);
  406. }
  407. //------------------------------------------------------------------------------------------------------------
  408. cmSigGen* cmSigGenAlloc( cmCtx* c, cmSigGen* p, unsigned procSmpCnt, double srate, unsigned wfId, double fundFrqHz, unsigned overToneCnt )
  409. {
  410. cmSigGen* op = cmObjAlloc( cmSigGen, c, p );
  411. if( procSmpCnt > 0 && srate > 0 && wfId != kInvalidWfId )
  412. if( cmSigGenInit( op, procSmpCnt, srate, wfId, fundFrqHz, overToneCnt ) != cmOkRC )
  413. cmObjFree(&op);
  414. return op;
  415. }
  416. cmRC_t cmSigGenFree( cmSigGen** pp )
  417. {
  418. cmRC_t rc = cmOkRC;
  419. if( pp != NULL && *pp != NULL )
  420. {
  421. cmSigGen* p = *pp;
  422. if((rc = cmSigGenFinal(p)) == cmOkRC )
  423. {
  424. cmMemPtrFree(&p->outV);
  425. cmObjFree(pp);
  426. }
  427. }
  428. return rc;
  429. }
  430. cmRC_t cmSigGenInit( cmSigGen* p, unsigned procSmpCnt, double srate, unsigned wfId, double fundFrqHz, unsigned overToneCnt )
  431. {
  432. assert( srate > 0 && procSmpCnt > 0 );
  433. p->outV = cmMemResize( cmSample_t, p->outV, procSmpCnt );
  434. p->outN = procSmpCnt;
  435. p->wfId = wfId;
  436. p->overToneCnt = overToneCnt;
  437. p->fundFrqHz = fundFrqHz;
  438. p->phase = 0;
  439. p->delaySmp = 0;
  440. p->srate = srate;
  441. return cmOkRC;
  442. }
  443. cmRC_t cmSigGenFinal( cmSigGen* p )
  444. { return cmOkRC; }
  445. cmRC_t cmSigGenExec( cmSigGen* p )
  446. {
  447. switch( p->wfId )
  448. {
  449. case kSineWfId: p->phase = cmVOS_SynthSine( p->outV, p->outN, p->phase, p->srate, p->fundFrqHz ); break;
  450. case kCosWfId: p->phase = cmVOS_SynthCosine( p->outV, p->outN, p->phase, p->srate, p->fundFrqHz ); break;
  451. case kSquareWfId: p->phase = cmVOS_SynthSquare( p->outV, p->outN, p->phase, p->srate, p->fundFrqHz, p->overToneCnt ); break;
  452. case kTriangleWfId: p->phase = cmVOS_SynthTriangle( p->outV, p->outN, p->phase, p->srate, p->fundFrqHz, p->overToneCnt ); break;
  453. case kSawtoothWfId: p->phase = cmVOS_SynthSawtooth( p->outV, p->outN, p->phase, p->srate, p->fundFrqHz, p->overToneCnt ); break;
  454. case kWhiteWfId: cmVOS_Random( p->outV, p->outN, -1.0, 1.0 ); break;
  455. case kPinkWfId: p->delaySmp = cmVOS_SynthPinkNoise(p->outV, p->outN, p->delaySmp ); break;
  456. case kPulseWfId: p->phase = cmVOS_SynthPulseCos( p->outV, p->outN, p->phase, p->srate, p->fundFrqHz, p->overToneCnt ); break;
  457. case kImpulseWfId: p->phase = cmVOS_SynthImpulse( p->outV, p->outN, p->phase, p->srate, p->fundFrqHz ); break;
  458. case kSilenceWfId: cmVOS_Fill( p->outV, p->outN, 0 ); break;
  459. case kPhasorWfId: p->phase = cmVOS_SynthPhasor( p->outV, p->outN, p->phase, p->srate, p->fundFrqHz ); break;
  460. case kSeqWfId: p->phase = cmVOS_Seq( p->outV, p->outN, p->phase, 1 ); break;
  461. case kInvalidWfId:
  462. default:
  463. return cmCtxRtAssertFailed( &p->obj, 0, "Invalid waveform shape.");
  464. }
  465. return cmOkRC;
  466. }
  467. //------------------------------------------------------------------------------------------------------------
  468. cmDelay* cmDelayAlloc( cmCtx* c, cmDelay* ap, unsigned procSmpCnt, unsigned delaySmpCnt )
  469. {
  470. cmDelay* p = cmObjAlloc( cmDelay, c, ap );
  471. if( procSmpCnt > 0 && delaySmpCnt > 0 )
  472. if( cmDelayInit( p,procSmpCnt,delaySmpCnt) != cmOkRC && ap == NULL )
  473. cmObjFree(&p);
  474. return p;
  475. }
  476. cmRC_t cmDelayFree( cmDelay** pp )
  477. {
  478. cmRC_t rc = cmOkRC;
  479. if( pp != NULL && *pp != NULL )
  480. {
  481. cmDelay* p = *pp;
  482. if((rc = cmDelayFinal(*pp)) == cmOkRC )
  483. {
  484. cmMemPtrFree(&p->bufPtr);
  485. cmObjFree(pp);
  486. }
  487. }
  488. return rc;
  489. }
  490. cmRC_t cmDelayInit( cmDelay* p, unsigned procSmpCnt, unsigned delaySmpCnt )
  491. {
  492. p->procSmpCnt = procSmpCnt;
  493. p->delaySmpCnt = delaySmpCnt;
  494. p->bufSmpCnt = delaySmpCnt + procSmpCnt;
  495. p->bufPtr = cmMemResizeZ( cmSample_t, p->bufPtr, p->bufSmpCnt);
  496. p->delayInIdx = 0;
  497. p->outCnt = 1;
  498. p->outV[0] = p->bufPtr;
  499. p->outN[0] = p->procSmpCnt;
  500. p->outV[1] = NULL;
  501. p->outN[1] = 0;
  502. return cmOkRC;
  503. }
  504. cmRC_t cmDelayFinal( cmDelay* p )
  505. { return cmOkRC; }
  506. cmRC_t cmDelayCopyIn( cmDelay* p, const cmSample_t* sp, unsigned sn )
  507. {
  508. assert(sn<=p->procSmpCnt);
  509. unsigned n0 = cmMin(sn,p->bufSmpCnt - p->delayInIdx);
  510. // copy as many samples as possible from the input to the delayInIdx
  511. cmVOS_Copy(p->bufPtr + p->delayInIdx, n0, sp);
  512. p->delayInIdx = (p->delayInIdx + n0) % p->bufSmpCnt;
  513. // if there was not enough room to copy all the samples into the end of the buffer ....
  514. if( n0 < sn )
  515. {
  516. assert( p->delayInIdx == 0 );
  517. // ... then copy the rest to the beginning of the buffer
  518. unsigned n1 = sn - n0;
  519. cmVOS_Copy(p->bufPtr,n1, sp + n0 );
  520. p->delayInIdx = (p->delayInIdx + n1) % p->bufSmpCnt;
  521. }
  522. return cmOkRC;
  523. }
  524. cmRC_t cmDelayAdvance( cmDelay* p, unsigned sn )
  525. {
  526. // advance the output by sn and make sn samples available
  527. int delayOutIdx = ((p->outV[0] - p->bufPtr) + sn) % p->bufSmpCnt;
  528. p->outV[0] = p->bufPtr + delayOutIdx;
  529. p->outN[0] = cmMin(p->bufSmpCnt - delayOutIdx , sn );
  530. p->outCnt = p->outN[0] == sn ? 1 : 2 ;
  531. p->outV[1] = p->outCnt == 1 ? NULL : p->bufPtr;
  532. p->outN[1] = p->outCnt == 1 ? 0 : sn - p->outN[0];
  533. return cmOkRC;
  534. }
  535. cmRC_t cmDelayExec( cmDelay* p, const cmSample_t* sp, unsigned sn, bool bypassFl )
  536. {
  537. cmRC_t rc = cmOkRC;
  538. if( bypassFl )
  539. memcpy(p->outV,sp,sn*sizeof(cmSample_t));
  540. else
  541. {
  542. cmDelayCopyIn(p,sp,sn);
  543. rc = cmDelayAdvance(p,sn);
  544. }
  545. return rc;
  546. }
  547. void cmDelayTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  548. {
  549. cmCtx ctx;
  550. cmDelay delay;
  551. cmSigGen sigGen;
  552. unsigned procCnt = 4;
  553. unsigned procSmpCnt = 5;
  554. unsigned delaySmpCnt = 3;
  555. unsigned i;
  556. cmCtx* c = cmCtxAlloc( &ctx, rpt, lhH, stH );
  557. cmDelay* dlp = cmDelayAlloc( c, &delay, procSmpCnt, delaySmpCnt );
  558. cmSigGen* sgp = cmSigGenAlloc( c, &sigGen, procSmpCnt, 0, kSeqWfId,0, 0);
  559. for(i=0; i<procCnt; ++i)
  560. {
  561. cmSigGenExec(sgp);
  562. cmDelayExec(dlp,sgp->outV,sgp->outN,false);
  563. //cmVOS_Print( c->outFp, 1, sgp->outN, sgp->outV, 5, 0 );
  564. cmCtxPrint(c,"%i %i : ",i,0);
  565. cmVOS_PrintE( rpt, 1, dlp->outN[0], dlp->outV[0] );
  566. if( dlp->outN[1] > 0 )
  567. {
  568. cmCtxPrint(c,"%i %i : ",i,1);
  569. cmVOS_PrintE( rpt, 1, dlp->outN[1], dlp->outV[1] );
  570. }
  571. }
  572. cmSigGenFinal(sgp);
  573. cmDelayFinal(dlp);
  574. cmCtxFinal(c);
  575. }
  576. //------------------------------------------------------------------------------------------------------------
  577. cmFIR* cmFIRAllocKaiser(cmCtx* c, cmFIR* p, unsigned procSmpCnt, double srate, double passHz, double stopHz, double passDb, double stopDb )
  578. {
  579. cmFIR* op = cmObjAlloc( cmFIR, c, p );
  580. if( procSmpCnt > 0 && srate > 0 )
  581. if( cmFIRInitKaiser(op,procSmpCnt,srate,passHz,stopHz,passDb,stopDb) != cmOkRC )
  582. cmObjFree(&op);
  583. return op;
  584. }
  585. cmFIR* cmFIRAllocSinc( cmCtx* c, cmFIR* p, unsigned procSmpCnt, double srate, unsigned sincSmpCnt, double fcHz, unsigned flags )
  586. {
  587. cmFIR* op = cmObjAlloc( cmFIR, c, p );
  588. if( srate > 0 && sincSmpCnt > 0 )
  589. if( cmFIRInitSinc(op,procSmpCnt,srate,sincSmpCnt,fcHz,flags) != cmOkRC )
  590. cmObjFree(&op);
  591. return op;
  592. }
  593. cmRC_t cmFIRFree( cmFIR** pp )
  594. {
  595. cmRC_t rc = cmOkRC;
  596. if( pp != NULL && *pp != NULL)
  597. {
  598. cmFIR* p = *pp;
  599. if((rc = cmFIRFinal(*pp)) != cmOkRC )
  600. {
  601. cmMemPtrFree(&p->coeffV);
  602. cmMemPtrFree(&p->outV);
  603. cmMemPtrFree(&p->delayV);
  604. cmObjFree(pp);
  605. }
  606. }
  607. return rc;
  608. }
  609. cmRC_t cmFIRInitKaiser( cmFIR* p, unsigned procSmpCnt, double srate, double passHz, double stopHz, double passDb, double stopDb )
  610. {
  611. // pass/stop frequencies above nyquist produce incorrect results
  612. assert( passHz <= srate/2 && stopHz<=srate/2);
  613. // based on Orfandis, Introduction to Signal Processing, p.551 Prentice Hall, 1996
  614. double fcHz = (passHz + stopHz) / 2; // fc is half way between passHz and stopHz
  615. double dHz = fabs(stopHz-passHz);
  616. //double signFcmt = stopHz > passHz ? 1 : -1;
  617. // convert ripple spec from db to linear
  618. double dPass = (pow(10,passDb/20)-1) / (pow(10,passDb/20)+1);
  619. double dStop = pow(10,-stopDb/20);
  620. // in prcmtice the ripple must be equal in the stop and pass band - so take the minimum between the two
  621. double d = cmMin(dPass,dStop);
  622. // convert the ripple bcmk to db
  623. double A = -20 * log10(d);
  624. // compute the kaiser alpha coeff
  625. double alpha = 0;
  626. if( A >= 50.0 ) // for ripple > 50
  627. alpha = 0.1102 * (A-8.7);
  628. else // for ripple <= 21
  629. {
  630. if( A > 21 )
  631. alpha = (0.5842 * (pow(A-21.0,0.4))) + (0.07886*(A-21));
  632. }
  633. double D = 0.922;
  634. if( A > 21 )
  635. D = (A - 7.95) / 14.36;
  636. // compute the filter order
  637. unsigned N = (unsigned)(floor(D * srate / dHz) + 1);
  638. //if N is even
  639. if( cmIsEvenU(N) )
  640. N = N + 1;
  641. //printf("fc=%f df=%f dPass=%f dStop=%f d=%f alpha=%f A=%f D=%f N=%i\n",fcHz,dHz,dPass,dStop,d,alpha,A,D,N);
  642. // form an ideal FIR LP impulse response based on a sinc function
  643. cmFIRInitSinc(p,procSmpCnt,srate,N,fcHz,0);
  644. // compute a kaiser function to truncate the sinc
  645. double wnd[ N ];
  646. cmVOD_Kaiser( wnd, N, alpha );
  647. // apply the kaiser window to the sinc function
  648. cmVOD_MultVV( p->coeffV, p->coeffCnt, wnd );
  649. double sum = cmVOD_Sum(p->coeffV,p->coeffCnt);
  650. cmVOD_DivVS(p->coeffV,p->coeffCnt,sum );
  651. //cmVOD_Print(stdout,1,p->coeffCnt,p->coeffV);
  652. return cmOkRC;
  653. }
  654. cmRC_t cmFIRInitSinc( cmFIR* p, unsigned procSmpCnt, double srate, unsigned sincSmpCnt, double fcHz, unsigned flags )
  655. {
  656. cmRC_t rc;
  657. if((rc = cmFIRFinal(p)) != cmOkRC )
  658. return rc;
  659. p->coeffCnt = sincSmpCnt;
  660. p->outV = cmMemResizeZ( cmSample_t, p->outV, procSmpCnt );
  661. p->outN = procSmpCnt;
  662. p->coeffV = cmMemResizeZ( double, p->coeffV, p->coeffCnt );
  663. p->delayV = cmMemResizeZ( double, p->delayV, p->coeffCnt-1 ); // there is always one less delay than coeff
  664. p->delayIdx = 0;
  665. cmVOD_LP_Sinc(p->coeffV, p->coeffCnt, srate, fcHz, cmIsFlag(flags,kHighPassFIRFl) ? kHighPass_LPSincFl : 0 );
  666. return cmOkRC;
  667. }
  668. cmRC_t cmFIRFinal( cmFIR* p )
  669. { return cmOkRC; }
  670. cmRC_t cmFIRExec( cmFIR* p, const cmSample_t* sbp, unsigned sn )
  671. {
  672. unsigned delayCnt = p->coeffCnt-1;
  673. int di = p->delayIdx;
  674. const cmSample_t* sep = sbp + sn;
  675. cmSample_t* op = p->outV;
  676. assert( di < delayCnt );
  677. assert( sn <= p->outN );
  678. // for each input sample
  679. while( sbp < sep )
  680. {
  681. // advance the delay line
  682. p->delayIdx = (p->delayIdx + 1) % delayCnt;
  683. const double* cbp = p->coeffV;
  684. const double* cep = cbp + p->coeffCnt;
  685. // mult input sample by coeff[0]
  686. double v = *sbp * *cbp++;
  687. // calc the output sample
  688. while( cbp<cep)
  689. {
  690. // note that the delay is being iterated bcmkwards
  691. if( di == -1 )
  692. di=delayCnt-1;
  693. v += *cbp++ * p->delayV[di];
  694. --di;
  695. }
  696. // store the output sample
  697. *op++ = v;
  698. // insert the input sample
  699. p->delayV[ p->delayIdx ] = *sbp++;
  700. // store the position of the newest ele in the delay line
  701. di = p->delayIdx;
  702. }
  703. return cmOkRC;
  704. }
  705. void cmFIRTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  706. {
  707. unsigned N = 512;
  708. cmKbRecd kb;
  709. cmCtx c;
  710. cmCtxInit(&c,rpt,lhH,stH);
  711. double srate = N;
  712. unsigned procSmpCnt = N;
  713. cmPlotSetup("Test Proc Impl",2,1);
  714. cmSample_t in[ procSmpCnt ];
  715. cmVOS_Fill(in,procSmpCnt,0);
  716. in[0] = 1;
  717. cmVOS_Random(in,procSmpCnt, -1.0, 1.0 );
  718. cmFIR* ffp = cmFIRAllocKaiser( &c, NULL, procSmpCnt,srate, srate*0.025, srate/2, 10, 60 );
  719. //cmFIR* ffp = cmFIRAllocSinc( &c, NULL, 32, 1000, 0 );
  720. cmFftSR* ftp = cmFftAllocSR( &c, NULL, ffp->outV, ffp->outN, kToPolarFftFl );
  721. cmFIRExec( ffp, in, procSmpCnt );
  722. cmFftExecSR( ftp, NULL, 0 );
  723. cmVOR_AmplitudeToDb(ftp->magV,ftp->binCnt,ftp->magV);
  724. printf("coeff cnt:%i\n",ffp->coeffCnt );
  725. cmPlotClear();
  726. cmPlotLineR( "test", NULL, ftp->magV, NULL, ftp->binCnt, NULL, kSolidPlotLineId );
  727. cmPlotDraw();
  728. cmKeyPress(&kb);
  729. cmFftFreeSR(&ftp);
  730. cmFIRFree(&ffp);
  731. }
  732. //------------------------------------------------------------------------------------------------------------
  733. cmFuncFilter* cmFuncFilterAlloc( cmCtx* c, cmFuncFilter* p, unsigned procSmpCnt, cmFuncFiltPtr_t funcPtr, void* userPtr, unsigned wndSmpCnt )
  734. {
  735. cmRC_t rc;
  736. cmFuncFilter* op = cmObjAlloc( cmFuncFilter,c, p );
  737. if( procSmpCnt > 0 && funcPtr != NULL && wndSmpCnt > 0 )
  738. {
  739. if( cmShiftBufAlloc(c,&p->shiftBuf,0,0,0) != NULL )
  740. if((rc = cmFuncFilterInit(op,procSmpCnt,funcPtr,userPtr,wndSmpCnt)) != cmOkRC )
  741. {
  742. cmShiftBuf* sbp = &p->shiftBuf;
  743. cmShiftBufFree(&sbp);
  744. cmObjFree(&op);
  745. }
  746. }
  747. return op;
  748. }
  749. cmRC_t cmFuncFilterFree( cmFuncFilter** pp )
  750. {
  751. cmRC_t rc = cmOkRC;
  752. if( pp!=NULL && *pp != NULL )
  753. {
  754. cmFuncFilter* p = *pp;
  755. if((rc = cmFuncFilterFinal(*pp)) == cmOkRC )
  756. {
  757. cmShiftBuf* sbp = &p->shiftBuf;
  758. cmShiftBufFree(&sbp);
  759. cmMemPtrFree(&p->outV);
  760. cmObjFree(pp);
  761. }
  762. }
  763. return rc;
  764. }
  765. cmRC_t cmFuncFilterInit( cmFuncFilter* p, unsigned procSmpCnt, cmFuncFiltPtr_t funcPtr, void* userPtr, unsigned wndSmpCnt )
  766. {
  767. cmRC_t rc;
  768. if(( rc = cmFuncFilterFinal(p)) != cmOkRC )
  769. return rc;
  770. // The shift buffer always consits of the p->wndSmpCnt-1 samples from the previous
  771. // exec followed by the latest procSmpCnt samples at the end of the buffer
  772. cmShiftBufInit( &p->shiftBuf, procSmpCnt, wndSmpCnt + procSmpCnt - 1, procSmpCnt );
  773. p->outV = cmMemResizeZ( cmSample_t, p->outV, procSmpCnt);
  774. p->outN = procSmpCnt;
  775. p->funcPtr = funcPtr;
  776. p->curWndSmpCnt = 1;
  777. p->wndSmpCnt = wndSmpCnt;
  778. return rc;
  779. }
  780. cmRC_t cmFuncFilterFinal( cmFuncFilter* p )
  781. { return cmOkRC; }
  782. cmRC_t cmFuncFilterExec( cmFuncFilter* p, const cmSample_t* sp, unsigned sn )
  783. {
  784. assert( sn <= p->outN);
  785. // The window used by this function is always causal. At the very beginning of the signal
  786. // the window length begins at 1 and increases until is has the length p->wndSmpCnt.
  787. // Note that this approach ignores any zeros automatically prepended to the beginning of the
  788. // signal by the shift buffer. The first window processed always has a length of 1 and
  789. // begins with the first actual sample given to the shift buffer. Successive windows increase
  790. // by one and start at the first actual sample until the full window length is available
  791. // from the shift buffer. At this point the window length remains constant and it is hopped
  792. // by one sample for each window.
  793. while(cmShiftBufExec(&p->shiftBuf,sp,sn))
  794. {
  795. const cmSample_t* fsp = p->shiftBuf.outV;
  796. cmSample_t* dp = p->outV;
  797. cmSample_t* ep = p->outV + sn; // produce as many output values as there are input samples
  798. // for each output sample
  799. while( dp < ep )
  800. {
  801. // the source range should never extend outside the shift buffer
  802. assert( fsp + p->curWndSmpCnt <= p->shiftBuf.outV + p->shiftBuf.wndSmpCnt );
  803. // calc the next output value
  804. *dp++ = p->funcPtr( fsp, p->curWndSmpCnt, p->userPtr );
  805. // if the window has not yet achieved its full length ...
  806. if( p->curWndSmpCnt < p->wndSmpCnt )
  807. ++p->curWndSmpCnt; // ... then increase its length by 1
  808. else
  809. ++fsp; // ... otherwise shift it ahead by 1
  810. }
  811. }
  812. return cmOkRC;
  813. }
  814. cmSample_t cmFuncFiltTestFunc( const cmSample_t* sp, unsigned sn, void* vp )
  815. {
  816. //printf("% f % f %p % i\n",*sp,*sp+(sn-1),sp,sn);
  817. cmSample_t v = cmVOS_Median(sp,sn);
  818. printf("%f ",v);
  819. return v;
  820. //return *sp;
  821. }
  822. void cmFuncFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  823. {
  824. unsigned procSmpCnt = 1;
  825. unsigned N = 32;
  826. cmSample_t v[N];
  827. cmCtx c;
  828. unsigned i;
  829. cmCtxAlloc(&c,rpt,lhH,stH);
  830. cmVOS_Seq(v,N,0,1);
  831. cmVOS_Print(rpt,1,32,v);
  832. cmFuncFilter* ffp = NULL;
  833. ffp = cmFuncFilterAlloc( &c, NULL, procSmpCnt, cmFuncFiltTestFunc, NULL, 5 );
  834. for(i=0; i<N; ++i)
  835. cmFuncFilterExec(ffp,v+(i*procSmpCnt),procSmpCnt);
  836. cmFuncFilterFree( &ffp );
  837. cmCtxFinal(&c);
  838. //unsigned v1n = 9;
  839. //cmSample_t v1[9] = { 1, 75, 91, 35, 6, 80, 40, 91, 79};
  840. //cmSample_t v1[10] = {53, 64, 48, 78, 30, 59, 7, 50, 71, 53 };
  841. //printf("Median: %f \n",cmVOS_Median(v1,v1+v1n));
  842. }
  843. //------------------------------------------------------------------------------------------------------------
  844. cmDhmm* cmDhmmAlloc( cmCtx* c, cmDhmm* ap, unsigned stateN, unsigned symN, cmReal_t* initV, cmReal_t* transM, cmReal_t* stsM )
  845. {
  846. cmDhmm* p = cmObjAlloc( cmDhmm, c, ap );
  847. if( stateN > 0 && symN > 0 )
  848. if( cmDhmmInit(p, stateN, symN, initV, transM, stsM ) != cmOkRC )
  849. cmObjFree(&p);
  850. return p;
  851. }
  852. cmRC_t cmDhmmFree( cmDhmm** pp )
  853. {
  854. cmRC_t rc = cmOkRC;
  855. cmDhmm* p = *pp;
  856. if( pp==NULL || *pp==NULL )
  857. return cmOkRC;
  858. if((rc = cmDhmmFinal(p)) != cmOkRC )
  859. return cmOkRC;
  860. cmObjFree(pp);
  861. return rc;
  862. }
  863. cmRC_t cmDhmmInit( cmDhmm* p, unsigned stateN, unsigned symN, cmReal_t* initV, cmReal_t* transM, cmReal_t* stsM )
  864. {
  865. cmRC_t rc;
  866. if((rc = cmDhmmFinal(p)) != cmOkRC )
  867. return rc;
  868. p->stateN = stateN;
  869. p->symN = symN;
  870. p->initV = initV;
  871. p->transM = transM;
  872. p->stsM = stsM;
  873. return cmOkRC;
  874. }
  875. cmRC_t cmDhmmFinal( cmDhmm* p )
  876. { return cmOkRC; }
  877. cmRC_t cmDhmmExec( cmDhmm* p )
  878. {
  879. return cmOkRC;
  880. }
  881. // Generate a random matrix with rows that sum to 1.0.
  882. void _cmDhmmGenRandMatrix( cmReal_t* dp, unsigned rn, unsigned cn )
  883. {
  884. cmReal_t v[ cn ];
  885. unsigned i,j;
  886. for(i=0; i<rn; ++i)
  887. {
  888. cmVOR_Random( v, cn, 0.0, 1.0 );
  889. cmVOR_NormalizeProbability( v, cn);
  890. for(j=0; j<cn; ++j)
  891. dp[ (j * rn) + i ] = v[j];
  892. }
  893. }
  894. enum { kEqualProbHmmFl=0x01, kRandProbHmmFl=0x02, kManualProbHmmFl=0x04 };
  895. void _cmDhmmGenProb( cmReal_t* dp, unsigned rn, unsigned cn, unsigned flags, const cmReal_t* sp )
  896. {
  897. switch( flags )
  898. {
  899. case kRandProbHmmFl:
  900. _cmDhmmGenRandMatrix( dp, rn, cn );
  901. break;
  902. case kEqualProbHmmFl:
  903. {
  904. // equal prob
  905. cmReal_t pr = 1.0/cn;
  906. unsigned i,j;
  907. for(i=0; i<rn; ++i)
  908. for(j=0; j<cn; ++j)
  909. dp[ (j*rn) + i ] = pr;
  910. }
  911. break;
  912. case kManualProbHmmFl:
  913. cmVOR_Copy( dp, (rn*cn), sp );
  914. break;
  915. default:
  916. assert(0);
  917. }
  918. }
  919. // generate a random integer in the range 0 to probN-1 where probV[ probN ] contains
  920. // the probability of generating each of the possible values.
  921. unsigned _cmDhmmGenRandInt( const cmReal_t* probV, unsigned probN, unsigned stride )
  922. {
  923. cmReal_t tmp[ probN ];
  924. cmReal_t cumSumV[ probN+1 ];
  925. const cmReal_t* sp = probV;
  926. cumSumV[0] = 0;
  927. if( stride > 1 )
  928. {
  929. cmVOR_CopyStride( tmp, probN, probV, stride );
  930. sp = tmp;
  931. }
  932. cmVOR_CumSum( cumSumV+1, probN, sp );
  933. return cmVOR_BinIndex( cumSumV, probN+1, (cmReal_t)rand()/RAND_MAX );
  934. }
  935. cmRC_t cmDhmmGenObsSequence( cmDhmm* p, unsigned* dbp, unsigned dn )
  936. {
  937. const unsigned* dep = dbp + dn;
  938. // generate the first state based on the init state prob. vector
  939. unsigned state = _cmDhmmGenRandInt( p->initV, p->stateN, 1 );
  940. // generate an observation from the state based on the symbol prob. vector
  941. *dbp++ = _cmDhmmGenRandInt( p->stsM + state, p->symN, p->stateN );
  942. while( dbp < dep )
  943. {
  944. // get the next state based on the previous state
  945. state = _cmDhmmGenRandInt( p->transM + state, p->stateN, p->stateN );
  946. // given a state generate an observation
  947. *dbp++ = _cmDhmmGenRandInt( p->stsM + state, p->symN, p->stateN );
  948. }
  949. return cmOkRC;
  950. }
  951. /// Perform a forward evaluation of the model given a set of observations.
  952. /// initPrV[ stateN ] is the probability of the model being in each state at the start of the evaluation.
  953. /// alphaM[ stateN, obsN ] is a return value and represents the probability of seeing each symbol at each time step.
  954. enum { kNoLogScaleHmmFl = 0x00, kLogScaleHmmFl = 0x01 };
  955. cmRC_t cmDHmmForwardEval( cmDhmm* p, const cmReal_t* initPrV, const unsigned* obsV, unsigned obsN, cmReal_t* alphaM, unsigned flags, cmReal_t* logProbPtr )
  956. {
  957. bool scaleFl = cmIsFlag(flags,kLogScaleHmmFl);
  958. cmReal_t logProb = 0;
  959. cmReal_t* abp = alphaM; // define first dest. column
  960. const cmReal_t* aep = abp + p->stateN;
  961. const cmReal_t* sts = p->stsM + (obsV[0] * p->stateN); // stsM[] column for assoc'd with first obs. symbol
  962. unsigned i;
  963. // calc the prob of begining in each state given the obs. symbol
  964. for(i=0; abp < aep; ++i )
  965. *abp++ = *initPrV++ * *sts++;
  966. // scale to prevent underflow
  967. if( scaleFl )
  968. {
  969. cmReal_t sum = cmVOR_Sum(abp-p->stateN,p->stateN);
  970. if( sum > 0 )
  971. {
  972. cmVOR_DivVS( abp-p->stateN,p->stateN,sum);
  973. logProb += log(sum);
  974. }
  975. }
  976. // for each time step
  977. for(i=1; i<obsN; ++i)
  978. {
  979. // next state 0 (first col, first row) is calc'd first
  980. const cmReal_t* tm = p->transM;
  981. // pick the stsM[] column assoc'd with ith observation symbol
  982. const cmReal_t* sts = p->stsM + (obsV[i] * p->stateN);
  983. // store a pointer to the alpha column assoc'd with obsV[i-1]
  984. const cmReal_t* app0 = abp - p->stateN;
  985. aep = abp + p->stateN;
  986. // for each dest state
  987. while( abp < aep )
  988. {
  989. // prob of being in each state on the previous time step
  990. const cmReal_t* app = app0;
  991. const cmReal_t* ape = app + p->stateN;
  992. *abp = 0;
  993. // for each src state - calc prob. of trans from src to dest
  994. while( app<ape )
  995. *abp += *app++ * *tm++;
  996. // calc prob of obs symbol in dest state
  997. *abp++ *= *sts++;
  998. }
  999. // scale to prevent underflow
  1000. if( scaleFl )
  1001. {
  1002. cmReal_t sum = cmVOR_Sum(abp-p->stateN,p->stateN);
  1003. if( sum > 0 )
  1004. {
  1005. cmVOR_DivVS( abp-p->stateN,p->stateN,sum);
  1006. logProb += log(sum);
  1007. }
  1008. }
  1009. }
  1010. if( logProbPtr != NULL )
  1011. *logProbPtr = logProb;
  1012. return cmOkRC;
  1013. }
  1014. cmRC_t cmDHmmBcmkwardEval( cmDhmm* p, const unsigned* obsV, unsigned obsN, cmReal_t* betaM, unsigned flags )
  1015. {
  1016. bool scaleFl = cmIsFlag(flags,kLogScaleHmmFl);
  1017. int i,j,t;
  1018. cmVOR_Fill(betaM+((obsN-1)*p->stateN),p->stateN,1.0);
  1019. // for each time step
  1020. for(t=obsN-2; t>=0; --t)
  1021. {
  1022. // for each state at t
  1023. for(i=0; i<p->stateN; ++i)
  1024. {
  1025. double Bt = 0;
  1026. // for each state at t+1
  1027. for(j=0; j<p->stateN; ++j)
  1028. {
  1029. double aij = p->transM[ (j * p->stateN) + i ];
  1030. double bj = p->stsM[ (obsV[t+1] * p->stateN) + j ];
  1031. double Bt1 = betaM[ ((t+1) * p->stateN) + j ];
  1032. Bt += aij * bj * Bt1;
  1033. }
  1034. betaM[ (t * p->stateN) + i ] = Bt;
  1035. }
  1036. if( scaleFl )
  1037. {
  1038. double* bp = betaM + (t * p->stateN);
  1039. double sum = cmVOR_Sum(bp, p->stateN );
  1040. if( sum > 0 )
  1041. cmVOR_DivVS( bp, p->stateN, sum );
  1042. }
  1043. }
  1044. return cmOkRC;
  1045. }
  1046. void _cmDhmmNormRow( cmReal_t* p, unsigned pn, unsigned stride, const cmReal_t* sp )
  1047. {
  1048. if( sp == NULL )
  1049. sp = p;
  1050. cmReal_t sum = 0;
  1051. unsigned n = pn * stride;
  1052. const cmReal_t* bp = sp;
  1053. const cmReal_t* ep = bp + n;
  1054. for(; bp<ep; bp+=stride)
  1055. sum += *bp;
  1056. for(ep = p+n; p<ep; p+=stride,sp+=stride)
  1057. *p = *sp / sum;
  1058. }
  1059. void _cmDhmmNormMtxRows( cmReal_t* dp, unsigned rn, unsigned cn, const cmReal_t* sp )
  1060. {
  1061. const cmReal_t* erp = sp + rn;
  1062. while( sp < erp )
  1063. _cmDhmmNormRow( dp++, cn, rn, sp++ );
  1064. }
  1065. cmRC_t cmDhmmTrainEM( cmDhmm* p, const unsigned* obsV, unsigned obsN, unsigned iterCnt, unsigned flags )
  1066. {
  1067. unsigned i,j,k,t;
  1068. cmReal_t alphaM[ p->stateN * obsN ];
  1069. cmReal_t betaM[ p->stateN * obsN ];
  1070. cmReal_t g[ p->stateN * obsN ];
  1071. cmReal_t q[ p->stateN * p->symN ];
  1072. cmReal_t E[ p->stateN * p->stateN ];
  1073. cmReal_t logProb = 0;
  1074. //cmDhmmReport(p->obj.ctx,p);
  1075. for(k=0; k<iterCnt; ++k)
  1076. {
  1077. cmVOR_Fill( q, (p->stateN * p->symN), 0 );
  1078. cmVOR_Fill( E, (p->stateN * p->stateN), 0 );
  1079. // calculate alpha and beta
  1080. cmDHmmForwardEval( p, p->initV, obsV, obsN, alphaM, flags, &logProb);
  1081. cmDHmmBcmkwardEval( p, obsV, obsN, betaM, flags );
  1082. // gamma[ stateN, obsN ] = alphaM .* betaM - gamma is the probability of being in each state at each time step
  1083. cmVOR_MultVVV( g,(p->stateN*obsN), alphaM, betaM );
  1084. // normalize gamma
  1085. for(i=0; i<obsN; ++i)
  1086. cmVOR_NormalizeProbability( g + (i*p->stateN), p->stateN );
  1087. //printf("ITER:%i logProb:%f\n",k,logProb);
  1088. // count the number of times state i emits obsV[0] in the starting location
  1089. cmVOR_Copy( q + (obsV[0] * p->stateN), p->stateN, g );
  1090. for(t=0; t<obsN-1; ++t)
  1091. {
  1092. // point to alpha[:,t] and beta[:,t+1]
  1093. const cmReal_t* alpha_t0 = alphaM + (t*p->stateN);
  1094. const cmReal_t* beta_t1 = betaM + ((t+1)*p->stateN);
  1095. cmReal_t Et[ p->stateN * p->stateN ];
  1096. cmReal_t Esum = 0;
  1097. // for each source state
  1098. for(i=0; i<p->stateN; ++i)
  1099. {
  1100. // for each dest state
  1101. for(j=0; j<p->stateN; ++j)
  1102. {
  1103. // prob of transitioning from state i to j and emitting obs[t] at time t
  1104. cmReal_t Eps = alpha_t0[i] * p->transM[ (j*p->stateN) + i ] * p->stsM[ (obsV[t+1]*p->stateN) + j ] * beta_t1[j];
  1105. // count the number of transitions from i to j
  1106. Et[ (j*p->stateN) + i ] = Eps;
  1107. Esum += Eps;
  1108. }
  1109. // count the number of times state i emits obsV[t]
  1110. q[ (obsV[t+1] * p->stateN) + i ] += g[ ((t+1)*p->stateN) + i ];
  1111. }
  1112. // normalize Et and sum it into E
  1113. cmVOR_DivVS( Et, (p->stateN*p->stateN), Esum );
  1114. cmVOR_AddVV( E, (p->stateN*p->stateN), Et );
  1115. }
  1116. // update the model
  1117. _cmDhmmNormMtxRows( p->initV, 1, p->stateN, g );
  1118. _cmDhmmNormMtxRows( p->transM, p->stateN, p->stateN, E );
  1119. _cmDhmmNormMtxRows( p->stsM, p->stateN, p->symN, q );
  1120. }
  1121. return cmOkRC;
  1122. }
  1123. cmRC_t cmDhmmReport( cmDhmm* p )
  1124. {
  1125. cmVOR_PrintL("initV:\n", p->obj.err.rpt, 1, p->stateN, p->initV );
  1126. cmVOR_PrintL("transM:\n", p->obj.err.rpt, p->stateN, p->stateN, p->transM );
  1127. cmVOR_PrintL("symM:\n", p->obj.err.rpt, p->stateN, p->symN, p->stsM );
  1128. return cmOkRC;
  1129. }
  1130. void cmDhmmTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  1131. {
  1132. unsigned stateN = 2;
  1133. unsigned symN = 3;
  1134. unsigned obsN = 4;
  1135. unsigned iterN = 10;
  1136. cmReal_t initV0[ stateN ];
  1137. cmReal_t transM0[ stateN * stateN ];
  1138. cmReal_t stsM0[ symN * stateN ];
  1139. cmReal_t initV1[ stateN ];
  1140. cmReal_t transM1[ stateN * stateN ];
  1141. cmReal_t stsM1[ symN * stateN ];
  1142. unsigned obsV[ obsN ];
  1143. unsigned hist[ symN ];
  1144. unsigned genFl = kManualProbHmmFl;
  1145. cmReal_t initV[] =
  1146. {
  1147. 0.44094,
  1148. 0.55906
  1149. };
  1150. cmReal_t transM[] =
  1151. {
  1152. 0.48336,
  1153. 0.81353,
  1154. 0.51664,
  1155. 0.18647,
  1156. };
  1157. cmReal_t stsM[] =
  1158. {
  1159. 0.20784,
  1160. 0.18698,
  1161. 0.43437,
  1162. 0.24102,
  1163. 0.35779,
  1164. 0.57199
  1165. };
  1166. unsigned obsM[] = { 2, 2, 2, 0 };
  1167. cmReal_t initV2[] = { 0.79060, 0.20940 };
  1168. cmReal_t transM2[] = { 0.508841, 0.011438, 0.491159, 0.988562 };
  1169. cmReal_t stsM2[] = { 0.25789, 0.35825, 0.61981, 0.42207, 0.12230, 0.21969 };
  1170. //srand( time(NULL) );
  1171. // generate a random HMM
  1172. _cmDhmmGenProb( initV0, 1, stateN, genFl, initV );
  1173. _cmDhmmGenProb( transM0, stateN, stateN, genFl, transM );
  1174. _cmDhmmGenProb( stsM0, stateN, symN, genFl, stsM );
  1175. cmCtx* c = cmCtxAlloc( NULL, rpt, lhH, stH);
  1176. cmDhmm* h0p = cmDhmmAlloc( c, NULL, stateN, symN, initV0, transM0, stsM0 );
  1177. // generate an observation sequence based on the random HMM
  1178. //cmDhmmGenObsSequence(c, h0p, obsV, obsN );
  1179. memcpy(obsV,obsM,obsN*sizeof(unsigned));
  1180. if( 0 )
  1181. {
  1182. // print the HMM
  1183. cmDhmmReport( h0p);
  1184. // print the observation symbols
  1185. cmVOU_PrintL("obs:\n", rpt, 1, obsN, obsV );
  1186. // print the histogram of the obs. symbols
  1187. cmVOU_Hist( hist, symN, obsV, obsN );
  1188. cmVOU_PrintL("hist:\n", rpt, 1, symN, hist );
  1189. // calc alpha (the forward probabilities)
  1190. cmReal_t alphaM[ h0p->stateN*obsN ];
  1191. cmReal_t logProb=0;
  1192. cmDHmmForwardEval( h0p, h0p->initV, obsV, obsN, alphaM, kLogScaleHmmFl, &logProb);
  1193. printf("log prob:%f\n alpha:\n", logProb );
  1194. cmVOR_Print( rpt, h0p->stateN, obsN, alphaM );
  1195. // calc beta (the bcmkward probabilities)
  1196. cmReal_t betaM[ h0p->stateN*obsN ];
  1197. logProb=0;
  1198. cmDHmmBcmkwardEval( h0p, obsV, obsN, betaM, kLogScaleHmmFl);
  1199. printf("log prob:%f\n beta:\n", logProb );
  1200. cmVOR_Print( h0p->obj.err.rpt, h0p->stateN, obsN, betaM );
  1201. }
  1202. // initialize a second HMM with random probabilities
  1203. _cmDhmmGenProb( initV1, 1, stateN, kManualProbHmmFl, initV2 );
  1204. _cmDhmmGenProb( transM1, stateN, stateN, kManualProbHmmFl, transM2 );
  1205. _cmDhmmGenProb( stsM1, stateN, symN, kManualProbHmmFl, stsM2 );
  1206. cmDhmm* h1p = cmDhmmAlloc( c, NULL, stateN, symN, initV1, transM1, stsM1 );
  1207. cmDhmmTrainEM( h1p, obsV, obsN, iterN, kLogScaleHmmFl );
  1208. cmDhmmFree(&h1p);
  1209. cmDhmmFree(&h0p);
  1210. cmCtxFree(&c);
  1211. }
  1212. //------------------------------------------------------------------------------------------------------------
  1213. cmConvolve* cmConvolveAlloc( cmCtx* c, cmConvolve* ap, const cmSample_t* h, unsigned hn, unsigned procSmpCnt )
  1214. {
  1215. cmConvolve* p = cmObjAlloc( cmConvolve, c, ap);
  1216. p->fft = cmFftAllocSR( c,NULL,NULL,0,kNoConvertFftFl);
  1217. p->ifft= cmIFftAllocRS(c,NULL,p->fft->binCnt);
  1218. if( hn > 0 && procSmpCnt > 0 )
  1219. if( cmConvolveInit(p,h,hn,procSmpCnt) != cmOkRC )
  1220. cmObjFree(&p);
  1221. return p;
  1222. }
  1223. cmRC_t cmConvolveFree( cmConvolve** pp )
  1224. {
  1225. cmRC_t rc;
  1226. cmConvolve* p = *pp;
  1227. if( pp == NULL || *pp == NULL )
  1228. return cmOkRC;
  1229. if((rc = cmConvolveFinal(p)) != cmOkRC )
  1230. return cmOkRC;
  1231. cmFftFreeSR(&p->fft);
  1232. cmIFftFreeRS(&p->ifft);
  1233. cmMemPtrFree(&p->H);
  1234. cmMemPtrFree(&p->outV);
  1235. cmObjFree(pp);
  1236. return cmOkRC;
  1237. }
  1238. cmRC_t cmConvolveInit( cmConvolve* p, const cmSample_t* h, unsigned hn, unsigned procSmpCnt )
  1239. {
  1240. cmRC_t rc;
  1241. unsigned i;
  1242. unsigned cn = cmNextPowerOfTwo( hn + procSmpCnt - 1 );
  1243. if((rc = cmConvolveFinal(p)) != cmOkRC )
  1244. return rc;
  1245. cmFftInitSR( p->fft, NULL, cn, kNoConvertFftFl );
  1246. cmIFftInitRS( p->ifft, p->fft->binCnt);
  1247. p->H = cmMemResizeZ( cmComplexR_t,p->H, p->fft->binCnt );
  1248. p->outV = cmMemResizeZ( cmSample_t,p->outV, cn );
  1249. p->olaV = p->outV + procSmpCnt;
  1250. p->outN = procSmpCnt;
  1251. p->hn = hn;
  1252. // take the FFT of the impulse response
  1253. cmFftExecSR( p->fft, h, hn );
  1254. // copy the FFT of the impulse response to p->H[]
  1255. for(i=0; i<p->fft->binCnt; ++i)
  1256. p->H[i] = p->fft->complexV[i] / p->fft->wndSmpCnt;
  1257. return cmOkRC;
  1258. }
  1259. cmRC_t cmConvolveFinal( cmConvolve* p )
  1260. { return cmOkRC; }
  1261. cmRC_t cmConvolveExec( cmConvolve* p, const cmSample_t* x, unsigned xn )
  1262. {
  1263. unsigned i;
  1264. // take FT of input signal
  1265. cmFftExecSR( p->fft, x, xn );
  1266. // multiply the signal spectra of the input signal and impulse response
  1267. for(i=0; i<p->fft->binCnt; ++i)
  1268. p->ifft->complexV[i] = p->H[i] * p->fft->complexV[i];
  1269. // take the IFFT of the convolved spectrum
  1270. cmIFftExecRS(p->ifft,NULL);
  1271. // sum with previous impulse response tail
  1272. cmVOS_AddVVV( p->outV, p->outN-1, p->olaV, p->ifft->outV );
  1273. // first sample of the impulse response tail is complete
  1274. p->outV[p->outN-1] = p->ifft->outV[p->outN-1];
  1275. // store the new impulse response tail
  1276. cmVOS_Copy(p->olaV,p->hn-1,p->ifft->outV + p->outN );
  1277. return cmOkRC;
  1278. }
  1279. cmRC_t cmConvolveSignal( cmCtx* c, const cmSample_t* h, unsigned hn, const cmSample_t* x, unsigned xn, cmSample_t* y, unsigned yn )
  1280. {
  1281. cmConvolve* p = cmConvolveAlloc(c,NULL,h,hn,xn);
  1282. cmConvolveExec(p,x,xn);
  1283. unsigned n = cmMin(p->outN,yn);
  1284. cmVOS_Copy(y,n,p->outV);
  1285. if( yn > p->outN )
  1286. {
  1287. unsigned m = cmMin(yn-p->outN,p->hn-1);
  1288. cmVOS_Copy(y+n,m,p->olaV);
  1289. }
  1290. cmConvolveFree(&p);
  1291. return cmOkRC;
  1292. }
  1293. cmRC_t cmConvolveTest(cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  1294. {
  1295. cmCtx *c = cmCtxAlloc(NULL,rpt,lhH,stH);
  1296. cmSample_t h[] = { 1, .5, .25, 0, 0 };
  1297. unsigned hn = sizeof(h) / sizeof(h[0]);
  1298. cmSample_t x[] = { 1, 0, 0, 0, 1, 0, 0, 0 };
  1299. unsigned xn = sizeof(x) / sizeof(x[0]);
  1300. unsigned yn = xn+hn-1;
  1301. cmSample_t y[yn];
  1302. //cmVOS_Hann(h,5);
  1303. cmConvolve* p = cmConvolveAlloc(c,NULL,h,hn,xn);
  1304. cmConvolveExec(p,x,xn);
  1305. cmVOS_Print( rpt, 1, p->outN, p->outV );
  1306. cmVOS_Print( rpt, 1, p->hn-1, p->olaV );
  1307. cmConvolveFree(&p);
  1308. cmConvolveSignal(c,h,hn,x,xn,y,yn);
  1309. cmVOS_Print( rpt, 1, hn+xn-1, y );
  1310. cmCtxFree(&c);
  1311. return cmOkRC;
  1312. }
  1313. //------------------------------------------------------------------------------------------------------------
  1314. cmBfcc* cmBfccAlloc( cmCtx* ctx, cmBfcc* ap, unsigned bandCnt, unsigned binCnt, double binHz )
  1315. {
  1316. cmBfcc* p = cmObjAlloc( cmBfcc, ctx, ap );
  1317. if( bandCnt > 0 )
  1318. if( cmBfccInit( p, bandCnt, binCnt, binHz ) != cmOkRC )
  1319. cmBfccFree(&p);
  1320. return p;
  1321. }
  1322. cmRC_t cmBfccFree( cmBfcc** pp )
  1323. {
  1324. cmRC_t rc;
  1325. if( pp== NULL || *pp==NULL)
  1326. return cmOkRC;
  1327. cmBfcc* p = *pp;
  1328. if((rc = cmBfccFinal(p)) != cmOkRC )
  1329. return rc;
  1330. cmMemPtrFree(&p->dctMtx);
  1331. cmMemPtrFree(&p->filtMask);
  1332. cmMemPtrFree(&p->outV);
  1333. cmObjFree(pp);
  1334. return rc;
  1335. }
  1336. cmRC_t cmBfccInit( cmBfcc* p, unsigned bandCnt, unsigned binCnt, double binHz )
  1337. {
  1338. cmRC_t rc;
  1339. if((rc = cmBfccFinal(p)) != cmOkRC )
  1340. return rc;
  1341. p->dctMtx = cmMemResizeZ( cmReal_t, p->dctMtx, bandCnt*bandCnt);
  1342. p->filtMask = cmMemResizeZ( cmReal_t, p->filtMask, bandCnt*binCnt);
  1343. p->outV = cmMemResizeZ( cmReal_t, p->outV, bandCnt );
  1344. p->binCnt = binCnt;
  1345. p->bandCnt = bandCnt;
  1346. cmVOR_BarkMask( p->filtMask, bandCnt, binCnt, binHz );
  1347. cmVOR_DctMatrix(p->dctMtx, bandCnt, bandCnt );
  1348. return rc;
  1349. }
  1350. cmRC_t cmBfccFinal( cmBfcc* p )
  1351. { return cmOkRC; }
  1352. cmRC_t cmBfccExec( cmBfcc* p, const cmReal_t* magV, unsigned binCnt )
  1353. {
  1354. assert( binCnt <= p->binCnt );
  1355. cmReal_t t[ p->bandCnt ];
  1356. cmReal_t v[ binCnt ];
  1357. // convert magnitude to power
  1358. cmVOR_PowVVS(v,binCnt,magV,2.0);
  1359. // apply the filter mask to the power spectrum
  1360. cmVOR_MultVMV( t, p->bandCnt, p->filtMask, binCnt, v );
  1361. //cmVOR_PrintL("\t:\n", p->obj.ctx->outFuncPtr, 1, p->bandCnt, t);
  1362. cmVOR_ReplaceLte( t, p->bandCnt, t, 0, 0.1e-5 );
  1363. cmVOR_LogV( t, p->bandCnt, t );
  1364. // decorellate the bands with a DCT
  1365. cmVOR_MultVMV( p->outV, p->bandCnt, p->dctMtx, p->bandCnt, t );
  1366. return cmOkRC;
  1367. }
  1368. void cmBfccTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  1369. {
  1370. double srate = 11025;
  1371. unsigned binCnt = 129;
  1372. double binHz = srate/(binCnt-1);
  1373. unsigned bandCnt = kDefaultBarkBandCnt;
  1374. cmCtx* c = cmCtxAlloc( NULL, rpt, lhH, stH );
  1375. cmBfcc* b = cmBfccAlloc( c, NULL, bandCnt, binCnt, binHz );
  1376. cmReal_t powV[] = {
  1377. 0.8402, 0.3944, 0.7831, 0.7984, 0.9116, 0.1976, 0.3352, 0.7682, 0.2778, 0.5540,
  1378. 0.4774, 0.6289, 0.3648, 0.5134, 0.9522, 0.9162, 0.6357, 0.7173, 0.1416, 0.6070,
  1379. 0.0163, 0.2429, 0.1372, 0.8042, 0.1567, 0.4009, 0.1298, 0.1088, 0.9989, 0.2183,
  1380. 0.5129, 0.8391, 0.6126, 0.2960, 0.6376, 0.5243, 0.4936, 0.9728, 0.2925, 0.7714,
  1381. 0.5267, 0.7699, 0.4002, 0.8915, 0.2833, 0.3525, 0.8077, 0.9190, 0.0698, 0.9493,
  1382. 0.5260, 0.0861, 0.1922, 0.6632, 0.8902, 0.3489, 0.0642, 0.0200, 0.4577, 0.0631,
  1383. 0.2383, 0.9706, 0.9022, 0.8509, 0.2667, 0.5398, 0.3752, 0.7602, 0.5125, 0.6677,
  1384. 0.5316, 0.0393, 0.4376, 0.9318, 0.9308, 0.7210, 0.2843, 0.7385, 0.6400, 0.3540,
  1385. 0.6879, 0.1660, 0.4401, 0.8801, 0.8292, 0.3303, 0.2290, 0.8934, 0.3504, 0.6867,
  1386. 0.9565, 0.5886, 0.6573, 0.8587, 0.4396, 0.9240, 0.3984, 0.8148, 0.6842, 0.9110,
  1387. 0.4825, 0.2158, 0.9503, 0.9201, 0.1477, 0.8811, 0.6411, 0.4320, 0.6196, 0.2811,
  1388. 0.7860, 0.3075, 0.4470, 0.2261, 0.1875, 0.2762, 0.5564, 0.4165, 0.1696, 0.9068,
  1389. 0.1032, 0.1261, 0.4954, 0.7605, 0.9848, 0.9350, 0.6844, 0.3832, 0.7498 };
  1390. //cmVOR_Random(powV, binCnt, 0.0, 1.0 );
  1391. cmBfccExec(b,powV,binCnt);
  1392. //cmVOR_PrintL("\nin:\n", rpt, 1, binCnt, powV);
  1393. //cmVOR_PrintL("\nfilt:\n", rpt, bandCnt, binCnt, b->filtMask);
  1394. //cmVOR_PrintL("\ndct:\n", rpt, bandCnt, bandCnt,b->dctMtx);
  1395. cmVOR_PrintL("\nbfcc:\n", rpt, 1, bandCnt, b->outV);
  1396. cmBfccFree(&b);
  1397. cmCtxFree(&c);
  1398. }
  1399. //------------------------------------------------------------------------------------------------------------
  1400. cmCeps* cmCepsAlloc( cmCtx* ctx, cmCeps* ap, unsigned binCnt, unsigned outN )
  1401. {
  1402. cmCeps* p = cmObjAlloc( cmCeps, ctx, ap );
  1403. //cmIFftAllocRR( ctx, &p->ft, 0 );
  1404. if( binCnt > 0 )
  1405. if( cmCepsInit( p, binCnt, outN ) != cmOkRC )
  1406. cmCepsFree(&p);
  1407. return p;
  1408. }
  1409. cmRC_t cmCepsFree( cmCeps** pp )
  1410. {
  1411. cmRC_t rc;
  1412. if( pp== NULL || *pp==NULL)
  1413. return cmOkRC;
  1414. cmCeps* p = *pp;
  1415. if((rc = cmCepsFinal(p)) != cmOkRC )
  1416. return rc;
  1417. //cmObjFreeStatic( cmIFftFreeRR, cmIFftRR, p->ft );
  1418. cmMemPtrFree(&p->dctM);
  1419. cmMemPtrFree(&p->outV);
  1420. cmObjFree(pp);
  1421. return rc;
  1422. }
  1423. cmRC_t cmCepsInit( cmCeps* p, unsigned binCnt, unsigned outN )
  1424. {
  1425. cmRC_t rc;
  1426. if((rc = cmCepsFinal(p)) != cmOkRC )
  1427. return rc;
  1428. //cmIFftInitRR( &p->ft, binCnt );
  1429. p->dct_cn = (binCnt-1)*2;
  1430. p->dctM = cmMemResize( cmReal_t, p->dctM, outN*p->dct_cn );
  1431. p->outN = outN; //p->ft.outN;
  1432. p->outV = cmMemResizeZ( cmReal_t, p->outV, outN ); //p->ft.outV;
  1433. p->binCnt = binCnt;
  1434. assert( outN <= p->dct_cn );
  1435. cmVOR_DctMatrix( p->dctM, outN, p->dct_cn );
  1436. return rc;
  1437. }
  1438. cmRC_t cmCepsFinal( cmCeps* p )
  1439. { return cmOkRC; }
  1440. cmRC_t cmCepsExec( cmCeps* p, const cmReal_t* magV, const cmReal_t* phsV, unsigned binCnt )
  1441. {
  1442. assert( binCnt == p->binCnt );
  1443. cmReal_t v[ p->dct_cn ];
  1444. // guard against zeros in the magn spectrum
  1445. cmVOR_ReplaceLte(v,binCnt,magV,0.0,0.00001);
  1446. // take the log of the spectrum
  1447. cmVOR_LogV(v,binCnt,v);
  1448. // reconstruct the negative frequencies
  1449. int i,j;
  1450. for(i=1,j=p->dct_cn-1; i<binCnt; ++i,--j)
  1451. v[j] = v[i];
  1452. // take the DCT
  1453. cmVOR_MultVMV( p->outV, p->outN, p->dctM, p->dct_cn, v );
  1454. //cmIFftExecPolarRR( &p->ft, v, phsV );
  1455. return cmOkRC;
  1456. }
  1457. //------------------------------------------------------------------------------------------------------------
  1458. cmOla* cmOlaAlloc( cmCtx* ctx, cmOla* ap, unsigned wndSmpCnt, unsigned hopSmpCnt, unsigned procSmpCnt, unsigned wndTypeId )
  1459. {
  1460. cmOla* p = cmObjAlloc( cmOla, ctx, ap );
  1461. cmWndFuncAlloc( ctx, &p->wf, kHannWndId, wndSmpCnt, 0);
  1462. if( wndSmpCnt > 0 )
  1463. if( cmOlaInit(p,wndSmpCnt,hopSmpCnt,procSmpCnt,wndTypeId) != cmOkRC )
  1464. cmOlaFree(&p);
  1465. return p;
  1466. }
  1467. cmRC_t cmOlaFree( cmOla** pp )
  1468. {
  1469. cmRC_t rc;
  1470. if( pp==NULL || *pp==NULL )
  1471. return cmOkRC;
  1472. cmOla* p = *pp;
  1473. if(( rc = cmOlaFinal(p)) != cmOkRC )
  1474. return rc;
  1475. cmMemPtrFree(&p->bufV);
  1476. cmMemPtrFree(&p->outV);
  1477. cmObjFreeStatic( cmWndFuncFree, cmWndFunc, p->wf );
  1478. cmObjFree(pp);
  1479. return rc;
  1480. }
  1481. cmRC_t cmOlaInit( cmOla* p, unsigned wndSmpCnt, unsigned hopSmpCnt, unsigned procSmpCnt, unsigned wndTypeId )
  1482. {
  1483. cmRC_t rc;
  1484. if((rc = cmOlaFinal(p)) != cmOkRC )
  1485. return rc;
  1486. if((rc = cmWndFuncInit( &p->wf, wndTypeId, wndSmpCnt, 0)) != cmOkRC )
  1487. return rc;
  1488. p->bufV = cmMemResizeZ( cmSample_t, p->bufV, wndSmpCnt );
  1489. p->outV = cmMemResizeZ( cmSample_t, p->outV, hopSmpCnt );
  1490. p->outPtr = p->outV + hopSmpCnt;
  1491. // hopSmpCnt must be an even multiple of procSmpCnt
  1492. assert( hopSmpCnt % procSmpCnt == 0 );
  1493. assert( wndSmpCnt >= hopSmpCnt );
  1494. p->wndSmpCnt = wndSmpCnt;
  1495. p->hopSmpCnt = hopSmpCnt;
  1496. p->procSmpCnt= procSmpCnt;
  1497. p->idx = 0;
  1498. return rc;
  1499. }
  1500. cmRC_t cmOlaFinal( cmOla* p )
  1501. { return cmOkRC; }
  1502. // The incoming buffer and the ola buf (bufV)
  1503. // can be divided into three logical parts:
  1504. //
  1505. // [head][middle][tail]
  1506. //
  1507. // head = hopSmpCnt values
  1508. // tail = hopSmpCnt values
  1509. // middle = wndSmpCnt - (2*hopSmpCnt) values
  1510. //
  1511. // Each exec can be broken into three phases:
  1512. //
  1513. // outV = bufV[tail] + in[head]
  1514. // bufV[middle] += in[middle]
  1515. // bufV[tail] = in[tail]
  1516. //
  1517. cmRC_t cmOlaExecS( cmOla* p, const cmSample_t* sp, unsigned sN )
  1518. {
  1519. assert( sN == p->wndSmpCnt );
  1520. cmRC_t rc = cmOkRC;
  1521. const cmSample_t* ep = sp + sN;
  1522. const cmSample_t* wp = p->wf.wndV;
  1523. int i,j,k,n;
  1524. // [Sum head of incoming samples with tail of ola buf]
  1525. // fill outV with the bufV[idx:idx+hopSmpCnt] + sp[hopSmpCnt]
  1526. for(i=0; i<p->hopSmpCnt; ++i)
  1527. {
  1528. p->outV[i] = p->bufV[p->idx++] + (*sp++ * *wp++);
  1529. if( p->idx == p->wndSmpCnt )
  1530. p->idx = 0;
  1531. }
  1532. // [Sum middle of incoming samples with middle of ola buf]
  1533. // sum next wndSmpCnt - hopSmpCnt samples of sp[] into bufV[]
  1534. n = p->wndSmpCnt - (2*p->hopSmpCnt);
  1535. k = p->idx;
  1536. for(j=0; j<n; ++j)
  1537. {
  1538. p->bufV[k++] += (*sp++ * *wp++);
  1539. if( k == p->wndSmpCnt )
  1540. k = 0;
  1541. }
  1542. // [Assign tail of incoming to tail of ola buf]
  1543. // assign ending samples from sp[] into bufV[]
  1544. while( sp < ep )
  1545. {
  1546. p->bufV[k++] = (*sp++ * *wp++);
  1547. if( k == p->wndSmpCnt )
  1548. k = 0;
  1549. }
  1550. p->outPtr = p->outV;
  1551. return rc;
  1552. }
  1553. cmRC_t cmOlaExecR( cmOla* p, const cmReal_t* sp, unsigned sN )
  1554. {
  1555. assert( sN == p->wndSmpCnt );
  1556. cmRC_t rc = cmOkRC;
  1557. const cmReal_t* ep = sp + sN;
  1558. const cmSample_t* wp = p->wf.wndV;
  1559. int i,j,k,n;
  1560. // fill outV with the bufV[idx:idx+hopSmpCnt] + sp[hopSmpCnt]
  1561. for(i=0; i<p->hopSmpCnt; ++i)
  1562. {
  1563. p->outV[i] = p->bufV[p->idx++] + (*sp++ * *wp++);
  1564. if( p->idx == p->wndSmpCnt )
  1565. p->idx = 0;
  1566. }
  1567. // sum next wndSmpCnt - hopSmpCnt samples of sp[] into bufV[]
  1568. n = p->wndSmpCnt - (2*p->hopSmpCnt);
  1569. k = p->idx;
  1570. for(j=0; j<n; ++j)
  1571. {
  1572. p->bufV[k++] += (*sp++ * *wp++);
  1573. if( k == p->wndSmpCnt )
  1574. k = 0;
  1575. }
  1576. // assign ending samples from sp[] into bufV[]
  1577. while( sp < ep )
  1578. {
  1579. p->bufV[k++] = (*sp++ * *wp++);
  1580. if( k == p->wndSmpCnt )
  1581. k = 0;
  1582. }
  1583. p->outPtr = p->outV;
  1584. return rc;
  1585. }
  1586. const cmSample_t* cmOlaExecOut(cmOla* p)
  1587. {
  1588. const cmSample_t* sp = p->outPtr;
  1589. if( sp >= p->outV + p->hopSmpCnt )
  1590. return NULL;
  1591. p->outPtr += p->procSmpCnt;
  1592. return sp;
  1593. }
  1594. //------------------------------------------------------------------------------------------------------------
  1595. cmPhsToFrq* cmPhsToFrqAlloc( cmCtx* c, cmPhsToFrq* ap, double srate, unsigned binCnt, unsigned hopSmpCnt )
  1596. {
  1597. cmPhsToFrq* p = cmObjAlloc( cmPhsToFrq, c, ap );
  1598. if( srate != 0 )
  1599. if( cmPhsToFrqInit( p, srate, binCnt, hopSmpCnt ) != cmOkRC )
  1600. cmPhsToFrqFree(&p);
  1601. return p;
  1602. }
  1603. cmRC_t cmPhsToFrqFree( cmPhsToFrq** pp )
  1604. {
  1605. cmRC_t rc = cmOkRC;
  1606. cmPhsToFrq* p = *pp;
  1607. if( pp==NULL || *pp== NULL )
  1608. return rc;
  1609. if((rc = cmPhsToFrqFinal(p)) != cmOkRC )
  1610. return rc;
  1611. cmMemPtrFree(&p->hzV);
  1612. cmMemPtrFree(&p->phsV);
  1613. cmMemPtrFree(&p->wV);
  1614. cmObjFree(pp);
  1615. return rc;
  1616. }
  1617. cmRC_t cmPhsToFrqInit( cmPhsToFrq* p, double srate, unsigned binCnt, unsigned hopSmpCnt )
  1618. {
  1619. cmRC_t rc;
  1620. unsigned i;
  1621. if((rc = cmPhsToFrqFinal(p)) != cmOkRC )
  1622. return rc;
  1623. p->hzV = cmMemResizeZ( cmReal_t, p->hzV, binCnt );
  1624. p->phsV = cmMemResizeZ( cmReal_t, p->phsV, binCnt );
  1625. p->wV = cmMemResizeZ( cmReal_t, p->wV, binCnt );
  1626. p->srate = srate;
  1627. p->binCnt = binCnt;
  1628. p->hopSmpCnt = hopSmpCnt;
  1629. for(i=0; i<binCnt; ++i)
  1630. p->wV[i] = M_PI * i * hopSmpCnt / (binCnt-1);
  1631. return rc;
  1632. }
  1633. cmRC_t cmPhsToFrqFinal(cmPhsToFrq* p )
  1634. { return cmOkRC; }
  1635. cmRC_t cmPhsToFrqExec( cmPhsToFrq* p, const cmReal_t* phsV )
  1636. {
  1637. cmRC_t rc = cmOkRC;
  1638. unsigned i;
  1639. double twoPi = 2.0 * M_PI;
  1640. double den = twoPi * p->hopSmpCnt;
  1641. for(i=0; i<p->binCnt; ++i)
  1642. {
  1643. cmReal_t dPhs = phsV[i] - p->phsV[i];
  1644. // unwrap phase - see phase_study.m for explanation
  1645. cmReal_t k = round( (p->wV[i] - dPhs) / twoPi);
  1646. // convert phase change to Hz
  1647. p->hzV[i] = (k * twoPi + dPhs) * p->srate / den;
  1648. // store phase for next iteration
  1649. p->phsV[i] = phsV[i];
  1650. }
  1651. return rc;
  1652. }
  1653. //------------------------------------------------------------------------------------------------------------
  1654. cmPvAnl* cmPvAnlAlloc( cmCtx* ctx, cmPvAnl* ap, unsigned procSmpCnt, double srate, unsigned wndSmpCnt, unsigned hopSmpCnt, unsigned flags )
  1655. {
  1656. cmRC_t rc;
  1657. cmPvAnl* p = cmObjAlloc( cmPvAnl, ctx, ap );
  1658. cmShiftBufAlloc(ctx, &p->sb, procSmpCnt, wndSmpCnt, hopSmpCnt );
  1659. cmWndFuncAlloc( ctx, &p->wf, kHannWndId, wndSmpCnt, 0);
  1660. cmFftAllocSR( ctx, &p->ft, p->wf.outV, wndSmpCnt, kToPolarFftFl);
  1661. cmPhsToFrqAlloc(ctx, &p->pf, srate, p->ft.binCnt, hopSmpCnt );
  1662. if( procSmpCnt > 0 )
  1663. if((rc = cmPvAnlInit(p,procSmpCnt,srate,wndSmpCnt,hopSmpCnt,flags)) != cmOkRC )
  1664. cmPvAnlFree(&p);
  1665. return p;
  1666. }
  1667. cmRC_t cmPvAnlFree( cmPvAnl** pp )
  1668. {
  1669. cmRC_t rc;
  1670. if( pp==NULL || *pp==NULL )
  1671. return cmOkRC;
  1672. cmPvAnl* p = *pp;
  1673. if((rc = cmPvAnlFinal(p) ) != cmOkRC )
  1674. return rc;
  1675. cmObjFreeStatic( cmPhsToFrqFree, cmPhsToFrq, p->pf );
  1676. cmObjFreeStatic( cmFftFreeSR, cmFftSR, p->ft );
  1677. cmObjFreeStatic( cmWndFuncFree, cmWndFunc, p->wf );
  1678. cmObjFreeStatic( cmShiftBufFree, cmShiftBuf, p->sb );
  1679. cmObjFree(pp);
  1680. return rc;
  1681. }
  1682. cmRC_t cmPvAnlInit( cmPvAnl* p, unsigned procSmpCnt, double srate, unsigned wndSmpCnt, unsigned hopSmpCnt, unsigned flags )
  1683. {
  1684. cmRC_t rc;
  1685. if((rc = cmPvAnlFinal(p)) != cmOkRC )
  1686. return rc;
  1687. if((rc = cmShiftBufInit( &p->sb, procSmpCnt, wndSmpCnt, hopSmpCnt )) != cmOkRC )
  1688. return rc;
  1689. if((rc = cmWndFuncInit( &p->wf, kHannWndId | kNormByLengthWndFl, wndSmpCnt, 0)) != cmOkRC )
  1690. return rc;
  1691. if((rc = cmFftInitSR( &p->ft, p->wf.outV, wndSmpCnt, kToPolarFftFl)) != cmOkRC )
  1692. return rc;
  1693. if((rc = cmPhsToFrqInit( &p->pf, srate, p->ft.binCnt, hopSmpCnt)) != cmOkRC )
  1694. return rc;
  1695. // if the window was just initialized
  1696. // divide the window to indirectly apply the magnitude normalization
  1697. //if( p->wndSmpCnt != wndSmpCnt )
  1698. // cmVOS_DivVS( p->wf.wndV, p->wf.outN, wndSmpCnt );
  1699. p->flags = flags;
  1700. p->procSmpCnt = procSmpCnt;
  1701. p->wndSmpCnt = wndSmpCnt;
  1702. p->hopSmpCnt = hopSmpCnt;
  1703. p->binCnt = p->ft.binCnt;
  1704. p->magV = p->ft.magV;
  1705. p->phsV = p->ft.phsV;
  1706. p->hzV = p->pf.hzV;
  1707. return rc;
  1708. }
  1709. cmRC_t cmPvAnlFinal(cmPvAnl* p )
  1710. { return cmOkRC; }
  1711. bool cmPvAnlExec( cmPvAnl* p, const cmSample_t* x, unsigned xN )
  1712. {
  1713. bool fl = false;
  1714. while( cmShiftBufExec(&p->sb,x,xN) )
  1715. {
  1716. cmWndFuncExec(&p->wf, p->sb.outV, p->sb.wndSmpCnt );
  1717. cmFftExecSR(&p->ft,NULL,0);
  1718. if( cmIsFlag(p->flags,kCalcHzPvaFl) )
  1719. cmPhsToFrqExec(&p->pf,p->phsV);
  1720. fl = true;
  1721. }
  1722. return fl;
  1723. }
  1724. //------------------------------------------------------------------------------------------------------------
  1725. cmPvSyn* cmPvSynAlloc( cmCtx* ctx, cmPvSyn* ap, unsigned procSmpCnt, double outSrate, unsigned wndSmpCnt, unsigned hopSmpCnt, unsigned wndTypeId )
  1726. {
  1727. cmRC_t rc;
  1728. cmPvSyn* p = cmObjAlloc( cmPvSyn, ctx, ap );
  1729. cmWndFuncAlloc( ctx, &p->wf, kHannWndId, wndSmpCnt, 0);
  1730. cmIFftAllocRS( ctx, &p->ft, wndSmpCnt/2+1 );
  1731. cmOlaAlloc( ctx, &p->ola, wndSmpCnt, hopSmpCnt, procSmpCnt, wndTypeId );
  1732. if( procSmpCnt )
  1733. if((rc = cmPvSynInit(p,procSmpCnt,outSrate,wndSmpCnt,hopSmpCnt,wndTypeId)) != cmOkRC )
  1734. cmPvSynFree(&p);
  1735. return p;
  1736. }
  1737. cmRC_t cmPvSynFree( cmPvSyn** pp )
  1738. {
  1739. cmRC_t rc;
  1740. if( pp==NULL || *pp==NULL )
  1741. return cmOkRC;
  1742. cmPvSyn* p = *pp;
  1743. if((rc = cmPvSynFinal(p)) != cmOkRC )
  1744. return rc;
  1745. cmMemPtrFree(&p->minRphV);
  1746. cmMemPtrFree(&p->maxRphV);
  1747. cmMemPtrFree(&p->itrV);
  1748. cmMemPtrFree(&p->phs0V);
  1749. cmMemPtrFree(&p->phsV);
  1750. cmMemPtrFree(&p->mag0V);
  1751. cmMemPtrFree(&p->magV);
  1752. cmObjFreeStatic( cmOlaFree, cmOla, p->ola);
  1753. cmObjFreeStatic( cmIFftFreeRS, cmIFftRS, p->ft );
  1754. cmObjFreeStatic( cmWndFuncFree, cmWndFunc, p->wf );
  1755. cmObjFree(pp);
  1756. return cmOkRC;
  1757. }
  1758. cmRC_t cmPvSynInit( cmPvSyn* p, unsigned procSmpCnt, double outSrate, unsigned wndSmpCnt, unsigned hopSmpCnt, unsigned wndTypeId )
  1759. {
  1760. cmRC_t rc;
  1761. int k;
  1762. double twoPi = 2.0 * M_PI;
  1763. bool useHannFl = true;
  1764. int m = useHannFl ? 2 : 1;
  1765. if((rc = cmPvSynFinal(p)) != cmOkRC )
  1766. return rc;
  1767. p->outSrate = outSrate;
  1768. p->procSmpCnt = procSmpCnt;
  1769. p->wndSmpCnt = wndSmpCnt;
  1770. p->hopSmpCnt = hopSmpCnt;
  1771. p->binCnt = wndSmpCnt / 2 + 1;
  1772. p->minRphV = cmMemResizeZ( cmReal_t, p->minRphV, p->binCnt );
  1773. p->maxRphV = cmMemResizeZ( cmReal_t, p->maxRphV, p->binCnt );
  1774. p->itrV = cmMemResizeZ( cmReal_t, p->itrV, p->binCnt );
  1775. p->phs0V = cmMemResizeZ( cmReal_t, p->phs0V, p->binCnt );
  1776. p->phsV = cmMemResizeZ( cmReal_t, p->phsV, p->binCnt );
  1777. p->mag0V = cmMemResizeZ( cmReal_t, p->mag0V, p->binCnt );
  1778. p->magV = cmMemResizeZ( cmReal_t, p->magV, p->binCnt );
  1779. if((rc = cmWndFuncInit( &p->wf, wndTypeId, wndSmpCnt, 0)) != cmOkRC )
  1780. return rc;
  1781. if((rc = cmIFftInitRS( &p->ft, p->binCnt )) != cmOkRC )
  1782. return rc;
  1783. if((rc = cmOlaInit( &p->ola, wndSmpCnt, hopSmpCnt, procSmpCnt, wndTypeId )) != cmOkRC )
  1784. return rc;
  1785. for(k=0; k<p->binCnt; ++k)
  1786. {
  1787. // complete revolutions per hop in radians
  1788. p->itrV[k] = twoPi * floor((double)k * hopSmpCnt / wndSmpCnt );
  1789. p->minRphV[k] = ((cmReal_t)(k-m)) * hopSmpCnt * twoPi / wndSmpCnt;
  1790. p->maxRphV[k] = ((cmReal_t)(k+m)) * hopSmpCnt * twoPi / wndSmpCnt;
  1791. //printf("%f %f %f\n",p->itrV[k],p->minRphV[k],p->maxRphV[k]);
  1792. }
  1793. return rc;
  1794. }
  1795. cmRC_t cmPvSynFinal(cmPvSyn* p )
  1796. { return cmOkRC; }
  1797. cmRC_t cmPvSynExec( cmPvSyn* p, const cmReal_t* magV, const cmReal_t* phsV )
  1798. {
  1799. double twoPi = 2.0 * M_PI;
  1800. unsigned k;
  1801. for(k=0; k<p->binCnt; ++k)
  1802. {
  1803. // phase dist between cur and prv frame
  1804. cmReal_t dp = phsV[k] - p->phs0V[k];
  1805. // dist must be positive (accum phase always increases)
  1806. if( dp < -0.00001 )
  1807. dp += twoPi;
  1808. // add in complete revolutions based on the bin frequency
  1809. // (these would have been lost from 'dp' due to phase wrap)
  1810. dp += p->itrV[k];
  1811. // constrain the phase change to lie within the range of the kth bin
  1812. if( dp < p->minRphV[k] )
  1813. dp += twoPi;
  1814. if( dp > p->maxRphV[k] )
  1815. dp -= twoPi;
  1816. p->phsV[k] = p->phs0V[k] + dp;
  1817. p->magV[k] = p->mag0V[k];
  1818. p->phs0V[k] = phsV[k];
  1819. p->mag0V[k] = magV[k];
  1820. }
  1821. cmIFftExecPolarRS( &p->ft, magV, phsV );
  1822. cmOlaExecS( &p->ola, p->ft.outV, p->ft.outN );
  1823. //printf("%i %i\n",p->binCnt,p->ft.binCnt );
  1824. //cmVOR_Print( p->obj.ctx->outFuncPtr, 1, p->binCnt, magV );
  1825. //cmVOR_Print( p->obj.ctx->outFuncPtr, 1, p->binCnt, p->phsV );
  1826. //cmVOS_Print( p->obj.ctx->outFuncPtr, 1, 10, p->ft.outV );
  1827. return cmOkRC;
  1828. }
  1829. cmRC_t cmPvSynDoIt( cmPvSyn* p, const cmSample_t* v )
  1830. {
  1831. cmOlaExecS( &p->ola, v, p->wndSmpCnt );
  1832. //printf("%f\n",cmVOS_RMS(s,p->wndSmpCnt,p->wndSmpCnt));
  1833. return cmOkRC;
  1834. }
  1835. const cmSample_t* cmPvSynExecOut(cmPvSyn* p )
  1836. { return cmOlaExecOut(&p->ola); }
  1837. //------------------------------------------------------------------------------------------------------------
  1838. cmMidiSynth* cmMidiSynthAlloc( cmCtx* ctx, cmMidiSynth* ap, const cmMidiSynthPgm* pgmArray, unsigned pgmCnt, unsigned voiceCnt, unsigned procSmpCnt, unsigned outChCnt, cmReal_t srate )
  1839. {
  1840. cmMidiSynth* p = cmObjAlloc( cmMidiSynth, ctx, ap );
  1841. if( pgmArray != NULL )
  1842. if( cmMidiSynthInit( p, pgmArray, pgmCnt, voiceCnt, procSmpCnt, outChCnt, srate ) != cmOkRC )
  1843. cmMidiSynthFree(&p);
  1844. return p;
  1845. }
  1846. cmRC_t cmMidiSynthFree( cmMidiSynth** pp )
  1847. {
  1848. cmRC_t rc;
  1849. if( pp==NULL || *pp==NULL)
  1850. return cmOkRC;
  1851. cmMidiSynth* p = *pp;
  1852. if((rc = cmMidiSynthFinal(p)) != cmOkRC )
  1853. return rc;
  1854. cmMemPtrFree(&p->voiceArray);
  1855. cmMemPtrFree(&p->outM);
  1856. cmMemPtrFree(&p->outChArray);
  1857. cmObjFree(pp);
  1858. return cmOkRC;
  1859. }
  1860. cmRC_t cmMidiSynthInit( cmMidiSynth* p, const cmMidiSynthPgm* pgmArray, unsigned pgmCnt, unsigned voiceCnt, unsigned procSmpCnt, unsigned outChCnt, cmReal_t srate )
  1861. {
  1862. // at least one pgm must be given
  1863. assert( pgmCnt > 0 );
  1864. unsigned i;
  1865. cmRC_t rc;
  1866. if((rc = cmMidiSynthFinal(p)) != cmOkRC )
  1867. return rc;
  1868. p->voiceArray = cmMemResizeZ( cmMidiVoice, p->voiceArray, voiceCnt );
  1869. p->outM = cmMemResizeZ( cmSample_t, p->outM, outChCnt * procSmpCnt );
  1870. p->outChArray = cmMemResizeZ( cmSample_t*, p->outChArray, outChCnt );
  1871. p->avail = p->voiceArray;
  1872. p->voiceCnt = voiceCnt;
  1873. p->activeVoiceCnt = 0;
  1874. p->voiceStealCnt = 0;
  1875. p->procSmpCnt = procSmpCnt;
  1876. p->outChCnt = outChCnt;
  1877. p->srate = srate;
  1878. for(i=0; i<outChCnt; ++i)
  1879. p->outChArray[i] = p->outM + (i*procSmpCnt);
  1880. for(i=0; i<kMidiChCnt; ++i)
  1881. {
  1882. p->chArray[i].pgm = 0;
  1883. p->chArray[i].active = NULL;
  1884. p->chArray[i].pitchBend = 0;
  1885. p->chArray[i].synthPtr = p;
  1886. memset(p->chArray[i].midiCtl, 0, kMidiCtlCnt * sizeof(cmMidiByte_t));
  1887. }
  1888. for(i=0; i<voiceCnt; ++i)
  1889. {
  1890. p->voiceArray[i].index = i;
  1891. p->voiceArray[i].flags = 0;
  1892. p->voiceArray[i].pitch = kInvalidMidiPitch;
  1893. p->voiceArray[i].velocity = kInvalidMidiVelocity;
  1894. p->voiceArray[i].pgm.pgm = kInvalidMidiPgm;
  1895. p->voiceArray[i].pgm.cbPtr = NULL;
  1896. p->voiceArray[i].pgm.cbDataPtr = NULL;
  1897. p->voiceArray[i].chPtr = NULL;
  1898. p->voiceArray[i].link = i<voiceCnt-1 ? p->voiceArray + i + 1 : NULL;
  1899. }
  1900. for(i=0; i<pgmCnt; ++i)
  1901. {
  1902. unsigned idx = pgmArray[i].pgm;
  1903. if( idx >= kMidiPgmCnt )
  1904. rc = cmCtxRtCondition( &p->obj, cmArgAssertRC, "MIDI program change values must be less than %i.",kMidiPgmCnt);
  1905. else
  1906. {
  1907. p->pgmArray[ idx ].cbPtr = pgmArray[i].cbPtr;
  1908. p->pgmArray[ idx ].cbDataPtr = pgmArray[i].cbDataPtr;
  1909. p->pgmArray[ idx ].pgm = idx;
  1910. }
  1911. }
  1912. return rc;
  1913. }
  1914. cmRC_t cmMidiSynthFinal( cmMidiSynth* p )
  1915. { return cmOkRC; }
  1916. cmRC_t _cmMidiSynthOnNoteOn( cmMidiSynth* p, cmMidiByte_t ch, cmMidiByte_t pitch, cmMidiByte_t vel )
  1917. {
  1918. assert( ch < kMidiChCnt );
  1919. if( p->activeVoiceCnt == p->voiceCnt )
  1920. {
  1921. ++p->voiceStealCnt;
  1922. return cmOkRC;
  1923. }
  1924. assert( p->avail != NULL );
  1925. cmMidiSynthCh* chPtr = p->chArray + ch;
  1926. cmMidiVoice* vp = p->avail;
  1927. ++p->activeVoiceCnt;
  1928. // update avail
  1929. p->avail = p->avail->link;
  1930. // update active
  1931. vp->flags |= kActiveMsFl | kKeyGateMsFl;
  1932. vp->pitch = pitch;
  1933. vp->velocity = vel;
  1934. vp->pgm = p->pgmArray[ chPtr->pgm ];
  1935. vp->chPtr = chPtr;
  1936. vp->link = chPtr->active;
  1937. chPtr->active = vp;
  1938. vp->pgm.cbPtr( vp, kAttackMsId, NULL, 0 );
  1939. return cmOkRC;
  1940. }
  1941. cmRC_t _cmMidiSynthOnNoteOff( cmMidiSynth* p, cmMidiByte_t ch, cmMidiByte_t pitch, cmMidiByte_t vel )
  1942. {
  1943. assert( ch < kMidiChCnt );
  1944. cmMidiSynthCh* cp = p->chArray + ch;
  1945. cmMidiVoice* vp = cp->active;
  1946. // find the voice for the given pitch
  1947. while( vp != NULL )
  1948. {
  1949. if( (vp->pitch == pitch) && (cmIsFlag(vp->flags,kKeyGateMsFl)==true) )
  1950. break;
  1951. vp = vp->link;
  1952. }
  1953. // if no voice had a key down on this pitch
  1954. if( vp == NULL )
  1955. {
  1956. return cmOkRC;
  1957. }
  1958. // mark the key as 'up'
  1959. vp->flags = cmClrFlag(vp->flags,kKeyGateMsFl);
  1960. // if the sustain pedal is up
  1961. if( cp->midiCtl[ kSustainCtlMdId ] == 0 )
  1962. vp->pgm.cbPtr( vp, kReleaseMsId, NULL, 0 );
  1963. return cmOkRC;
  1964. }
  1965. cmRC_t _cmMidiSynthOnCtl( cmMidiSynth* p, cmMidiByte_t ch, cmMidiByte_t ctlId, cmMidiByte_t ctlValue )
  1966. {
  1967. assert( ch < kMidiChCnt && ctlId < kMidiCtlCnt );
  1968. cmMidiSynthCh* cp = p->chArray + ch;
  1969. cp->midiCtl[ ctlId ] = ctlValue;
  1970. // if the sustain pedal is going up
  1971. if( ctlId == kSustainCtlMdId && ctlValue == 0 )
  1972. {
  1973. cmMidiVoice* vp = cp->active;
  1974. while(vp != NULL)
  1975. {
  1976. if( cmIsFlag(vp->flags,kKeyGateMsFl)==false )
  1977. vp->pgm.cbPtr(vp, kReleaseMsId, NULL, 0 );
  1978. vp = vp->link;
  1979. }
  1980. }
  1981. //printf("%i %i %f\n",ctlId,ctlValue,ctlValue/127.0);
  1982. return cmOkRC;
  1983. }
  1984. cmRC_t cmMidiSynthOnMidi( cmMidiSynth* p, const cmMidiPacket_t* pktArray, unsigned pktCnt )
  1985. {
  1986. unsigned i=0;
  1987. for(i=0; i<pktCnt; ++i)
  1988. if( pktArray[i].msgArray != NULL )
  1989. {
  1990. unsigned j;
  1991. for(j=0; j<pktArray[i].msgCnt; ++j)
  1992. {
  1993. const cmMidiMsg* mp = pktArray[i].msgArray + j;
  1994. cmMidiByte_t ch = mp->status & 0x0f;
  1995. cmMidiByte_t status = mp->status & 0xf0;
  1996. switch( status )
  1997. {
  1998. case kNoteOnMdId:
  1999. if( mp->d1 != 0 )
  2000. {
  2001. _cmMidiSynthOnNoteOn(p,ch,mp->d0,mp->d1);
  2002. break;
  2003. }
  2004. // fall through
  2005. case kNoteOffMdId:
  2006. _cmMidiSynthOnNoteOff(p,ch,mp->d0,mp->d1);
  2007. break;
  2008. case kPolyPresMdId:
  2009. break;
  2010. case kCtlMdId:
  2011. _cmMidiSynthOnCtl( p, ch, mp->d0, mp->d1 );
  2012. break;
  2013. case kPgmMdId:
  2014. break;
  2015. case kChPresMdId:
  2016. break;
  2017. case kPbendMdId:
  2018. break;
  2019. default:
  2020. printf("Unknown MIDI status:%i %i\n",(int)status,(int)mp->status);
  2021. break;
  2022. }
  2023. }
  2024. }
  2025. return cmOkRC;
  2026. }
  2027. cmRC_t cmMidiSynthExec( cmMidiSynth* p, cmSample_t* outChArray[], unsigned outChCnt )
  2028. {
  2029. unsigned i;
  2030. cmSample_t** chArray = outChArray == NULL ? p->outChArray : outChArray;
  2031. unsigned chCnt = outChArray == NULL ? p->outChCnt : outChCnt;
  2032. // FIX: make one active chain attached to cmMidiSynth rather than many
  2033. // active chains attached to each channel - this will avoid the extra
  2034. // iterations below.
  2035. // for each channel
  2036. for(i=0; i<kMidiChCnt; ++i)
  2037. {
  2038. cmMidiVoice* vp = p->chArray[i].active;
  2039. cmMidiVoice* prv = NULL;
  2040. // for each voice assigned to this channel
  2041. while(vp != NULL)
  2042. {
  2043. // tell the voice to perform its DSP function - returns 0 if the voice is no longer active
  2044. if( vp->pgm.cbPtr( vp, kDspMsId, chArray, chCnt ) )
  2045. {
  2046. prv = vp;
  2047. vp = vp->link;
  2048. }
  2049. else
  2050. {
  2051. cmMidiVoice* nvp = vp->link;
  2052. // remove vp from the active chain
  2053. if( prv != NULL )
  2054. prv->link = vp->link;
  2055. else
  2056. {
  2057. assert( vp == p->chArray[i].active );
  2058. // vp is first recd on active chain, nvp becomes first ...
  2059. p->chArray[i].active = vp->link;
  2060. prv = NULL; // ... so prv must be NULL
  2061. }
  2062. // insert this voice on the available chain
  2063. vp->link = p->avail;
  2064. p->avail = vp;
  2065. --p->activeVoiceCnt;
  2066. vp = nvp;
  2067. }
  2068. }
  2069. }
  2070. return cmOkRC;
  2071. }
  2072. //------------------------------------------------------------------------------------------------------------
  2073. cmWtVoice* cmWtVoiceAlloc( cmCtx* ctx, cmWtVoice* ap, unsigned procSmpCnt, cmReal_t hz )
  2074. {
  2075. cmWtVoice* p = cmObjAlloc( cmWtVoice, ctx, ap );
  2076. if( procSmpCnt != 0 )
  2077. if( cmWtVoiceInit( p, procSmpCnt, hz ) != cmOkRC )
  2078. cmWtVoiceFree(&p);
  2079. return p;
  2080. }
  2081. cmRC_t cmWtVoiceFree( cmWtVoice** pp )
  2082. {
  2083. cmRC_t rc = cmOkRC;
  2084. if( pp==NULL || *pp==NULL )
  2085. return cmOkRC;
  2086. cmWtVoice* p = *pp;
  2087. if((rc = cmWtVoiceFinal(p)) != cmOkRC )
  2088. return rc;
  2089. cmMemPtrFree(&p->outV);
  2090. cmObjFree(pp);
  2091. return rc;
  2092. }
  2093. cmRC_t cmWtVoiceInit( cmWtVoice* p, unsigned procSmpCnt, cmReal_t hz )
  2094. {
  2095. p->outV = cmMemResizeZ( cmSample_t, p->outV, procSmpCnt );
  2096. p->outN = procSmpCnt;
  2097. p->hz = hz;
  2098. p->level = 0;
  2099. p->durSmpCnt = 0;
  2100. p->phase = 0;
  2101. p->state = kOffWtId;
  2102. return cmOkRC;
  2103. }
  2104. cmRC_t cmWtVoiceFinal( cmWtVoice* p )
  2105. { return cmOkRC; }
  2106. int cmWtVoiceExec( cmWtVoice* p, struct cmMidiVoice_str* mvp, unsigned sel, cmSample_t* outChArray[], unsigned outChCnt )
  2107. {
  2108. switch( sel )
  2109. {
  2110. case kAttackMsId:
  2111. p->state = kAtkWtId;
  2112. p->hz = (13.75 * pow(2,(-9.0/12.0))) * pow(2,((double)mvp->pitch / 12));
  2113. //printf("%fhz\n",p->hz);
  2114. break;
  2115. case kReleaseMsId:
  2116. p->state = kRlsWtId;
  2117. //printf("rls:%f\n",p->phase);
  2118. break;
  2119. case kDspMsId:
  2120. {
  2121. if( p->state == kRlsWtId )
  2122. {
  2123. p->state = kOffWtId;
  2124. return 0;
  2125. }
  2126. cmMidiSynth* sp = mvp->chPtr->synthPtr;
  2127. cmSample_t* dp = outChCnt == 0 ? p->outV : outChArray[0];
  2128. cmSample_t* ep = dp + p->outN;
  2129. cmReal_t rps = (2.0 * M_PI * p->hz) / sp->srate;
  2130. cmReal_t sum=0;
  2131. unsigned i=0;
  2132. for(; dp < ep; ++dp)
  2133. {
  2134. *dp += (cmSample_t)(0.5 * sin( p->phase ));
  2135. sum += *dp;
  2136. ++i;
  2137. p->phase += rps;
  2138. }
  2139. //printf("(%f %f %i %i %p) ",p->phase,sum,i,p->outN,outChArray[0] );
  2140. }
  2141. break;
  2142. default:
  2143. assert(0);
  2144. break;
  2145. }
  2146. return 1;
  2147. }
  2148. //------------------------------------------------------------------------------------------------------------
  2149. cmWtVoiceBank* cmWtVoiceBankAlloc( cmCtx* ctx, cmWtVoiceBank* ap, double srate, unsigned procSmpCnt, unsigned voiceCnt, unsigned chCnt )
  2150. {
  2151. cmWtVoiceBank* p = cmObjAlloc( cmWtVoiceBank, ctx, ap );
  2152. if( srate != 0 )
  2153. if( cmWtVoiceBankInit( p, srate, procSmpCnt, voiceCnt, chCnt ) != cmOkRC )
  2154. cmWtVoiceBankFree(&p);
  2155. return p;
  2156. }
  2157. cmRC_t cmWtVoiceBankFree( cmWtVoiceBank** pp )
  2158. {
  2159. cmRC_t rc;
  2160. if( pp==NULL || *pp==NULL)
  2161. return cmOkRC;
  2162. cmWtVoiceBank* p = *pp;
  2163. if((rc = cmWtVoiceBankFinal(p)) != cmOkRC )
  2164. return rc;
  2165. cmMemPtrFree(&p->voiceArray);
  2166. cmMemPtrFree(&p->chArray);
  2167. cmMemPtrFree(&p->buf);
  2168. cmObjFree(pp);
  2169. return rc;
  2170. }
  2171. cmRC_t cmWtVoiceBankInit( cmWtVoiceBank* p, double srate, unsigned procSmpCnt, unsigned voiceCnt, unsigned chCnt )
  2172. {
  2173. cmRC_t rc;
  2174. unsigned i;
  2175. if((rc = cmWtVoiceBankFinal(p)) != cmOkRC )
  2176. return rc;
  2177. p->voiceArray = cmMemResizeZ( cmWtVoice*, p->voiceArray, voiceCnt );
  2178. for(i=0; i<voiceCnt; ++i)
  2179. p->voiceArray[i] = cmWtVoiceAlloc(p->obj.ctx,NULL,procSmpCnt,0);
  2180. p->voiceCnt = voiceCnt;
  2181. p->buf = cmMemResizeZ( cmSample_t, p->buf, chCnt * procSmpCnt );
  2182. p->chArray = cmMemResizeZ( cmSample_t*, p->chArray, chCnt );
  2183. for(i=0; i<chCnt; ++i)
  2184. p->chArray[i] = p->buf + (i*procSmpCnt);
  2185. p->chCnt = chCnt;
  2186. p->procSmpCnt = procSmpCnt;
  2187. return cmOkRC;
  2188. }
  2189. cmRC_t cmWtVoiceBankFinal( cmWtVoiceBank* p )
  2190. {
  2191. unsigned i;
  2192. for(i=0; i<p->voiceCnt; ++i)
  2193. cmWtVoiceFree(&p->voiceArray[i]);
  2194. return cmOkRC;
  2195. }
  2196. int cmWtVoiceBankExec( cmWtVoiceBank* p, struct cmMidiVoice_str* voicePtr, unsigned sel, cmSample_t* outChArray[], unsigned outChCnt )
  2197. {
  2198. cmWtVoice* vp = p->voiceArray[ voicePtr->index ];
  2199. bool fl = outChArray==NULL || outChCnt==0;
  2200. cmSample_t** chArray = fl ? p->chArray : outChArray;
  2201. unsigned chCnt = fl ? p->chCnt : outChCnt;
  2202. return cmWtVoiceExec( vp, voicePtr, sel, chArray, chCnt );
  2203. }
  2204. //------------------------------------------------------------------------------------------------------------
  2205. cmAudioFileBuf* cmAudioFileBufAlloc( cmCtx* ctx, cmAudioFileBuf* ap, unsigned procSmpCnt, const char* fn, unsigned audioChIdx, unsigned begSmpIdx, unsigned durSmpCnt )
  2206. {
  2207. cmAudioFileBuf* p = cmObjAlloc( cmAudioFileBuf, ctx, ap );
  2208. if( procSmpCnt != 0 )
  2209. if( cmAudioFileBufInit( p, procSmpCnt, fn, audioChIdx, begSmpIdx, durSmpCnt ) != cmOkRC )
  2210. cmAudioFileBufFree(&p);
  2211. return p;
  2212. }
  2213. cmRC_t cmAudioFileBufFree( cmAudioFileBuf** pp )
  2214. {
  2215. cmRC_t rc;
  2216. if( pp==NULL || *pp==NULL)
  2217. return cmOkRC;
  2218. cmAudioFileBuf* p = *pp;
  2219. if((rc = cmAudioFileBufFinal(p)) != cmOkRC )
  2220. return rc;
  2221. cmMemPtrFree(&p->bufV);
  2222. cmMemPtrFree(&p->fn);
  2223. cmObjFree(pp);
  2224. return rc;
  2225. }
  2226. cmRC_t cmAudioFileBufInit( cmAudioFileBuf* p, unsigned procSmpCnt, const char* fn, unsigned audioChIdx, unsigned begSmpIdx, unsigned durSmpCnt )
  2227. {
  2228. cmAudioFileH_t afH;
  2229. cmRC_t rc;
  2230. if((rc = cmAudioFileBufFinal(p)) != cmOkRC )
  2231. return rc;
  2232. // open the audio file for reading
  2233. if( cmAudioFileIsValid( afH = cmAudioFileNewOpen( fn, &p->info, &rc, p->obj.err.rpt ))==false || rc != kOkAfRC )
  2234. return cmCtxRtCondition(&p->obj, cmArgAssertRC,"The audio file '%s' could not be opend.",fn );
  2235. // validate the audio channel
  2236. if( audioChIdx >= p->info.chCnt )
  2237. return cmCtxRtCondition(&p->obj, cmArgAssertRC,"The audio file channel index %i is out of range for the audio file '%s'.",audioChIdx,fn);
  2238. // validate the start sample index
  2239. if( begSmpIdx > p->info.frameCnt )
  2240. return cmCtxRtCondition(&p->obj, cmOkRC, "The start sample index %i is past the end of the audio file '%s'.",begSmpIdx,fn);
  2241. if( durSmpCnt == cmInvalidCnt )
  2242. durSmpCnt = p->info.frameCnt - begSmpIdx;
  2243. // validate the duration
  2244. if( begSmpIdx + durSmpCnt > p->info.frameCnt )
  2245. {
  2246. unsigned newDurSmpCnt = p->info.frameCnt - begSmpIdx;
  2247. cmCtxRtCondition(&p->obj, cmOkRC, "The selected sample duration %i is past the end of the audio file '%s' and has been shorted to %i samples.",durSmpCnt,fn,newDurSmpCnt);
  2248. durSmpCnt = newDurSmpCnt;
  2249. }
  2250. // seek to the starting sample
  2251. if( cmAudioFileSeek( afH, begSmpIdx ) != kOkAfRC )
  2252. return cmCtxRtCondition(&p->obj, cmArgAssertRC,"Seek to sample index %i failed on the audio file '%s'.",begSmpIdx,fn);
  2253. // allocate the buffer memory
  2254. p->bufV = cmMemResizeZ( cmSample_t, p->bufV, durSmpCnt );
  2255. p->fn = cmMemResize( char, p->fn, strlen(fn)+1 );
  2256. p->bufN = durSmpCnt;
  2257. p->begSmpIdx = begSmpIdx;
  2258. p->chIdx = audioChIdx;
  2259. strcpy(p->fn,fn);
  2260. cmSample_t* outV = p->bufV;
  2261. // read the file into the buffer
  2262. unsigned rdSmpCnt = cmMin(4096,durSmpCnt);
  2263. unsigned cmc = 0;
  2264. while( cmc < durSmpCnt )
  2265. {
  2266. unsigned actualReadCnt = 0;
  2267. unsigned n = rdSmpCnt;
  2268. cmSample_t* chArray[] = {outV};
  2269. if( cmc + n > durSmpCnt )
  2270. n = durSmpCnt - cmc;
  2271. if((rc=cmAudioFileReadSample( afH, n, audioChIdx, 1, chArray, &actualReadCnt)) != kOkAfRC )
  2272. break;
  2273. cmc += actualReadCnt;
  2274. outV += actualReadCnt;
  2275. }
  2276. if( rc==kOkAfRC || (rc != kOkAfRC && cmAudioFileIsEOF(afH)))
  2277. rc = cmOkRC;
  2278. return rc;
  2279. }
  2280. cmRC_t cmAudioFileBufFinal(cmAudioFileBuf* p )
  2281. { return cmOkRC; }
  2282. unsigned cmAudioFileBufExec( cmAudioFileBuf* p, unsigned smpIdx, cmSample_t* outV, unsigned outN, bool sumIntoOutFl )
  2283. {
  2284. if( outV == NULL || outN == 0 || smpIdx >= p->bufN )
  2285. return 0;
  2286. unsigned n = cmMin(outN,p->bufN-smpIdx);
  2287. if( sumIntoOutFl )
  2288. cmVOS_AddVV(outV,n,p->bufV + smpIdx);
  2289. else
  2290. cmVOS_Copy(outV,n,p->bufV + smpIdx );
  2291. if( n < outN )
  2292. memset(outV+n,0,(outN-n)*sizeof(cmSample_t));
  2293. return n;
  2294. }
  2295. //------------------------------------------------------------------------------------------------------------
  2296. cmMDelay* cmMDelayAlloc( cmCtx* ctx, cmMDelay* ap, unsigned procSmpCnt, cmReal_t srate, cmReal_t fbCoeff, unsigned delayCnt, const cmReal_t* delayMsArray, const cmReal_t* delayGainArray )
  2297. {
  2298. cmMDelay* p = cmObjAlloc( cmMDelay, ctx, ap );
  2299. if( procSmpCnt != 0 )
  2300. if( cmMDelayInit( p, procSmpCnt, srate, fbCoeff, delayCnt, delayMsArray, delayGainArray ) != cmOkRC )
  2301. cmMDelayFree(&p);
  2302. return p;
  2303. }
  2304. cmRC_t cmMDelayFree( cmMDelay** pp )
  2305. {
  2306. cmRC_t rc;
  2307. if( pp == NULL || *pp==NULL)
  2308. return cmOkRC;
  2309. cmMDelay* p = *pp;
  2310. if((rc = cmMDelayFinal(p)) != cmOkRC )
  2311. return rc;
  2312. unsigned i;
  2313. for(i=0; i<p->delayCnt; ++i)
  2314. cmMemPtrFree(&p->delayArray[i].delayBuf);
  2315. cmMemPtrFree(&p->delayArray);
  2316. cmMemPtrFree(&p->outV);
  2317. cmObjFree(pp);
  2318. return cmOkRC;
  2319. }
  2320. cmRC_t cmMDelayInit( cmMDelay* p, unsigned procSmpCnt, cmReal_t srate, cmReal_t fbCoeff, unsigned delayCnt, const cmReal_t* delayMsArray, const cmReal_t* delayGainArray )
  2321. {
  2322. cmRC_t rc;
  2323. if((rc = cmMDelayFinal(p)) != cmOkRC )
  2324. return rc;
  2325. if( delayCnt <= 0 )
  2326. return rc;
  2327. p->delayArray = cmMemResizeZ( cmMDelayHead, p->delayArray, delayCnt );
  2328. unsigned i;
  2329. for(i=0; i<delayCnt; ++i)
  2330. {
  2331. p->delayArray[i].delayGain = delayGainArray == NULL ? 1.0 : delayGainArray[i];
  2332. p->delayArray[i].delayMs = delayMsArray[i];
  2333. p->delayArray[i].delaySmpFrac = delayMsArray[i] * srate / 1000.0;
  2334. p->delayArray[i].delayBufSmpCnt = ceil(delayMsArray[i] * srate / 1000)+2;
  2335. p->delayArray[i].delayBuf = cmMemResizeZ( cmSample_t, p->delayArray[i].delayBuf, p->delayArray[i].delayBufSmpCnt );
  2336. p->delayArray[i].inIdx = 0;
  2337. }
  2338. p->delayCnt= delayCnt;
  2339. p->outV = cmMemResizeZ( cmSample_t, p->outV, procSmpCnt );
  2340. p->outN = procSmpCnt;
  2341. p->fbCoeff = fbCoeff;
  2342. p->srate = srate;
  2343. return cmOkRC;
  2344. }
  2345. cmRC_t cmMDelayFinal( cmMDelay* p )
  2346. { return cmOkRC; }
  2347. void _cmMDelayExec( cmMDelay* p, cmMDelayHead* hp, const cmSample_t inV[], cmSample_t outV[], unsigned sigN )
  2348. {
  2349. cmSample_t* dl = hp->delayBuf; // ptr to the base of the delay line
  2350. cmReal_t dfi = (cmReal_t)(hp->inIdx - hp->delaySmpFrac) + hp->delayBufSmpCnt; // fractional delay in samples
  2351. int dii0 = ((int)dfi) % hp->delayBufSmpCnt; // index to the sample just before the delay position
  2352. int dii1 = (dii0 + 1) % hp->delayBufSmpCnt; // index to the sample just after the delay position
  2353. //cmReal_t frac = 0; //dfi - dii0; // interpolation coeff.
  2354. unsigned i;
  2355. for(i=0; i<sigN; i++)
  2356. {
  2357. /*
  2358. outPtr[i] = -(((f+0)*(f-1)*(f-2)/6) * _wtPtr[iPhs0])
  2359. +(((f+1)*(f-1)*(f-2)/2) * _wtPtr[iPhs0+1])
  2360. -(((f+1)*(f-0)*(f-2)/2) * _wtPtr[iPhs0+2])
  2361. +(((f+1)*(f-0)*(f-1)/6) * _wtPtr[iPhs0+3]);
  2362. */
  2363. cmSample_t outSmp = dl[dii0]; // + (frac * (dl[dii1]-dl[dii0]));
  2364. outV[i] += outSmp/p->delayCnt;
  2365. dl[hp->inIdx] = (p->fbCoeff * outSmp) + inV[i];
  2366. hp->inIdx = (hp->inIdx+1) % hp->delayBufSmpCnt;
  2367. dii0 = (dii0+1) % hp->delayBufSmpCnt;
  2368. dii1 = (dii1+1) % hp->delayBufSmpCnt;
  2369. }
  2370. }
  2371. cmRC_t cmMDelayExec( cmMDelay* p, const cmSample_t* inV, cmSample_t* outV, unsigned sigN, bool bypassFl )
  2372. {
  2373. assert( sigN <= p->outN);
  2374. if( outV == NULL )
  2375. {
  2376. outV = p->outV;
  2377. sigN = cmMin(sigN,p->outN);
  2378. cmVOS_Fill(outV,sigN,0);
  2379. }
  2380. else
  2381. {
  2382. cmVOS_Zero(outV,sigN);
  2383. }
  2384. if( inV == NULL )
  2385. return cmOkRC;
  2386. if( bypassFl )
  2387. {
  2388. memcpy(outV,inV,sigN*sizeof(cmSample_t));
  2389. return cmOkRC;
  2390. }
  2391. unsigned di;
  2392. for( di=0; di<p->delayCnt; ++di)
  2393. {
  2394. cmMDelayHead* hp = p->delayArray + di;
  2395. hp->delaySmpFrac = hp->delayMs * p->srate / 1000.0;
  2396. _cmMDelayExec(p,hp,inV,outV,sigN);
  2397. }
  2398. return cmOkRC;
  2399. }
  2400. void cmMDelaySetTapMs( cmMDelay* p, unsigned tapIdx, cmReal_t ms )
  2401. {
  2402. assert( tapIdx < p->delayCnt );
  2403. p->delayArray[tapIdx].delayMs = ms;
  2404. }
  2405. void cmMDelaySetTapGain(cmMDelay* p, unsigned tapIdx, cmReal_t gain )
  2406. {
  2407. assert( tapIdx < p->delayCnt );
  2408. p->delayArray[tapIdx].delayGain = gain;
  2409. }
  2410. void cmMDelayReport( cmMDelay* p, cmRpt_t* rpt )
  2411. {
  2412. cmRptPrintf(rpt,"tap cnt:%i fb:%f sr:%f\n",p->delayCnt,p->fbCoeff,p->srate);
  2413. }
  2414. //------------------------------------------------------------------------------------------------------------
  2415. cmAudioSegPlayer* cmAudioSegPlayerAlloc( cmCtx* ctx, cmAudioSegPlayer* ap, unsigned procSmpCnt, unsigned outChCnt )
  2416. {
  2417. cmAudioSegPlayer* p = cmObjAlloc( cmAudioSegPlayer, ctx, ap );
  2418. if( procSmpCnt != 0 )
  2419. if( cmAudioSegPlayerInit( p, procSmpCnt, outChCnt ) != cmOkRC )
  2420. cmAudioSegPlayerFree(&p);
  2421. return p;
  2422. }
  2423. cmRC_t cmAudioSegPlayerFree( cmAudioSegPlayer** pp )
  2424. {
  2425. if( pp == NULL || *pp == NULL )
  2426. return cmOkRC;
  2427. cmAudioSegPlayer* p = *pp;
  2428. cmMemPtrFree(&p->segArray);
  2429. cmMemPtrFree(&p->outM);
  2430. cmObjFree(pp);
  2431. return cmOkRC;
  2432. }
  2433. cmRC_t cmAudioSegPlayerInit( cmAudioSegPlayer* p, unsigned procSmpCnt, unsigned outChCnt )
  2434. {
  2435. cmRC_t rc = cmOkRC;
  2436. if((rc = cmAudioSegPlayerFinal(p)) != cmOkRC )
  2437. return rc;
  2438. p->procSmpCnt = procSmpCnt;
  2439. p->outChCnt = outChCnt;
  2440. p->segCnt = 0;
  2441. if( outChCnt )
  2442. {
  2443. unsigned i;
  2444. p->outM = cmMemResizeZ( cmSample_t, p->outM, procSmpCnt * outChCnt );
  2445. p->outChArray = cmMemResizeZ( cmSample_t*, p->outChArray, outChCnt );
  2446. for(i=0; i<outChCnt; ++i)
  2447. p->outChArray[i] = p->outM + (i*procSmpCnt);
  2448. }
  2449. return rc;
  2450. }
  2451. cmRC_t cmAudioSegPlayerFinal( cmAudioSegPlayer* p )
  2452. { return cmOkRC; }
  2453. cmRC_t _cmAudioSegPlayerSegSetup( cmAudioSeg* sp, unsigned id, cmAudioFileBuf* bufPtr, unsigned smpIdx, unsigned smpCnt, unsigned outChIdx )
  2454. {
  2455. sp->bufPtr = bufPtr;
  2456. sp->id = id;
  2457. sp->smpIdx = smpIdx;
  2458. sp->smpCnt = smpCnt;
  2459. sp->outChIdx = outChIdx;
  2460. sp->outSmpIdx = 0;
  2461. sp->flags = 0;
  2462. return cmOkRC;
  2463. }
  2464. cmAudioSeg* _cmAudioSegPlayerIdToSegPtr( cmAudioSegPlayer* p, unsigned id, bool ignoreErrFl )
  2465. {
  2466. unsigned i = 0;
  2467. for(i=0; i<p->segCnt; ++i)
  2468. if( p->segArray[i].id == id )
  2469. return p->segArray + i;
  2470. if( !ignoreErrFl )
  2471. cmCtxRtCondition(&p->obj, cmArgAssertRC,"Unable to locate an audio segment with id=%i.",id);
  2472. return NULL;
  2473. }
  2474. cmRC_t cmAudioSegPlayerInsert( cmAudioSegPlayer* p, unsigned id, cmAudioFileBuf* bufPtr, unsigned smpIdx, unsigned smpCnt, unsigned outChIdx )
  2475. {
  2476. cmRC_t rc;
  2477. assert( _cmAudioSegPlayerIdToSegPtr( p, id, true ) == NULL );
  2478. p->segArray = cmMemResizePZ( cmAudioSeg, p->segArray, p->segCnt + 1 );
  2479. cmAudioSeg* sp = p->segArray + p->segCnt;
  2480. if((rc = _cmAudioSegPlayerSegSetup( sp, id, bufPtr, smpIdx, smpCnt, outChIdx )) == cmOkRC )
  2481. ++p->segCnt;
  2482. return rc;
  2483. }
  2484. cmRC_t cmAudioSegPlayerEdit( cmAudioSegPlayer* p, unsigned id, cmAudioFileBuf* bufPtr, unsigned smpIdx, unsigned smpCnt, unsigned outChIdx )
  2485. {
  2486. cmAudioSeg* sp = _cmAudioSegPlayerIdToSegPtr(p,id,false);
  2487. return _cmAudioSegPlayerSegSetup( sp, id, bufPtr, smpIdx, smpCnt, outChIdx );
  2488. }
  2489. cmRC_t cmAudioSegPlayerRemove( cmAudioSegPlayer* p, unsigned id, bool delFl )
  2490. {
  2491. cmAudioSeg* sp = _cmAudioSegPlayerIdToSegPtr(p,id,false);
  2492. if( sp == NULL )
  2493. return cmArgAssertRC;
  2494. sp->flags = cmEnaFlag( sp->flags, kDelAspFl, delFl );
  2495. return cmOkRC;
  2496. }
  2497. cmRC_t cmAudioSegPlayerEnable( cmAudioSegPlayer* p, unsigned id, bool enableFl, unsigned outSmpIdx )
  2498. {
  2499. cmAudioSeg* sp = _cmAudioSegPlayerIdToSegPtr(p,id,false);
  2500. if( sp == NULL )
  2501. return cmArgAssertRC;
  2502. if( outSmpIdx != cmInvalidIdx )
  2503. sp->outSmpIdx = outSmpIdx;
  2504. sp->flags = cmEnaFlag( sp->flags, kEnableAspFl, enableFl );
  2505. return cmOkRC;
  2506. }
  2507. void _cmAudioSegPlayerResetSeg( cmAudioSeg* sp )
  2508. {
  2509. sp->outSmpIdx = 0;
  2510. sp->flags = cmClrFlag(sp->flags, kEnableAspFl );
  2511. }
  2512. cmRC_t cmAudioSegPlayerReset( cmAudioSegPlayer* p )
  2513. {
  2514. unsigned i;
  2515. for(i=0; i<p->segCnt; ++i)
  2516. {
  2517. cmAudioSeg* sp = p->segArray + i;
  2518. _cmAudioSegPlayerResetSeg(sp);
  2519. }
  2520. return cmOkRC;
  2521. }
  2522. cmRC_t cmAudioSegPlayerExec( cmAudioSegPlayer* p, cmSample_t** outChPtr, unsigned outChCnt, unsigned procSmpCnt )
  2523. {
  2524. unsigned i;
  2525. if( outChPtr == NULL || outChCnt == 0 )
  2526. {
  2527. assert( p->outChCnt > 0 );
  2528. outChPtr = p->outChArray;
  2529. outChCnt = p->outChCnt;
  2530. assert( p->procSmpCnt <= procSmpCnt );
  2531. }
  2532. for(i=0; i<p->segCnt; ++i)
  2533. {
  2534. cmAudioSeg* sp = p->segArray + i;
  2535. // if the output channel is valid and the segment is enabled and not deleted
  2536. if( sp->outChIdx < outChCnt && (sp->flags & (kEnableAspFl | kDelAspFl)) == kEnableAspFl )
  2537. {
  2538. unsigned bufSmpIdx = sp->smpIdx + sp->outSmpIdx;
  2539. unsigned bufSmpCnt = 0;
  2540. // if all the samples have been played
  2541. if( sp->bufPtr->bufN <= bufSmpIdx )
  2542. _cmAudioSegPlayerResetSeg(sp);
  2543. else
  2544. {
  2545. // prevent playing past the end of the buffer
  2546. bufSmpCnt = cmMin( procSmpCnt, sp->bufPtr->bufN - bufSmpIdx );
  2547. // limit the number of samples to the segment length
  2548. bufSmpCnt = cmMin( bufSmpCnt, sp->smpCnt - sp->outSmpIdx );
  2549. // sum the samples into the output channel
  2550. cmVOS_AddVV( outChPtr[ sp->outChIdx ], bufSmpCnt, sp->bufPtr->bufV + bufSmpIdx );
  2551. // incr the next output sample index
  2552. sp->outSmpIdx += bufSmpCnt;
  2553. }
  2554. if( bufSmpCnt < procSmpCnt )
  2555. cmVOS_Zero( outChPtr[ sp->outChIdx ] + bufSmpCnt, procSmpCnt - bufSmpCnt );
  2556. }
  2557. }
  2558. return cmOkRC;
  2559. }
  2560. //------------------------------------------------------------------------------------------------------------
  2561. /*
  2562. cmCluster0* cmCluster0Alloc( cmCtx* ctx, cmCluster0* ap, unsigned stateCnt, unsigned binCnt, unsigned flags, cmCluster0DistFunc_t distFunc, void* dstUserPtr )
  2563. {
  2564. cmCluster0* p = cmObjAlloc( cmCluster0, ctx, ap );
  2565. if( stateCnt != 0 )
  2566. if( cmCluster0Init( p, stateCnt, binCnt, flags, distFunc, distUserPtr ) != cmOkRC )
  2567. cmCluster0Free(&p);
  2568. return p;
  2569. }
  2570. cmRC_t cmCluster0Free( cmCluster0** pp )
  2571. {
  2572. if( pp == NULL || *pp == NULL )
  2573. return cmOkRC;
  2574. cmCluster0* p = *pp;
  2575. cmMemPtrFree(&p->oM);
  2576. cmMemPtrFree(&p->tM);
  2577. cmMemPtrFree(&p->dV);
  2578. cmObjFree(pp);
  2579. return cmOkRC;
  2580. }
  2581. cmRC_t cmCluster0Init( cmCluster0* p, unsigned stateCnt, unsigned binCnt, unsigned flags, cmCluster0DistFunc_t distFunc, void* distUserPtr )
  2582. {
  2583. cmRC_t rc;
  2584. if((rc = cmCluster0Final(p)) != cmOkRC )
  2585. return rc;
  2586. p->oM = cmMemResizeZ( cmReal_t, p->oM, binCnt * stateCnt );
  2587. p->tM = cmMemResizeZ( cmReal_t, p->tM, stateCnt * stateCnt );
  2588. p->stateCnt = stateCnt;
  2589. p->binCnt = binCnt;
  2590. p->flags = flags;
  2591. p->distFunc = distFunc;
  2592. p->distUserPtr = distUserPtr;
  2593. p->cnt = 0;
  2594. }
  2595. cmRC_t cmCluster0Final( cmCluster0* p )
  2596. { return cmOkRC; }
  2597. cmRC_t cmCluster0Exec( cmCluster0* p, const cmReal_t* v, unsigned vn )
  2598. {
  2599. assert( vn <= p->binCnt );
  2600. ++cnt;
  2601. if( cnt <= stateCnt )
  2602. {
  2603. cmVOR_Copy( p->oM + ((cnt-1)*binCnt), vn, v );
  2604. return cmOkRC;
  2605. }
  2606. return cmOkRC;
  2607. }
  2608. */
  2609. cmNmf_t* cmNmfAlloc( cmCtx* ctx, cmNmf_t* ap, unsigned n, unsigned m, unsigned r, unsigned maxIterCnt, unsigned convergeCnt )
  2610. {
  2611. cmNmf_t* p = cmObjAlloc( cmNmf_t, ctx, ap );
  2612. if( n != 0 )
  2613. if( cmNmfInit( p, n, m, r, maxIterCnt, convergeCnt ) != cmOkRC )
  2614. cmNmfFree(&p);
  2615. return p;
  2616. }
  2617. cmRC_t cmNmfFree( cmNmf_t** pp )
  2618. {
  2619. if( pp== NULL || *pp == NULL )
  2620. return cmOkRC;
  2621. cmNmf_t* p = *pp;
  2622. cmMemPtrFree(&p->V);
  2623. cmMemPtrFree(&p->W);
  2624. cmMemPtrFree(&p->H);
  2625. cmMemPtrFree(&p->tr);
  2626. cmMemPtrFree(&p->x);
  2627. cmMemPtrFree(&p->t0nm);
  2628. cmMemPtrFree(&p->t1nm);
  2629. cmMemPtrFree(&p->Wt);
  2630. cmMemPtrFree(&p->trm);
  2631. cmMemPtrFree(&p->crm);
  2632. cmMemPtrFree(&p->c0);
  2633. cmMemPtrFree(&p->c1);
  2634. cmMemPtrFree(&p->idxV);
  2635. cmObjFree(pp);
  2636. return cmOkRC;
  2637. }
  2638. cmRC_t cmNmfInit( cmNmf_t* p, unsigned n, unsigned m, unsigned r, unsigned maxIterCnt, unsigned convergeCnt )
  2639. {
  2640. cmRC_t rc;
  2641. if((rc = cmNmfFinal(p)) != cmOkRC )
  2642. return rc;
  2643. p->n = n;
  2644. p->m = m;
  2645. p->r = r;
  2646. p->maxIterCnt = maxIterCnt;
  2647. p->convergeCnt= convergeCnt;
  2648. p->V = cmMemResizeZ(cmReal_t, p->V, n*m );
  2649. p->W = cmMemResize( cmReal_t, p->W, n*r );
  2650. p->H = cmMemResize( cmReal_t, p->H, r*m );
  2651. p->tr = cmMemResize( cmReal_t, p->tr, r );
  2652. p->x = cmMemResize( cmReal_t, p->x, r*cmMax(m,n) );
  2653. p->t0nm = cmMemResize( cmReal_t, p->t0nm, cmMax(r,n)*m );
  2654. p->Ht = p->t0nm;
  2655. p->t1nm = cmMemResize( cmReal_t, p->t1nm, n*m );
  2656. p->Wt = cmMemResize( cmReal_t, p->Wt, r*n );
  2657. p->trm = cmMemResize( cmReal_t, p->trm, r*cmMax(m,n) );
  2658. p->crm = cmMemResizeZ(unsigned, p->crm, r*m);
  2659. p->tnr = p->trm;
  2660. p->c0 = cmMemResizeZ(unsigned, p->c0, m*m);
  2661. p->c1 = cmMemResizeZ(unsigned, p->c1, m*m);
  2662. p->idxV = cmMemResizeZ(unsigned, p->idxV, m );
  2663. p->c0m = p->c0;
  2664. p->c1m = p->c1;
  2665. cmVOR_Random(p->W,n*r,0.0,1.0);
  2666. cmVOR_Random(p->H,r*m,0.0,1.0);
  2667. return rc;
  2668. }
  2669. cmRC_t cmNmfFinal(cmNmf_t* p )
  2670. { return cmOkRC; }
  2671. // NMF base on: Lee and Seung, 2001, Algo's for Non-negative Matrix Fcmtorization
  2672. // Connectivity stopping technique based on: http://www.broadinstitute.org/mpr/publications/projects/NMF/nmf.m
  2673. cmRC_t cmNmfExec( cmNmf_t* p, const cmReal_t* vM, unsigned cn )
  2674. {
  2675. cmRC_t rc = cmOkRC;
  2676. unsigned i,j,k;
  2677. unsigned n = p->n;
  2678. unsigned m = p->m;
  2679. unsigned r = p->r;
  2680. unsigned stopIter = 0;
  2681. assert(cn <= m );
  2682. // shift in the incoming columns of V[]
  2683. if( cn < m )
  2684. cmVOR_Shift(p->V, n*m, n*cn,0);
  2685. cmVOR_Copy( p->V, n*cn,vM );
  2686. // shift H[] by the same amount as V[]
  2687. if( cn < m )
  2688. cmVOR_Shift( p->H, r*m, r*cn,0);
  2689. cmVOR_Random(p->H, r*cn, 0.0, 1.0 );
  2690. cmVOU_Zero( p->c1m, m*m );
  2691. for(i=0,j=0; i<p->maxIterCnt && stopIter<p->convergeCnt; ++i)
  2692. {
  2693. // x[r,m] =repmat(sum(W,1)',1,m);
  2694. cmVOR_SumM( p->W, n, r, p->tr );
  2695. for(j=0; j<m; ++j)
  2696. cmVOR_Copy( p->x + (j*r), r, p->tr );
  2697. cmVOR_Transpose(p->Wt,p->W,n,r);
  2698. //H=H.*(W'*(V./(W*H)))./x;
  2699. cmVOR_MultMMM(p->t0nm,n,m,p->W,p->H,r); // t0nm[n,m] = W*H
  2700. cmVOR_DivVVV( p->t1nm,n*m,p->V,p->t0nm); // t1nm[n,m] = V./(W*H)
  2701. cmVOR_MultMMM(p->trm,r,m,p->Wt,p->t1nm,n); // trm[r,m] = W'*(V./(W*H))
  2702. cmVOR_MultVV(p->H,r*m,p->trm); // H[r,m] = H .* (W'*(V./(W*H)))
  2703. cmVOR_DivVV(p->H,r*m, p->x ); // H[r,m] = (H .* (W'*(V./(W*H)))) ./ x
  2704. // x[n,r]=repmat(sum(H,2)',n,1);
  2705. cmVOR_SumMN(p->H, r, m, p->tr );
  2706. for(j=0; j<n; ++j)
  2707. cmVOR_CopyN(p->x + j, r, n, p->tr, 1 );
  2708. cmVOR_Transpose(p->Ht,p->H,r,m);
  2709. // W=W.*((V./(W*H))*H')./x;
  2710. cmVOR_MultMMM(p->tnr,n,r,p->t1nm,p->Ht,m); // tnr[n,r] = (V./(W*H))*Ht
  2711. cmVOR_MultVV(p->W,n*r,p->tnr); // W[n,r] = W.*(V./(W*H))*Ht
  2712. cmVOR_DivVV(p->W,n*r,p->x); // W[n,r] = W.*(V./(W*H))*Ht ./x
  2713. if( i % 10 == 0 )
  2714. {
  2715. cmVOR_ReplaceLte( p->H, r*m, p->H, 2.2204e-16, 2.2204e-16 );
  2716. cmVOR_ReplaceLte( p->W, n*r, p->W, 2.2204e-16, 2.2204e-16 );
  2717. cmVOR_MaxIndexM( p->idxV, p->H, r, m );
  2718. unsigned mismatchCnt = 0;
  2719. for(j=0; j<m; ++j)
  2720. for(k=0; k<m; ++k)
  2721. {
  2722. unsigned c_idx = (j*m)+k;
  2723. p->c0m[ c_idx ] = p->idxV[j] == p->idxV[k];
  2724. mismatchCnt += p->c0m[ c_idx ] != p->c1m[ c_idx ];
  2725. }
  2726. if( mismatchCnt == 0 )
  2727. ++stopIter;
  2728. else
  2729. stopIter = 0;
  2730. printf("%i %i %i\n",i,stopIter,mismatchCnt);
  2731. fflush(stdout);
  2732. unsigned* tcm = p->c0m;
  2733. p->c0m = p->c1m;
  2734. p->c1m = tcm;
  2735. }
  2736. }
  2737. return rc;
  2738. }
  2739. //------------------------------------------------------------------------------------------------------------
  2740. cmRC_t _cmVectArrayAppend( cmVectArray_t* p, const void* v, unsigned typeByteCnt, unsigned valCnt )
  2741. {
  2742. cmRC_t rc = cmSubSysFailRC;
  2743. cmVectArrayVect_t* ep = NULL;
  2744. unsigned byteCnt = typeByteCnt * valCnt;
  2745. if( byteCnt == 0 || v == NULL )
  2746. return rc;
  2747. // verify that all vectors written to this vector array contain the same data type.
  2748. if( (cmIsFlag(p->flags,kFloatVaFl) && typeByteCnt!=sizeof(float)) || (cmIsFlag(p->flags,kDoubleVaFl) && typeByteCnt!=sizeof(double)))
  2749. return cmCtxRtCondition(&p->obj,cmInvalidArgRC,"All data stored to a cmVectArray_t must be a consistent type.");
  2750. // allocate space for the link record
  2751. if((ep = cmMemAllocZ(cmVectArrayVect_t,1)) == NULL )
  2752. goto errLabel;
  2753. // allocate space for the vector data
  2754. if((ep->u.v = cmMemAlloc(char,typeByteCnt*valCnt)) == NULL )
  2755. goto errLabel;
  2756. // append the link recd to the end of the element list
  2757. if( p->ep != NULL )
  2758. p->ep->link = ep;
  2759. else
  2760. {
  2761. p->bp = ep;
  2762. p->cur = p->bp;
  2763. }
  2764. p->ep = ep;
  2765. // store the length of the vector
  2766. ep->n = valCnt;
  2767. // copy in the vector data
  2768. memcpy(ep->u.v,v,byteCnt);
  2769. // track the number of vectors stored
  2770. p->vectCnt += 1;
  2771. // track the longest data vector
  2772. if( valCnt > p->maxEleCnt )
  2773. p->maxEleCnt = valCnt;
  2774. rc = cmOkRC;
  2775. errLabel:
  2776. if(rc != cmOkRC )
  2777. {
  2778. cmMemFree(ep->u.v);
  2779. cmMemFree(ep);
  2780. }
  2781. return rc;
  2782. }
  2783. cmVectArray_t* cmVectArrayAlloc( cmCtx* ctx, unsigned flags )
  2784. {
  2785. cmRC_t rc = cmOkRC;
  2786. cmVectArray_t* p = cmObjAlloc(cmVectArray_t,ctx,NULL);
  2787. assert(p != NULL);
  2788. switch( flags & (kFloatVaFl | kDoubleVaFl | kSampleVaFl | kRealVaFl ) )
  2789. {
  2790. case kFloatVaFl:
  2791. p->flags |= kFloatVaFl;
  2792. p->typeByteCnt = sizeof(float);
  2793. break;
  2794. case kDoubleVaFl:
  2795. p->flags |= kDoubleVaFl;
  2796. p->typeByteCnt = sizeof(double);
  2797. break;
  2798. case kSampleVaFl:
  2799. if( sizeof(cmSample_t) == sizeof(float) )
  2800. p->flags |= kFloatVaFl;
  2801. else
  2802. {
  2803. if( sizeof(cmSample_t) == sizeof(double))
  2804. p->flags |= kDoubleVaFl;
  2805. else
  2806. rc = cmCtxRtCondition(&p->obj,cmInvalidArgRC,"The size of the cmSample_t is not consistent with float or double.");
  2807. }
  2808. p->typeByteCnt = sizeof(cmSample_t);
  2809. break;
  2810. case kRealVaFl:
  2811. if( sizeof(cmReal_t) == sizeof(float) )
  2812. p->flags |= kFloatVaFl;
  2813. else
  2814. {
  2815. if( sizeof(cmReal_t) == sizeof(double))
  2816. p->flags |= kDoubleVaFl;
  2817. else
  2818. rc = cmCtxRtCondition(&p->obj,cmInvalidArgRC,"The size of the cmReal_t is not consistent with float or double.");
  2819. }
  2820. p->typeByteCnt = sizeof(cmReal_t);
  2821. break;
  2822. default:
  2823. rc = cmCtxRtCondition(&p->obj,cmInvalidArgRC,"The vector array value type flag was not recognized.");
  2824. }
  2825. if(rc != cmOkRC)
  2826. cmVectArrayFree(&p);
  2827. return p;
  2828. }
  2829. cmVectArray_t* cmVectArrayAllocFromFile(cmCtx* ctx, const char* fn )
  2830. {
  2831. cmRC_t rc = cmOkRC;
  2832. FILE* fp = NULL;
  2833. char* buf = NULL;
  2834. cmVectArray_t* p = NULL;
  2835. unsigned hn = 4;
  2836. unsigned hdr[hn];
  2837. // create the file
  2838. if((fp = fopen(fn,"rb")) == NULL )
  2839. {
  2840. rc = cmCtxRtCondition(&ctx->obj,cmSystemErrorRC,"The vector array file '%s' could not be opened.",cmStringNullGuard(fn));
  2841. goto errLabel;
  2842. }
  2843. if( fread(hdr,sizeof(unsigned),hn,fp) != hn )
  2844. {
  2845. rc = cmCtxRtCondition(&ctx->obj,cmSystemErrorRC,"The vector array file header could not be read from '%s'.",cmStringNullGuard(fn));
  2846. goto errLabel;
  2847. }
  2848. unsigned flags = hdr[0];
  2849. unsigned typeByteCnt = hdr[1];
  2850. unsigned vectCnt = hdr[2];
  2851. unsigned maxEleCnt = hdr[3];
  2852. unsigned i;
  2853. buf = cmMemAlloc(char,maxEleCnt*typeByteCnt);
  2854. if((p = cmVectArrayAlloc(ctx, flags )) == NULL )
  2855. goto errLabel;
  2856. for(i=0; i<vectCnt; ++i)
  2857. {
  2858. unsigned vn;
  2859. if( fread(&vn,sizeof(unsigned),1,fp) != 1 )
  2860. {
  2861. rc = cmCtxRtCondition(&p->obj,cmSystemErrorRC,"The vector array file element count read failed on vector index:%i in '%s'.",i,cmStringNullGuard(fn));
  2862. goto errLabel;
  2863. }
  2864. assert( vn <= maxEleCnt );
  2865. if( fread(buf,typeByteCnt,vn,fp) != vn )
  2866. {
  2867. rc = cmCtxRtCondition(&p->obj,cmSystemErrorRC,"The vector array data read failed on vector index:%i in '%s'.",i,cmStringNullGuard(fn));
  2868. goto errLabel;
  2869. }
  2870. if((rc = _cmVectArrayAppend(p,buf, typeByteCnt, vn )) != cmOkRC )
  2871. {
  2872. rc = cmCtxRtCondition(&p->obj,rc,"The vector array data store failed on vector index:%i in '%s'.",i,cmStringNullGuard(fn));
  2873. goto errLabel;
  2874. }
  2875. }
  2876. errLabel:
  2877. if( fp != NULL )
  2878. fclose(fp);
  2879. cmMemFree(buf);
  2880. if(rc != cmOkRC && p != NULL)
  2881. cmVectArrayFree(&p);
  2882. return p;
  2883. }
  2884. cmRC_t cmVectArrayFree( cmVectArray_t** pp )
  2885. {
  2886. cmRC_t rc = cmOkRC;
  2887. if( pp == NULL || *pp == NULL )
  2888. return rc;
  2889. cmVectArray_t* p = *pp;
  2890. if((rc = cmVectArrayClear(p)) != cmOkRC )
  2891. return rc;
  2892. cmMemFree(p->tempV);
  2893. cmObjFree(pp);
  2894. return rc;
  2895. }
  2896. cmRC_t cmVectArrayClear( cmVectArray_t* p )
  2897. {
  2898. cmVectArrayVect_t* ep = p->bp;
  2899. while( ep!=NULL )
  2900. {
  2901. cmVectArrayVect_t* np = ep->link;
  2902. cmMemFree(ep->u.v);
  2903. cmMemFree(ep);
  2904. ep = np;
  2905. }
  2906. p->bp = NULL;
  2907. p->ep = NULL;
  2908. p->maxEleCnt = 0;
  2909. p->vectCnt = 0;
  2910. return cmOkRC;
  2911. }
  2912. unsigned cmVectArrayCount( const cmVectArray_t* p )
  2913. { return p->vectCnt; }
  2914. /*
  2915. unsigned cmVectArrayEleCount( const cmVectArray_t* p )
  2916. {
  2917. cmVectArrayVect_t* ep = p->bp;
  2918. unsigned n = 0;
  2919. for(; ep!=NULL; ep=ep->link )
  2920. n += ep->n;
  2921. return n;
  2922. }
  2923. */
  2924. cmRC_t cmVectArrayAppendS( cmVectArray_t* p, const cmSample_t* v, unsigned vn )
  2925. { return _cmVectArrayAppend(p,v,sizeof(v[0]),vn); }
  2926. cmRC_t cmVectArrayAppendR( cmVectArray_t* p, const cmReal_t* v, unsigned vn )
  2927. { return _cmVectArrayAppend(p,v,sizeof(v[0]),vn); }
  2928. cmRC_t cmVectArrayAppendF( cmVectArray_t* p, const float* v, unsigned vn )
  2929. { return _cmVectArrayAppend(p,v,sizeof(v[0]),vn); }
  2930. cmRC_t cmVectArrayAppendD( cmVectArray_t* p, const double* v, unsigned vn )
  2931. { return _cmVectArrayAppend(p,v,sizeof(v[0]),vn); }
  2932. cmRC_t cmVectArrayAppendI( cmVectArray_t* p, const int* v, unsigned vn )
  2933. {
  2934. p->tempV = cmMemResize(double,p->tempV,vn);
  2935. unsigned i;
  2936. for(i=0; i<vn; ++i)
  2937. p->tempV[i] = v[i];
  2938. cmRC_t rc = cmVectArrayAppendD(p,p->tempV,vn);
  2939. return rc;
  2940. }
  2941. cmRC_t cmVectArrayAppendU( cmVectArray_t* p, const unsigned* v, unsigned vn )
  2942. {
  2943. p->tempV = cmMemResize(double,p->tempV,vn);
  2944. unsigned i;
  2945. for(i=0; i<vn; ++i)
  2946. p->tempV[i] = v[i];
  2947. cmRC_t rc = cmVectArrayAppendD(p,p->tempV,vn);
  2948. return rc;
  2949. }
  2950. cmRC_t cmVectArrayWrite( cmVectArray_t* p, const char* fn )
  2951. {
  2952. cmRC_t rc = cmOkRC;
  2953. FILE* fp = NULL;
  2954. cmVectArrayVect_t* ep;
  2955. unsigned i;
  2956. unsigned hn = 4;
  2957. unsigned hdr[hn];
  2958. hdr[0] = p->flags;
  2959. hdr[1] = p->typeByteCnt;
  2960. hdr[2] = p->vectCnt;
  2961. hdr[3] = p->maxEleCnt;
  2962. // create the file
  2963. if((fp = fopen(fn,"wb")) == NULL )
  2964. return cmCtxRtCondition(&p->obj,cmSystemErrorRC,"The vector array file '%s' could not be created.",cmStringNullGuard(fn));
  2965. // write the header
  2966. if( fwrite(hdr,sizeof(unsigned),hn,fp) != hn )
  2967. {
  2968. rc = cmCtxRtCondition(&p->obj,cmSystemErrorRC,"Vector array file header write failed in '%s'.",cmStringNullGuard(fn));
  2969. goto errLabel;
  2970. }
  2971. // write each vector element
  2972. for(ep=p->bp,i=0; ep!=NULL; ep=ep->link,++i)
  2973. {
  2974. // write the count of data values in the vector
  2975. if( fwrite(&ep->n,sizeof(ep->n),1,fp) != 1 )
  2976. {
  2977. rc = cmCtxRtCondition(&p->obj,cmSystemErrorRC,"Vector array file write failed on element header %i in '%s'.",i,cmStringNullGuard(fn));
  2978. goto errLabel;
  2979. }
  2980. // write the vector
  2981. if(fwrite(ep->u.v,p->typeByteCnt,ep->n,fp) != ep->n )
  2982. {
  2983. rc = cmCtxRtCondition(&p->obj,cmSystemErrorRC,"Vector array file write failed on data vector %i in '%s'.",i,cmStringNullGuard(fn));
  2984. goto errLabel;
  2985. }
  2986. }
  2987. errLabel:
  2988. if( fp != NULL )
  2989. fclose(fp);
  2990. return rc;
  2991. }
  2992. unsigned cmVectArrayForEachS( cmVectArray_t* p, unsigned idx, unsigned cnt, cmVectArrayForEachFuncS_t func, void* arg )
  2993. {
  2994. cmVectArrayVect_t* ep = p->bp;
  2995. unsigned i = 0;
  2996. unsigned n = 0;
  2997. // for each sub-array
  2998. for(; ep!=NULL && n<cnt; ep=ep->link )
  2999. {
  3000. // if the cur sub-array is in the range of idx:idx+cnt
  3001. if( i <= idx && idx < i + ep->n )
  3002. {
  3003. unsigned j = idx - i; // starting idx into cur sub-array
  3004. assert(j<ep->n);
  3005. unsigned m = cmMin(ep->n - j,cnt-n); // cnt of ele's to send from cur sub-array
  3006. // do callback
  3007. if( func(arg, idx, ep->u.sV + j, m ) != cmOkRC )
  3008. break;
  3009. idx += m;
  3010. n += m;
  3011. }
  3012. i += ep->n;
  3013. }
  3014. return n;
  3015. }
  3016. cmRC_t cmVectArrayWriteVectorS( cmCtx* ctx, const char* fn, const cmSample_t* v, unsigned vn )
  3017. {
  3018. cmRC_t rc = cmOkRC;
  3019. cmVectArray_t* p;
  3020. if((p = cmVectArrayAlloc(ctx,kSampleVaFl)) == NULL )
  3021. return cmCtxRtCondition(&ctx->obj,cmSubSysFailRC,"Unable to allocate an cmVectArray_t in cmVectArrayWriteVectorS().");
  3022. if((rc = cmVectArrayAppendS(p,v,vn)) != cmOkRC )
  3023. {
  3024. rc = cmCtxRtCondition(&p->obj,rc,"Vector append failed in cmVectArrayWriteVectorS().");
  3025. goto errLabel;
  3026. }
  3027. if((rc = cmVectArrayWrite(p,fn)) != cmOkRC )
  3028. rc = cmCtxRtCondition(&p->obj,rc,"Vector array write failed in cmVectArrayWriteVectorS().");
  3029. errLabel:
  3030. if((rc = cmVectArrayFree(&p)) != cmOkRC )
  3031. rc = cmCtxRtCondition(&ctx->obj,rc,"Free failed on cmVectArrayFree() in cmVectArrayWriteVectorS().");
  3032. return rc;
  3033. }
  3034. cmRC_t cmVectArrayWriteMatrixS( cmCtx* ctx, const char* fn, const cmSample_t* m, unsigned rn, unsigned cn )
  3035. {
  3036. cmRC_t rc = cmOkRC;
  3037. cmVectArray_t* p;
  3038. unsigned i;
  3039. if((p = cmVectArrayAlloc(ctx,kSampleVaFl)) == NULL )
  3040. return cmCtxRtCondition(&ctx->obj,cmSubSysFailRC,"Unable to allocate an cmVectArray_t in cmVectArrayWriteVectorS().");
  3041. for(i=0; i<cn; ++i)
  3042. {
  3043. if((rc = cmVectArrayAppendS(p,m + (i*rn), rn)) != cmOkRC )
  3044. {
  3045. rc = cmCtxRtCondition(&p->obj,rc,"Vector append failed in cmVectArrayWriteVectorS().");
  3046. goto errLabel;
  3047. }
  3048. }
  3049. if((rc = cmVectArrayWrite(p,fn)) != cmOkRC )
  3050. rc = cmCtxRtCondition(&p->obj,rc,"Vector array write failed in cmVectArrayWriteVectorS().");
  3051. errLabel:
  3052. if((rc = cmVectArrayFree(&p)) != cmOkRC )
  3053. rc = cmCtxRtCondition(&ctx->obj,rc,"Free failed on cmVectArrayFree() in cmVectArrayWriteVectorS().");
  3054. return rc;
  3055. }
  3056. cmRC_t cmVectArrayWriteMatrixR( cmCtx* ctx, const char* fn, const cmReal_t* m, unsigned rn, unsigned cn )
  3057. {
  3058. cmRC_t rc = cmOkRC;
  3059. cmVectArray_t* p;
  3060. unsigned i;
  3061. if((p = cmVectArrayAlloc(ctx,kRealVaFl)) == NULL )
  3062. return cmCtxRtCondition(&ctx->obj,cmSubSysFailRC,"Unable to allocate an cmVectArray_t in cmVectArrayWriteVectorR().");
  3063. for(i=0; i<cn; ++i)
  3064. {
  3065. if((rc = cmVectArrayAppendR(p,m + (i*rn), rn)) != cmOkRC )
  3066. {
  3067. rc = cmCtxRtCondition(&p->obj,rc,"Vector append failed in cmVectArrayWriteVectorR().");
  3068. goto errLabel;
  3069. }
  3070. }
  3071. if((rc = cmVectArrayWrite(p,fn)) != cmOkRC )
  3072. rc = cmCtxRtCondition(&p->obj,rc,"Vector array write failed in cmVectArrayWriteVectorR().");
  3073. errLabel:
  3074. if((rc = cmVectArrayFree(&p)) != cmOkRC )
  3075. rc = cmCtxRtCondition(&ctx->obj,rc,"Free failed on cmVectArrayFree() in cmVectArrayWriteVectorR().");
  3076. return rc;
  3077. }
  3078. cmRC_t cmVectArrayWriteVectorI( cmCtx* ctx, const char* fn, const int* v, unsigned vn )
  3079. {
  3080. cmRC_t rc = cmOkRC;
  3081. cmVectArray_t* p;
  3082. if((p = cmVectArrayAlloc(ctx,kIntVaFl)) == NULL )
  3083. return cmCtxRtCondition(&ctx->obj,cmSubSysFailRC,"Unable to allocate an cmVectArray_t in cmVectArrayWriteVectorS().");
  3084. if((rc = cmVectArrayAppendI(p,v,vn)) != cmOkRC )
  3085. {
  3086. rc = cmCtxRtCondition(&p->obj,rc,"Vector append failed in cmVectArrayWriteVectorS().");
  3087. goto errLabel;
  3088. }
  3089. if((rc = cmVectArrayWrite(p,fn)) != cmOkRC )
  3090. rc = cmCtxRtCondition(&p->obj,rc,"Vector array write failed in cmVectArrayWriteVectorS().");
  3091. errLabel:
  3092. if((rc = cmVectArrayFree(&p)) != cmOkRC )
  3093. rc = cmCtxRtCondition(&ctx->obj,rc,"Free failed on cmVectArrayFree() in cmVectArrayWriteVectorS().");
  3094. return rc;
  3095. }
  3096. cmRC_t cmVectArrayWriteMatrixI( cmCtx* ctx, const char* fn, const int* m, unsigned rn, unsigned cn )
  3097. {
  3098. cmRC_t rc = cmOkRC;
  3099. cmVectArray_t* p;
  3100. unsigned i;
  3101. if((p = cmVectArrayAlloc(ctx,kIntVaFl)) == NULL )
  3102. return cmCtxRtCondition(&ctx->obj,cmSubSysFailRC,"Unable to allocate an cmVectArray_t in cmVectArrayWriteVectorI().");
  3103. for(i=0; i<cn; ++i)
  3104. {
  3105. if((rc = cmVectArrayAppendI(p,m + (i*rn), rn)) != cmOkRC )
  3106. {
  3107. rc = cmCtxRtCondition(&p->obj,rc,"Vector append failed in cmVectArrayWriteVectorI().");
  3108. goto errLabel;
  3109. }
  3110. }
  3111. if((rc = cmVectArrayWrite(p,fn)) != cmOkRC )
  3112. rc = cmCtxRtCondition(&p->obj,rc,"Vector array write failed in cmVectArrayWriteVectorI().");
  3113. errLabel:
  3114. if((rc = cmVectArrayFree(&p)) != cmOkRC )
  3115. rc = cmCtxRtCondition(&ctx->obj,rc,"Free failed on cmVectArrayFree() in cmVectArrayWriteVectorI().");
  3116. return rc;
  3117. }
  3118. cmRC_t cmVectArrayForEachTextFuncS( void* arg, unsigned idx, const cmSample_t* xV, unsigned xN )
  3119. {
  3120. assert(0);
  3121. return cmOkRC;
  3122. }
  3123. cmRC_t cmVectArrayRewind( cmVectArray_t* p )
  3124. {
  3125. p->cur = p->bp;
  3126. return cmOkRC;
  3127. }
  3128. cmRC_t cmVectArrayAdvance( cmVectArray_t* p, unsigned n )
  3129. {
  3130. unsigned i;
  3131. for(i=0; i<n; ++i)
  3132. {
  3133. if( p->cur == NULL )
  3134. break;
  3135. p->cur = p->cur->link;
  3136. }
  3137. return cmOkRC;
  3138. }
  3139. bool cmVectArrayIsEOL( const cmVectArray_t* p )
  3140. { return p->cur == NULL; }
  3141. unsigned cmVectArrayEleCount( const cmVectArray_t* p )
  3142. {
  3143. if( p->cur == NULL )
  3144. return 0;
  3145. return p->cur->n;
  3146. }
  3147. cmRC_t cmVectArrayGetF( cmVectArray_t* p, float* v, unsigned* vnRef )
  3148. {
  3149. assert( cmIsFlag(p->flags,kFloatVaFl) );
  3150. if( cmVectArrayIsEOL(p) )
  3151. return cmCtxRtCondition(&p->obj,cmSubSysFailRC,"VectArray get failed because the state is EOL.");
  3152. unsigned n = cmMin(*vnRef,p->cur->n);
  3153. cmVOF_Copy(v,n,p->cur->u.fV);
  3154. *vnRef = n;
  3155. return cmOkRC;
  3156. }
  3157. cmRC_t cmVectArrayGetD( cmVectArray_t* p, double* v, unsigned* vnRef )
  3158. {
  3159. assert( cmIsFlag(p->flags,kDoubleVaFl) );
  3160. if( cmVectArrayIsEOL(p) )
  3161. return cmCtxRtCondition(&p->obj,cmSubSysFailRC,"VectArray get failed because the state is EOL.");
  3162. unsigned n = cmMin(*vnRef,p->cur->n);
  3163. cmVOD_Copy(v,n,p->cur->u.dV);
  3164. *vnRef = n;
  3165. return cmOkRC;
  3166. }
  3167. cmRC_t cmVectArrayGetI( cmVectArray_t* p, int* v, unsigned* vnRef )
  3168. {
  3169. if( cmVectArrayIsEOL(p) )
  3170. return cmCtxRtCondition(&p->obj,cmSubSysFailRC,"VectArray get failed because the state is EOL.");
  3171. unsigned n = cmMin(*vnRef,p->cur->n);
  3172. unsigned i;
  3173. if( cmIsFlag(p->flags,kDoubleVaFl) )
  3174. {
  3175. for(i=0; i<n; ++i)
  3176. v[i] = (int)(p->cur->u.dV[i]);
  3177. }
  3178. else
  3179. {
  3180. if( cmIsFlag(p->flags,kFloatVaFl) )
  3181. {
  3182. for(i=0; i<n; ++i)
  3183. v[i] = (int)(p->cur->u.fV[i]);
  3184. }
  3185. else
  3186. {
  3187. assert(0);
  3188. }
  3189. }
  3190. *vnRef = n;
  3191. return cmOkRC;
  3192. }
  3193. cmRC_t cmVectArrayGetU( cmVectArray_t* p, unsigned* v, unsigned* vnRef )
  3194. {
  3195. if( cmVectArrayIsEOL(p) )
  3196. return cmCtxRtCondition(&p->obj,cmSubSysFailRC,"VectArray get failed because the state is EOL.");
  3197. unsigned n = cmMin(*vnRef,p->cur->n);
  3198. unsigned i;
  3199. if( cmIsFlag(p->flags,kDoubleVaFl) )
  3200. {
  3201. for(i=0; i<n; ++i)
  3202. v[i] = (unsigned)(p->cur->u.dV[i]);
  3203. }
  3204. else
  3205. {
  3206. if( cmIsFlag(p->flags,kFloatVaFl) )
  3207. {
  3208. for(i=0; i<n; ++i)
  3209. v[i] = (unsigned)(p->cur->u.fV[i]);
  3210. }
  3211. else
  3212. {
  3213. assert(0);
  3214. }
  3215. }
  3216. *vnRef = n;
  3217. return cmOkRC;
  3218. }
  3219. unsigned cmVectArrayVectEleCount( cmVectArray_t* p, unsigned groupIdx, unsigned groupCnt )
  3220. {
  3221. unsigned n = 0;
  3222. cmVectArrayVect_t* pos = p->cur;
  3223. if( cmVectArrayRewind(p) != cmOkRC )
  3224. goto errLabel;
  3225. if( cmVectArrayAdvance(p,groupIdx) != cmOkRC )
  3226. goto errLabel;
  3227. while( !cmVectArrayIsEOL(p) )
  3228. {
  3229. n += cmVectArrayEleCount(p);
  3230. if(cmVectArrayAdvance(p,groupCnt) != cmOkRC )
  3231. goto errLabel;
  3232. }
  3233. errLabel:
  3234. p->cur = pos;
  3235. return n;
  3236. }
  3237. cmRC_t cmVectArrayFormVectF( cmVectArray_t* p, unsigned groupIdx, unsigned groupCnt, float** vRef, unsigned* vnRef )
  3238. {
  3239. cmRC_t rc = cmOkRC;
  3240. *vRef = NULL;
  3241. *vnRef = 0;
  3242. unsigned N = cmVectArrayVectEleCount(p,groupIdx,groupCnt);
  3243. if( N == 0 )
  3244. return rc;
  3245. float* v = cmMemAllocZ(float,N);
  3246. unsigned i = 0;
  3247. cmVectArrayVect_t* pos = p->cur;
  3248. if( cmVectArrayRewind(p) != cmOkRC )
  3249. goto errLabel;
  3250. if( cmVectArrayAdvance(p,groupIdx) != cmOkRC )
  3251. goto errLabel;
  3252. while( !cmVectArrayIsEOL(p) )
  3253. {
  3254. unsigned n = cmVectArrayEleCount(p);
  3255. assert(i+n <= N);
  3256. cmVectArrayGetF(p,v+i,&n);
  3257. i += n;
  3258. cmVectArrayAdvance(p,groupCnt);
  3259. }
  3260. *vRef = v;
  3261. *vnRef = i;
  3262. errLabel:
  3263. p->cur = pos;
  3264. return rc;
  3265. }
  3266. cmRC_t cmVectArrayFormVectColF( cmVectArray_t* p, unsigned groupIdx, unsigned groupCnt, unsigned colIdx, float** vRef, unsigned* vnRef )
  3267. {
  3268. cmRC_t rc = cmOkRC;
  3269. *vRef = NULL;
  3270. *vnRef = 0;
  3271. // assume there will be one output element for each group
  3272. unsigned N = cmVectArrayCount(p)/groupCnt + 1;
  3273. if( N == 0 )
  3274. return rc;
  3275. float* v = cmMemAllocZ(float,N);
  3276. unsigned i = 0;
  3277. cmVectArrayVect_t* pos = p->cur;
  3278. if( cmVectArrayRewind(p) != cmOkRC )
  3279. goto errLabel;
  3280. if( cmVectArrayAdvance(p,groupIdx) != cmOkRC )
  3281. goto errLabel;
  3282. while( i<N && !cmVectArrayIsEOL(p) )
  3283. {
  3284. unsigned tn = cmVectArrayEleCount(p);
  3285. float tv[tn];
  3286. // read the sub-vector
  3287. cmVectArrayGetF(p,tv,&tn);
  3288. // store the output value
  3289. if( colIdx < tn )
  3290. {
  3291. v[i] = tv[colIdx];
  3292. i += 1;
  3293. }
  3294. cmVectArrayAdvance(p,groupCnt);
  3295. }
  3296. *vRef = v;
  3297. *vnRef = i;
  3298. errLabel:
  3299. p->cur = pos;
  3300. return rc;
  3301. }
  3302. cmRC_t cmVectArrayFormVectColU( cmVectArray_t* p, unsigned groupIdx, unsigned groupCnt, unsigned colIdx, unsigned** vRef, unsigned* vnRef )
  3303. {
  3304. cmRC_t rc = cmOkRC;
  3305. *vRef = NULL;
  3306. *vnRef = 0;
  3307. // assume there will be one output element for each group
  3308. unsigned N = cmVectArrayCount(p)/groupCnt + 1;
  3309. if( N == 0 )
  3310. return rc;
  3311. unsigned* v = cmMemAllocZ(unsigned,N);
  3312. unsigned i = 0;
  3313. cmVectArrayVect_t* pos = p->cur;
  3314. if( cmVectArrayRewind(p) != cmOkRC )
  3315. goto errLabel;
  3316. if( cmVectArrayAdvance(p,groupIdx) != cmOkRC )
  3317. goto errLabel;
  3318. while( i<N && !cmVectArrayIsEOL(p) )
  3319. {
  3320. unsigned tn = cmVectArrayEleCount(p);
  3321. unsigned tv[tn];
  3322. // read the sub-vector
  3323. cmVectArrayGetU(p,tv,&tn);
  3324. assert( colIdx < tn );
  3325. // store the output value
  3326. if( colIdx < tn )
  3327. v[i++] = tv[colIdx];
  3328. cmVectArrayAdvance(p,groupCnt);
  3329. }
  3330. *vRef = v;
  3331. *vnRef = i;
  3332. errLabel:
  3333. p->cur = pos;
  3334. return rc;
  3335. }
  3336. cmRC_t cmVectArrayTest( cmCtx* ctx, const char* fn )
  3337. {
  3338. cmRC_t rc = cmOkRC;
  3339. cmVectArray_t* p = NULL;
  3340. unsigned flags = kSampleVaFl;
  3341. cmSample_t v[] = { 0, 1, 2, 3, 4, 5 };
  3342. if( fn == NULL || strlen(fn)==0 )
  3343. return cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Invalid test output file name.");
  3344. if( (p = cmVectArrayAlloc(ctx,flags)) == NULL )
  3345. return cmCtxRtCondition(&p->obj,cmSubSysFailRC,"The vectory array object allocation failed.");
  3346. if( cmVectArrayAppendS(p,v,1) != cmOkRC )
  3347. {
  3348. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Vector append 1 failed.");
  3349. goto errLabel;
  3350. }
  3351. if( cmVectArrayAppendS(p,v+1,2) != cmOkRC )
  3352. {
  3353. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Vector append 2 failed.");
  3354. goto errLabel;
  3355. }
  3356. if( cmVectArrayAppendS(p,v+3,3) != cmOkRC )
  3357. {
  3358. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Vector append 3 failed.");
  3359. goto errLabel;
  3360. }
  3361. if( cmVectArrayWrite(p,fn) != cmOkRC )
  3362. {
  3363. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Vector array write failed.");
  3364. goto errLabel;
  3365. }
  3366. //cmVectArrayForEachS(p,0,cmVectArrayEleCount(p),cmVectArrayForEachTextFuncS,&ctx->printRpt);
  3367. if( cmVectArrayFree(&p) != cmOkRC )
  3368. {
  3369. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"The vectory array release failed.");
  3370. goto errLabel;
  3371. }
  3372. if((p = cmVectArrayAllocFromFile(ctx, fn )) == NULL )
  3373. {
  3374. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"VectArray alloc from file failed.");
  3375. goto errLabel;
  3376. }
  3377. while(!cmVectArrayIsEOL(p))
  3378. {
  3379. unsigned n = cmVectArrayEleCount(p);
  3380. cmSample_t v[n];
  3381. if( cmVectArrayGetS(p,v,&n) != cmOkRC )
  3382. {
  3383. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"VectArrayGetS() failed.");
  3384. goto errLabel;
  3385. }
  3386. cmVOS_PrintL("v:",NULL,1,n,v);
  3387. cmVectArrayAdvance(p,1);
  3388. }
  3389. errLabel:
  3390. if( cmVectArrayFree(&p) != cmOkRC )
  3391. rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"The vectory array release failed.");
  3392. return rc;
  3393. }
  3394. //-----------------------------------------------------------------------------------------------------------------------
  3395. cmWhFilt* cmWhFiltAlloc( cmCtx* c, cmWhFilt* p, unsigned binCnt, cmReal_t binHz, cmReal_t coeff, cmReal_t maxHz )
  3396. {
  3397. cmWhFilt* op = cmObjAlloc(cmWhFilt,c,p);
  3398. if( binCnt > 0 )
  3399. if( cmWhFiltInit(op,binCnt,binHz,coeff,maxHz) != cmOkRC )
  3400. cmWhFiltFree(&op);
  3401. return op;
  3402. }
  3403. cmRC_t cmWhFiltFree( cmWhFilt** pp )
  3404. {
  3405. cmRC_t rc = cmOkRC;
  3406. if( pp==NULL || *pp==NULL )
  3407. return rc;
  3408. cmWhFilt* p = *pp;
  3409. if((rc = cmWhFiltFinal(p)) != cmOkRC )
  3410. return rc;
  3411. cmMemFree(p->whM);
  3412. cmMemFree(p->whiV);
  3413. cmMemFree(p->iV);
  3414. cmObjFree(pp);
  3415. return rc;
  3416. }
  3417. cmRC_t cmWhFiltInit( cmWhFilt* p, unsigned binCnt, cmReal_t binHz, cmReal_t coeff, cmReal_t maxHz )
  3418. {
  3419. cmRC_t rc;
  3420. if((rc = cmWhFiltFinal(p)) != cmOkRC )
  3421. return rc;
  3422. p->binCnt = binCnt;
  3423. p->binHz = binHz;
  3424. p->bandCnt = maxHz == 0 ? 34 : ceil(log10(maxHz/229.0 + 1) * 21.4 - 1)-1;
  3425. if( p->bandCnt <= 0 )
  3426. return cmCtxRtCondition(&p->obj, cmInvalidArgRC, "Max. Hz too low to form any frequency bands.");
  3427. cmReal_t flV[ p->bandCnt ];
  3428. cmReal_t fcV[ p->bandCnt ];
  3429. cmReal_t fhV[ p->bandCnt ];
  3430. int i;
  3431. for(i=0; i<p->bandCnt; ++i)
  3432. {
  3433. fcV[i] = 229.0 * (pow(10.0,(i+2)/21.4) - 1.0);
  3434. flV[i] = i==0 ? 0 : fcV[i-1];
  3435. }
  3436. for(i=0; i<p->bandCnt-1; ++i)
  3437. fhV[i] = fcV[i+1];
  3438. fhV[p->bandCnt-1] = fcV[p->bandCnt-1] + (fcV[p->bandCnt-1] - fcV[p->bandCnt-2]);
  3439. //cmVOR_PrintL("flV",NULL,1,p->bandCnt,flV);
  3440. //cmVOR_PrintL("fcV",NULL,1,p->bandCnt,fcV);
  3441. //cmVOR_PrintL("fhV",NULL,1,p->bandCnt,fhV);
  3442. cmReal_t* tM = cmMemAlloc(cmReal_t,p->bandCnt * p->binCnt);
  3443. p->whM = cmMemResizeZ(cmReal_t,p->whM,p->binCnt * p->bandCnt);
  3444. p->iV = cmMemResizeZ(cmReal_t,p->iV,p->binCnt);
  3445. // generate the bin index values
  3446. for(i=0; i<p->binCnt; ++i)
  3447. p->iV[i] = i;
  3448. cmReal_t stSpread = 0; // set stSpread to 0 to use flV/fhV[]
  3449. cmVOR_TriangleMask(tM, p->bandCnt, p->binCnt, fcV, p->binHz, stSpread, flV, fhV );
  3450. cmVOR_Transpose(p->whM, tM, p->bandCnt, p->binCnt );
  3451. cmMemFree(tM);
  3452. //cmVOR_PrintL("whM",NULL,p->bandCnt,p->binCnt,p->whM);
  3453. //cmVectArrayWriteMatrixR(p->obj.ctx, "/home/kevin/temp/frqtrk/whM.va", p->whM, p->binCnt, p->bandCnt );
  3454. unsigned whiN = p->bandCnt+2;
  3455. p->whiV = cmMemResizeZ(cmReal_t,p->whiV,whiN);
  3456. for(i=0; i<whiN; ++i)
  3457. {
  3458. if( i == 0 )
  3459. p->whiV[i] = 0;
  3460. else
  3461. if( i == whiN-1 )
  3462. p->whiV[i] = fhV[p->bandCnt-1]/binHz;
  3463. else
  3464. p->whiV[i] = fcV[i-1]/binHz;
  3465. }
  3466. //cmVOR_PrintL("whiV",NULL,1,whiN,p->whiV);
  3467. //cmVectArrayWriteMatrixR(p->obj.ctx, "/home/kevin/temp/frqtrk/whiV.va", p->whiV, whiN, 1 );
  3468. return rc;
  3469. }
  3470. cmRC_t cmWhFiltFinal( cmWhFilt* p )
  3471. { return cmOkRC; }
  3472. cmRC_t cmWhFiltExec( cmWhFilt* p, const cmReal_t* xV, cmReal_t* yV, unsigned xyN )
  3473. {
  3474. assert( xyN == p->binCnt);
  3475. cmRC_t rc = cmOkRC;
  3476. unsigned whiN = p->bandCnt + 2;
  3477. unsigned mbi = cmMin(xyN, floor(p->whiV[whiN-1]));
  3478. // calculate the level in each band to form a composite filter
  3479. cmReal_t y0V[ whiN ];
  3480. cmReal_t* b0V = y0V + 1;
  3481. cmVOR_MultVVM(b0V, p->bandCnt, xV, p->binCnt, p->whM );
  3482. //cmVOR_PrintL("b0V",NULL,1,p->bandCnt,b0V);
  3483. // BEWARE: zeros in b0V will generate Inf's when sent
  3484. // through the cmVOR_PowVS() function.
  3485. int i;
  3486. for(i=0; i<p->bandCnt; ++i)
  3487. if( b0V[i] < 0.000001 )
  3488. b0V[i] = 0.000001;
  3489. // apply a non-linear expansion function to each band
  3490. cmVOR_PowVS(b0V,p->bandCnt,p->coeff-1);
  3491. //cmVOR_PrintL("b0V",NULL,1,p->bandCnt,b0V);
  3492. // add edge values to the filter
  3493. y0V[0] = b0V[0];
  3494. y0V[whiN-1] = b0V[p->bandCnt-1];
  3495. //cmVOR_PrintL("y0V",NULL,1,whiN,y0V);
  3496. cmVOR_Interp1(yV,p->iV,p->binCnt,p->whiV,y0V,whiN);
  3497. cmVOR_Fill(yV+mbi,xyN-mbi,1.0);
  3498. //cmVOR_PrintL("yV",NULL,1,p->binCnt,yV);
  3499. cmVOR_MultVV(yV,xyN,xV);
  3500. return rc;
  3501. }
  3502. //-----------------------------------------------------------------------------------------------------------------------
  3503. cmFrqTrk* cmFrqTrkAlloc( cmCtx* c, cmFrqTrk* p, const cmFrqTrkArgs_t* a )
  3504. {
  3505. cmFrqTrk* op = cmObjAlloc(cmFrqTrk,c,p);
  3506. op->logVa = cmVectArrayAlloc(c,kRealVaFl);
  3507. op->levelVa = cmVectArrayAlloc(c,kRealVaFl);
  3508. op->specVa = cmVectArrayAlloc(c,kRealVaFl);
  3509. op->attenVa = cmVectArrayAlloc(c,kRealVaFl);
  3510. op->wf = cmWhFiltAlloc(c,NULL,0,0,0,0);
  3511. if( a != NULL )
  3512. if( cmFrqTrkInit(op,a) != cmOkRC )
  3513. cmFrqTrkFree(&op);
  3514. return op;
  3515. }
  3516. cmRC_t cmFrqTrkFree( cmFrqTrk** pp )
  3517. {
  3518. cmRC_t rc = cmOkRC;
  3519. if( pp==NULL || *pp==NULL )
  3520. return rc;
  3521. cmFrqTrk* p = *pp;
  3522. if((rc = cmFrqTrkFinal(p)) != cmOkRC )
  3523. return rc;
  3524. unsigned i;
  3525. for(i=0; i<p->a.chCnt; ++i)
  3526. {
  3527. cmMemFree(p->ch[i].dbV);
  3528. cmMemFree(p->ch[i].hzV);
  3529. }
  3530. cmMemFree(p->ch);
  3531. cmMemFree(p->dbM);
  3532. cmMemFree(p->pkiV);
  3533. cmMemFree(p->dbV);
  3534. cmMemFree(p->aV);
  3535. cmVectArrayFree(&p->logVa);
  3536. cmVectArrayFree(&p->levelVa);
  3537. cmVectArrayFree(&p->specVa);
  3538. cmVectArrayFree(&p->attenVa);
  3539. cmWhFiltFree(&p->wf);
  3540. cmMemFree(p->logFn);
  3541. cmMemFree(p->levelFn);
  3542. cmMemFree(p->specFn);
  3543. cmObjFree(pp);
  3544. return rc;
  3545. }
  3546. cmRC_t cmFrqTrkInit( cmFrqTrk* p, const cmFrqTrkArgs_t* a )
  3547. {
  3548. cmRC_t rc;
  3549. if((rc = cmFrqTrkFinal(p)) != cmOkRC )
  3550. return rc;
  3551. p->a = *a;
  3552. p->ch = cmMemResizeZ(cmFrqTrkCh_t,p->ch,a->chCnt );
  3553. p->hN = cmMax(1,a->wndSecs * a->srate / a->hopSmpCnt );
  3554. p->sN = 4*p->hN;
  3555. p->binHz = a->srate / ((p->a.binCnt-1)*2);
  3556. p->bN = cmMin( p->a.binCnt, ceil(p->a.pkMaxHz / p->binHz ));
  3557. p->dbM = cmMemResizeZ(cmReal_t,p->dbM,p->hN*p->bN);
  3558. p->hi = 0;
  3559. p->fN = 0;
  3560. p->dbV = cmMemResizeZ(cmReal_t,p->dbV,p->bN);
  3561. p->pkiV = cmMemResizeZ(unsigned,p->pkiV,p->bN);
  3562. p->deadN_max = a->maxTrkDeadSec * a->srate / a->hopSmpCnt;
  3563. p->minTrkN = a->minTrkSec * a->srate / a->hopSmpCnt;
  3564. p->nextTrkId = 1;
  3565. p->aV = cmMemResizeZ(cmReal_t,p->aV,p->a.binCnt);
  3566. p->attenPhsMax = cmMax(3,a->attenAtkSec * a->srate / a->hopSmpCnt );
  3567. if( a->logFn != NULL )
  3568. p->logFn = cmMemResizeStr(p->logFn,a->logFn);
  3569. if( a->levelFn != NULL )
  3570. p->levelFn = cmMemResizeStr(p->levelFn,a->levelFn);
  3571. if( a->specFn != NULL )
  3572. p->specFn = cmMemResizeStr(p->specFn,a->specFn);
  3573. if( a->attenFn != NULL )
  3574. p->attenFn = cmMemResizeStr(p->attenFn,a->attenFn);
  3575. if(cmWhFiltInit(p->wf,p->bN,p->binHz,p->a.whFiltCoeff,p->a.pkMaxHz) != cmOkRC )
  3576. cmCtxRtCondition(&p->obj, cmSubSysFailRC, "Whitening filter intitialization failed.");
  3577. unsigned i;
  3578. for(i=0; i<p->a.chCnt; ++i)
  3579. {
  3580. p->ch[i].dbV = cmMemResizeZ(cmReal_t,p->ch[i].dbV,p->sN);
  3581. p->ch[i].hzV = cmMemResizeZ(cmReal_t,p->ch[i].hzV,p->sN);
  3582. }
  3583. return rc;
  3584. }
  3585. cmRC_t cmFrqTrkFinal( cmFrqTrk* p )
  3586. {
  3587. cmRC_t rc = cmOkRC;
  3588. if( p->logFn != NULL )
  3589. cmVectArrayWrite(p->logVa,p->logFn);
  3590. if( p->levelFn != NULL )
  3591. cmVectArrayWrite(p->levelVa,p->levelFn);
  3592. if( p->specFn != NULL )
  3593. cmVectArrayWrite(p->specVa,p->specFn);
  3594. if( p->attenFn != NULL )
  3595. cmVectArrayWrite(p->attenVa,p->attenFn);
  3596. cmWhFiltFinal(p->wf);
  3597. return rc;
  3598. }
  3599. // Return an available channel record or NULL if all channel records are in use.
  3600. cmFrqTrkCh_t* _cmFrqTrkFindAvailCh( cmFrqTrk* p )
  3601. {
  3602. unsigned i;
  3603. for(i=0; i<p->a.chCnt; ++i)
  3604. if( p->ch[i].activeFl == false )
  3605. return p->ch + i;
  3606. return NULL;
  3607. }
  3608. // Estimate the peak frequency by parabolic interpolotion into hzV[p->bN]
  3609. void _cmFrqTrkMagnToHz( cmFrqTrk* p, const cmReal_t* dbV, unsigned* pkiV, unsigned pkN, cmReal_t* hzV )
  3610. {
  3611. unsigned i;
  3612. for(i=0; i<pkN; ++i)
  3613. if( pkiV[i] != cmInvalidIdx )
  3614. {
  3615. unsigned pki = pkiV[i];
  3616. cmReal_t y0 = pki>0 ? dbV[ pki-1 ] : dbV[pki];
  3617. cmReal_t y1 = dbV[ pki ];
  3618. cmReal_t y2 = pki<p->bN-1 ? dbV[ pki+1 ] : dbV[pki];
  3619. cmReal_t den = y0 - (2.*y1) + y2;
  3620. cmReal_t offs = den==0 ? 0 : 0.5 * ((y0 - y2) / den);
  3621. hzV[pki] = p->binHz * (pki+offs);
  3622. //if( hzV[pki] < 0 )
  3623. //{
  3624. // printf("%f : %f %f %f : %f %f\n",hzV[pki],y0,y1,y2,den,offs);
  3625. //}
  3626. }
  3627. }
  3628. unsigned _cmFrqTrkActiveChCount( cmFrqTrk* p )
  3629. {
  3630. unsigned n = 0;
  3631. unsigned i;
  3632. for(i=0; i<p->a.chCnt; ++i)
  3633. if( p->ch[i].activeFl )
  3634. ++n;
  3635. return n;
  3636. }
  3637. void _cmFrqTrkWriteLevel( cmFrqTrk* p, const cmReal_t* dbV, const cmReal_t* hzV, unsigned bN )
  3638. {
  3639. if( p->levelFn != NULL )
  3640. {
  3641. double maxHz = 5000.0;
  3642. unsigned maxBinIdx = cmMin(bN,maxHz / p->binHz);
  3643. unsigned vn = 3;
  3644. cmReal_t v[vn];
  3645. unsigned idx = cmVOR_MaxIndex(dbV,maxBinIdx,1);
  3646. v[0] = cmVOR_Mean(dbV,maxBinIdx);
  3647. v[1] = dbV[idx];
  3648. v[2] = hzV[idx];
  3649. cmVectArrayAppendR(p->levelVa,v,vn);
  3650. }
  3651. }
  3652. void _cmFrqTrkWriteLog( cmFrqTrk* p )
  3653. {
  3654. unsigned n;
  3655. cmReal_t* vb = NULL;
  3656. if( p->logFn == NULL )
  3657. return;
  3658. if((n = _cmFrqTrkActiveChCount(p)) > 0 )
  3659. {
  3660. unsigned i,j;
  3661. // sn = count of elements in the summary sub-vector
  3662. unsigned sn = 3;
  3663. // each active channel will emit 7 values
  3664. unsigned nn = 1 + n*7 + sn;
  3665. // allocate the row vector
  3666. vb = cmMemResize(cmReal_t,vb,nn);
  3667. // row format
  3668. // [ nn idV[n] hzV[n] ... hsV[n] smV[sn] ]
  3669. // n = (nn - (1 + sn)) / 7
  3670. *vb = nn; // the first element in the vector contains the length of the row
  3671. cmReal_t* v = vb + 1;
  3672. // setup the base pointer to each sub-vector
  3673. cmReal_t* idV = v + n * 0;
  3674. cmReal_t* hzV = v + n * 1;
  3675. cmReal_t* dbV = v + n * 2;
  3676. cmReal_t* stV = v + n * 3;
  3677. cmReal_t* dsV = v + n * 4;
  3678. cmReal_t* hsV = v + n * 5;
  3679. cmReal_t* agV = v + n * 6;
  3680. cmReal_t* smV = v + n * 7; // summary information
  3681. smV[0] = p->newTrkCnt;
  3682. smV[1] = p->curTrkCnt;
  3683. smV[2] = p->deadTrkCnt;
  3684. // for each active channel
  3685. for(i=0,j=0; i<p->a.chCnt; ++i)
  3686. if( p->ch[i].activeFl )
  3687. {
  3688. assert(j < n);
  3689. // elements of each sub-vector associated with a given
  3690. // index refer to the same track record - element i therefore
  3691. // refers to active track index i.
  3692. idV[j] = p->ch[i].id;
  3693. hzV[j] = p->ch[i].hz;
  3694. dbV[j] = p->ch[i].db;
  3695. stV[j] = p->ch[i].dN;
  3696. dsV[j] = p->ch[i].db_std;
  3697. hsV[j] = p->ch[i].hz_std;
  3698. agV[j] = p->ch[i].attenGain;
  3699. ++j;
  3700. }
  3701. cmVectArrayAppendR(p->logVa, vb, nn );
  3702. }
  3703. cmMemFree(vb);
  3704. }
  3705. void _cmFrqTrkPrintChs( const cmFrqTrk* p )
  3706. {
  3707. unsigned i;
  3708. for(i=0; i<p->a.chCnt; ++i)
  3709. {
  3710. cmFrqTrkCh_t* c = p->ch + i;
  3711. printf("%i : %i tN:%i hz:%f db:%f\n",i,c->activeFl,c->tN,c->hz,c->db);
  3712. }
  3713. }
  3714. // Used to sort the channels into descending dB order.
  3715. int _cmFrqTrkChCompare( const void* p0, const void* p1 )
  3716. { return ((cmFrqTrkCh_t*)p0)->db - ((cmFrqTrkCh_t*)p1)->db; }
  3717. // Return the index of the peak associated with pkiV[i] which best matches the tracker 'c'
  3718. // or cmInvalidIdx if no valid peaks were found.
  3719. // pkiV[ pkN ] holds the indexes into dbV[] and hzV[] which are peaks.
  3720. // Some elements of pkiV[] may be set to cmInvalidIdx if the associated peak has already
  3721. // been selected by another tracker.
  3722. unsigned _cmFrqTrkFindPeak( cmFrqTrk* p, const cmFrqTrkCh_t* c, const cmReal_t* dbV, const cmReal_t* hzV, unsigned* pkiV, unsigned pkN )
  3723. {
  3724. unsigned i,pki;
  3725. cmReal_t d_max = p->a.pkThreshDb;
  3726. unsigned d_idx = cmInvalidIdx;
  3727. cmReal_t hz_min = c->hz * pow(2,-p->a.stRange/12.0);
  3728. cmReal_t hz_max = c->hz * pow(2, p->a.stRange/12.0);
  3729. // find the peak with the most energy inside the frequency range hz_min to hz_max.
  3730. for(i=0; i<pkN; ++i)
  3731. if( ((pki = pkiV[i]) != cmInvalidIdx) && hz_min <= hzV[pki] && hzV[pki] <= hz_max && dbV[pki]>d_max )
  3732. {
  3733. d_max= dbV[pki];
  3734. d_idx = i;
  3735. }
  3736. return d_idx;
  3737. }
  3738. void _cmFrqTrkScoreChs( cmFrqTrk* p )
  3739. {
  3740. unsigned i;
  3741. for(i=0; i<p->a.chCnt; ++i)
  3742. if( p->ch[i].activeFl )
  3743. {
  3744. cmFrqTrkCh_t* c = p->ch + i;
  3745. c->dbV[ c->si ] = c->db;
  3746. c->hzV[ c->si ] = c->hz;
  3747. c->si = (c->si + 1) % p->sN;
  3748. c->sn += 1;
  3749. unsigned n = cmMin(c->sn,p->sN);
  3750. c->db_mean = cmVOR_Mean(c->dbV,n);
  3751. c->db_std = sqrt(cmVOR_Variance( c->dbV,n,&c->db_mean));
  3752. c->hz_mean = cmVOR_Mean(c->hzV,n);
  3753. c->hz_std = sqrt(cmVOR_Variance( c->hzV,n,&c->hz_mean));
  3754. c->score = c->db / ((cmMin(0.1,c->db_std) + cmMin(0.1,c->hz_std))/2);
  3755. }
  3756. }
  3757. // Generate a filter that is wider for higher frequencies than lower frequencies.
  3758. unsigned _cmFrqTrkFillMap( cmFrqTrk* p, cmReal_t* map, unsigned maxN, cmReal_t hz )
  3759. {
  3760. assert( maxN % 2 == 1 );
  3761. unsigned i;
  3762. cmReal_t maxHz = p->a.srate/2;
  3763. unsigned mapN = cmMin(maxN,ceil(hz/maxHz * maxN));
  3764. if( mapN % 2 == 0 )
  3765. mapN += 1;
  3766. mapN = cmMin(maxN,mapN);
  3767. unsigned N = floor(mapN/2);
  3768. double COEFF = 0.3;
  3769. for(i=0; i<N; ++i)
  3770. {
  3771. map[i] = pow(((double)i+1)/(N+1),COEFF);
  3772. map[mapN-(i+1)] = map[i];
  3773. }
  3774. map[N] = 1.0;
  3775. return mapN;
  3776. }
  3777. void _cmFrqTrkApplyAtten( cmFrqTrk* p, cmReal_t* aV, cmReal_t gain, cmReal_t hz )
  3778. {
  3779. int cbi = cmMin(p->a.binCnt,cmMax(0,round(hz/p->binHz)));
  3780. //cmReal_t map[] = { .25, .5, 1, .5, .25 };
  3781. //int mapN = sizeof(map)/sizeof(map[0]);
  3782. unsigned maxN = 30; // must be odd
  3783. cmReal_t map[ maxN ];
  3784. int mapN = _cmFrqTrkFillMap(p, map, maxN, hz );
  3785. int j;
  3786. int ai = cbi - mapN/2;
  3787. for(j=0; j<mapN; ++j,++ai)
  3788. if( 0 <= ai && ai < p->a.binCnt )
  3789. aV[ai] *= 1.0 - (map[j] * gain);
  3790. }
  3791. void _cmFrqTrkUpdateFilter( cmFrqTrk* p )
  3792. {
  3793. unsigned i;
  3794. cmVOR_Fill(p->aV,p->a.binCnt,1.0);
  3795. for(i=0; i<p->a.chCnt; ++i)
  3796. if( p->ch[i].activeFl )
  3797. {
  3798. cmFrqTrkCh_t* c = p->ch + i;
  3799. //
  3800. if( c->score >= p->a.attenThresh && c->state == kNoStateFrqTrkId )
  3801. {
  3802. c->attenPhsIdx = 0;
  3803. c->state = kAtkFrqTrkId;
  3804. }
  3805. switch( c->state )
  3806. {
  3807. case kNoStateFrqTrkId:
  3808. break;
  3809. case kAtkFrqTrkId:
  3810. if( c->attenPhsIdx < p->attenPhsMax )
  3811. {
  3812. c->attenGain = cmMin(1.0,p->a.attenGain * c->attenPhsIdx / p->attenPhsMax);
  3813. _cmFrqTrkApplyAtten(p, p->aV, c->attenGain, c->hz);
  3814. }
  3815. c->attenPhsIdx += 1;
  3816. if( c->attenPhsIdx >= p->attenPhsMax )
  3817. c->state = kSusFrqTrkId;
  3818. break;
  3819. case kSusFrqTrkId:
  3820. if( c->dN > 0 )
  3821. {
  3822. if( c->attenPhsIdx > 0 )
  3823. {
  3824. c->attenPhsIdx -= 1;
  3825. c->attenGain = cmMin(1.0,p->a.attenGain * c->attenPhsIdx / p->attenPhsMax);
  3826. }
  3827. }
  3828. _cmFrqTrkApplyAtten(p,p->aV, c->attenGain, c->hz);
  3829. if( c->dN >= p->deadN_max )
  3830. c->state = kDcyFrqTrkId;
  3831. break;
  3832. case kDcyFrqTrkId:
  3833. if( c->attenPhsIdx > 0 )
  3834. {
  3835. c->attenPhsIdx -= 1;
  3836. c->attenGain = cmMin(1.0,p->a.attenGain * c->attenPhsIdx / p->attenPhsMax);
  3837. _cmFrqTrkApplyAtten(p,p->aV, c->attenGain, c->hz);
  3838. }
  3839. if( c->attenPhsIdx == 0 )
  3840. c->activeFl = false;
  3841. break;
  3842. }
  3843. }
  3844. }
  3845. // Extend the existing trackers
  3846. void _cmFrqTrkExtendChs( cmFrqTrk* p, const cmReal_t* dbV, const cmReal_t* hzV, unsigned* pkiV, unsigned pkN )
  3847. {
  3848. unsigned i;
  3849. p->curTrkCnt = 0;
  3850. p->deadTrkCnt = 0;
  3851. // sort the channels in descending order
  3852. qsort(p->ch,p->a.chCnt,sizeof(cmFrqTrkCh_t),_cmFrqTrkChCompare);
  3853. // for each active channel
  3854. for(i=0; i<p->a.chCnt; ++i)
  3855. {
  3856. cmFrqTrkCh_t* c = p->ch + i;
  3857. if( c->activeFl )
  3858. {
  3859. unsigned pki;
  3860. // find the best peak to extend tracker 'c'.
  3861. if((pki = _cmFrqTrkFindPeak(p,c,dbV,hzV,pkiV,pkN)) == cmInvalidIdx )
  3862. {
  3863. // no valid track was found to extend tracker 'c'
  3864. c->dN += 1;
  3865. c->tN += 1;
  3866. if( c->dN >= p->deadN_max )
  3867. {
  3868. if( c->attenPhsIdx == 0 )
  3869. c->activeFl = false;
  3870. p->deadTrkCnt += 1;
  3871. }
  3872. }
  3873. else // ... update the tracker using the matching peak
  3874. {
  3875. unsigned j = pkiV[pki];
  3876. c->dN = 0;
  3877. c->db = dbV[ j ];
  3878. c->hz = hzV[ j ];
  3879. c->tN += 1;
  3880. pkiV[pki] = cmInvalidIdx; // mark the peak as unavailable.
  3881. p->curTrkCnt += 1;
  3882. }
  3883. }
  3884. }
  3885. }
  3886. // disable peaks which are within 'stRange' semitones of the frequency of active trackers.
  3887. void _cmFrqTrkDisableClosePeaks( cmFrqTrk* p, const cmReal_t* dbV, const cmReal_t* hzV, unsigned* pkiV, unsigned pkN )
  3888. {
  3889. unsigned i;
  3890. for(i=0; i<p->a.chCnt; ++i)
  3891. {
  3892. const cmFrqTrkCh_t* c = p->ch + i;
  3893. if( !c->activeFl )
  3894. continue;
  3895. cmReal_t hz_min = c->hz * pow(2,-p->a.stRange/12.0);
  3896. cmReal_t hz_max = c->hz * pow(2, p->a.stRange/12.0);
  3897. unsigned j;
  3898. // find all peaks within the frequency range hz_min to hz_max.
  3899. for(j=0; j<pkN; ++j)
  3900. if( pkiV[j] != cmInvalidIdx && hz_min <= c->hz && c->hz <= hz_max )
  3901. pkiV[j] = cmInvalidIdx;
  3902. }
  3903. }
  3904. // Return the index into pkiV[] of the maximum energy peak in dbV[]
  3905. // that is also above kAtkThreshDb.
  3906. unsigned _cmFrqTrkMaxEnergyPeakIndex( const cmFrqTrk* p, const cmReal_t* dbV, const cmReal_t* hzV, const unsigned* pkiV, unsigned pkN )
  3907. {
  3908. cmReal_t mv = p->a.pkAtkThreshDb;
  3909. unsigned mi = cmInvalidIdx;
  3910. unsigned i;
  3911. for(i=0; i<pkN; ++i)
  3912. if( pkiV[i] != cmInvalidIdx && dbV[pkiV[i]] >= mv && hzV[pkiV[i]] < p->a.pkMaxHz )
  3913. {
  3914. mi = i;
  3915. mv = dbV[pkiV[i]];
  3916. }
  3917. return mi;
  3918. }
  3919. // start new trackers
  3920. void _cmFrqTrkNewChs( cmFrqTrk* p, const cmReal_t* dbV, const cmReal_t* hzV, unsigned* pkiV, unsigned pkN )
  3921. {
  3922. p->newTrkCnt = 0;
  3923. while(1)
  3924. {
  3925. unsigned db_max_idx;
  3926. cmFrqTrkCh_t* c;
  3927. // find an inactive channel
  3928. if((c = _cmFrqTrkFindAvailCh(p)) == NULL )
  3929. break;
  3930. // find the largest peak that is above pkAtkThreshDb && less than pkAtkHz.
  3931. if((db_max_idx = _cmFrqTrkMaxEnergyPeakIndex(p,dbV,hzV,pkiV,pkN)) == cmInvalidIdx )
  3932. break;
  3933. // activate a new channel
  3934. c->activeFl = true;
  3935. c->tN = 1;
  3936. c->dN = 0;
  3937. c->hz = hzV[ pkiV[ db_max_idx ] ];
  3938. c->db = dbV[ pkiV[ db_max_idx ] ];
  3939. c->id = p->nextTrkId++;
  3940. c->si = 0;
  3941. c->sn = 0;
  3942. c->score = 0;
  3943. c->state = kNoStateFrqTrkId;
  3944. c->attenPhsIdx = cmInvalidIdx;
  3945. c->attenGain = 1.0;
  3946. // mark the peak as unavailable
  3947. pkiV[ db_max_idx ] = cmInvalidIdx;
  3948. p->newTrkCnt += 1;
  3949. }
  3950. }
  3951. void _cmFrqTrkApplyFrqBias( cmFrqTrk* p, cmReal_t* xV )
  3952. {
  3953. // 1+2*([0:.01:1].^4)
  3954. unsigned i;
  3955. for(i=0; i<p->bN; ++i)
  3956. xV[i] = cmMax(0.0, (20*log10( cmMax(xV[i]/1.5,0.00001)) + 100.0)/100.0);
  3957. }
  3958. cmRC_t cmFrqTrkExec( cmFrqTrk* p, const cmReal_t* magV, const cmReal_t* phsV, const cmReal_t* hertzV )
  3959. {
  3960. cmRC_t rc = cmOkRC;
  3961. cmReal_t hzV[ p->bN ];
  3962. //cmReal_t powV[ p->bN ];
  3963. //cmReal_t yV[ p->bN];
  3964. //cmVOR_MultVVV(powV,p->bN,magV,magV);
  3965. //cmWhFiltExec(p->wf,powV,p->dbV,p->bN);
  3966. // convert magV to Decibels
  3967. //cmVOR_AmplToDbVV(p->dbV,p->bN, magV, -200.0);
  3968. // copy p->dbV to dbM[hi,:]
  3969. //cmVOR_CopyN(p->dbM + p->hi, p->bN, p->hN, p->dbV, 1 );
  3970. //cmVOR_CopyN(p->dbM + p->hi, p->bN, p->hN, whV, 1 );
  3971. if( 1 )
  3972. {
  3973. cmReal_t powV[ p->bN ];
  3974. cmVOR_MultVVV(powV,p->bN,magV,magV);
  3975. cmWhFiltExec(p->wf,powV,p->dbV,p->bN);
  3976. _cmFrqTrkApplyFrqBias(p,p->dbV);
  3977. }
  3978. else
  3979. {
  3980. // convert magV to Decibels
  3981. cmVOR_AmplToDbVV(p->dbV,p->bN, magV, -200.0);
  3982. }
  3983. // copy p->dbV to dbM[hi,:]
  3984. cmVOR_CopyN(p->dbM + p->hi, p->bN, p->hN, p->dbV, 1 );
  3985. // increment hi
  3986. p->hi = (p->hi + 1) % p->hN;
  3987. // Form the spectral magnitude profile by taking the mean over time
  3988. // of the last hN magnitude vectors
  3989. cmVOR_MeanM2(p->dbV, p->dbM, p->hN, p->bN, 0, cmMin(p->fN+1,p->hN));
  3990. //cmVOR_MeanM(p->dbV, p->dbM, p->hN, p->bN, 0);
  3991. if( p->fN >= p->hN )
  3992. {
  3993. // set the indexes of the peaks above pkThreshDb in i0[]
  3994. unsigned pkN = cmVOR_PeakIndexes(p->pkiV, p->bN, p->dbV, p->bN, p->a.pkThreshDb );
  3995. // generate the peak frequencies from the magnitude
  3996. _cmFrqTrkMagnToHz(p, p->dbV, p->pkiV, pkN, hzV );
  3997. // extend the existing trackers
  3998. _cmFrqTrkExtendChs(p, p->dbV, hzV, p->pkiV, pkN );
  3999. //_cmFrqTrkDisableClosePeaks(p, p->dbV, hzV, p->pkiV, pkN );
  4000. // create new trackers
  4001. _cmFrqTrkNewChs(p,p->dbV,hzV,p->pkiV,pkN);
  4002. //
  4003. _cmFrqTrkScoreChs(p);
  4004. //
  4005. _cmFrqTrkUpdateFilter(p);
  4006. /*
  4007. // write the log file
  4008. _cmFrqTrkWriteLog(p);
  4009. // write the spectrum output file
  4010. if( p->specFn != NULL )
  4011. cmVectArrayAppendR(p->specVa,p->dbV,p->bN);
  4012. // write the atten output file
  4013. if( p->attenFn != NULL )
  4014. cmVectArrayAppendR(p->attenVa,p->aV,p->bN);
  4015. // write the the level file
  4016. _cmFrqTrkWriteLevel(p,p->dbV,hzV,p->bN);
  4017. */
  4018. }
  4019. p->fN += 1;
  4020. return rc;
  4021. }
  4022. void cmFrqTrkPrint( cmFrqTrk* p )
  4023. {
  4024. printf("srate: %f\n",p->a.srate);
  4025. printf("chCnt: %i\n",p->a.chCnt);
  4026. printf("binCnt: %i (bN=%i)\n",p->a.binCnt,p->bN);
  4027. printf("hopSmpCnt: %i\n",p->a.hopSmpCnt);
  4028. printf("stRange: %f\n",p->a.stRange);
  4029. printf("wndSecs: %f (%i)\n",p->a.wndSecs,p->hN);
  4030. printf("minTrkSec: %f (%i)\n",p->a.minTrkSec,p->minTrkN);
  4031. printf("maxTrkDeadSec: %f (%i)\n",p->a.maxTrkDeadSec,p->deadN_max);
  4032. printf("pkThreshDb: %f\n",p->a.pkThreshDb);
  4033. printf("pkAtkThreshDb: %f\n",p->a.pkAtkThreshDb);
  4034. }
  4035. //------------------------------------------------------------------------------------------------------------
  4036. cmFbCtl_t* cmFbCtlAlloc( cmCtx* c, cmFbCtl_t* ap, const cmFbCtlArgs_t* a )
  4037. {
  4038. cmFbCtl_t* p = cmObjAlloc( cmFbCtl_t, c, ap );
  4039. p->sva = cmVectArrayAlloc(c,kRealVaFl);
  4040. p->uva = cmVectArrayAlloc(c,kRealVaFl);
  4041. if( a != NULL )
  4042. {
  4043. if( cmFbCtlInit( p, a ) != cmOkRC )
  4044. cmFbCtlFree(&p);
  4045. }
  4046. return p;
  4047. }
  4048. cmRC_t cmFbCtlFree( cmFbCtl_t** pp )
  4049. {
  4050. if( pp == NULL || *pp == NULL )
  4051. return cmOkRC;
  4052. cmFbCtl_t* p = *pp;
  4053. cmVectArrayWrite(p->sva, "/home/kevin/temp/frqtrk/fb_ctl_s.va");
  4054. cmVectArrayWrite(p->uva, "/home/kevin/temp/frqtrk/fb_ctl_u.va");
  4055. cmMemFree(p->bM);
  4056. cmMemFree(p->rmsV);
  4057. cmVectArrayFree(&p->sva);
  4058. cmVectArrayFree(&p->uva);
  4059. cmObjFree(pp);
  4060. return cmOkRC;
  4061. }
  4062. cmRC_t cmFbCtlInit( cmFbCtl_t* p, const cmFbCtlArgs_t* a )
  4063. {
  4064. cmRC_t rc;
  4065. if((rc = cmFbCtlFinal(p)) != cmOkRC )
  4066. return rc;
  4067. double binHz = a->srate / ((a->binCnt-1)*2);
  4068. p->a = *a;
  4069. p->frmCnt = (a->bufMs * a->srate / 1000.0) /a->hopSmpCnt;
  4070. p->binCnt = cmMin(p->a.binCnt, a->maxHz/binHz);
  4071. p->bM = cmMemResizeZ(cmReal_t, p->bM, p->binCnt*p->frmCnt);
  4072. p->rmsV = cmMemResizeZ(cmReal_t, p->rmsV, p->frmCnt);
  4073. p->sV = cmMemResizeZ(cmReal_t, p->sV, p->binCnt);
  4074. p->uV = cmMemResizeZ(cmReal_t, p->uV, p->binCnt);
  4075. printf("cmFbCtl: frmCnt:%i binCnt:%i \n",p->frmCnt,p->binCnt);
  4076. return rc;
  4077. }
  4078. cmRC_t cmFbCtlFinal(cmFbCtl_t* p )
  4079. { return cmOkRC; }
  4080. cmRC_t cmFbCtlExec( cmFbCtl_t* p, const cmReal_t* x0V )
  4081. {
  4082. unsigned i;
  4083. cmRC_t rc = cmOkRC;
  4084. cmReal_t xV[ p->binCnt ];
  4085. cmVOR_AmplToDbVV(xV, p->binCnt, x0V, -1000.0 );
  4086. cmVOR_Shift( p->rmsV, p->frmCnt, -1, 0 );
  4087. p->rmsV[0] = cmVOR_Mean(xV,p->binCnt);
  4088. cmVOR_CopyN(p->bM + p->bfi, p->binCnt, p->frmCnt, xV, 1 );
  4089. p->bfi = (p->bfi + 1) % p->frmCnt;
  4090. p->bfN = cmMin(p->bfN+1,p->frmCnt);
  4091. for(i=0; i<p->binCnt; ++i)
  4092. {
  4093. const cmReal_t* v = p->bM + i * p->frmCnt;
  4094. cmReal_t u = cmVOR_Mean(v, p->bfN );
  4095. cmReal_t s = sqrt(cmVOR_Variance(v, p->bfN,&u));
  4096. p->sV[i] = (0.0002 - s);
  4097. p->uV[i] = u;
  4098. }
  4099. cmVectArrayAppendR(p->sva,p->sV,p->binCnt);
  4100. cmVectArrayAppendR(p->uva,p->uV,p->binCnt);
  4101. return rc;
  4102. }
  4103. //------------------------------------------------------------------------------------------------------------
  4104. cmSpecDist_t* cmSpecDistAlloc( cmCtx* ctx,cmSpecDist_t* ap, unsigned procSmpCnt, double srate, unsigned wndSmpCnt, unsigned hopFcmt, unsigned olaWndTypeId )
  4105. {
  4106. cmSpecDist_t* p = cmObjAlloc( cmSpecDist_t, ctx, ap );
  4107. p->oSpecVa = cmVectArrayAlloc(ctx,kRealVaFl);
  4108. if( procSmpCnt != 0 )
  4109. {
  4110. if( cmSpecDistInit( p, procSmpCnt, srate, wndSmpCnt, hopFcmt, olaWndTypeId ) != cmOkRC )
  4111. cmSpecDistFree(&p);
  4112. }
  4113. return p;
  4114. }
  4115. cmRC_t cmSpecDistFree( cmSpecDist_t** pp )
  4116. {
  4117. if( pp == NULL || *pp == NULL )
  4118. return cmOkRC;
  4119. cmSpecDist_t* p = *pp;
  4120. cmSpecDistFinal(p);
  4121. cmVectArrayFree(&p->oSpecVa);
  4122. cmMemPtrFree(&p->hzV);
  4123. cmObjFree(pp);
  4124. return cmOkRC;
  4125. }
  4126. cmRC_t cmSpecDistInit( cmSpecDist_t* p, unsigned procSmpCnt, double srate, unsigned wndSmpCnt, unsigned hopFcmt, unsigned olaWndTypeId )
  4127. {
  4128. cmFrqTrkArgs_t fta;
  4129. cmRC_t rc;
  4130. if((rc = cmSpecDistFinal(p)) != cmOkRC )
  4131. return rc;
  4132. unsigned flags = 0;
  4133. p->wndSmpCnt = wndSmpCnt;
  4134. p->hopSmpCnt = (unsigned)floor(wndSmpCnt/hopFcmt);
  4135. p->procSmpCnt = procSmpCnt;
  4136. p->mode = kBasicModeSdId;
  4137. p->thresh = 60;
  4138. p->offset = 0;
  4139. p->invertFl = false;
  4140. p->uprSlope = 0.0;
  4141. p->lwrSlope = 2.0;
  4142. p->pva = cmPvAnlAlloc( p->obj.ctx, NULL, procSmpCnt, srate, wndSmpCnt, p->hopSmpCnt, flags );
  4143. p->pvs = cmPvSynAlloc( p->obj.ctx, NULL, procSmpCnt, srate, wndSmpCnt, p->hopSmpCnt, olaWndTypeId );
  4144. fta.srate = srate;
  4145. fta.chCnt = 50;
  4146. fta.binCnt = p->pva->binCnt;
  4147. fta.hopSmpCnt = p->pva->hopSmpCnt;
  4148. fta.stRange = 0.25;
  4149. fta.wndSecs = 0.25;
  4150. fta.minTrkSec = 0.25;
  4151. fta.maxTrkDeadSec = 0.25;
  4152. fta.pkThreshDb = 0.1; //-110.0;
  4153. fta.pkAtkThreshDb = 0.4; //-60.0;
  4154. fta.pkMaxHz = 20000;
  4155. fta.whFiltCoeff = 0.33;
  4156. fta.attenThresh = 900.0;
  4157. fta.attenGain = 1.0;
  4158. fta.attenAtkSec = 0.25;
  4159. fta.logFn = "/home/kevin/temp/frqtrk/trk_log.va";
  4160. fta.levelFn = "/home/kevin/temp/frqtrk/level.va";
  4161. fta.specFn = "/home/kevin/temp/frqtrk/spec.va";
  4162. fta.attenFn = "/home/kevin/temp/frqtrk/atten.va";
  4163. p->ft = cmFrqTrkAlloc( p->obj.ctx, NULL, &fta );
  4164. cmFrqTrkPrint(p->ft);
  4165. cmFbCtlArgs_t fba;
  4166. fba.srate = srate;
  4167. fba.binCnt = p->pva->binCnt;
  4168. fba.hopSmpCnt = p->hopSmpCnt;
  4169. fba.bufMs = 500;
  4170. fba.maxHz = 5000;
  4171. p->fbc = cmFbCtlAlloc( p->obj.ctx, NULL, &fba );
  4172. p->spcBwHz = cmMin(srate/2,10000);
  4173. p->spcSmArg = 0.05;
  4174. p->spcMin = p->spcBwHz;
  4175. p->spcMax = 0.0;
  4176. p->spcSum = 0.0;
  4177. p->spcCnt = 0;
  4178. double binHz = srate / p->pva->wndSmpCnt;
  4179. p->spcBinCnt = (unsigned)floor(p->spcBwHz / binHz);
  4180. p->hzV = cmMemResizeZ(cmReal_t,p->hzV,p->spcBinCnt);
  4181. cmVOR_Seq( p->hzV, p->spcBinCnt, 0, 1 );
  4182. cmVOR_MultVS( p->hzV, p->spcBinCnt, binHz );
  4183. p->aeUnit = 0;
  4184. p->aeMin = 1000;
  4185. p->aeMax = -1000;
  4186. //p->bypOut = cmMemResizeZ(cmSample_t, p->bypOut, procSmpCnt );
  4187. return rc;
  4188. }
  4189. cmRC_t cmSpecDistFinal(cmSpecDist_t* p )
  4190. {
  4191. cmRC_t rc = cmOkRC;
  4192. cmVectArrayWrite(p->oSpecVa, "/home/kevin/temp/frqtrk/oSpec.va");
  4193. cmPvAnlFree(&p->pva);
  4194. cmPvSynFree(&p->pvs);
  4195. cmFrqTrkFree(&p->ft);
  4196. cmFbCtlFree(&p->fbc);
  4197. return rc;
  4198. }
  4199. void _cmSpecDistBasicMode0(cmSpecDist_t* p, cmReal_t* X1m, unsigned binCnt, cmReal_t thresh )
  4200. {
  4201. // octavez> thresh = 60;
  4202. // octave> X1m = [-62 -61 -60 -59];
  4203. // octave> -abs(abs(X1m+thresh)-(X1m+thresh)) - thresh
  4204. // octave> ans = -64 -62 -60 -60
  4205. unsigned i=0;
  4206. for(i=0; i<binCnt; ++i)
  4207. {
  4208. cmReal_t a = fabs(X1m[i]);
  4209. cmReal_t d = a - thresh;
  4210. X1m[i] = -thresh;
  4211. if( d > 0 )
  4212. X1m[i] -= 2*d;
  4213. }
  4214. }
  4215. void _cmSpecDistBasicMode(cmSpecDist_t* p, cmReal_t* X1m, unsigned binCnt, cmReal_t thresh )
  4216. {
  4217. unsigned i=0;
  4218. if( p->lwrSlope < 0.3 )
  4219. p->lwrSlope = 0.3;
  4220. for(i=0; i<binCnt; ++i)
  4221. {
  4222. cmReal_t a = fabs(X1m[i]);
  4223. cmReal_t d = a - thresh;
  4224. X1m[i] = -thresh;
  4225. if( d > 0 )
  4226. X1m[i] -= (p->lwrSlope*d);
  4227. else
  4228. X1m[i] -= (p->uprSlope*d);
  4229. }
  4230. }
  4231. cmReal_t _cmSpecDistCentMode( cmSpecDist_t* p, cmReal_t* X1m )
  4232. {
  4233. // calc the spectral centroid
  4234. double num = cmVOR_MultSumVV( p->pva->magV, p->hzV, p->spcBinCnt );
  4235. double den = cmVOR_Sum( p->pva->magV, p->spcBinCnt );
  4236. double result = 0;
  4237. if( den != 0 )
  4238. result = num/den;
  4239. // apply smoothing filter to spectral centroid
  4240. p->spc = (result * p->spcSmArg) + (p->spc * (1.0-p->spcSmArg));
  4241. // track spec. cetr. min and max
  4242. p->spcMin = cmMin(p->spcMin,p->spc);
  4243. p->spcMax = cmMax(p->spcMax,p->spc);
  4244. //-----------------------------------------------------
  4245. ++p->spcCnt;
  4246. p->spcSum += p->spc;
  4247. p->spcSqSum += p->spc * p->spc;
  4248. // use the one-pass std-dev calc. trick
  4249. //double mean = p->spcSum / p->spcCnt;
  4250. //double variance = p->spcSqSum / p->spcCnt - mean * mean;
  4251. //double std_dev = sqrt(variance);
  4252. double smin = p->spcMin;
  4253. double smax = p->spcMax;
  4254. //smin = mean - std_dev;
  4255. //smax = mean + std_dev;
  4256. //-----------------------------------------------------
  4257. // convert spec. cent. to unit range
  4258. double spcUnit = (p->spc - smin) / (smax - smin);
  4259. spcUnit = cmMin(1.0,cmMax(0.0,spcUnit));
  4260. if( p->invertFl )
  4261. spcUnit = 1.0 - spcUnit;
  4262. //if( p->spcMin==p->spc || p->spcMax==p->spc )
  4263. // printf("min:%f avg:%f sd:%f max:%f\n",p->spcMin,p->spcSum/p->spcCnt,std_dev,p->spcMax);
  4264. return spcUnit;
  4265. }
  4266. void _cmSpecDistBump( cmSpecDist_t* p, cmReal_t* x, unsigned binCnt, double thresh)
  4267. {
  4268. /*
  4269. thresh *= -1;
  4270. minDb = -100;
  4271. if db < minDb
  4272. db = minDb;
  4273. endif
  4274. if db > thresh
  4275. y = 1;
  4276. else
  4277. x = (minDb - db)/(minDb - thresh);
  4278. y = x + (x - (x.^coeff));
  4279. endif
  4280. y = minDb + abs(minDb) * y;
  4281. */
  4282. unsigned i=0;
  4283. //printf("%f %f %f\n",thresh,p->lwrSlope,x[0]);
  4284. double minDb = -100.0;
  4285. thresh = -thresh;
  4286. for(i=0; i<binCnt; ++i)
  4287. {
  4288. double y;
  4289. if( x[i] < minDb )
  4290. x[i] = minDb;
  4291. if( x[i] > thresh )
  4292. y = 1;
  4293. else
  4294. {
  4295. y = (minDb - x[i])/(minDb - thresh);
  4296. y += y - pow(y,p->lwrSlope);
  4297. }
  4298. x[i] = minDb + (-minDb) * y;
  4299. }
  4300. }
  4301. void _cmSpecDistAmpEnvMode( cmSpecDist_t* p, cmReal_t* X1m )
  4302. {
  4303. cmReal_t smCoeff = 0.1;
  4304. //
  4305. cmReal_t mx = cmVOR_Max(X1m,p->pva->binCnt,1);
  4306. p->aeSmMax = (mx * smCoeff) + (p->aeSmMax * (1.0-smCoeff));
  4307. cmReal_t a = cmVOR_Mean(X1m,p->pva->binCnt);
  4308. p->ae = (a * smCoeff) + (p->ae * (1.0-smCoeff));
  4309. p->aeMin = cmMin(p->ae,p->aeMin);
  4310. p->aeMax = cmMax(p->ae,p->aeMax);
  4311. p->aeUnit = (p->ae - p->aeMin) / (p->aeMax-p->aeMin);
  4312. p->aeUnit = cmMin(1.0,cmMax(0.0,p->aeUnit));
  4313. if( p->invertFl )
  4314. p->aeUnit = 1.0 - p->aeUnit;
  4315. //printf("%f\n",p->aeSmMax);
  4316. }
  4317. cmRC_t cmSpecDistExec( cmSpecDist_t* p, const cmSample_t* sp, unsigned sn )
  4318. {
  4319. assert( sn == p->procSmpCnt );
  4320. // cmPvAnlExec() returns true when it calc's a new spectral output frame
  4321. if( cmPvAnlExec( p->pva, sp, sn ) )
  4322. {
  4323. cmReal_t X1m[p->pva->binCnt];
  4324. cmReal_t u0 = cmVOR_Mean(p->pva->magV,p->pva->binCnt);
  4325. //cmFrqTrkExec(p->ft, p->pva->magV, p->pva->phsV, NULL );
  4326. // apply the freq track suppression filter
  4327. //cmVOR_MultVVV(X1m, p->pva->binCnt,p->pva->magV, p->ft->aV );
  4328. cmVOR_AmplToDbVV(X1m, p->pva->binCnt, p->pva->magV, -1000.0 );
  4329. //cmVOR_AmplToDbVV(X1m, p->pva->binCnt, X1m, -1000.0 );
  4330. switch( p->mode )
  4331. {
  4332. case kBypassModeSdId:
  4333. break;
  4334. case kBasicModeSdId:
  4335. _cmSpecDistBasicMode(p,X1m,p->pva->binCnt,p->thresh);
  4336. break;
  4337. case kSpecCentSdId:
  4338. {
  4339. _cmSpecDistAmpEnvMode(p,X1m);
  4340. double spcUnit = _cmSpecDistCentMode(p,X1m);
  4341. double thresh = fabs(p->aeSmMax) - (spcUnit*p->offset);
  4342. _cmSpecDistBasicMode(p,X1m,p->pva->binCnt, thresh);
  4343. }
  4344. break;
  4345. case kAmpEnvSdId:
  4346. {
  4347. _cmSpecDistAmpEnvMode(p,X1m);
  4348. //double thresh = fabs(p->aeSmMax) - p->offset;
  4349. double thresh = fabs(p->aeSmMax) - (p->aeUnit*p->offset);
  4350. thresh = fabs(p->thresh) - (p->aeUnit*p->offset);
  4351. _cmSpecDistBasicMode(p,X1m,p->pva->binCnt, thresh);
  4352. }
  4353. break;
  4354. case kBumpSdId:
  4355. _cmSpecDistBump(p,X1m, p->pva->binCnt, p->offset);
  4356. _cmSpecDistBasicMode(p,X1m,p->pva->binCnt,p->thresh);
  4357. break;
  4358. case 5:
  4359. break;
  4360. default:
  4361. break;
  4362. }
  4363. //cmVectArrayAppendR(p->oSpecVa,X1m,p->pva->binCnt);
  4364. cmVOR_DbToAmplVV(X1m, p->pva->binCnt, X1m );
  4365. // run and apply the tracker/supressor
  4366. cmFrqTrkExec(p->ft, X1m, p->pva->phsV, NULL );
  4367. cmVOR_MultVV(X1m, p->pva->binCnt,p->ft->aV );
  4368. cmReal_t idb = 20*log10(u0);
  4369. cmReal_t u1 = cmVOR_Mean(X1m,p->pva->binCnt);
  4370. if( idb > -150.0 )
  4371. {
  4372. p->ogain = u0/u1;
  4373. }
  4374. else
  4375. {
  4376. cmReal_t a0 = 0.9;
  4377. p->ogain *= a0;
  4378. }
  4379. //cmReal_t v[] = { u0, u1, idb, 20*log10(u1), p->ogain };
  4380. //unsigned vn = sizeof(v)/sizeof(v[0]);
  4381. //cmVectArrayAppendR(p->oSpecVa,v,vn);
  4382. cmVOR_MultVS(X1m,p->pva->binCnt,cmMin(4.0,p->ogain));
  4383. //cmFbCtlExec(p->fbc,X1m);
  4384. cmPvSynExec(p->pvs, X1m, p->pva->phsV );
  4385. }
  4386. return cmOkRC;
  4387. }
  4388. const cmSample_t* cmSpecDistOut( cmSpecDist_t* p )
  4389. {
  4390. return cmPvSynExecOut(p->pvs);
  4391. }
  4392. //------------------------------------------------------------------------------------------------------------
  4393. cmRC_t _cmBinMtxFileWriteHdr( cmBinMtxFile_t* p )
  4394. {
  4395. cmFileRC_t fileRC;
  4396. unsigned n = 3;
  4397. unsigned hdr[n];
  4398. hdr[0] = p->rowCnt;
  4399. hdr[1] = p->maxRowEleCnt;
  4400. hdr[2] = p->eleByteCnt;
  4401. if((fileRC = cmFileSeek(p->fh,kBeginFileFl,0)) != kOkFileRC )
  4402. return cmCtxRtCondition(&p->obj, fileRC, "File seek failed on matrix file:'%s'.", cmStringNullGuard(cmFileName(p->fh)));
  4403. if((fileRC = cmFileWriteUInt(p->fh,hdr,n)) != kOkFileRC )
  4404. return cmCtxRtCondition( &p->obj, fileRC, "Header write failed on matrix file:'%s'.", cmStringNullGuard(cmFileName(p->fh)) );
  4405. return cmOkRC;
  4406. }
  4407. cmBinMtxFile_t* cmBinMtxFileAlloc( cmCtx* ctx, cmBinMtxFile_t* ap, const cmChar_t* fn )
  4408. {
  4409. cmBinMtxFile_t* p = cmObjAlloc( cmBinMtxFile_t, ctx, ap );
  4410. if( fn != NULL )
  4411. if( cmBinMtxFileInit( p, fn ) != cmOkRC )
  4412. cmBinMtxFileFree(&p);
  4413. return p;
  4414. }
  4415. cmRC_t cmBinMtxFileFree( cmBinMtxFile_t** pp )
  4416. {
  4417. cmRC_t rc;
  4418. if( pp==NULL || *pp == NULL )
  4419. return cmOkRC;
  4420. cmBinMtxFile_t* p = *pp;
  4421. if((rc = cmBinMtxFileFinal(p)) == cmOkRC )
  4422. {
  4423. cmObjFree(pp);
  4424. }
  4425. return rc;
  4426. }
  4427. cmRC_t cmBinMtxFileInit( cmBinMtxFile_t* p, const cmChar_t* fn )
  4428. {
  4429. cmRC_t rc;
  4430. cmFileRC_t fileRC;
  4431. if((rc = cmBinMtxFileFinal(p)) != cmOkRC )
  4432. return rc;
  4433. // open the output file for writing
  4434. if((fileRC = cmFileOpen(&p->fh,fn,kWriteFileFl | kBinaryFileFl, p->obj.err.rpt)) != kOkFileRC )
  4435. return cmCtxRtCondition( &p->obj, fileRC, "Unable to open the matrix file:'%s'", cmStringNullGuard(fn) );
  4436. // iniitlaize the object
  4437. p->rowCnt = 0;
  4438. p->maxRowEleCnt = 0;
  4439. p->eleByteCnt = 0;
  4440. // write the blank header as place holder
  4441. if((rc = _cmBinMtxFileWriteHdr(p)) != cmOkRC )
  4442. return rc;
  4443. return rc;
  4444. }
  4445. cmRC_t cmBinMtxFileFinal( cmBinMtxFile_t* p )
  4446. {
  4447. cmRC_t rc;
  4448. cmFileRC_t fileRC;
  4449. if( p != NULL && cmFileIsValid(p->fh))
  4450. {
  4451. // re-write the file header
  4452. if((rc = _cmBinMtxFileWriteHdr(p)) != cmOkRC )
  4453. return rc;
  4454. // close the file
  4455. if((fileRC = cmFileClose(&p->fh)) != kOkFileRC )
  4456. return cmCtxRtCondition(&p->obj, fileRC, "Matrix file close failed on:'%s'",cmStringNullGuard(cmFileName(p->fh)));
  4457. }
  4458. return cmOkRC;
  4459. }
  4460. bool cmBinMtxFileIsValid( cmBinMtxFile_t* p )
  4461. { return p != NULL && cmFileIsValid(p->fh); }
  4462. cmRC_t _cmBinMtxFileWriteRow( cmBinMtxFile_t* p, const void* buf, unsigned eleCnt, unsigned eleByteCnt )
  4463. {
  4464. cmFileRC_t fileRC;
  4465. if((fileRC = cmFileWrite(p->fh,&eleCnt,sizeof(eleCnt))) != kOkFileRC )
  4466. return cmCtxRtCondition(&p->obj, fileRC, "Matrix file row at index %i element count write failed on '%s'.", p->rowCnt, cmStringNullGuard(cmFileName(p->fh)));
  4467. if((fileRC = cmFileWrite(p->fh,buf,eleCnt*eleByteCnt)) != kOkFileRC )
  4468. return cmCtxRtCondition(&p->obj, fileRC, "Matrix file row at index %i data write failed on '%s'.", p->rowCnt, cmStringNullGuard(cmFileName(p->fh)));
  4469. if( eleCnt > p->maxRowEleCnt )
  4470. p->maxRowEleCnt = eleCnt;
  4471. ++p->rowCnt;
  4472. return cmOkRC;
  4473. }
  4474. cmRC_t cmBinMtxFileExecS( cmBinMtxFile_t* p, const cmSample_t* x, unsigned xn )
  4475. {
  4476. // verify that all rows are written as cmSample_t
  4477. assert( p->eleByteCnt == 0 || p->eleByteCnt == sizeof(cmSample_t));
  4478. p->eleByteCnt = sizeof(cmSample_t);
  4479. return _cmBinMtxFileWriteRow(p,x,xn,p->eleByteCnt);
  4480. }
  4481. cmRC_t cmBinMtxFileExecR( cmBinMtxFile_t* p, const cmReal_t* x, unsigned xn )
  4482. {
  4483. // verify that all rows are written as cmReal_t
  4484. assert( p->eleByteCnt == 0 || p->eleByteCnt == sizeof(cmReal_t));
  4485. p->eleByteCnt = sizeof(cmReal_t);
  4486. return _cmBinMtxFileWriteRow(p,x,xn,p->eleByteCnt);
  4487. }
  4488. cmRC_t cmBinMtxFileWrite( const cmChar_t* fn, unsigned rowCnt, unsigned colCnt, const cmSample_t* sp, const cmReal_t* rp, cmCtx* ctx, cmRpt_t* rpt )
  4489. {
  4490. assert( sp == NULL || rp == NULL );
  4491. cmCtx* ctxp = NULL;
  4492. cmBinMtxFile_t* bp = NULL;
  4493. if( ctx == NULL )
  4494. ctx = ctxp = cmCtxAlloc(NULL,rpt,cmLHeapNullHandle,cmSymTblNullHandle);
  4495. if((bp = cmBinMtxFileAlloc(ctx,NULL,fn)) != NULL )
  4496. {
  4497. unsigned i = 0;
  4498. cmSample_t* sbp = sp == NULL ? NULL : cmMemAlloc(cmSample_t,colCnt);
  4499. cmReal_t* rbp = rp == NULL ? NULL : cmMemAlloc(cmReal_t,colCnt);
  4500. for(i=0; i<rowCnt; ++i)
  4501. {
  4502. if( sp!=NULL )
  4503. {
  4504. cmVOS_CopyN(sbp,colCnt,1,sp+i,rowCnt);
  4505. cmBinMtxFileExecS(bp,sbp,colCnt);
  4506. }
  4507. if( rp!=NULL )
  4508. {
  4509. cmVOR_CopyN(rbp,colCnt,1,rp+i,rowCnt);
  4510. cmBinMtxFileExecR(bp,rbp,colCnt);
  4511. }
  4512. }
  4513. cmMemPtrFree(&sbp);
  4514. cmMemPtrFree(&rbp);
  4515. cmBinMtxFileFree(&bp);
  4516. }
  4517. if( ctxp != NULL )
  4518. cmCtxFree(&ctxp);
  4519. return cmOkRC;
  4520. }
  4521. cmRC_t _cmBinMtxFileReadHdr( cmCtx_t* ctx, cmFileH_t h, unsigned* rowCntPtr, unsigned* colCntPtr, unsigned* eleByteCntPtr, const cmChar_t* fn )
  4522. {
  4523. cmRC_t rc = cmOkRC;
  4524. unsigned hdr[3];
  4525. if( cmFileRead(h,&hdr,sizeof(hdr)) != kOkFileRC )
  4526. {
  4527. rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"Binary matrix file header read failed on '%s'.",cmStringNullGuard(fn));
  4528. goto errLabel;
  4529. }
  4530. if( rowCntPtr != NULL )
  4531. *rowCntPtr = hdr[0];
  4532. if( colCntPtr != NULL )
  4533. *colCntPtr = hdr[1];
  4534. if( eleByteCntPtr != NULL )
  4535. *eleByteCntPtr = hdr[2];
  4536. errLabel:
  4537. return rc;
  4538. }
  4539. cmRC_t cmBinMtxFileSize( cmCtx_t* ctx, const cmChar_t* fn, unsigned* rowCntPtr, unsigned* colCntPtr, unsigned* eleByteCntPtr )
  4540. {
  4541. cmFileH_t h = cmFileNullHandle;
  4542. cmRC_t rc = cmOkRC;
  4543. if(cmFileOpen(&h,fn,kReadFileFl | kBinaryFileFl, ctx->err.rpt) != kOkFileRC )
  4544. {
  4545. rc = cmErrMsg(&ctx->err,cmSubSysFailRC,"Binary matrix file:%s open failed.",cmStringNullGuard(fn));
  4546. goto errLabel;
  4547. }
  4548. rc = _cmBinMtxFileReadHdr(ctx,h,rowCntPtr,colCntPtr,eleByteCntPtr,fn);
  4549. errLabel:
  4550. cmFileClose(&h);
  4551. return rc;
  4552. }
  4553. cmRC_t cmBinMtxFileRead( cmCtx_t* ctx, const cmChar_t* fn, unsigned mRowCnt, unsigned mColCnt, unsigned mEleByteCnt, void* retBuf, unsigned* colCntV )
  4554. {
  4555. cmFileH_t h = cmFileNullHandle;
  4556. cmRC_t rc = cmOkRC;
  4557. char* rowBuf = NULL;
  4558. unsigned rowCnt,colCnt,eleByteCnt,i;
  4559. cmErr_t err;
  4560. cmErrSetup(&err,ctx->err.rpt,"Binary Matrix File Reader");
  4561. if(cmFileOpen(&h,fn,kReadFileFl | kBinaryFileFl, err.rpt) != kOkFileRC )
  4562. {
  4563. rc = cmErrMsg(&err,cmSubSysFailRC,"Binary matrix file:%s open failed.",cmStringNullGuard(fn));
  4564. goto errLabel;
  4565. }
  4566. if((rc = _cmBinMtxFileReadHdr(ctx,h,&rowCnt,&colCnt,&eleByteCnt,fn)) != cmOkRC )
  4567. goto errLabel;
  4568. if( mRowCnt != rowCnt )
  4569. rc = cmErrMsg(&err,cmArgAssertRC,"The binary matrix file row count and the return buffer row count are not the same.");
  4570. if( mColCnt != colCnt )
  4571. rc = cmErrMsg(&err,cmArgAssertRC,"The binary matrix file column count and the return buffer column count are not the same.");
  4572. if( mEleByteCnt != eleByteCnt )
  4573. rc = cmErrMsg(&err,cmArgAssertRC,"The binary matrix file element byte count and the return buffer element byte count are not the same.");
  4574. if( rc == cmOkRC )
  4575. {
  4576. rowBuf = cmMemAllocZ(char,colCnt*eleByteCnt);
  4577. for(i=0; i<rowCnt; ++i)
  4578. {
  4579. unsigned cn;
  4580. // read the row length
  4581. if( cmFileReadUInt(h,&cn,1) != kOkFileRC )
  4582. {
  4583. rc = cmErrMsg(&err,cmSubSysFailRC,"Error reading row length at row index:%i.",i);
  4584. goto errLabel;
  4585. }
  4586. if( colCntV != NULL )
  4587. colCntV[i] = cn;
  4588. // verify the actual col count does not exceed the max col count
  4589. if( cn > colCnt )
  4590. {
  4591. rc = cmErrMsg(&err,cmSubSysFailRC,"The actual column count:%i exceeds the max column count:%i.",cn,colCnt);
  4592. goto errLabel;
  4593. }
  4594. //read the row data
  4595. if( cmFileReadChar(h,rowBuf,cn*eleByteCnt) != kOkFileRC )
  4596. {
  4597. rc = cmErrMsg(&err,cmSubSysFailRC,"File read failed at row index:%i.",i);
  4598. goto errLabel;
  4599. }
  4600. char* dp = ((char*)retBuf) + i * eleByteCnt;
  4601. // the data is read in row-major order but the matrix must be
  4602. // returned on col major order - rearrange the columns here.
  4603. switch(eleByteCnt)
  4604. {
  4605. case sizeof(cmSample_t):
  4606. cmVOS_CopyN(((cmSample_t*)dp), cn, rowCnt, (cmSample_t*)rowBuf, 1 );
  4607. break;
  4608. case sizeof(cmReal_t):
  4609. cmVOR_CopyN(((cmReal_t*)dp), cn, rowCnt, (cmReal_t*)rowBuf, 1 );
  4610. break;
  4611. default:
  4612. rc = cmErrMsg(&err,cmSubSysFailRC,"Invalid element byte count:%i.",eleByteCnt);
  4613. goto errLabel;
  4614. }
  4615. }
  4616. }
  4617. errLabel:
  4618. cmMemPtrFree(&rowBuf);
  4619. cmFileClose(&h);
  4620. return rc;
  4621. }