libcm is a C development framework with an emphasis on audio signal processing applications.
選択できるのは25トピックまでです。 トピックは、先頭が英数字で、英数字とダッシュ('-')を使用した35文字以内のものにしてください。

cmMath.c 9.6KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469
  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. }
  350. //=================================================================
  351. // Base on: http://stackoverflow.com/questions/3874627/floating-point-comparison-functions-for-c-sharp
  352. bool cmIsCloseD( double x0, double x1, double eps )
  353. {
  354. double d = fabs(x0-x1);
  355. if( x0 == x1 )
  356. return true;
  357. if( x0==0 || x1==0 || d<DBL_MIN )
  358. return d < (eps * DBL_MIN);
  359. return (d / cmMin( fabs(x0) + fabs(x1), DBL_MAX)) < eps;
  360. }
  361. bool cmIsCloseF( float x0, float x1, double eps_d )
  362. {
  363. float eps = (float)eps_d;
  364. float d = fabsf(x0-x1);
  365. if( x0 == x1 )
  366. return true;
  367. if( x0==0 || x1==0 || d<FLT_MIN )
  368. return d < (eps * FLT_MIN);
  369. return (d / cmMin( fabsf(x0) + fabsf(x1), FLT_MAX)) < eps;
  370. }
  371. bool cmIsCloseI( int x0, int x1, double eps )
  372. {
  373. if( x0 == x1 )
  374. return true;
  375. return abs(x0-x1)/(abs(x0)+abs(x1)) < eps;
  376. }
  377. bool cmIsCloseU( unsigned x0, unsigned x1, double eps )
  378. {
  379. if( x0 == x1 )
  380. return true;
  381. return abs(x0-x1)/(x0+x1) < eps;
  382. }