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.

cmMath.c 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610
  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 "cmFloatTypes.h"
  9. #include "cmMath.h"
  10. #include <sys/types.h> // u_char
  11. // TODO: rewrite to avoid copying
  12. // this code comes via csound source ...
  13. double cmX80ToDouble( unsigned char rate[10] )
  14. {
  15. char sign;
  16. short exp = 0;
  17. unsigned long mant1 = 0;
  18. unsigned long mant0 = 0;
  19. double val;
  20. unsigned char* p = (unsigned char*)rate;
  21. exp = *p++;
  22. exp <<= 8;
  23. exp |= *p++;
  24. sign = (exp & 0x8000) ? 1 : 0;
  25. exp &= 0x7FFF;
  26. mant1 = *p++;
  27. mant1 <<= 8;
  28. mant1 |= *p++;
  29. mant1 <<= 8;
  30. mant1 |= *p++;
  31. mant1 <<= 8;
  32. mant1 |= *p++;
  33. mant0 = *p++;
  34. mant0 <<= 8;
  35. mant0 |= *p++;
  36. mant0 <<= 8;
  37. mant0 |= *p++;
  38. mant0 <<= 8;
  39. mant0 |= *p++;
  40. /* special test for all bits zero meaning zero
  41. - else pow(2,-16383) bombs */
  42. if (mant1 == 0 && mant0 == 0 && exp == 0 && sign == 0)
  43. return 0.0;
  44. else {
  45. val = ((double)mant0) * pow(2.0,-63.0);
  46. val += ((double)mant1) * pow(2.0,-31.0);
  47. val *= pow(2.0,((double) exp) - 16383.0);
  48. return sign ? -val : val;
  49. }
  50. }
  51. // TODO: rewrite to avoid copying
  52. /*
  53. * Convert double to IEEE 80 bit floating point
  54. * Should be portable to all C compilers.
  55. * 19aug91 aldel/dpwe covered for MSB bug in Ultrix 'cc'
  56. */
  57. void cmDoubleToX80(double val, unsigned char rate[10])
  58. {
  59. char sign = 0;
  60. short exp = 0;
  61. unsigned long mant1 = 0;
  62. unsigned long mant0 = 0;
  63. unsigned char* p = (unsigned char*)rate;
  64. if (val < 0.0) { sign = 1; val = -val; }
  65. if (val != 0.0) /* val identically zero -> all elements zero */
  66. {
  67. exp = (short)(log(val)/log(2.0) + 16383.0);
  68. val *= pow(2.0, 31.0+16383.0-(double)exp);
  69. mant1 =((unsigned)val);
  70. val -= ((double)mant1);
  71. val *= pow(2.0, 32.0);
  72. mant0 =((double)val);
  73. }
  74. *p++ = ((sign<<7)|(exp>>8));
  75. *p++ = (u_char)(0xFF & exp);
  76. *p++ = (u_char)(0xFF & (mant1>>24));
  77. *p++ = (u_char)(0xFF & (mant1>>16));
  78. *p++ = (u_char)(0xFF & (mant1>> 8));
  79. *p++ = (u_char)(0xFF & (mant1));
  80. *p++ = (u_char)(0xFF & (mant0>>24));
  81. *p++ = (u_char)(0xFF & (mant0>>16));
  82. *p++ = (u_char)(0xFF & (mant0>> 8));
  83. *p++ = (u_char)(0xFF & (mant0));
  84. }
  85. bool cmIsPowerOfTwo( unsigned x )
  86. {
  87. return !( (x < 2) || (x & (x-1)) );
  88. }
  89. unsigned cmNextPowerOfTwo( unsigned val )
  90. {
  91. unsigned i;
  92. unsigned mask = 1;
  93. unsigned msb = 0;
  94. unsigned cnt = 0;
  95. // if val is a power of two return it
  96. if( cmIsPowerOfTwo(val) )
  97. return val;
  98. // next pow of zero is 2
  99. if( val == 0 )
  100. return 2;
  101. // if the next power of two can't be represented in 32 bits
  102. if( val > 0x80000000)
  103. {
  104. assert(0);
  105. return 0;
  106. }
  107. // find most sig. bit that is set - the number with only the next msb set is next pow 2
  108. for(i=0; i<31; i++,mask<<=1)
  109. if( mask & val )
  110. {
  111. msb = i;
  112. cnt++;
  113. }
  114. return 1 << (msb + 1);
  115. }
  116. unsigned cmNearPowerOfTwo( unsigned i )
  117. {
  118. unsigned vh = cmNextPowerOfTwo(i);
  119. if( vh == 2 )
  120. return vh;
  121. unsigned vl = vh / 2;
  122. if( vh - i < i - vl )
  123. return vh;
  124. return vl;
  125. }
  126. bool cmIsOddU( unsigned v ) { return v % 2 == 1; }
  127. bool cmIsEvenU( unsigned v ) { return !cmIsOddU(v); }
  128. unsigned cmNextOddU( unsigned v ) { return cmIsOddU(v) ? v : v+1; }
  129. unsigned cmPrevOddU( unsigned v ) { return cmIsOddU(v) ? v : v-1; }
  130. unsigned cmNextEvenU( unsigned v ) { return cmIsEvenU(v) ? v : v+1; }
  131. unsigned cmPrevEvenU( unsigned v ) { return cmIsEvenU(v) ? v : v-1; }
  132. unsigned cmModIncr(int idx, int delta, int maxN )
  133. {
  134. int sum = idx + delta;
  135. if( sum >= maxN )
  136. return sum - maxN;
  137. if( sum < 0 )
  138. return maxN + sum;
  139. return sum;
  140. }
  141. // modified bessel function of first kind, order 0
  142. // ref: orfandis appendix B io.m
  143. double cmBessel0( double x )
  144. {
  145. double eps = pow(10.0,-9.0);
  146. double n = 1.0;
  147. double S = 1.0;
  148. double D = 1.0;
  149. while(D > eps*S)
  150. {
  151. double T = x /(2.0*n);
  152. n = n+1;
  153. D = D * pow(T,2.0);
  154. S = S + D;
  155. }
  156. return S;
  157. }
  158. //=================================================================
  159. // The following elliptic-related function approximations come from
  160. // Parks & Burrus, Digital Filter Design, Appendix program 9, pp. 317-326
  161. // which in turn draws directly on other sources
  162. // calculate complete elliptic integral (quarter period) K
  163. // given *complimentary* modulus kc
  164. cmReal_t cmEllipK( cmReal_t kc )
  165. {
  166. cmReal_t a = 1, b = kc, c = 1, tmp;
  167. while( c > cmReal_EPSILON )
  168. {
  169. c = 0.5*(a-b);
  170. tmp = 0.5*(a+b);
  171. b = sqrt(a*b);
  172. a = tmp;
  173. }
  174. return M_PI/(2*a);
  175. }
  176. // calculate elliptic modulus k
  177. // given ratio of complete elliptic integrals r = K/K'
  178. // (solves the "degree equation" for fixed N = K*K1'/K'K1)
  179. cmReal_t cmEllipDeg( cmReal_t r )
  180. {
  181. cmReal_t q,a,b,c,d;
  182. a = b = c = 1;
  183. d = q = exp(-M_PI*r);
  184. while( c > cmReal_EPSILON )
  185. {
  186. a = a + 2*c*d;
  187. c = c*d*d;
  188. b = b + c;
  189. d = d*q;
  190. }
  191. return 4*sqrt(q)*pow(b/a,2);
  192. }
  193. // calculate arc elliptic tangent u (elliptic integral of the 1st kind)
  194. // given argument x = sc(u,k) and *complimentary* modulus kc
  195. cmReal_t cmEllipArcSc( cmReal_t x, cmReal_t kc )
  196. {
  197. cmReal_t a = 1, b = kc, y = 1/x, tmp;
  198. unsigned L = 0;
  199. while( true )
  200. {
  201. tmp = a*b;
  202. a += b;
  203. b = 2*sqrt(tmp);
  204. y -= tmp/y;
  205. if( y == 0 )
  206. y = sqrt(tmp) * 1E-10;
  207. if( fabs(a-b)/a < cmReal_EPSILON )
  208. break;
  209. L *= 2;
  210. if( y < 0 )
  211. L++;
  212. }
  213. if( y < 0 )
  214. L++;
  215. return (atan(a/y) + M_PI*L)/a;
  216. }
  217. // calculate Jacobi elliptic functions sn, cn, and dn
  218. // given argument u and *complimentary* modulus kc
  219. cmRC_t cmEllipJ( cmReal_t u, cmReal_t kc, cmReal_t* sn, cmReal_t* cn, cmReal_t* dn )
  220. {
  221. assert( sn != NULL || cn != NULL || dn != NULL );
  222. if( u == 0 )
  223. {
  224. if( sn != NULL ) *sn = 0;
  225. if( cn != NULL ) *cn = 1;
  226. if( dn != NULL ) *dn = 1;
  227. return cmOkRC;
  228. }
  229. int i;
  230. cmReal_t a,b,c,d,e,tmp,_sn,_cn,_dn;
  231. cmReal_t aa[16], bb[16];
  232. a = 1;
  233. b = kc;
  234. for( i = 0; i < 16; i++ )
  235. {
  236. aa[i] = a;
  237. bb[i] = b;
  238. tmp = (a+b)/2;
  239. b = sqrt(a*b);
  240. a = tmp;
  241. if( (a-b)/a < cmReal_EPSILON )
  242. break;
  243. }
  244. c = a/tan(u*a);
  245. d = 1;
  246. for( ; i >= 0; i-- )
  247. {
  248. e = c*c/a;
  249. c = c*d;
  250. a = aa[i];
  251. d = (e + bb[i]) / (e+a);
  252. }
  253. _sn = 1/sqrt(1+c*c);
  254. _cn = _sn*c;
  255. _dn = d;
  256. if( sn != NULL ) *sn = _sn;
  257. if( cn != NULL ) *cn = _cn;
  258. if( dn != NULL ) *dn = _dn;
  259. return cmOkRC;
  260. }
  261. //=================================================================
  262. // bilinear transform
  263. // z = (2*sr + s)/(2*sr - s)
  264. cmRC_t cmBlt( unsigned n, cmReal_t sr, cmReal_t* rp, cmReal_t* ip )
  265. {
  266. unsigned i;
  267. cmReal_t a = 2*sr,
  268. tr, ti, td;
  269. for( i = 0; i < n; i++ )
  270. {
  271. tr = rp[i];
  272. ti = ip[i];
  273. td = pow(a-tr, 2) + ti*ti;
  274. rp[i] = (a*a - tr*tr - ti*ti)/td;
  275. ip[i] = 2*a*ti/td;
  276. if( tr < -1E15 )
  277. rp[i] = 0;
  278. if( fabs(ti) > 1E15 )
  279. ip[i] = 0;
  280. }
  281. return cmOkRC;
  282. }
  283. unsigned cmHzToMidi( double hz )
  284. {
  285. float midi = 12.0 * log2(hz/13.75) + 9;
  286. if( midi < 0 )
  287. midi = 0;
  288. if( midi > 127 )
  289. midi = 127;
  290. return (unsigned)lround(midi);
  291. }
  292. float cmMidiToHz( unsigned midi )
  293. {
  294. double m = midi <= 127 ? midi : 127;
  295. return (float)( 13.75 * pow(2.0,(m - 9.0)/12.0));
  296. }
  297. //=================================================================
  298. // Floating point byte swapping
  299. // Unions used to type-pun the swapping functions and thereby
  300. // avoid strict aliasing problems with -O2. Using unions for
  301. // this purpose is apparently legal under C99 but not C++.
  302. typedef union
  303. {
  304. unsigned u;
  305. float f;
  306. } _cmMathU_t;
  307. typedef union
  308. {
  309. unsigned long long u;
  310. double f;
  311. } _cmMathUL_t;
  312. unsigned cmFfSwapFloatToUInt( float v )
  313. {
  314. assert( sizeof(float) == sizeof(unsigned));
  315. _cmMathU_t u;
  316. u.f=v;
  317. return cmSwap32(u.u);
  318. }
  319. float cmFfSwapUIntToFloat( unsigned v )
  320. {
  321. assert( sizeof(float) == sizeof(unsigned));
  322. _cmMathU_t u;
  323. u.u = cmSwap32(v);
  324. return u.f;
  325. }
  326. unsigned long long cmFfSwapDoubleToULLong( double v )
  327. {
  328. assert( sizeof(double) == sizeof(unsigned long long));
  329. _cmMathUL_t u;
  330. u.f = v;
  331. return cmSwap64(u.u);
  332. }
  333. double cmFfSwapULLongToDouble( unsigned long long v )
  334. {
  335. assert( sizeof(double) == sizeof(unsigned long long));
  336. _cmMathUL_t u;
  337. u.u = cmSwap64(v);
  338. return u.f;
  339. }
  340. int cmRandInt( int min, int max )
  341. {
  342. assert( min <= max );
  343. int offs = max - min;
  344. return min + cmMax(0,cmMin(offs,(int)round(offs * (double)rand() / RAND_MAX)));
  345. }
  346. unsigned cmRandUInt( unsigned min, unsigned max )
  347. {
  348. assert( min <= max );
  349. unsigned offs = max - min;
  350. return min + cmMax(0,cmMin(offs,(unsigned)round(offs * (double)rand() / RAND_MAX)));
  351. }
  352. float cmRandFloat( float min, float max )
  353. {
  354. assert( min <= max );
  355. float offs = max - min;
  356. return min + cmMax(0,cmMin(offs,(float)(offs * (double)rand() / RAND_MAX)));
  357. }
  358. double cmRandDouble( double min, double max )
  359. {
  360. assert( min <= max );
  361. double offs = max - min;
  362. return min + cmMax(0,cmMin(offs,(offs * (double)rand() / RAND_MAX)));
  363. }
  364. //=================================================================
  365. // Base on: http://stackoverflow.com/questions/3874627/floating-point-comparison-functions-for-c-sharp
  366. bool cmIsCloseD( double x0, double x1, double eps )
  367. {
  368. double d = fabs(x0-x1);
  369. if( x0 == x1 )
  370. return true;
  371. if( x0==0 || x1==0 || d<DBL_MIN )
  372. return d < (eps * DBL_MIN);
  373. return (d / cmMin( fabs(x0) + fabs(x1), DBL_MAX)) < eps;
  374. }
  375. bool cmIsCloseF( float x0, float x1, double eps_d )
  376. {
  377. float eps = (float)eps_d;
  378. float d = fabsf(x0-x1);
  379. if( x0 == x1 )
  380. return true;
  381. if( x0==0 || x1==0 || d<FLT_MIN )
  382. return d < (eps * FLT_MIN);
  383. return (d / cmMin( fabsf(x0) + fabsf(x1), FLT_MAX)) < eps;
  384. }
  385. bool cmIsCloseI( int x0, int x1, double eps )
  386. {
  387. if( x0 == x1 )
  388. return true;
  389. return abs(x0-x1)/(abs(x0)+abs(x1)) < eps;
  390. }
  391. bool cmIsCloseU( unsigned x0, unsigned x1, double eps )
  392. {
  393. if( x0 == x1 )
  394. return true;
  395. if( x0 > x1 )
  396. return (x0-x1)/(x0+x1) < eps;
  397. else
  398. return (x1-x0)/(x0+x1) < eps;
  399. }
  400. //=================================================================
  401. // cmLFSR() implementation based on note at bottom of:
  402. // http://www.ece.cmu.edu/~koopman/lfsr/index.html
  403. void cmLFSR( unsigned lfsrN, unsigned tapMask, unsigned seed, unsigned* yV, unsigned yN )
  404. {
  405. assert( 0 < lfsrN && lfsrN < 32 );
  406. unsigned i;
  407. for(i=0; i<yN; ++i)
  408. {
  409. if( (yV[i] = seed & 1)==1 )
  410. seed = (seed >> 1) ^ tapMask;
  411. else
  412. seed = (seed >> 1);
  413. }
  414. }
  415. bool cmMLS_IsBalanced( const unsigned* xV, int xN)
  416. {
  417. int a = 0;
  418. unsigned i;
  419. for(i=0; i<xN; ++i)
  420. if( xV[i] == 1 )
  421. ++a;
  422. return abs(a - (xN-a)) == 1;
  423. }
  424. unsigned _cmGenGoldCopy( int* y, unsigned yi, unsigned yN, unsigned* x, unsigned xN)
  425. {
  426. unsigned i;
  427. for(i=0; i<xN; ++i,++yi)
  428. y[yi] = x[i]==1 ? -1 : 1;
  429. assert(yi <= yN);
  430. return yi;
  431. }
  432. bool cmGenGoldCodes( unsigned lfsrN, unsigned poly_coeff0, unsigned poly_coeff1, unsigned goldN, int* yM, unsigned mlsN )
  433. {
  434. bool retFl = true;
  435. unsigned yi = 0;
  436. unsigned yN = goldN * mlsN;
  437. unsigned* mls0V = cmMemAllocZ(unsigned,mlsN);
  438. unsigned* mls1V = cmMemAllocZ(unsigned,mlsN);
  439. unsigned* xorV = cmMemAllocZ(unsigned,mlsN);
  440. unsigned i,j;
  441. cmLFSR(lfsrN, poly_coeff0, 1 << (lfsrN-1), mls0V, mlsN);
  442. cmLFSR(lfsrN, poly_coeff1, 1 << (lfsrN-1), mls1V, mlsN);
  443. if( cmMLS_IsBalanced(mls0V,mlsN) )
  444. yi = _cmGenGoldCopy(yM, yi, yN, mls0V, mlsN);
  445. if( yi<yN && cmMLS_IsBalanced(mls1V,mlsN) )
  446. yi = _cmGenGoldCopy(yM, yi, yN, mls1V, mlsN);
  447. for(i=0; yi < yN && i<mlsN-1; ++i )
  448. {
  449. for(j=0; j<mlsN; ++j)
  450. xorV[j] = (mls0V[j] + mls1V[ (i+j) % mlsN ]) % 2;
  451. if( cmMLS_IsBalanced(xorV,mlsN) )
  452. yi = _cmGenGoldCopy(yM,yi,yN,xorV,mlsN);
  453. }
  454. if(yi < yN )
  455. {
  456. //rc = cmErrMsg(err,kOpFailAtRC,"Gold code generation failed. Insuffient balanced pairs.");
  457. retFl = false;
  458. }
  459. cmMemFree(mls0V);
  460. cmMemFree(mls1V);
  461. cmMemFree(xorV);
  462. return retFl;
  463. }
  464. bool cmLFSR_Test()
  465. {
  466. // lfsrN = 5; % 5 6 7;
  467. // poly_coeff0 = 0x12; % 0x12 0x21 0x41;
  468. // poly_coeff1 = 0x1e; % 0x1e 0x36 0x72;
  469. unsigned lfsrN = 7;
  470. unsigned pc0 = 0x41;
  471. unsigned pc1 = 0x72;
  472. unsigned mlsN = (1 << lfsrN)-1;
  473. unsigned yN = mlsN*2;
  474. unsigned yV[ yN ];
  475. unsigned i;
  476. cmLFSR( lfsrN, pc0, 1 << (lfsrN-1), yV, yN );
  477. for(i=0; i<mlsN; ++i)
  478. if( yV[i] != yV[i+mlsN] )
  479. return false;
  480. //atVOU_PrintL(NULL,"0x12",yV,mlsN,2);
  481. cmLFSR( lfsrN, pc1, 1 << (lfsrN-1), yV, yN );
  482. //atVOU_PrintL(NULL,"0x17",yV,mlsN,2);
  483. for(i=0; i<mlsN; ++i)
  484. if( yV[i] != yV[i+mlsN] )
  485. return false;
  486. return true;
  487. }