libcm is a C development framework with an emphasis on audio signal processing applications.
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.

cmMath.c 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596
  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. // modified bessel function of first kind, order 0
  133. // ref: orfandis appendix B io.m
  134. double cmBessel0( double x )
  135. {
  136. double eps = pow(10.0,-9.0);
  137. double n = 1.0;
  138. double S = 1.0;
  139. double D = 1.0;
  140. while(D > eps*S)
  141. {
  142. double T = x /(2.0*n);
  143. n = n+1;
  144. D = D * pow(T,2.0);
  145. S = S + D;
  146. }
  147. return S;
  148. }
  149. //=================================================================
  150. // The following elliptic-related function approximations come from
  151. // Parks & Burrus, Digital Filter Design, Appendix program 9, pp. 317-326
  152. // which in turn draws directly on other sources
  153. // calculate complete elliptic integral (quarter period) K
  154. // given *complimentary* modulus kc
  155. cmReal_t cmEllipK( cmReal_t kc )
  156. {
  157. cmReal_t a = 1, b = kc, c = 1, tmp;
  158. while( c > cmReal_EPSILON )
  159. {
  160. c = 0.5*(a-b);
  161. tmp = 0.5*(a+b);
  162. b = sqrt(a*b);
  163. a = tmp;
  164. }
  165. return M_PI/(2*a);
  166. }
  167. // calculate elliptic modulus k
  168. // given ratio of complete elliptic integrals r = K/K'
  169. // (solves the "degree equation" for fixed N = K*K1'/K'K1)
  170. cmReal_t cmEllipDeg( cmReal_t r )
  171. {
  172. cmReal_t q,a,b,c,d;
  173. a = b = c = 1;
  174. d = q = exp(-M_PI*r);
  175. while( c > cmReal_EPSILON )
  176. {
  177. a = a + 2*c*d;
  178. c = c*d*d;
  179. b = b + c;
  180. d = d*q;
  181. }
  182. return 4*sqrt(q)*pow(b/a,2);
  183. }
  184. // calculate arc elliptic tangent u (elliptic integral of the 1st kind)
  185. // given argument x = sc(u,k) and *complimentary* modulus kc
  186. cmReal_t cmEllipArcSc( cmReal_t x, cmReal_t kc )
  187. {
  188. cmReal_t a = 1, b = kc, y = 1/x, tmp;
  189. unsigned L = 0;
  190. while( true )
  191. {
  192. tmp = a*b;
  193. a += b;
  194. b = 2*sqrt(tmp);
  195. y -= tmp/y;
  196. if( y == 0 )
  197. y = sqrt(tmp) * 1E-10;
  198. if( fabs(a-b)/a < cmReal_EPSILON )
  199. break;
  200. L *= 2;
  201. if( y < 0 )
  202. L++;
  203. }
  204. if( y < 0 )
  205. L++;
  206. return (atan(a/y) + M_PI*L)/a;
  207. }
  208. // calculate Jacobi elliptic functions sn, cn, and dn
  209. // given argument u and *complimentary* modulus kc
  210. cmRC_t cmEllipJ( cmReal_t u, cmReal_t kc, cmReal_t* sn, cmReal_t* cn, cmReal_t* dn )
  211. {
  212. assert( sn != NULL || cn != NULL || dn != NULL );
  213. if( u == 0 )
  214. {
  215. if( sn != NULL ) *sn = 0;
  216. if( cn != NULL ) *cn = 1;
  217. if( dn != NULL ) *dn = 1;
  218. return cmOkRC;
  219. }
  220. int i;
  221. cmReal_t a,b,c,d,e,tmp,_sn,_cn,_dn;
  222. cmReal_t aa[16], bb[16];
  223. a = 1;
  224. b = kc;
  225. for( i = 0; i < 16; i++ )
  226. {
  227. aa[i] = a;
  228. bb[i] = b;
  229. tmp = (a+b)/2;
  230. b = sqrt(a*b);
  231. a = tmp;
  232. if( (a-b)/a < cmReal_EPSILON )
  233. break;
  234. }
  235. c = a/tan(u*a);
  236. d = 1;
  237. for( ; i >= 0; i-- )
  238. {
  239. e = c*c/a;
  240. c = c*d;
  241. a = aa[i];
  242. d = (e + bb[i]) / (e+a);
  243. }
  244. _sn = 1/sqrt(1+c*c);
  245. _cn = _sn*c;
  246. _dn = d;
  247. if( sn != NULL ) *sn = _sn;
  248. if( cn != NULL ) *cn = _cn;
  249. if( dn != NULL ) *dn = _dn;
  250. return cmOkRC;
  251. }
  252. //=================================================================
  253. // bilinear transform
  254. // z = (2*sr + s)/(2*sr - s)
  255. cmRC_t cmBlt( unsigned n, cmReal_t sr, cmReal_t* rp, cmReal_t* ip )
  256. {
  257. unsigned i;
  258. cmReal_t a = 2*sr,
  259. tr, ti, td;
  260. for( i = 0; i < n; i++ )
  261. {
  262. tr = rp[i];
  263. ti = ip[i];
  264. td = pow(a-tr, 2) + ti*ti;
  265. rp[i] = (a*a - tr*tr - ti*ti)/td;
  266. ip[i] = 2*a*ti/td;
  267. if( tr < -1E15 )
  268. rp[i] = 0;
  269. if( fabs(ti) > 1E15 )
  270. ip[i] = 0;
  271. }
  272. return cmOkRC;
  273. }
  274. unsigned cmHzToMidi( double hz )
  275. {
  276. float midi = 12.0 * log2(hz/13.75) + 9;
  277. if( midi < 0 )
  278. midi = 0;
  279. if( midi > 127 )
  280. midi = 127;
  281. return (unsigned)lround(midi);
  282. }
  283. float cmMidiToHz( unsigned midi )
  284. {
  285. double m = midi <= 127 ? midi : 127;
  286. return (float)( 13.75 * pow(2.0,(m - 9.0)/12.0));
  287. }
  288. //=================================================================
  289. // Floating point byte swapping
  290. // Unions used to type-pun the swapping functions and thereby
  291. // avoid strict aliasing problems with -O2. Using unions for
  292. // this purpose is apparently legal under C99 but not C++.
  293. typedef union
  294. {
  295. unsigned u;
  296. float f;
  297. } _cmMathU_t;
  298. typedef union
  299. {
  300. unsigned long long u;
  301. double f;
  302. } _cmMathUL_t;
  303. unsigned cmFfSwapFloatToUInt( float v )
  304. {
  305. assert( sizeof(float) == sizeof(unsigned));
  306. _cmMathU_t u;
  307. u.f=v;
  308. return cmSwap32(u.u);
  309. }
  310. float cmFfSwapUIntToFloat( unsigned v )
  311. {
  312. assert( sizeof(float) == sizeof(unsigned));
  313. _cmMathU_t u;
  314. u.u = cmSwap32(v);
  315. return u.f;
  316. }
  317. unsigned long long cmFfSwapDoubleToULLong( double v )
  318. {
  319. assert( sizeof(double) == sizeof(unsigned long long));
  320. _cmMathUL_t u;
  321. u.f = v;
  322. return cmSwap64(u.u);
  323. }
  324. double cmFfSwapULLongToDouble( unsigned long long v )
  325. {
  326. assert( sizeof(double) == sizeof(unsigned long long));
  327. _cmMathUL_t u;
  328. u.u = cmSwap64(v);
  329. return u.f;
  330. }
  331. int cmRandInt( int min, int max )
  332. {
  333. assert( min <= max );
  334. int offs = max - min;
  335. return min + cmMax(0,cmMin(offs,(int)round(offs * (double)rand() / RAND_MAX)));
  336. }
  337. unsigned cmRandUInt( unsigned min, unsigned max )
  338. {
  339. assert( min <= max );
  340. unsigned offs = max - min;
  341. return min + cmMax(0,cmMin(offs,(unsigned)round(offs * (double)rand() / RAND_MAX)));
  342. }
  343. float cmRandFloat( float min, float max )
  344. {
  345. assert( min <= max );
  346. float offs = max - min;
  347. return min + cmMax(0,cmMin(offs,(float)(offs * (double)rand() / RAND_MAX)));
  348. }
  349. double cmRandDouble( double min, double max )
  350. {
  351. assert( min <= max );
  352. double offs = max - min;
  353. return min + cmMax(0,cmMin(offs,(offs * (double)rand() / RAND_MAX)));
  354. }
  355. //=================================================================
  356. // Base on: http://stackoverflow.com/questions/3874627/floating-point-comparison-functions-for-c-sharp
  357. bool cmIsCloseD( double x0, double x1, double eps )
  358. {
  359. double d = fabs(x0-x1);
  360. if( x0 == x1 )
  361. return true;
  362. if( x0==0 || x1==0 || d<DBL_MIN )
  363. return d < (eps * DBL_MIN);
  364. return (d / cmMin( fabs(x0) + fabs(x1), DBL_MAX)) < eps;
  365. }
  366. bool cmIsCloseF( float x0, float x1, double eps_d )
  367. {
  368. float eps = (float)eps_d;
  369. float d = fabsf(x0-x1);
  370. if( x0 == x1 )
  371. return true;
  372. if( x0==0 || x1==0 || d<FLT_MIN )
  373. return d < (eps * FLT_MIN);
  374. return (d / cmMin( fabsf(x0) + fabsf(x1), FLT_MAX)) < eps;
  375. }
  376. bool cmIsCloseI( int x0, int x1, double eps )
  377. {
  378. if( x0 == x1 )
  379. return true;
  380. return abs(x0-x1)/(abs(x0)+abs(x1)) < eps;
  381. }
  382. bool cmIsCloseU( unsigned x0, unsigned x1, double eps )
  383. {
  384. if( x0 == x1 )
  385. return true;
  386. if( x0 > x1 )
  387. return (x0-x1)/(x0+x1) < eps;
  388. else
  389. return (x1-x0)/(x0+x1) < eps;
  390. }
  391. //=================================================================
  392. // cmLFSR() implementation based on note at bottom of:
  393. // http://www.ece.cmu.edu/~koopman/lfsr/index.html
  394. void cmLFSR( unsigned lfsrN, unsigned tapMask, unsigned seed, unsigned* yV, unsigned yN )
  395. {
  396. assert( 0 < lfsrN && lfsrN < 32 );
  397. unsigned i;
  398. for(i=0; i<yN; ++i)
  399. {
  400. if( (yV[i] = seed & 1)==1 )
  401. seed = (seed >> 1) ^ tapMask;
  402. else
  403. seed = (seed >> 1);
  404. }
  405. }
  406. bool cmMLS_IsBalanced( const unsigned* xV, int xN)
  407. {
  408. int a = 0;
  409. unsigned i;
  410. for(i=0; i<xN; ++i)
  411. if( xV[i] == 1 )
  412. ++a;
  413. return abs(a - (xN-a)) == 1;
  414. }
  415. unsigned _cmGenGoldCopy( int* y, unsigned yi, unsigned yN, unsigned* x, unsigned xN)
  416. {
  417. unsigned i;
  418. for(i=0; i<xN; ++i,++yi)
  419. y[yi] = x[i]==1 ? -1 : 1;
  420. assert(yi <= yN);
  421. return yi;
  422. }
  423. bool cmGenGoldCodes( unsigned lfsrN, unsigned poly_coeff0, unsigned poly_coeff1, unsigned goldN, int* yM, unsigned mlsN )
  424. {
  425. bool retFl = true;
  426. unsigned yi = 0;
  427. unsigned yN = goldN * mlsN;
  428. unsigned* mls0V = cmMemAllocZ(unsigned,mlsN);
  429. unsigned* mls1V = cmMemAllocZ(unsigned,mlsN);
  430. unsigned* xorV = cmMemAllocZ(unsigned,mlsN);
  431. unsigned i,j;
  432. cmLFSR(lfsrN, poly_coeff0, 1 << (lfsrN-1), mls0V, mlsN);
  433. cmLFSR(lfsrN, poly_coeff1, 1 << (lfsrN-1), mls1V, mlsN);
  434. if( cmMLS_IsBalanced(mls0V,mlsN) )
  435. yi = _cmGenGoldCopy(yM, yi, yN, mls0V, mlsN);
  436. if( yi<yN && cmMLS_IsBalanced(mls1V,mlsN) )
  437. yi = _cmGenGoldCopy(yM, yi, yN, mls1V, mlsN);
  438. for(i=0; yi < yN && i<mlsN-1; ++i )
  439. {
  440. for(j=0; j<mlsN; ++j)
  441. xorV[j] = (mls0V[j] + mls1V[ (i+j) % mlsN ]) % 2;
  442. if( cmMLS_IsBalanced(xorV,mlsN) )
  443. yi = _cmGenGoldCopy(yM,yi,yN,xorV,mlsN);
  444. }
  445. if(yi < yN )
  446. {
  447. //rc = cmErrMsg(err,kOpFailAtRC,"Gold code generation failed. Insuffient balanced pairs.");
  448. retFl = false;
  449. }
  450. cmMemFree(mls0V);
  451. cmMemFree(mls1V);
  452. cmMemFree(xorV);
  453. return retFl;
  454. }
  455. bool cmLFSR_Test()
  456. {
  457. // lfsrN = 5; % 5 6 7;
  458. // poly_coeff0 = 0x12; % 0x12 0x21 0x41;
  459. // poly_coeff1 = 0x1e; % 0x1e 0x36 0x72;
  460. unsigned lfsrN = 7;
  461. unsigned pc0 = 0x41;
  462. unsigned pc1 = 0x72;
  463. unsigned mlsN = (1 << lfsrN)-1;
  464. unsigned yN = mlsN*2;
  465. unsigned yV[ yN ];
  466. unsigned i;
  467. cmLFSR( lfsrN, pc0, 1 << (lfsrN-1), yV, yN );
  468. for(i=0; i<mlsN; ++i)
  469. if( yV[i] != yV[i+mlsN] )
  470. return false;
  471. //atVOU_PrintL(NULL,"0x12",yV,mlsN,2);
  472. cmLFSR( lfsrN, pc1, 1 << (lfsrN-1), yV, yN );
  473. //atVOU_PrintL(NULL,"0x17",yV,mlsN,2);
  474. for(i=0; i<mlsN; ++i)
  475. if( yV[i] != yV[i+mlsN] )
  476. return false;
  477. return true;
  478. }