Fork of the espurna firmware for `mhsw` switches
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

636 lines
13 KiB

  1. /**
  2. * This code is available at
  3. * http://www.mindspring.com/~pfilandr/C/fs_math/
  4. * and it is believed to be public domain.
  5. */
  6. /* BEGIN fs_math.c */
  7. #include "fs_math.h"
  8. #include <float.h>
  9. /*
  10. ** pi == (atan(1.0 / 3) + atan(1.0 / 2)) * 4
  11. */
  12. static double fs_pi(void);
  13. static long double fs_pil(void);
  14. double fs_sqrt(double x)
  15. {
  16. int n;
  17. double a, b;
  18. if (x > 0 && DBL_MAX >= x) {
  19. for (n = 0; x > 2; x /= 4) {
  20. ++n;
  21. }
  22. while (0.5 > x) {
  23. --n;
  24. x *= 4;
  25. }
  26. a = x;
  27. b = (1 + x) / 2;
  28. do {
  29. x = b;
  30. b = (a / x + x) / 2;
  31. } while (x > b);
  32. while (n > 0) {
  33. x *= 2;
  34. --n;
  35. }
  36. while (0 > n) {
  37. x /= 2;
  38. ++n;
  39. }
  40. } else {
  41. if (x != 0) {
  42. x = DBL_MAX;
  43. }
  44. }
  45. return x;
  46. }
  47. double fs_log(double x)
  48. {
  49. int n;
  50. double a, b, c, epsilon;
  51. static double A, B, C;
  52. static int initialized;
  53. if (x > 0 && DBL_MAX >= x) {
  54. if (!initialized) {
  55. initialized = 1;
  56. A = fs_sqrt(2);
  57. B = A / 2;
  58. C = fs_log(A);
  59. }
  60. for (n = 0; x > A; x /= 2) {
  61. ++n;
  62. }
  63. while (B > x) {
  64. --n;
  65. x *= 2;
  66. }
  67. a = (x - 1) / (x + 1);
  68. x = C * n + a;
  69. c = a * a;
  70. n = 1;
  71. epsilon = DBL_EPSILON * x;
  72. if (0 > a) {
  73. if (epsilon > 0) {
  74. epsilon = -epsilon;
  75. }
  76. do {
  77. n += 2;
  78. a *= c;
  79. b = a / n;
  80. x += b;
  81. } while (epsilon > b);
  82. } else {
  83. if (0 > epsilon) {
  84. epsilon = -epsilon;
  85. }
  86. do {
  87. n += 2;
  88. a *= c;
  89. b = a / n;
  90. x += b;
  91. } while (b > epsilon);
  92. }
  93. x *= 2;
  94. } else {
  95. x = -DBL_MAX;
  96. }
  97. return x;
  98. }
  99. double fs_log10(double x)
  100. {
  101. static double log_10;
  102. static int initialized;
  103. if (!initialized) {
  104. initialized = 1;
  105. log_10 = fs_log(10);
  106. }
  107. return x > 0 && DBL_MAX >= x ? fs_log(x) / log_10 : fs_log(x);
  108. }
  109. double fs_exp(double x)
  110. {
  111. unsigned n, square;
  112. double b, e;
  113. static double x_max, x_min, epsilon;
  114. static int initialized;
  115. if (!initialized) {
  116. initialized = 1;
  117. x_max = fs_log(DBL_MAX);
  118. x_min = fs_log(DBL_MIN);
  119. epsilon = DBL_EPSILON / 4;
  120. }
  121. if (x_max >= x && x >= x_min) {
  122. for (square = 0; x > 1; x /= 2) {
  123. ++square;
  124. }
  125. while (-1 > x) {
  126. ++square;
  127. x /= 2;
  128. }
  129. e = b = n = 1;
  130. do {
  131. b /= n++;
  132. b *= x;
  133. e += b;
  134. b /= n++;
  135. b *= x;
  136. e += b;
  137. } while (b > epsilon);
  138. while (square-- != 0) {
  139. e *= e;
  140. }
  141. } else {
  142. e = x > 0 ? DBL_MAX : 0;
  143. }
  144. return e;
  145. }
  146. double fs_modf(double value, double *iptr)
  147. {
  148. double a, b;
  149. const double c = value;
  150. if (0 > c) {
  151. value = -value;
  152. }
  153. if (DBL_MAX >= value) {
  154. for (*iptr = 0; value >= 1; value -= b) {
  155. a = value / 2;
  156. b = 1;
  157. while (a >= b) {
  158. b *= 2;
  159. }
  160. *iptr += b;
  161. }
  162. } else {
  163. *iptr = value;
  164. value = 0;
  165. }
  166. if (0 > c) {
  167. *iptr = -*iptr;
  168. value = -value;
  169. }
  170. return value;
  171. }
  172. double fs_fmod(double x, double y)
  173. {
  174. double a, b;
  175. const double c = x;
  176. if (0 > c) {
  177. x = -x;
  178. }
  179. if (0 > y) {
  180. y = -y;
  181. }
  182. if (y != 0 && DBL_MAX >= y && DBL_MAX >= x) {
  183. while (x >= y) {
  184. a = x / 2;
  185. b = y;
  186. while (a >= b) {
  187. b *= 2;
  188. }
  189. x -= b;
  190. }
  191. } else {
  192. x = 0;
  193. }
  194. return 0 > c ? -x : x;
  195. }
  196. double fs_pow(double x, double y)
  197. {
  198. double p = 0;
  199. if (0 > x && fs_fmod(y, 1) == 0) {
  200. if (fs_fmod(y, 2) == 0) {
  201. p = fs_exp(fs_log(-x) * y);
  202. } else {
  203. p = -fs_exp(fs_log(-x) * y);
  204. }
  205. } else {
  206. if (x != 0 || 0 >= y) {
  207. p = fs_exp(fs_log( x) * y);
  208. }
  209. }
  210. return p;
  211. }
  212. static double fs_pi(void)
  213. {
  214. unsigned n;
  215. double a, b, epsilon;
  216. static double p;
  217. static int initialized;
  218. if (!initialized) {
  219. initialized = 1;
  220. epsilon = DBL_EPSILON / 4;
  221. n = 1;
  222. a = 3;
  223. do {
  224. a /= 9;
  225. b = a / n;
  226. n += 2;
  227. a /= 9;
  228. b -= a / n;
  229. n += 2;
  230. p += b;
  231. } while (b > epsilon);
  232. epsilon = DBL_EPSILON / 2;
  233. n = 1;
  234. a = 2;
  235. do {
  236. a /= 4;
  237. b = a / n;
  238. n += 2;
  239. a /= 4;
  240. b -= a / n;
  241. n += 2;
  242. p += b;
  243. } while (b > epsilon);
  244. p *= 4;
  245. }
  246. return p;
  247. }
  248. double fs_cos(double x)
  249. {
  250. unsigned n;
  251. int negative, sine;
  252. double a, b, c;
  253. static double pi, two_pi, half_pi, third_pi, epsilon;
  254. static int initialized;
  255. if (0 > x) {
  256. x = -x;
  257. }
  258. if (DBL_MAX >= x) {
  259. if (!initialized) {
  260. initialized = 1;
  261. pi = fs_pi();
  262. two_pi = 2 * pi;
  263. half_pi = pi / 2;
  264. third_pi = pi / 3;
  265. epsilon = DBL_EPSILON / 2;
  266. }
  267. if (x > two_pi) {
  268. x = fs_fmod(x, two_pi);
  269. }
  270. if (x > pi) {
  271. x = two_pi - x;
  272. }
  273. if (x > half_pi) {
  274. x = pi - x;
  275. negative = 1;
  276. } else {
  277. negative = 0;
  278. }
  279. if (x > third_pi) {
  280. x = half_pi - x;
  281. sine = 1;
  282. } else {
  283. sine = 0;
  284. }
  285. c = x * x;
  286. x = n = 0;
  287. a = 1;
  288. do {
  289. b = a;
  290. a *= c;
  291. a /= ++n;
  292. a /= ++n;
  293. b -= a;
  294. a *= c;
  295. a /= ++n;
  296. a /= ++n;
  297. x += b;
  298. } while (b > epsilon);
  299. if (sine) {
  300. x = fs_sqrt((1 - x) * (1 + x));
  301. }
  302. if (negative) {
  303. x = -x;
  304. }
  305. } else {
  306. x = -DBL_MAX;
  307. }
  308. return x;
  309. }
  310. double fs_log2(double x)
  311. {
  312. static double log_2;
  313. static int initialized;
  314. if (!initialized) {
  315. initialized = 1;
  316. log_2 = fs_log(2);
  317. }
  318. return x > 0 && DBL_MAX >= x ? fs_log(x) / log_2 : fs_log(x);
  319. }
  320. double fs_exp2(double x)
  321. {
  322. static double log_2;
  323. static int initialized;
  324. if (!initialized) {
  325. initialized = 1;
  326. log_2 = fs_log(2);
  327. }
  328. return fs_exp(x * log_2);
  329. }
  330. long double fs_powl(long double x, long double y)
  331. {
  332. long double p;
  333. if (0 > x && fs_fmodl(y, 1) == 0) {
  334. if (fs_fmodl(y, 2) == 0) {
  335. p = fs_expl(fs_logl(-x) * y);
  336. } else {
  337. p = -fs_expl(fs_logl(-x) * y);
  338. }
  339. } else {
  340. if (x != 0 || 0 >= y) {
  341. p = fs_expl(fs_logl( x) * y);
  342. } else {
  343. p = 0;
  344. }
  345. }
  346. return p;
  347. }
  348. long double fs_sqrtl(long double x)
  349. {
  350. long int n;
  351. long double a, b;
  352. if (x > 0 && LDBL_MAX >= x) {
  353. for (n = 0; x > 2; x /= 4) {
  354. ++n;
  355. }
  356. while (0.5 > x) {
  357. --n;
  358. x *= 4;
  359. }
  360. a = x;
  361. b = (1 + x) / 2;
  362. do {
  363. x = b;
  364. b = (a / x + x) / 2;
  365. } while (x > b);
  366. while (n > 0) {
  367. x *= 2;
  368. --n;
  369. }
  370. while (0 > n) {
  371. x /= 2;
  372. ++n;
  373. }
  374. } else {
  375. if (x != 0) {
  376. x = LDBL_MAX;
  377. }
  378. }
  379. return x;
  380. }
  381. long double fs_logl(long double x)
  382. {
  383. long int n;
  384. long double a, b, c, epsilon;
  385. static long double A, B, C;
  386. static int initialized;
  387. if (x > 0 && LDBL_MAX >= x) {
  388. if (!initialized) {
  389. initialized = 1;
  390. B = 1.5;
  391. do {
  392. A = B;
  393. B = 1 / A + A / 2;
  394. } while (A > B);
  395. B /= 2;
  396. C = fs_logl(A);
  397. }
  398. for (n = 0; x > A; x /= 2) {
  399. ++n;
  400. }
  401. while (B > x) {
  402. --n;
  403. x *= 2;
  404. }
  405. a = (x - 1) / (x + 1);
  406. x = C * n + a;
  407. c = a * a;
  408. n = 1;
  409. epsilon = LDBL_EPSILON * x;
  410. if (0 > a) {
  411. if (epsilon > 0) {
  412. epsilon = -epsilon;
  413. }
  414. do {
  415. n += 2;
  416. a *= c;
  417. b = a / n;
  418. x += b;
  419. } while (epsilon > b);
  420. } else {
  421. if (0 > epsilon) {
  422. epsilon = -epsilon;
  423. }
  424. do {
  425. n += 2;
  426. a *= c;
  427. b = a / n;
  428. x += b;
  429. } while (b > epsilon);
  430. }
  431. x *= 2;
  432. } else {
  433. x = -LDBL_MAX;
  434. }
  435. return x;
  436. }
  437. long double fs_expl(long double x)
  438. {
  439. long unsigned n, square;
  440. long double b, e;
  441. static long double x_max, x_min, epsilon;
  442. static int initialized;
  443. if (!initialized) {
  444. initialized = 1;
  445. x_max = fs_logl(LDBL_MAX);
  446. x_min = fs_logl(LDBL_MIN);
  447. epsilon = LDBL_EPSILON / 4;
  448. }
  449. if (x_max >= x && x >= x_min) {
  450. for (square = 0; x > 1; x /= 2) {
  451. ++square;
  452. }
  453. while (-1 > x) {
  454. ++square;
  455. x /= 2;
  456. }
  457. e = b = n = 1;
  458. do {
  459. b /= n++;
  460. b *= x;
  461. e += b;
  462. b /= n++;
  463. b *= x;
  464. e += b;
  465. } while (b > epsilon);
  466. while (square-- != 0) {
  467. e *= e;
  468. }
  469. } else {
  470. e = x > 0 ? LDBL_MAX : 0;
  471. }
  472. return e;
  473. }
  474. static long double fs_pil(void)
  475. {
  476. long unsigned n;
  477. long double a, b, epsilon;
  478. static long double p;
  479. static int initialized;
  480. if (!initialized) {
  481. initialized = 1;
  482. epsilon = LDBL_EPSILON / 4;
  483. n = 1;
  484. a = 3;
  485. do {
  486. a /= 9;
  487. b = a / n;
  488. n += 2;
  489. a /= 9;
  490. b -= a / n;
  491. n += 2;
  492. p += b;
  493. } while (b > epsilon);
  494. epsilon = LDBL_EPSILON / 2;
  495. n = 1;
  496. a = 2;
  497. do {
  498. a /= 4;
  499. b = a / n;
  500. n += 2;
  501. a /= 4;
  502. b -= a / n;
  503. n += 2;
  504. p += b;
  505. } while (b > epsilon);
  506. p *= 4;
  507. }
  508. return p;
  509. }
  510. long double fs_cosl(long double x)
  511. {
  512. long unsigned n;
  513. int negative, sine;
  514. long double a, b, c;
  515. static long double pi, two_pi, half_pi, third_pi, epsilon;
  516. static int initialized;
  517. if (0 > x) {
  518. x = -x;
  519. }
  520. if (LDBL_MAX >= x) {
  521. if (!initialized) {
  522. initialized = 1;
  523. pi = fs_pil();
  524. two_pi = 2 * pi;
  525. half_pi = pi / 2;
  526. third_pi = pi / 3;
  527. epsilon = LDBL_EPSILON / 2;
  528. }
  529. if (x > two_pi) {
  530. x = fs_fmodl(x, two_pi);
  531. }
  532. if (x > pi) {
  533. x = two_pi - x;
  534. }
  535. if (x > half_pi) {
  536. x = pi - x;
  537. negative = 1;
  538. } else {
  539. negative = 0;
  540. }
  541. if (x > third_pi) {
  542. x = half_pi - x;
  543. sine = 1;
  544. } else {
  545. sine = 0;
  546. }
  547. c = x * x;
  548. x = n = 0;
  549. a = 1;
  550. do {
  551. b = a;
  552. a *= c;
  553. a /= ++n;
  554. a /= ++n;
  555. b -= a;
  556. a *= c;
  557. a /= ++n;
  558. a /= ++n;
  559. x += b;
  560. } while (b > epsilon);
  561. if (sine) {
  562. x = fs_sqrtl((1 - x) * (1 + x));
  563. }
  564. if (negative) {
  565. x = -x;
  566. }
  567. } else {
  568. x = -LDBL_MAX;
  569. }
  570. return x;
  571. }
  572. long double fs_fmodl(long double x, long double y)
  573. {
  574. long double a, b;
  575. const long double c = x;
  576. if (0 > c) {
  577. x = -x;
  578. }
  579. if (0 > y) {
  580. y = -y;
  581. }
  582. if (y != 0 && LDBL_MAX >= y && LDBL_MAX >= x) {
  583. while (x >= y) {
  584. a = x / 2;
  585. b = y;
  586. while (a >= b) {
  587. b *= 2;
  588. }
  589. x -= b;
  590. }
  591. } else {
  592. x = 0;
  593. }
  594. return 0 > c ? -x : x;
  595. }
  596. /* END fs_math.c */