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 8.6KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421
  1. #include "cmPrefix.h"
  2. #include "cmGlobal.h"
  3. #include "cmFloatTypes.h"
  4. #include "cmMath.h"
  5. #include <sys/types.h> // u_char
  6. // TODO: rewrite to avoid copying
  7. // this code comes via csound source ...
  8. double cmX80ToDouble( unsigned char rate[10] )
  9. {
  10. char sign;
  11. short exp = 0;
  12. unsigned long mant1 = 0;
  13. unsigned long mant0 = 0;
  14. double val;
  15. unsigned char* p = (unsigned char*)rate;
  16. exp = *p++;
  17. exp <<= 8;
  18. exp |= *p++;
  19. sign = (exp & 0x8000) ? 1 : 0;
  20. exp &= 0x7FFF;
  21. mant1 = *p++;
  22. mant1 <<= 8;
  23. mant1 |= *p++;
  24. mant1 <<= 8;
  25. mant1 |= *p++;
  26. mant1 <<= 8;
  27. mant1 |= *p++;
  28. mant0 = *p++;
  29. mant0 <<= 8;
  30. mant0 |= *p++;
  31. mant0 <<= 8;
  32. mant0 |= *p++;
  33. mant0 <<= 8;
  34. mant0 |= *p++;
  35. /* special test for all bits zero meaning zero
  36. - else pow(2,-16383) bombs */
  37. if (mant1 == 0 && mant0 == 0 && exp == 0 && sign == 0)
  38. return 0.0;
  39. else {
  40. val = ((double)mant0) * pow(2.0,-63.0);
  41. val += ((double)mant1) * pow(2.0,-31.0);
  42. val *= pow(2.0,((double) exp) - 16383.0);
  43. return sign ? -val : val;
  44. }
  45. }
  46. // TODO: rewrite to avoid copying
  47. /*
  48. * Convert double to IEEE 80 bit floating point
  49. * Should be portable to all C compilers.
  50. * 19aug91 aldel/dpwe covered for MSB bug in Ultrix 'cc'
  51. */
  52. void cmDoubleToX80(double val, unsigned char rate[10])
  53. {
  54. char sign = 0;
  55. short exp = 0;
  56. unsigned long mant1 = 0;
  57. unsigned long mant0 = 0;
  58. unsigned char* p = (unsigned char*)rate;
  59. if (val < 0.0) { sign = 1; val = -val; }
  60. if (val != 0.0) /* val identically zero -> all elements zero */
  61. {
  62. exp = (short)(log(val)/log(2.0) + 16383.0);
  63. val *= pow(2.0, 31.0+16383.0-(double)exp);
  64. mant1 =((unsigned)val);
  65. val -= ((double)mant1);
  66. val *= pow(2.0, 32.0);
  67. mant0 =((double)val);
  68. }
  69. *p++ = ((sign<<7)|(exp>>8));
  70. *p++ = (u_char)(0xFF & exp);
  71. *p++ = (u_char)(0xFF & (mant1>>24));
  72. *p++ = (u_char)(0xFF & (mant1>>16));
  73. *p++ = (u_char)(0xFF & (mant1>> 8));
  74. *p++ = (u_char)(0xFF & (mant1));
  75. *p++ = (u_char)(0xFF & (mant0>>24));
  76. *p++ = (u_char)(0xFF & (mant0>>16));
  77. *p++ = (u_char)(0xFF & (mant0>> 8));
  78. *p++ = (u_char)(0xFF & (mant0));
  79. }
  80. bool cmIsPowerOfTwo( unsigned x )
  81. {
  82. return !( (x < 2) || (x & (x-1)) );
  83. }
  84. unsigned cmNextPowerOfTwo( unsigned val )
  85. {
  86. unsigned i;
  87. unsigned mask = 1;
  88. unsigned msb = 0;
  89. unsigned cnt = 0;
  90. // if val is a power of two return it
  91. if( cmIsPowerOfTwo(val) )
  92. return val;
  93. // next pow of zero is 2
  94. if( val == 0 )
  95. return 2;
  96. // if the next power of two can't be represented in 32 bits
  97. if( val > 0x80000000)
  98. {
  99. assert(0);
  100. return 0;
  101. }
  102. // find most sig. bit that is set - the number with only the next msb set is next pow 2
  103. for(i=0; i<31; i++,mask<<=1)
  104. if( mask & val )
  105. {
  106. msb = i;
  107. cnt++;
  108. }
  109. return 1 << (msb + 1);
  110. }
  111. unsigned cmNearPowerOfTwo( unsigned i )
  112. {
  113. unsigned vh = cmNextPowerOfTwo(i);
  114. if( vh == 2 )
  115. return vh;
  116. unsigned vl = vh / 2;
  117. if( vh - i < i - vl )
  118. return vh;
  119. return vl;
  120. }
  121. bool cmIsOddU( unsigned v ) { return v % 2 == 1; }
  122. bool cmIsEvenU( unsigned v ) { return !cmIsOddU(v); }
  123. unsigned cmNextOddU( unsigned v ) { return cmIsOddU(v) ? v : v+1; }
  124. unsigned cmPrevOddU( unsigned v ) { return cmIsOddU(v) ? v : v-1; }
  125. unsigned cmNextEvenU( unsigned v ) { return cmIsEvenU(v) ? v : v+1; }
  126. unsigned cmPrevEvenU( unsigned v ) { return cmIsEvenU(v) ? v : v-1; }
  127. // modified bessel function of first kind, order 0
  128. // ref: orfandis appendix B io.m
  129. double cmBessel0( double x )
  130. {
  131. double eps = pow(10.0,-9.0);
  132. double n = 1.0;
  133. double S = 1.0;
  134. double D = 1.0;
  135. while(D > eps*S)
  136. {
  137. double T = x /(2.0*n);
  138. n = n+1;
  139. D = D * pow(T,2.0);
  140. S = S + D;
  141. }
  142. return S;
  143. }
  144. //=================================================================
  145. // The following elliptic-related function approximations come from
  146. // Parks & Burrus, Digital Filter Design, Appendix program 9, pp. 317-326
  147. // which in turn draws directly on other sources
  148. // calculate complete elliptic integral (quarter period) K
  149. // given *complimentary* modulus kc
  150. cmReal_t cmEllipK( cmReal_t kc )
  151. {
  152. cmReal_t a = 1, b = kc, c = 1, tmp;
  153. while( c > cmReal_EPSILON )
  154. {
  155. c = 0.5*(a-b);
  156. tmp = 0.5*(a+b);
  157. b = sqrt(a*b);
  158. a = tmp;
  159. }
  160. return M_PI/(2*a);
  161. }
  162. // calculate elliptic modulus k
  163. // given ratio of complete elliptic integrals r = K/K'
  164. // (solves the "degree equation" for fixed N = K*K1'/K'K1)
  165. cmReal_t cmEllipDeg( cmReal_t r )
  166. {
  167. cmReal_t q,a,b,c,d;
  168. a = b = c = 1;
  169. d = q = exp(-M_PI*r);
  170. while( c > cmReal_EPSILON )
  171. {
  172. a = a + 2*c*d;
  173. c = c*d*d;
  174. b = b + c;
  175. d = d*q;
  176. }
  177. return 4*sqrt(q)*pow(b/a,2);
  178. }
  179. // calculate arc elliptic tangent u (elliptic integral of the 1st kind)
  180. // given argument x = sc(u,k) and *complimentary* modulus kc
  181. cmReal_t cmEllipArcSc( cmReal_t x, cmReal_t kc )
  182. {
  183. cmReal_t a = 1, b = kc, y = 1/x, tmp;
  184. unsigned L = 0;
  185. while( true )
  186. {
  187. tmp = a*b;
  188. a += b;
  189. b = 2*sqrt(tmp);
  190. y -= tmp/y;
  191. if( y == 0 )
  192. y = sqrt(tmp) * 1E-10;
  193. if( fabs(a-b)/a < cmReal_EPSILON )
  194. break;
  195. L *= 2;
  196. if( y < 0 )
  197. L++;
  198. }
  199. if( y < 0 )
  200. L++;
  201. return (atan(a/y) + M_PI*L)/a;
  202. }
  203. // calculate Jacobi elliptic functions sn, cn, and dn
  204. // given argument u and *complimentary* modulus kc
  205. cmRC_t cmEllipJ( cmReal_t u, cmReal_t kc, cmReal_t* sn, cmReal_t* cn, cmReal_t* dn )
  206. {
  207. assert( sn != NULL || cn != NULL || dn != NULL );
  208. if( u == 0 )
  209. {
  210. if( sn != NULL ) *sn = 0;
  211. if( cn != NULL ) *cn = 1;
  212. if( dn != NULL ) *dn = 1;
  213. return cmOkRC;
  214. }
  215. int i;
  216. cmReal_t a,b,c,d,e,tmp,_sn,_cn,_dn;
  217. cmReal_t aa[16], bb[16];
  218. a = 1;
  219. b = kc;
  220. for( i = 0; i < 16; i++ )
  221. {
  222. aa[i] = a;
  223. bb[i] = b;
  224. tmp = (a+b)/2;
  225. b = sqrt(a*b);
  226. a = tmp;
  227. if( (a-b)/a < cmReal_EPSILON )
  228. break;
  229. }
  230. c = a/tan(u*a);
  231. d = 1;
  232. for( ; i >= 0; i-- )
  233. {
  234. e = c*c/a;
  235. c = c*d;
  236. a = aa[i];
  237. d = (e + bb[i]) / (e+a);
  238. }
  239. _sn = 1/sqrt(1+c*c);
  240. _cn = _sn*c;
  241. _dn = d;
  242. if( sn != NULL ) *sn = _sn;
  243. if( cn != NULL ) *cn = _cn;
  244. if( dn != NULL ) *dn = _dn;
  245. return cmOkRC;
  246. }
  247. //=================================================================
  248. // bilinear transform
  249. // z = (2*sr + s)/(2*sr - s)
  250. cmRC_t cmBlt( unsigned n, cmReal_t sr, cmReal_t* rp, cmReal_t* ip )
  251. {
  252. unsigned i;
  253. cmReal_t a = 2*sr,
  254. tr, ti, td;
  255. for( i = 0; i < n; i++ )
  256. {
  257. tr = rp[i];
  258. ti = ip[i];
  259. td = pow(a-tr, 2) + ti*ti;
  260. rp[i] = (a*a - tr*tr - ti*ti)/td;
  261. ip[i] = 2*a*ti/td;
  262. if( tr < -1E15 )
  263. rp[i] = 0;
  264. if( fabs(ti) > 1E15 )
  265. ip[i] = 0;
  266. }
  267. return cmOkRC;
  268. }
  269. unsigned cmHzToMidi( double hz )
  270. {
  271. float midi = 12.0 * log2(hz/13.75) + 9;
  272. if( midi < 0 )
  273. midi = 0;
  274. if( midi > 127 )
  275. midi = 127;
  276. return (unsigned)lround(midi);
  277. }
  278. float cmMidiToHz( unsigned midi )
  279. {
  280. double m = midi <= 127 ? midi : 127;
  281. return (float)( 13.75 * pow(2.0,(m - 9.0)/12.0));
  282. }
  283. //=================================================================
  284. // Floating point byte swapping
  285. // Unions used to type-pun the swapping functions and thereby
  286. // avoid strict aliasing problems with -O2. Using unions for
  287. // this purpose is apparently legal under C99 but not C++.
  288. typedef union
  289. {
  290. unsigned u;
  291. float f;
  292. } _cmMathU_t;
  293. typedef union
  294. {
  295. unsigned long long u;
  296. double f;
  297. } _cmMathUL_t;
  298. unsigned cmFfSwapFloatToUInt( float v )
  299. {
  300. assert( sizeof(float) == sizeof(unsigned));
  301. _cmMathU_t u;
  302. u.f=v;
  303. return cmSwap32(u.u);
  304. }
  305. float cmFfSwapUIntToFloat( unsigned v )
  306. {
  307. assert( sizeof(float) == sizeof(unsigned));
  308. _cmMathU_t u;
  309. u.u = cmSwap32(v);
  310. return u.f;
  311. }
  312. unsigned long long cmFfSwapDoubleToULLong( double v )
  313. {
  314. assert( sizeof(double) == sizeof(unsigned long long));
  315. _cmMathUL_t u;
  316. u.f = v;
  317. return cmSwap64(u.u);
  318. }
  319. double cmFfSwapULLongToDouble( unsigned long long v )
  320. {
  321. assert( sizeof(double) == sizeof(unsigned long long));
  322. _cmMathUL_t u;
  323. u.u = cmSwap64(v);
  324. return u.f;
  325. }
  326. int cmRandInt( int min, int max )
  327. {
  328. assert( min <= max );
  329. int offs = max - min;
  330. return min + cmMax(0,cmMin(offs,(int)round(offs * (double)rand() / RAND_MAX)));
  331. }
  332. unsigned cmRandUInt( unsigned min, unsigned max )
  333. {
  334. assert( min <= max );
  335. unsigned offs = max - min;
  336. return min + cmMax(0,cmMin(offs,(unsigned)round(offs * (double)rand() / RAND_MAX)));
  337. }
  338. float cmRandFloat( float min, float max )
  339. {
  340. assert( min <= max );
  341. float offs = max - min;
  342. return min + cmMax(0,cmMin(offs,(float)(offs * (double)rand() / RAND_MAX)));
  343. }
  344. double cmRandDouble( double min, double max )
  345. {
  346. assert( min <= max );
  347. double offs = max - min;
  348. return min + cmMax(0,cmMin(offs,(offs * (double)rand() / RAND_MAX)));
  349. }