Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 20 | pmbaty | 1 | #include "fixed.h" |
| 2 | #include "harness/trace.h" |
||
| 3 | |||
| 4 | #include <assert.h> |
||
| 5 | |||
| 6 | #define fixed_cos_table ((unsigned short*)&fixed_sin_table[64]) |
||
| 7 | |||
| 8 | static unsigned short fixed_sin_table[] = { |
||
| 9 | // start sin table |
||
| 10 | 0x0000, 0x0324, 0x0647, 0x096a, 0x0c8b, 0x0fab, 0x12c7, 0x15e1, |
||
| 11 | 0x18f8, 0x1c0b, 0x1f19, 0x2223, 0x2527, 0x2826, 0x2b1e, 0x2e10, |
||
| 12 | 0x30fb, 0x33de, 0x36b9, 0x398c, 0x3c56, 0x3f16, 0x41cd, 0x447a, |
||
| 13 | 0x471c, 0x49b3, 0x4c3f, 0x4ebf, 0x5133, 0x539a, 0x55f4, 0x5842, |
||
| 14 | 0x5a81, 0x5cb3, 0x5ed6, 0x60eb, 0x62f1, 0x64e7, 0x66ce, 0x68a5, |
||
| 15 | 0x6a6c, 0x6c23, 0x6dc9, 0x6f5e, 0x70e1, 0x7254, 0x73b5, 0x7503, |
||
| 16 | 0x7640, 0x776b, 0x7883, 0x7989, 0x7a7c, 0x7b5c, 0x7c29, 0x7ce2, |
||
| 17 | 0x7d89, 0x7e1c, 0x7e9c, 0x7f08, 0x7f61, 0x7fa6, 0x7fd7, 0x7ff5, |
||
| 18 | // start cos table |
||
| 19 | 0x7fff, 0x7ff5, 0x7fd7, 0x7fa6, 0x7f61, 0x7f08, 0x7e9c, 0x7e1c, |
||
| 20 | 0x7d89, 0x7ce2, 0x7c29, 0x7b5c, 0x7a7c, 0x7989, 0x7883, 0x776b, |
||
| 21 | 0x7640, 0x7503, 0x73b5, 0x7254, 0x70e1, 0x6f5e, 0x6dc9, 0x6c23, |
||
| 22 | 0x6a6c, 0x68a5, 0x66ce, 0x64e7, 0x62f1, 0x60eb, 0x5ed6, 0x5cb3, |
||
| 23 | 0x5a81, 0x5842, 0x55f4, 0x539a, 0x5133, 0x4ebf, 0x4c3f, 0x49b3, |
||
| 24 | 0x471c, 0x447a, 0x41cd, 0x3f16, 0x3c56, 0x398c, 0x36b9, 0x33de, |
||
| 25 | 0x30fb, 0x2e10, 0x2b1e, 0x2826, 0x2527, 0x2223, 0x1f19, 0x1c0b, |
||
| 26 | 0x18f8, 0x15e1, 0x12c7, 0x0fab, 0x0c8b, 0x096a, 0x0647, 0x0324, |
||
| 27 | 0x0000, 0xfcdc, 0xf9b9, 0xf696, 0xf375, 0xf055, 0xed39, 0xea1f, |
||
| 28 | 0xe708, 0xe3f5, 0xe0e7, 0xdddd, 0xdad9, 0xd7da, 0xd4e2, 0xd1f0, |
||
| 29 | 0xcf05, 0xcc22, 0xc947, 0xc674, 0xc3aa, 0xc0ea, 0xbe33, 0xbb86, |
||
| 30 | 0xb8e4, 0xb64d, 0xb3c1, 0xb141, 0xaecd, 0xac66, 0xaa0c, 0xa7be, |
||
| 31 | 0xa57f, 0xa34d, 0xa12a, 0x9f15, 0x9d0f, 0x9b19, 0x9932, 0x975b, |
||
| 32 | 0x9594, 0x93dd, 0x9237, 0x90a2, 0x8f1f, 0x8dac, 0x8c4b, 0x8afd, |
||
| 33 | 0x89c0, 0x8895, 0x877d, 0x8677, 0x8584, 0x84a4, 0x83d7, 0x831e, |
||
| 34 | 0x8277, 0x81e4, 0x8164, 0x80f8, 0x809f, 0x805a, 0x8029, 0x800b, |
||
| 35 | 0x8001, 0x800b, 0x8029, 0x805a, 0x809f, 0x80f8, 0x8164, 0x81e4, |
||
| 36 | 0x8277, 0x831e, 0x83d7, 0x84a4, 0x8584, 0x8677, 0x877d, 0x8895, |
||
| 37 | 0x89c0, 0x8afd, 0x8c4b, 0x8dac, 0x8f1f, 0x90a2, 0x9237, 0x93dd, |
||
| 38 | 0x9594, 0x975b, 0x9932, 0x9b19, 0x9d0f, 0x9f15, 0xa12a, 0xa34d, |
||
| 39 | 0xa57f, 0xa7be, 0xaa0c, 0xac66, 0xaecd, 0xb141, 0xb3c1, 0xb64d, |
||
| 40 | 0xb8e4, 0xbb86, 0xbe33, 0xc0ea, 0xc3aa, 0xc674, 0xc947, 0xcc22, |
||
| 41 | 0xcf05, 0xd1f0, 0xd4e2, 0xd7da, 0xdad9, 0xdddd, 0xe0e7, 0xe3f5, |
||
| 42 | 0xe708, 0xea1f, 0xed39, 0xf055, 0xf375, 0xf696, 0xf9b9, 0xfcdc, |
||
| 43 | 0x0000, 0x0324, 0x0647, 0x096a, 0x0c8b, 0x0fab, 0x12c7, 0x15e1, |
||
| 44 | 0x18f8, 0x1c0b, 0x1f19, 0x2223, 0x2527, 0x2826, 0x2b1e, 0x2e10, |
||
| 45 | 0x30fb, 0x33de, 0x36b9, 0x398c, 0x3c56, 0x3f16, 0x41cd, 0x447a, |
||
| 46 | 0x471c, 0x49b3, 0x4c3f, 0x4ebf, 0x5133, 0x539a, 0x55f4, 0x5842, |
||
| 47 | 0x5a81, 0x5cb3, 0x5ed6, 0x60eb, 0x62f1, 0x64e7, 0x66ce, 0x68a5, |
||
| 48 | 0x6a6c, 0x6c23, 0x6dc9, 0x6f5e, 0x70e1, 0x7254, 0x73b5, 0x7503, |
||
| 49 | 0x7640, 0x776b, 0x7883, 0x7989, 0x7a7c, 0x7b5c, 0x7c29, 0x7ce2, |
||
| 50 | 0x7d89, 0x7e1c, 0x7e9c, 0x7f08, 0x7f61, 0x7fa6, 0x7fd7, 0x7ff5, |
||
| 51 | 0x7fff, 0x7fff, |
||
| 52 | }; |
||
| 53 | |||
| 54 | static unsigned short fixed_asin_table[] = { |
||
| 55 | 0xc001, 0xc519, 0xc737, 0xc8d7, 0xca37, 0xcb6d, 0xcc87, 0xcd8a, |
||
| 56 | 0xce7c, 0xcf5f, 0xd037, 0xd104, 0xd1c9, 0xd286, 0xd33c, 0xd3ed, |
||
| 57 | 0xd498, 0xd53e, 0xd5df, 0xd67c, 0xd716, 0xd7ac, 0xd83f, 0xd8cf, |
||
| 58 | 0xd95c, 0xd9e7, 0xda6f, 0xdaf4, 0xdb78, 0xdbf9, 0xdc79, 0xdcf7, |
||
| 59 | 0xdd73, 0xdded, 0xde66, 0xdedd, 0xdf53, 0xdfc8, 0xe03b, 0xe0ad, |
||
| 60 | 0xe11e, 0xe18d, 0xe1fc, 0xe26a, 0xe2d6, 0xe342, 0xe3ac, 0xe416, |
||
| 61 | 0xe47f, 0xe4e7, 0xe54e, 0xe5b4, 0xe61a, 0xe67f, 0xe6e3, 0xe746, |
||
| 62 | 0xe7a9, 0xe80c, 0xe86d, 0xe8ce, 0xe92f, 0xe98f, 0xe9ee, 0xea4d, |
||
| 63 | 0xeaab, 0xeb09, 0xeb66, 0xebc3, 0xec20, 0xec7c, 0xecd7, 0xed33, |
||
| 64 | 0xed8d, 0xede8, 0xee42, 0xee9c, 0xeef5, 0xef4e, 0xefa7, 0xefff, |
||
| 65 | 0xf057, 0xf0af, 0xf106, 0xf15d, 0xf1b4, 0xf20b, 0xf261, 0xf2b8, |
||
| 66 | 0xf30d, 0xf363, 0xf3b9, 0xf40e, 0xf463, 0xf4b8, 0xf50c, 0xf561, |
||
| 67 | 0xf5b5, 0xf609, 0xf65d, 0xf6b1, 0xf704, 0xf758, 0xf7ab, 0xf7fe, |
||
| 68 | 0xf851, 0xf8a4, 0xf8f7, 0xf949, 0xf99c, 0xf9ee, 0xfa41, 0xfa93, |
||
| 69 | 0xfae5, 0xfb37, 0xfb89, 0xfbdb, 0xfc2d, 0xfc7f, 0xfcd1, 0xfd23, |
||
| 70 | 0xfd74, 0xfdc6, 0xfe17, 0xfe69, 0xfeba, 0xff0c, 0xff5e, 0xffaf, |
||
| 71 | 0x0000, 0x0051, 0x00a2, 0x00f4, 0x0146, 0x0197, 0x01e9, 0x023a, |
||
| 72 | 0x028c, 0x02dd, 0x032f, 0x0381, 0x03d3, 0x0425, 0x0477, 0x04c9, |
||
| 73 | 0x051b, 0x056d, 0x05bf, 0x0612, 0x0664, 0x06b7, 0x0709, 0x075c, |
||
| 74 | 0x07af, 0x0802, 0x0855, 0x08a8, 0x08fc, 0x094f, 0x09a3, 0x09f7, |
||
| 75 | 0x0a4b, 0x0a9f, 0x0af4, 0x0b48, 0x0b9d, 0x0bf2, 0x0c47, 0x0c9d, |
||
| 76 | 0x0cf3, 0x0d48, 0x0d9f, 0x0df5, 0x0e4c, 0x0ea3, 0x0efa, 0x0f51, |
||
| 77 | 0x0fa9, 0x1001, 0x1059, 0x10b2, 0x110b, 0x1164, 0x11be, 0x1218, |
||
| 78 | 0x1273, 0x12cd, 0x1329, 0x1384, 0x13e0, 0x143d, 0x149a, 0x14f7, |
||
| 79 | 0x1555, 0x15b3, 0x1612, 0x1671, 0x16d1, 0x1732, 0x1793, 0x17f4, |
||
| 80 | 0x1857, 0x18ba, 0x191d, 0x1981, 0x19e6, 0x1a4c, 0x1ab2, 0x1b19, |
||
| 81 | 0x1b81, 0x1bea, 0x1c54, 0x1cbe, 0x1d2a, 0x1d96, 0x1e04, 0x1e73, |
||
| 82 | 0x1ee2, 0x1f53, 0x1fc5, 0x2038, 0x20ad, 0x2123, 0x219a, 0x2213, |
||
| 83 | 0x228d, 0x2309, 0x2387, 0x2407, 0x2488, 0x250c, 0x2591, 0x2619, |
||
| 84 | 0x26a4, 0x2731, 0x27c1, 0x2854, 0x28ea, 0x2984, 0x2a21, 0x2ac2, |
||
| 85 | 0x2b68, 0x2c13, 0x2cc4, 0x2d7a, 0x2e37, 0x2efc, 0x2fc9, 0x30a1, |
||
| 86 | 0x3184, 0x3276, 0x3379, 0x3493, 0x35c9, 0x3729, 0x38c9, 0x3ae7, |
||
| 87 | 0x4000, |
||
| 88 | }; |
||
| 89 | |||
| 90 | static unsigned short fixed_acos_table[] = { |
||
| 91 | 0x7fff, 0x7ae7, 0x78c9, 0x7729, 0x75c9, 0x7493, 0x7379, 0x7276, |
||
| 92 | 0x7184, 0x70a1, 0x6fc9, 0x6efc, 0x6e37, 0x6d7a, 0x6cc4, 0x6c13, |
||
| 93 | 0x6b68, 0x6ac2, 0x6a21, 0x6984, 0x68ea, 0x6854, 0x67c1, 0x6731, |
||
| 94 | 0x66a4, 0x6619, 0x6591, 0x650c, 0x6488, 0x6407, 0x6387, 0x6309, |
||
| 95 | 0x628d, 0x6213, 0x619a, 0x6123, 0x60ad, 0x6038, 0x5fc5, 0x5f53, |
||
| 96 | 0x5ee2, 0x5e73, 0x5e04, 0x5d96, 0x5d2a, 0x5cbe, 0x5c54, 0x5bea, |
||
| 97 | 0x5b81, 0x5b19, 0x5ab2, 0x5a4c, 0x59e6, 0x5981, 0x591d, 0x58ba, |
||
| 98 | 0x5857, 0x57f4, 0x5793, 0x5732, 0x56d1, 0x5671, 0x5612, 0x55b3, |
||
| 99 | 0x5555, 0x54f7, 0x549a, 0x543d, 0x53e0, 0x5384, 0x5329, 0x52cd, |
||
| 100 | 0x5273, 0x5218, 0x51be, 0x5164, 0x510b, 0x50b2, 0x5059, 0x5001, |
||
| 101 | 0x4fa9, 0x4f51, 0x4efa, 0x4ea3, 0x4e4c, 0x4df5, 0x4d9f, 0x4d48, |
||
| 102 | 0x4cf3, 0x4c9d, 0x4c47, 0x4bf2, 0x4b9d, 0x4b48, 0x4af4, 0x4a9f, |
||
| 103 | 0x4a4b, 0x49f7, 0x49a3, 0x494f, 0x48fc, 0x48a8, 0x4855, 0x4802, |
||
| 104 | 0x47af, 0x475c, 0x4709, 0x46b7, 0x4664, 0x4612, 0x45bf, 0x456d, |
||
| 105 | 0x451b, 0x44c9, 0x4477, 0x4425, 0x43d3, 0x4381, 0x432f, 0x42dd, |
||
| 106 | 0x428c, 0x423a, 0x41e9, 0x4197, 0x4146, 0x40f4, 0x40a2, 0x4051, |
||
| 107 | 0x3fff, 0x3fae, 0x3f5d, 0x3f0b, 0x3eb9, 0x3e68, 0x3e16, 0x3dc5, |
||
| 108 | 0x3d73, 0x3d22, 0x3cd0, 0x3c7e, 0x3c2c, 0x3bda, 0x3b88, 0x3b36, |
||
| 109 | 0x3ae4, 0x3a92, 0x3a40, 0x39ed, 0x399b, 0x3948, 0x38f6, 0x38a3, |
||
| 110 | 0x3850, 0x37fd, 0x37aa, 0x3757, 0x3703, 0x36b0, 0x365c, 0x3608, |
||
| 111 | 0x35b4, 0x3560, 0x350b, 0x34b7, 0x3462, 0x340d, 0x33b8, 0x3362, |
||
| 112 | 0x330c, 0x32b7, 0x3260, 0x320a, 0x31b3, 0x315c, 0x3105, 0x30ae, |
||
| 113 | 0x3056, 0x2ffe, 0x2fa6, 0x2f4d, 0x2ef4, 0x2e9b, 0x2e41, 0x2de7, |
||
| 114 | 0x2d8c, 0x2d32, 0x2cd6, 0x2c7b, 0x2c1f, 0x2bc2, 0x2b65, 0x2b08, |
||
| 115 | 0x2aaa, 0x2a4c, 0x29ed, 0x298e, 0x292e, 0x28cd, 0x286c, 0x280b, |
||
| 116 | 0x27a8, 0x2745, 0x26e2, 0x267e, 0x2619, 0x25b3, 0x254d, 0x24e6, |
||
| 117 | 0x247e, 0x2415, 0x23ab, 0x2341, 0x22d5, 0x2269, 0x21fb, 0x218c, |
||
| 118 | 0x211d, 0x20ac, 0x203a, 0x1fc7, 0x1f52, 0x1edc, 0x1e65, 0x1dec, |
||
| 119 | 0x1d72, 0x1cf6, 0x1c78, 0x1bf8, 0x1b77, 0x1af3, 0x1a6e, 0x19e6, |
||
| 120 | 0x195b, 0x18ce, 0x183e, 0x17ab, 0x1715, 0x167b, 0x15de, 0x153d, |
||
| 121 | 0x1497, 0x13ec, 0x133b, 0x1285, 0x11c8, 0x1103, 0x1036, 0x0f5e, |
||
| 122 | 0x0e7b, 0x0d89, 0x0c86, 0x0b6c, 0x0a36, 0x08d6, 0x0736, 0x0518, |
||
| 123 | 0x0000, |
||
| 124 | }; |
||
| 125 | |||
| 126 | static unsigned short fixed_atan_table[] = { |
||
| 127 | 0x0000, 0x0028, 0x0051, 0x007a, 0x00a2, 0x00cb, 0x00f4, 0x011d, |
||
| 128 | 0x0145, 0x016e, 0x0197, 0x01bf, 0x01e8, 0x0211, 0x0239, 0x0262, |
||
| 129 | 0x028b, 0x02b3, 0x02dc, 0x0304, 0x032d, 0x0355, 0x037e, 0x03a6, |
||
| 130 | 0x03ce, 0x03f7, 0x041f, 0x0448, 0x0470, 0x0498, 0x04c0, 0x04e8, |
||
| 131 | 0x0511, 0x0539, 0x0561, 0x0589, 0x05b1, 0x05d9, 0x0601, 0x0628, |
||
| 132 | 0x0650, 0x0678, 0x06a0, 0x06c7, 0x06ef, 0x0716, 0x073e, 0x0765, |
||
| 133 | 0x078d, 0x07b4, 0x07db, 0x0803, 0x082a, 0x0851, 0x0878, 0x089f, |
||
| 134 | 0x08c6, 0x08ed, 0x0913, 0x093a, 0x0961, 0x0987, 0x09ae, 0x09d4, |
||
| 135 | 0x09fb, 0x0a21, 0x0a47, 0x0a6d, 0x0a94, 0x0aba, 0x0ae0, 0x0b05, |
||
| 136 | 0x0b2b, 0x0b51, 0x0b77, 0x0b9c, 0x0bc2, 0x0be7, 0x0c0c, 0x0c32, |
||
| 137 | 0x0c57, 0x0c7c, 0x0ca1, 0x0cc6, 0x0ceb, 0x0d0f, 0x0d34, 0x0d58, |
||
| 138 | 0x0d7d, 0x0da1, 0x0dc6, 0x0dea, 0x0e0e, 0x0e32, 0x0e56, 0x0e7a, |
||
| 139 | 0x0e9e, 0x0ec1, 0x0ee5, 0x0f08, 0x0f2c, 0x0f4f, 0x0f72, 0x0f95, |
||
| 140 | 0x0fb8, 0x0fdb, 0x0ffe, 0x1021, 0x1044, 0x1066, 0x1089, 0x10ab, |
||
| 141 | 0x10cd, 0x10ef, 0x1111, 0x1133, 0x1155, 0x1177, 0x1199, 0x11ba, |
||
| 142 | 0x11dc, 0x11fd, 0x121e, 0x123f, 0x1260, 0x1281, 0x12a2, 0x12c3, |
||
| 143 | 0x12e4, 0x1304, 0x1325, 0x1345, 0x1365, 0x1385, 0x13a5, 0x13c5, |
||
| 144 | 0x13e5, 0x1405, 0x1424, 0x1444, 0x1463, 0x1483, 0x14a2, 0x14c1, |
||
| 145 | 0x14e0, 0x14ff, 0x151e, 0x153c, 0x155b, 0x1579, 0x1598, 0x15b6, |
||
| 146 | 0x15d4, 0x15f2, 0x1610, 0x162e, 0x164c, 0x166a, 0x1687, 0x16a5, |
||
| 147 | 0x16c2, 0x16df, 0x16fc, 0x1719, 0x1736, 0x1753, 0x1770, 0x178c, |
||
| 148 | 0x17a9, 0x17c5, 0x17e2, 0x17fe, 0x181a, 0x1836, 0x1852, 0x186e, |
||
| 149 | 0x188a, 0x18a5, 0x18c1, 0x18dc, 0x18f7, 0x1913, 0x192e, 0x1949, |
||
| 150 | 0x1964, 0x197f, 0x1999, 0x19b4, 0x19ce, 0x19e9, 0x1a03, 0x1a1d, |
||
| 151 | 0x1a37, 0x1a51, 0x1a6b, 0x1a85, 0x1a9f, 0x1ab9, 0x1ad2, 0x1aec, |
||
| 152 | 0x1b05, 0x1b1e, 0x1b37, 0x1b50, 0x1b69, 0x1b82, 0x1b9b, 0x1bb4, |
||
| 153 | 0x1bcc, 0x1be5, 0x1bfd, 0x1c16, 0x1c2e, 0x1c46, 0x1c5e, 0x1c76, |
||
| 154 | 0x1c8e, 0x1ca5, 0x1cbd, 0x1cd5, 0x1cec, 0x1d04, 0x1d1b, 0x1d32, |
||
| 155 | 0x1d49, 0x1d60, 0x1d77, 0x1d8e, 0x1da5, 0x1dbb, 0x1dd2, 0x1de9, |
||
| 156 | 0x1dff, 0x1e15, 0x1e2c, 0x1e42, 0x1e58, 0x1e6e, 0x1e84, 0x1e99, |
||
| 157 | 0x1eaf, 0x1ec5, 0x1eda, 0x1ef0, 0x1f05, 0x1f1b, 0x1f30, 0x1f45, |
||
| 158 | 0x1f5a, 0x1f6f, 0x1f84, 0x1f99, 0x1fad, 0x1fc2, 0x1fd7, 0x1feb, |
||
| 159 | 0x2000, |
||
| 160 | }; |
||
| 161 | |||
| 162 | static uint16_t fast_sqrt_table[] = { |
||
| 163 | 0x8000, 0x80ff, 0x81fc, 0x82f7, 0x83f0, 0x84e7, 0x85dd, 0x86d1, |
||
| 164 | 0x87c3, 0x88b4, 0x89a3, 0x8a90, 0x8b7c, 0x8c66, 0x8d4e, 0x8e36, |
||
| 165 | 0x8f1b, 0x9000, 0x90e2, 0x91c4, 0x92a4, 0x9383, 0x9460, 0x953c, |
||
| 166 | 0x9617, 0x96f1, 0x97ca, 0x98a1, 0x9977, 0x9a4c, 0x9b20, 0x9bf2, |
||
| 167 | 0x9cc4, 0x9d94, 0x9e64, 0x9f32, 0xa000, 0xa0cc, 0xa197, 0xa261, |
||
| 168 | 0xa32b, 0xa3f3, 0xa4ba, 0xa581, 0xa646, 0xa70b, 0xa7cf, 0xa892, |
||
| 169 | 0xa953, 0xaa15, 0xaad5, 0xab94, 0xac53, 0xad11, 0xadcd, 0xae8a, |
||
| 170 | 0xaf45, 0xb000, 0xb0b9, 0xb172, 0xb22b, 0xb2e2, 0xb399, 0xb44f, |
||
| 171 | 0xb504, 0xb5b9, 0xb66d, 0xb720, 0xb7d3, 0xb885, 0xb936, 0xb9e7, |
||
| 172 | 0xba97, 0xbb46, 0xbbf5, 0xbca3, 0xbd50, 0xbdfd, 0xbea9, 0xbf55, |
||
| 173 | 0xc000, 0xc0aa, 0xc154, 0xc1fd, 0xc2a5, 0xc34e, 0xc3f5, 0xc49c, |
||
| 174 | 0xc542, 0xc5e8, 0xc68e, 0xc732, 0xc7d7, 0xc87a, 0xc91d, 0xc9c0, |
||
| 175 | 0xca62, 0xcb04, 0xcba5, 0xcc46, 0xcce6, 0xcd86, 0xce25, 0xcec3, |
||
| 176 | 0xcf62, 0xd000, 0xd09d, 0xd13a, 0xd1d6, 0xd272, 0xd30d, 0xd3a8, |
||
| 177 | 0xd443, 0xd4dd, 0xd577, 0xd610, 0xd6a9, 0xd742, 0xd7da, 0xd871, |
||
| 178 | 0xd908, 0xd99f, 0xda35, 0xdacb, 0xdb61, 0xdbf6, 0xdc8b, 0xdd1f, |
||
| 179 | 0xddb3, 0xde47, 0xdeda, 0xdf6d, 0xe000, 0xe092, 0xe123, 0xe1b5, |
||
| 180 | 0xe246, 0xe2d6, 0xe367, 0xe3f7, 0xe486, 0xe515, 0xe5a4, 0xe633, |
||
| 181 | 0xe6c1, 0xe74f, 0xe7dc, 0xe869, 0xe8f6, 0xe983, 0xea0f, 0xea9b, |
||
| 182 | 0xeb26, 0xebb1, 0xec3c, 0xecc7, 0xed51, 0xeddb, 0xee65, 0xeeee, |
||
| 183 | 0xef77, 0xf000, 0xf088, 0xf110, 0xf198, 0xf21f, 0xf2a6, 0xf32d, |
||
| 184 | 0xf3b4, 0xf43a, 0xf4c0, 0xf546, 0xf5cb, 0xf651, 0xf6d6, 0xf75a, |
||
| 185 | 0xf7de, 0xf863, 0xf8e6, 0xf96a, 0xf9ed, 0xfa70, 0xfaf3, 0xfb75, |
||
| 186 | 0xfbf7, 0xfc79, 0xfcfb, 0xfd7c, 0xfdfd, 0xfe7e, 0xfeff, 0xff7f, |
||
| 187 | }; |
||
| 188 | |||
| 189 | static uint16_t fast_rsqrt_table[] = { |
||
| 190 | 0xffff, 0xfe05, 0xfc17, 0xfa33, 0xf85b, 0xf68c, 0xf4c8, 0xf30d, |
||
| 191 | 0xf15b, 0xefb3, 0xee13, 0xec7b, 0xeaeb, 0xe964, 0xe7e3, 0xe66b, |
||
| 192 | 0xe4f9, 0xe38e, 0xe229, 0xe0cc, 0xdf74, 0xde23, 0xdcd7, 0xdb91, |
||
| 193 | 0xda51, 0xd916, 0xd7e0, 0xd6b0, 0xd584, 0xd45e, 0xd33c, 0xd21e, |
||
| 194 | 0xd105, 0xcff1, 0xcee1, 0xcdd4, 0xcccc, 0xcbc8, 0xcac8, 0xc9cb, |
||
| 195 | 0xc8d2, 0xc7dd, 0xc6eb, 0xc5fc, 0xc511, 0xc429, 0xc344, 0xc263, |
||
| 196 | 0xc184, 0xc0a8, 0xbfd0, 0xbefa, 0xbe26, 0xbd56, 0xbc88, 0xbbbd, |
||
| 197 | 0xbaf4, 0xba2e, 0xb96a, 0xb8a9, 0xb7ea, 0xb72d, 0xb673, 0xb5bb, |
||
| 198 | 0xb504, 0xb450, 0xb39f, 0xb2ef, 0xb241, 0xb195, 0xb0eb, 0xb043, |
||
| 199 | 0xaf9d, 0xaef8, 0xae56, 0xadb5, 0xad16, 0xac79, 0xabdd, 0xab43, |
||
| 200 | 0xaaaa, 0xaa13, 0xa97e, 0xa8ea, 0xa858, 0xa7c7, 0xa737, 0xa6a9, |
||
| 201 | 0xa61d, 0xa592, 0xa508, 0xa47f, 0xa3f8, 0xa372, 0xa2ee, 0xa26a, |
||
| 202 | 0xa1e8, 0xa167, 0xa0e7, 0xa069, 0x9fec, 0x9f6f, 0x9ef4, 0x9e7a, |
||
| 203 | 0x9e01, 0x9d89, 0x9d13, 0x9c9d, 0x9c28, 0x9bb4, 0x9b42, 0x9ad0, |
||
| 204 | 0x9a5f, 0x99ef, 0x9981, 0x9913, 0x98a6, 0x983a, 0x97ce, 0x9764, |
||
| 205 | 0x96fb, 0x9692, 0x962a, 0x95c3, 0x955d, 0x94f8, 0x9493, 0x9430, |
||
| 206 | 0x93cd, 0x936b, 0x9309, 0x92a9, 0x9249, 0x91e9, 0x918b, 0x912d, |
||
| 207 | 0x90d0, 0x9074, 0x9018, 0x8fbd, 0x8f63, 0x8f09, 0x8eb0, 0x8e58, |
||
| 208 | 0x8e00, 0x8da9, 0x8d53, 0x8cfd, 0x8ca8, 0x8c53, 0x8bff, 0x8bac, |
||
| 209 | 0x8b59, 0x8b06, 0x8ab5, 0x8a64, 0x8a13, 0x89c3, 0x8973, 0x8924, |
||
| 210 | 0x88d6, 0x8888, 0x883b, 0x87ee, 0x87a1, 0x8755, 0x870a, 0x86bf, |
||
| 211 | 0x8675, 0x862b, 0x85e1, 0x8598, 0x8550, 0x8508, 0x84c0, 0x8479, |
||
| 212 | 0x8432, 0x83ec, 0x83a6, 0x8361, 0x831c, 0x82d7, 0x8293, 0x824f, |
||
| 213 | 0x820c, 0x81c9, 0x8186, 0x8144, 0x8103, 0x80c1, 0x8080, 0x8040, |
||
| 214 | }; |
||
| 215 | |||
| 216 | br_fixed_ls BrFixedAbs(br_fixed_ls a) { |
||
| 217 | LOG_TRACE("(%08x)", a); |
||
| 218 | |||
| 219 | if (a < 0) { |
||
| 220 | a = -a; |
||
| 221 | } |
||
| 222 | return a; |
||
| 223 | } |
||
| 224 | |||
| 225 | br_fixed_ls BrFixedMul(br_fixed_ls a, br_fixed_ls b) { |
||
| 226 | LOG_TRACE("(%08x, %08x)", a, b); |
||
| 227 | |||
| 228 | return ((long long)a * (long long)b) >> 16; |
||
| 229 | } |
||
| 230 | |||
| 231 | br_fixed_ls BrFixedMac2(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d) { |
||
| 232 | LOG_TRACE("(%08x, %08x, %08x, %08x)", a, b, c, d); |
||
| 233 | |||
| 234 | return ((long long)a * (long long)b + (long long)c * (long long)d) >> 16; |
||
| 235 | } |
||
| 236 | |||
| 237 | br_fixed_ls BrFixedMac3(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d, br_fixed_ls e, br_fixed_ls f) { |
||
| 238 | LOG_TRACE("(%08x, %08x, %08x, %08x, %08x, %08x)", a, b, c, d, e, f); |
||
| 239 | |||
| 240 | return ((long long)a * (long long)b + (long long)c * (long long)d + (long long)e * (long long)f) >> 16; |
||
| 241 | } |
||
| 242 | |||
| 243 | br_fixed_ls BrFixedMac4(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d, br_fixed_ls e, br_fixed_ls f, br_fixed_ls g, br_fixed_ls h) { |
||
| 244 | LOG_TRACE("(%08x, %08x, %08x, %08x, %08x, %08x, %08x, %08x)", a, b, c, d, e, f, g, h); |
||
| 245 | |||
| 246 | return ((long long)a * (long long)b + (long long)c * (long long)d + (long long)e * (long long)f + (long long)g * (long long)h) >> 16; |
||
| 247 | } |
||
| 248 | |||
| 249 | br_fixed_ls BrFixedLength2(br_fixed_ls a, br_fixed_ls b) { |
||
| 250 | LOG_TRACE("(%08x, %08x)", a, b); |
||
| 251 | |||
| 252 | return _BrISqrt64((long long)a * (long long)a + (long long)b * (long long)b); |
||
| 253 | } |
||
| 254 | |||
| 255 | br_fixed_ls BrFixedLength3(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c) { |
||
| 256 | LOG_TRACE("(%08x, %08x, %08x)", a, b, c); |
||
| 257 | |||
| 258 | return _BrISqrt64((long long)a * (long long)a + (long long)b * (long long)b + (long long)c * (long long)c); |
||
| 259 | } |
||
| 260 | |||
| 261 | br_fixed_ls BrFixedLength4(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d) { |
||
| 262 | LOG_TRACE("(%08x, %08x, %08x, %08x)", a, b, c, d); |
||
| 263 | |||
| 264 | return _BrISqrt64((long long)a * (long long)a + (long long)b * (long long)b + (long long)c * (long long)c + (long long)d * (long long)d); |
||
| 265 | } |
||
| 266 | |||
| 267 | br_fixed_ls BrFixedRLength2(br_fixed_ls a, br_fixed_ls b) { |
||
| 268 | LOG_TRACE("(%08x, %08x)", a, b); |
||
| 269 | |||
| 270 | return _BrFastRSqrt64((long long)a * (long long)a + (long long)b * (long long)b); |
||
| 271 | } |
||
| 272 | |||
| 273 | br_fixed_ls BrFixedRLength3(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c) { |
||
| 274 | LOG_TRACE("(%08x, %08x, %08x)", a, b, c); |
||
| 275 | |||
| 276 | return _BrFastRSqrt64((long long)a * (long long)a + (long long)b * (long long)b + (long long)c * (long long)c); |
||
| 277 | } |
||
| 278 | |||
| 279 | br_fixed_ls BrFixedRLength4(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d) { |
||
| 280 | LOG_TRACE("(%08x, %08x, %08x, %08x)", a, b, c, d); |
||
| 281 | |||
| 282 | return _BrFastRSqrt64((long long)a * (long long)a + (long long)b * (long long)b + (long long)c * (long long)c + (long long)d * (long long)d); |
||
| 283 | } |
||
| 284 | |||
| 285 | br_fixed_ls BrFixedDiv(br_fixed_ls a, br_fixed_ls b) { |
||
| 286 | LOG_TRACE("(%08x, %08x)", a, b); |
||
| 287 | |||
| 288 | return (((long long)a) << 16) / (long long)b; |
||
| 289 | } |
||
| 290 | |||
| 291 | br_fixed_ls BrFixedDivR(br_fixed_ls a, br_fixed_ls b) { |
||
| 292 | LOG_TRACE("(%08x, %08x)", a, b); |
||
| 293 | |||
| 294 | return (((long long)a) << 16 | ((((uint32_t)(a)) & 0x80000000) ? 0xffff : 0x0)) / b; |
||
| 295 | } |
||
| 296 | |||
| 297 | br_fixed_ls BrFixedDivF(br_fixed_ls a, br_fixed_ls b) { |
||
| 298 | LOG_TRACE("(%08x, %08x)", a, b); |
||
| 299 | |||
| 300 | return (((long long)a) << 31) / b; |
||
| 301 | } |
||
| 302 | |||
| 303 | br_fixed_ls BrFixedMulDiv(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c) { |
||
| 304 | LOG_TRACE("(%08x, %08x, %08x)", a, b, c); |
||
| 305 | |||
| 306 | return ((long long)a * (long long)b) / (long long)c; |
||
| 307 | } |
||
| 308 | |||
| 309 | br_fixed_ls BrFixedMac2Div(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d, br_fixed_ls e) { |
||
| 310 | LOG_TRACE("(%08x, %08x, %08x, %08x, %08x)", a, b, c, d, e); |
||
| 311 | |||
| 312 | return ((long long)a * (long long)b + (long long)c * (long long)d) / (long long)e; |
||
| 313 | } |
||
| 314 | |||
| 315 | br_fixed_ls BrFixedMac3Div(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d, br_fixed_ls e, br_fixed_ls f, br_fixed_ls g) { |
||
| 316 | LOG_TRACE("(%08x, %08x, %08x, %08x, %08x, %08x, %08x)", a, b, c, d, e, f, g); |
||
| 317 | |||
| 318 | return ((long long)a * (long long)b + (long long)c * (long long)d + (long long)e * (long long)f) / (long long)g; |
||
| 319 | } |
||
| 320 | |||
| 321 | br_fixed_ls BrFixedMac4Div(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d, br_fixed_ls e, br_fixed_ls f, br_fixed_ls g, br_fixed_ls h, br_fixed_ls i) { |
||
| 322 | LOG_TRACE("(%08x, %08x, %08x, %08x, %08x, %08x, %08x, %08x, %08x)", a, b, c, d, e, f, g, h, i); |
||
| 323 | |||
| 324 | return ((long long)a * (long long)b + (long long)c * (long long)d + (long long)e * (long long)f + (long long)g * (long long)h) / (long long)i; |
||
| 325 | } |
||
| 326 | |||
| 327 | br_fixed_ls BrFixedFMac2(br_fixed_lsf a, br_fixed_ls b, br_fixed_lsf c, br_fixed_ls d) { |
||
| 328 | LOG_TRACE("(%08x, %08x, %08x, %08x)", a, b, c, d); |
||
| 329 | |||
| 330 | return ((long long)a * (long long)b + (long long)c * (long long)d) >> 15; |
||
| 331 | } |
||
| 332 | |||
| 333 | br_fixed_ls BrFixedFMac3(br_fixed_lsf a, br_fixed_ls b, br_fixed_lsf c, br_fixed_ls d, br_fixed_lsf e, br_fixed_ls f) { |
||
| 334 | LOG_TRACE("(%08x, %08x, %08x, %08x, %08x, %08x)", a, b, c, d, e, f); |
||
| 335 | |||
| 336 | return ((long long)a * (long long)b + (long long)c * (long long)d + (long long)e * (long long)f) >> 15; |
||
| 337 | } |
||
| 338 | |||
| 339 | br_fixed_ls BrFixedFMac4(br_fixed_lsf a, br_fixed_ls b, br_fixed_lsf c, br_fixed_ls d, br_fixed_lsf e, br_fixed_ls f, br_fixed_lsf g, br_fixed_ls h) { |
||
| 340 | LOG_TRACE("(%08x, %08x, %08x, %08x, %08x, %08x, %08x, %08x, %08x)", a, b, c, d, e, f, g, h); |
||
| 341 | |||
| 342 | return ((long long)a * (long long)b + (long long)c * (long long)d + (long long)e * (long long)f + (long long)g * (long long)h) >> 15; |
||
| 343 | } |
||
| 344 | |||
| 345 | br_fixed_ls BrFixedRcp(br_fixed_ls a) { |
||
| 346 | LOG_TRACE("(%08x)", a); |
||
| 347 | |||
| 348 | return (br_fixed_ls)(((long long)0x100000000) / a); |
||
| 349 | } |
||
| 350 | |||
| 351 | br_fixed_ls BrFixedSqr(br_fixed_ls a) { |
||
| 352 | LOG_TRACE("(%08x)", a); |
||
| 353 | |||
| 354 | return ((long long)a * (long long)a) >> 16; |
||
| 355 | } |
||
| 356 | |||
| 357 | br_fixed_ls BrFixedSqr2(br_fixed_ls a, br_fixed_ls b) { |
||
| 358 | LOG_TRACE("(%08x, %08x)", a, b); |
||
| 359 | |||
| 360 | return ((long long)a * (long long)a + (long long)b * (long long)b) >> 16; |
||
| 361 | } |
||
| 362 | |||
| 363 | br_fixed_ls BrFixedSqr3(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c) { |
||
| 364 | LOG_TRACE("(%08x, %08x, %08x)", a, b, c); |
||
| 365 | |||
| 366 | return ((long long)a * (long long)a + (long long)b * (long long)b + (long long)c * (long long)c) >> 16; |
||
| 367 | } |
||
| 368 | |||
| 369 | br_fixed_ls BrFixedSqr4(br_fixed_ls a, br_fixed_ls b, br_fixed_ls c, br_fixed_ls d) { |
||
| 370 | LOG_TRACE("(%08x, %08x, %08x, %08x)", a, b, c, d); |
||
| 371 | |||
| 372 | return ((long long)a * (long long)a + (long long)b * (long long)b + (long long)c * (long long)c + (long long)d * (long long)d) >> 16; |
||
| 373 | } |
||
| 374 | |||
| 375 | static int16_t fixed_lut_interpolate(uint16_t *table, uint32_t value) { |
||
| 376 | uint16_t idx = value >> 8; |
||
| 377 | int16_t basew = table[idx]; |
||
| 378 | int16_t deltaw = table[idx+1] - basew; |
||
| 379 | int16_t errw = (deltaw * (value & 0xff)) >> 8; |
||
| 380 | return basew + errw; |
||
| 381 | } |
||
| 382 | |||
| 383 | br_fixed_ls BrFixedSin(br_angle a) { |
||
| 384 | LOG_TRACE("(%04x)", a); |
||
| 385 | |||
| 386 | return 2 * fixed_lut_interpolate(fixed_sin_table, a); |
||
| 387 | } |
||
| 388 | |||
| 389 | br_fixed_ls BrFixedCos(br_angle a) { |
||
| 390 | LOG_TRACE("(%04x)", a); |
||
| 391 | |||
| 392 | return 2 * fixed_lut_interpolate(fixed_cos_table, a); |
||
| 393 | } |
||
| 394 | |||
| 395 | br_angle BrFixedASin(br_fixed_ls a) { |
||
| 396 | LOG_TRACE("(%08x)", a); |
||
| 397 | |||
| 398 | assert(a <= BrFloatToFixed(1.f) && "a must be <= BrFloatToFixed(1.f)"); |
||
| 399 | assert(a >= BrFloatToFixed(-1.f) && "a must be >= BrFloatToFixed(-1.f)"); |
||
| 400 | |||
| 401 | return fixed_lut_interpolate(fixed_asin_table, (a+0x10000) >> 1); |
||
| 402 | } |
||
| 403 | |||
| 404 | br_angle BrFixedACos(br_fixed_ls a) { |
||
| 405 | LOG_TRACE("(%08x)", a); |
||
| 406 | |||
| 407 | assert(a <= BrFloatToFixed(1.f) && "a must be <= BrFloatToFixed(1.f)"); |
||
| 408 | assert(a >= BrFloatToFixed(-1.f) && "a must be >= BrFloatToFixed(-1.f)"); |
||
| 409 | |||
| 410 | return fixed_lut_interpolate(fixed_acos_table, (a+0x10000) >> 1); |
||
| 411 | } |
||
| 412 | |||
| 413 | br_angle BrFixedATan2(br_fixed_ls x, br_fixed_ls y) { |
||
| 414 | LOG_TRACE("(%08x %08x)", x, y); |
||
| 415 | |||
| 416 | if (y > 0) { |
||
| 417 | if (x >= 0) { |
||
| 418 | if (x > y) { |
||
| 419 | return fixed_lut_interpolate(fixed_atan_table, ((((unsigned long long)y)<<32) / x) >> 16); // atan(y / x) |
||
| 420 | } else if (x == y) { |
||
| 421 | return 0x2000; // PI / 4 |
||
| 422 | } else { |
||
| 423 | return 0x4000 - fixed_lut_interpolate(fixed_atan_table, ((((unsigned long long)x)<<32) / y) >> 16); // PI/2 - atan(x / y) |
||
| 424 | } |
||
| 425 | } else { |
||
| 426 | x = -x; |
||
| 427 | if (x < y) { |
||
| 428 | return 0x4000 + fixed_lut_interpolate(fixed_atan_table, ((((unsigned long long)x)<<32) / y) >> 16); // PI/2 + atan(x / y) |
||
| 429 | } else if (x == y) { |
||
| 430 | return 0x6000; // 3 * PI / 4 |
||
| 431 | } else { |
||
| 432 | return 0x8000 - fixed_lut_interpolate(fixed_atan_table, ((((unsigned long long)y)<<32) / x) >> 16); // PI - atan(y / x) |
||
| 433 | } |
||
| 434 | } |
||
| 435 | } else if (y < 0) { |
||
| 436 | y = -y; |
||
| 437 | if (x < 0) { |
||
| 438 | x = -x; |
||
| 439 | if (x > y) { |
||
| 440 | return 0x8000 + fixed_lut_interpolate(fixed_atan_table, ((((unsigned long long)y)<<32) / x) >> 16); // PI + atan(y / x) |
||
| 441 | } else if (x == y) { |
||
| 442 | return 0xa000; // 5 * PI / 4 |
||
| 443 | } else { |
||
| 444 | return 0xc000 - fixed_lut_interpolate(fixed_atan_table, ((((unsigned long long)x)<<32) / y) >> 16); // 3 * PI / 2 - atan(x / y) |
||
| 445 | } |
||
| 446 | } else { |
||
| 447 | if (x < y) { |
||
| 448 | return 0xc000 + fixed_lut_interpolate(fixed_atan_table, ((((unsigned long long)x)<<32) / y) >> 16); // 3 * PI / 2 + atan(x / y) |
||
| 449 | } else if (x == y) { |
||
| 450 | return 0xe000; // 7 * PI / 4 |
||
| 451 | } else { |
||
| 452 | return -fixed_lut_interpolate(fixed_atan_table, ((((unsigned long long)y)<<32) / x) >> 16); // 2 * pi - atan(y / x) |
||
| 453 | } |
||
| 454 | } |
||
| 455 | } else { |
||
| 456 | if (x >= 0) { |
||
| 457 | return 0; // 0 |
||
| 458 | } else { |
||
| 459 | return 0x8000; // PI |
||
| 460 | } |
||
| 461 | } |
||
| 462 | } |
||
| 463 | |||
| 464 | br_angle BrFixedATan2Fast(br_fixed_ls x, br_fixed_ls y) { |
||
| 465 | LOG_TRACE("(%08x %08x)", x, y); |
||
| 466 | if (y > 0) { |
||
| 467 | if (x >= 0) { |
||
| 468 | if (x > y) { |
||
| 469 | return ((((unsigned long long)y)<<32) / x) >> 19; // atan(y / x) |
||
| 470 | } else if (x == y) { |
||
| 471 | return 0x2000; // PI / 4 |
||
| 472 | } else { |
||
| 473 | return 0x4000 - (((((unsigned long long)x)<<32) / y) >> 19); // PI/2 - atan(x / y) |
||
| 474 | } |
||
| 475 | } else { |
||
| 476 | x = -x; |
||
| 477 | if (x < y) { |
||
| 478 | return 0x4000 + (((((unsigned long long)x)<<32) / y) >> 19); // PI/2 + atan(x / y) |
||
| 479 | } else if (x == y) { |
||
| 480 | return 0x6000; // 3 * PI / 4 |
||
| 481 | } else { |
||
| 482 | return 0x8000 - (((((unsigned long long)y)<<32) / x) >> 19); // PI - atan(y / x) |
||
| 483 | } |
||
| 484 | } |
||
| 485 | } else if (y < 0) { |
||
| 486 | y = -y; |
||
| 487 | if (x < 0) { |
||
| 488 | x = -x; |
||
| 489 | if (x > y) { |
||
| 490 | return 0x8000 + (((((unsigned long long)y)<<32) / x) >> 19); // PI + atan(y / x) |
||
| 491 | } else if (x == y) { |
||
| 492 | return 0xa000; // 5 * PI / 4 |
||
| 493 | } else { |
||
| 494 | return 0xc000 - (((((unsigned long long)x)<<32) / y) >> 19); // 3 * PI / 2 - atan(x / y) |
||
| 495 | } |
||
| 496 | } else { |
||
| 497 | if (x < y) { |
||
| 498 | return 0xc000 + (((((unsigned long long)x)<<32) / y) >> 19); // 3 * PI / 2 + atan(x / y) |
||
| 499 | } else if (x == y) { |
||
| 500 | return 0xe000; // 7 * PI / 4 |
||
| 501 | } else { |
||
| 502 | return (br_angle)(0x10000 - (((((unsigned long long)y)<<32) / x) >> 19)); // 2 * pi - atan(y / x) |
||
| 503 | } |
||
| 504 | } |
||
| 505 | } else { |
||
| 506 | if (x >= 0) { |
||
| 507 | return 0; // 0 |
||
| 508 | } else { |
||
| 509 | return 0x8000; // PI |
||
| 510 | } |
||
| 511 | } |
||
| 512 | return 0; |
||
| 513 | } |
||
| 514 | |||
| 515 | uint16_t _BrISqrt32(uint32_t a) { |
||
| 516 | // Shifting nth root algorithm |
||
| 517 | uint32_t err; |
||
| 518 | uint32_t est; |
||
| 519 | uint32_t est_2; |
||
| 520 | int i; |
||
| 521 | LOG_TRACE("(%u)", a); |
||
| 522 | |||
| 523 | err = 0; |
||
| 524 | est = 0; |
||
| 525 | est_2 = 0; |
||
| 526 | |||
| 527 | // 2 MSB bits |
||
| 528 | err = a >> 30; |
||
| 529 | a <<= 2; |
||
| 530 | if (err != 0) { |
||
| 531 | err -= 1; |
||
| 532 | est = 1; |
||
| 533 | est_2 = 2; |
||
| 534 | } |
||
| 535 | // 14 iterations: 14*2-bit pairs=28 bits |
||
| 536 | for (i = 0; i < 14; i++) { |
||
| 537 | err = (err << 2) | (a >> 30); |
||
| 538 | a <<= 2; |
||
| 539 | est <<= 1; |
||
| 540 | est_2 <<= 1; |
||
| 541 | if (err > est_2) { |
||
| 542 | err -= est_2 + 1; |
||
| 543 | est += 1; |
||
| 544 | est_2 += 2; |
||
| 545 | } |
||
| 546 | } |
||
| 547 | // 2 LSB bits |
||
| 548 | err = (err << 2) | (a >> 30); |
||
| 549 | est <<= 1; |
||
| 550 | est_2 <<= 1; |
||
| 551 | if (err > est_2) { |
||
| 552 | est += 1; |
||
| 553 | } |
||
| 554 | return est; |
||
| 555 | } |
||
| 556 | #include <stdio.h> |
||
| 557 | br_uint_32 _BrISqrt64(uint64_t a) { |
||
| 558 | // Shifting nth root algorithm |
||
| 559 | uint64_t err; |
||
| 560 | uint64_t est; |
||
| 561 | uint64_t est_2; |
||
| 562 | int i; |
||
| 563 | LOG_TRACE("(%lu)", a); |
||
| 564 | |||
| 565 | err = 0; |
||
| 566 | est = 0; |
||
| 567 | est_2 = 0; |
||
| 568 | |||
| 569 | // 32 iterations (32 * 2bits) |
||
| 570 | for (i = 0; i < 32; i++) { |
||
| 571 | err = (err << 2) | (a >> 62); |
||
| 572 | a <<= 2; |
||
| 573 | est <<= 1; |
||
| 574 | est_2 <<= 1; |
||
| 575 | if (err > est_2) { |
||
| 576 | err -= est_2 + 1; |
||
| 577 | est += 1; |
||
| 578 | est_2 += 2; |
||
| 579 | } |
||
| 580 | } |
||
| 581 | return (br_uint_32)est; |
||
| 582 | } |
||
| 583 | |||
| 584 | #if 0 |
||
| 585 | // working inprecise |
||
| 586 | uint16_t _BrFastSqrt32(uint32_t v) { |
||
| 587 | uint32_t exp; |
||
| 588 | LOG_TRACE("(%u)", v); |
||
| 589 | |||
| 590 | if (v == 0) { |
||
| 591 | return v; |
||
| 592 | } |
||
| 593 | for (exp = 31; (v >> exp) == 0; exp--) { |
||
| 594 | } |
||
| 595 | // Exponent must be multiple of 2 (because square root divides exponent by 2) |
||
| 596 | exp = (exp | 1) + 1; |
||
| 597 | // Normalise number: exponent in [0..31], mantissa in [0x40-0xff] |
||
| 598 | v = (v << (31 - exp)) >> 24; |
||
| 599 | v = fast_sqrt_table[2*v-0x40]; |
||
| 600 | return (v << 16) >> (32 - (exp >> 1)); |
||
| 601 | } |
||
| 602 | #endif |
||
| 603 | |||
| 604 | uint16_t _BrFastSqrt32(uint32_t v) { |
||
| 605 | uint32_t exp; |
||
| 606 | LOG_TRACE("(%u)", v); |
||
| 607 | |||
| 608 | if (v == 0) { |
||
| 609 | return v; |
||
| 610 | } |
||
| 611 | for (exp = 31; (v >> exp) == 0; exp--) { |
||
| 612 | } |
||
| 613 | // Exponent must be multiple of 2 (because square root divides exponent by 2) |
||
| 614 | exp = (exp | 1) + 1; |
||
| 615 | // Normalise number: exponent in [0..31], mantissa in [0x40-0xff] |
||
| 616 | v = (v << (32 - exp)) >> 24; |
||
| 617 | v = fast_sqrt_table[v-0x40]; |
||
| 618 | return (v << 16) >> (32 - (exp >> 1)); |
||
| 619 | } |
||
| 620 | |||
| 621 | uint32_t _BrFastRSqrt32(uint32_t v) { |
||
| 622 | uint32_t exp; |
||
| 623 | LOG_TRACE("(0x%08x)", v); |
||
| 624 | |||
| 625 | if (v == 0) { |
||
| 626 | return v; |
||
| 627 | } |
||
| 628 | for (exp = 31; (v >> exp) == 0; exp--) { |
||
| 629 | } |
||
| 630 | exp = exp | 1; |
||
| 631 | v = (v << (31 - exp)) >> 24; |
||
| 632 | v = fast_rsqrt_table[v-0x40]; |
||
| 633 | return v >> (exp >> 1); |
||
| 634 | } |
||
| 635 | |||
| 636 | br_uint_32 _BrFastRSqrt64(uint64_t v) { |
||
| 637 | uint32_t exp; |
||
| 638 | uint32_t tmp; |
||
| 639 | LOG_TRACE("(0x%016llx)", v); |
||
| 640 | |||
| 641 | if (v == 0) { |
||
| 642 | return 0; |
||
| 643 | } |
||
| 644 | tmp = v >> 32; |
||
| 645 | if (tmp != 0) { |
||
| 646 | for (exp = 31; (tmp >> exp) == 0; exp--) { |
||
| 647 | } |
||
| 648 | exp = (31-exp) & 0xfe; |
||
| 649 | v = (v << exp) >> 56; |
||
| 650 | fflush(stdout); |
||
| 651 | v = fast_rsqrt_table[v-0x40]; |
||
| 652 | return (br_uint_32)(v >> ((30 - exp) >> 1)); |
||
| 653 | } |
||
| 654 | tmp = (br_uint_32)v; |
||
| 655 | for (exp = 31; (tmp >> exp) == 0; exp--) { |
||
| 656 | } |
||
| 657 | exp = exp | 1; |
||
| 658 | v = (v << (31 - exp)) >> 24; |
||
| 659 | v = fast_rsqrt_table[v-0x40]; |
||
| 660 | return (br_uint_32)((v << 16) >> (exp >> 1)); |
||
| 661 | } |