libcm is a C development framework with an emphasis on audio signal processing applications.
Du kan inte välja fler än 25 ämnen Ämnen måste starta med en bokstav eller siffra, kan innehålla bindestreck ('-') och vara max 35 tecken långa.

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. }