Fixed point arc tangent. This uses the CORDIC algorithm in vectoring mode. \param my y coordinate as Q0.30 \param mx x coordinate as Q0.30 \param n number of iterations (at most 31) \return arc tangent of \a my / \a mx as Q1.30
| 1580 | /// \param n number of iterations (at most 31) |
| 1581 | /// \return arc tangent of \a my / \a mx as Q1.30 |
| 1582 | inline uint32 atan2(uint32 my, uint32 mx, unsigned int n = 31) |
| 1583 | { |
| 1584 | static const uint32 angles[] = { |
| 1585 | 0x3243F6A9, 0x1DAC6705, 0x0FADBAFD, 0x07F56EA7, 0x03FEAB77, 0x01FFD55C, 0x00FFFAAB, 0x007FFF55, |
| 1586 | 0x003FFFEB, 0x001FFFFD, 0x00100000, 0x00080000, 0x00040000, 0x00020000, 0x00010000, 0x00008000, |
| 1587 | 0x00004000, 0x00002000, 0x00001000, 0x00000800, 0x00000400, 0x00000200, 0x00000100, 0x00000080, |
| 1588 | 0x00000040, 0x00000020, 0x00000010, 0x00000008, 0x00000004, 0x00000002, 0x00000001 }; |
| 1589 | uint32 mz = 0; |
| 1590 | for(unsigned int i=0; i<n; ++i) |
| 1591 | { |
| 1592 | uint32 sign = sign_mask(my); |
| 1593 | uint32 tx = mx + (arithmetic_shift(my, i)^sign) - sign; |
| 1594 | uint32 ty = my - (arithmetic_shift(mx, i)^sign) + sign; |
| 1595 | mx = tx; my = ty; mz += (angles[i]^sign) - sign; |
| 1596 | } |
| 1597 | return mz; |
| 1598 | } |
| 1599 | |
| 1600 | /// Reduce argument for trigonometric functions. |
| 1601 | /// \param abs half-precision floating-point value |