Fixed point sine and cosine. This uses the CORDIC algorithm in rotation mode. \param mz angle in [-pi/2,pi/2] as Q1.30 \param n number of iterations (at most 31) \return sine and cosine of \a mz as Q1.30
| 1556 | /// \param n number of iterations (at most 31) |
| 1557 | /// \return sine and cosine of \a mz as Q1.30 |
| 1558 | inline std::pair<uint32,uint32> sincos(uint32 mz, unsigned int n = 31) |
| 1559 | { |
| 1560 | static const uint32 angles[] = { |
| 1561 | 0x3243F6A9, 0x1DAC6705, 0x0FADBAFD, 0x07F56EA7, 0x03FEAB77, 0x01FFD55C, 0x00FFFAAB, 0x007FFF55, |
| 1562 | 0x003FFFEB, 0x001FFFFD, 0x00100000, 0x00080000, 0x00040000, 0x00020000, 0x00010000, 0x00008000, |
| 1563 | 0x00004000, 0x00002000, 0x00001000, 0x00000800, 0x00000400, 0x00000200, 0x00000100, 0x00000080, |
| 1564 | 0x00000040, 0x00000020, 0x00000010, 0x00000008, 0x00000004, 0x00000002, 0x00000001 }; |
| 1565 | uint32 mx = 0x26DD3B6A, my = 0; |
| 1566 | for(unsigned int i=0; i<n; ++i) |
| 1567 | { |
| 1568 | uint32 sign = sign_mask(mz); |
| 1569 | uint32 tx = mx - (arithmetic_shift(my, i)^sign) + sign; |
| 1570 | uint32 ty = my + (arithmetic_shift(mx, i)^sign) - sign; |
| 1571 | mx = tx; my = ty; mz -= (angles[i]^sign) - sign; |
| 1572 | } |
| 1573 | return std::make_pair(my, mx); |
| 1574 | } |
| 1575 | |
| 1576 | /// Fixed point arc tangent. |
| 1577 | /// This uses the CORDIC algorithm in vectoring mode. |