andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. |
| 3 | * |
| 4 | * Use of this source code is governed by a BSD-style license |
| 5 | * that can be found in the LICENSE file in the root of the source |
| 6 | * tree. An additional intellectual property rights grant can be found |
| 7 | * in the file PATENTS. All contributing project authors may |
| 8 | * be found in the AUTHORS file in the root of the source tree. |
| 9 | */ |
| 10 | |
| 11 | |
| 12 | /* |
| 13 | * This file contains the function WebRtcSpl_Sqrt(). |
| 14 | * The description header can be found in signal_processing_library.h |
| 15 | * |
| 16 | */ |
| 17 | |
pbos@webrtc.org | abf0cd8 | 2013-05-27 09:49:58 +0000 | [diff] [blame] | 18 | #include "webrtc/common_audio/signal_processing/include/signal_processing_library.h" |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 19 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 20 | int32_t WebRtcSpl_SqrtLocal(int32_t in); |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 21 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 22 | int32_t WebRtcSpl_SqrtLocal(int32_t in) |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 23 | { |
| 24 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 25 | int16_t x_half, t16; |
| 26 | int32_t A, B, x2; |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 27 | |
| 28 | /* The following block performs: |
| 29 | y=in/2 |
| 30 | x=y-2^30 |
| 31 | x_half=x/2^31 |
| 32 | t = 1 + (x_half) - 0.5*((x_half)^2) + 0.5*((x_half)^3) - 0.625*((x_half)^4) |
| 33 | + 0.875*((x_half)^5) |
| 34 | */ |
| 35 | |
| 36 | B = in; |
| 37 | |
| 38 | B = WEBRTC_SPL_RSHIFT_W32(B, 1); // B = in/2 |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 39 | B = B - ((int32_t)0x40000000); // B = in/2 - 1/2 |
| 40 | x_half = (int16_t)WEBRTC_SPL_RSHIFT_W32(B, 16);// x_half = x/2 = (in-1)/2 |
| 41 | B = B + ((int32_t)0x40000000); // B = 1 + x/2 |
| 42 | B = B + ((int32_t)0x40000000); // Add 0.5 twice (since 1.0 does not exist in Q31) |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 43 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 44 | x2 = ((int32_t)x_half) * ((int32_t)x_half) * 2; // A = (x/2)^2 |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 45 | A = -x2; // A = -(x/2)^2 |
| 46 | B = B + (A >> 1); // B = 1 + x/2 - 0.5*(x/2)^2 |
| 47 | |
| 48 | A = WEBRTC_SPL_RSHIFT_W32(A, 16); |
| 49 | A = A * A * 2; // A = (x/2)^4 |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 50 | t16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(A, 16); |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 51 | B = B + WEBRTC_SPL_MUL_16_16(-20480, t16) * 2; // B = B - 0.625*A |
| 52 | // After this, B = 1 + x/2 - 0.5*(x/2)^2 - 0.625*(x/2)^4 |
| 53 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 54 | t16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(A, 16); |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 55 | A = WEBRTC_SPL_MUL_16_16(x_half, t16) * 2; // A = (x/2)^5 |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 56 | t16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(A, 16); |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 57 | B = B + WEBRTC_SPL_MUL_16_16(28672, t16) * 2; // B = B + 0.875*A |
| 58 | // After this, B = 1 + x/2 - 0.5*(x/2)^2 - 0.625*(x/2)^4 + 0.875*(x/2)^5 |
| 59 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 60 | t16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(x2, 16); |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 61 | A = WEBRTC_SPL_MUL_16_16(x_half, t16) * 2; // A = x/2^3 |
| 62 | |
| 63 | B = B + (A >> 1); // B = B + 0.5*A |
| 64 | // After this, B = 1 + x/2 - 0.5*(x/2)^2 + 0.5*(x/2)^3 - 0.625*(x/2)^4 + 0.875*(x/2)^5 |
| 65 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 66 | B = B + ((int32_t)32768); // Round off bit |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 67 | |
| 68 | return B; |
| 69 | } |
| 70 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 71 | int32_t WebRtcSpl_Sqrt(int32_t value) |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 72 | { |
| 73 | /* |
| 74 | Algorithm: |
| 75 | |
| 76 | Six term Taylor Series is used here to compute the square root of a number |
| 77 | y^0.5 = (1+x)^0.5 where x = y-1 |
| 78 | = 1+(x/2)-0.5*((x/2)^2+0.5*((x/2)^3-0.625*((x/2)^4+0.875*((x/2)^5) |
| 79 | 0.5 <= x < 1 |
| 80 | |
| 81 | Example of how the algorithm works, with ut=sqrt(in), and |
| 82 | with in=73632 and ut=271 (even shift value case): |
| 83 | |
| 84 | in=73632 |
| 85 | y= in/131072 |
| 86 | x=y-1 |
| 87 | t = 1 + (x/2) - 0.5*((x/2)^2) + 0.5*((x/2)^3) - 0.625*((x/2)^4) + 0.875*((x/2)^5) |
| 88 | ut=t*(1/sqrt(2))*512 |
| 89 | |
| 90 | or: |
| 91 | |
| 92 | in=73632 |
| 93 | in2=73632*2^14 |
| 94 | y= in2/2^31 |
| 95 | x=y-1 |
| 96 | t = 1 + (x/2) - 0.5*((x/2)^2) + 0.5*((x/2)^3) - 0.625*((x/2)^4) + 0.875*((x/2)^5) |
| 97 | ut=t*(1/sqrt(2)) |
| 98 | ut2=ut*2^9 |
| 99 | |
| 100 | which gives: |
| 101 | |
| 102 | in = 73632 |
| 103 | in2 = 1206386688 |
| 104 | y = 0.56176757812500 |
| 105 | x = -0.43823242187500 |
| 106 | t = 0.74973506527313 |
| 107 | ut = 0.53014274874797 |
| 108 | ut2 = 2.714330873589594e+002 |
| 109 | |
| 110 | or: |
| 111 | |
| 112 | in=73632 |
| 113 | in2=73632*2^14 |
| 114 | y=in2/2 |
| 115 | x=y-2^30 |
| 116 | x_half=x/2^31 |
| 117 | t = 1 + (x_half) - 0.5*((x_half)^2) + 0.5*((x_half)^3) - 0.625*((x_half)^4) |
| 118 | + 0.875*((x_half)^5) |
| 119 | ut=t*(1/sqrt(2)) |
| 120 | ut2=ut*2^9 |
| 121 | |
| 122 | which gives: |
| 123 | |
| 124 | in = 73632 |
| 125 | in2 = 1206386688 |
| 126 | y = 603193344 |
| 127 | x = -470548480 |
| 128 | x_half = -0.21911621093750 |
| 129 | t = 0.74973506527313 |
| 130 | ut = 0.53014274874797 |
| 131 | ut2 = 2.714330873589594e+002 |
| 132 | |
| 133 | */ |
| 134 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 135 | int16_t x_norm, nshift, t16, sh; |
| 136 | int32_t A; |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 137 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 138 | int16_t k_sqrt_2 = 23170; // 1/sqrt2 (==5a82) |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 139 | |
| 140 | A = value; |
| 141 | |
| 142 | if (A == 0) |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 143 | return (int32_t)0; // sqrt(0) = 0 |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 144 | |
| 145 | sh = WebRtcSpl_NormW32(A); // # shifts to normalize A |
| 146 | A = WEBRTC_SPL_LSHIFT_W32(A, sh); // Normalize A |
| 147 | if (A < (WEBRTC_SPL_WORD32_MAX - 32767)) |
| 148 | { |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 149 | A = A + ((int32_t)32768); // Round off bit |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 150 | } else |
| 151 | { |
| 152 | A = WEBRTC_SPL_WORD32_MAX; |
| 153 | } |
| 154 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 155 | x_norm = (int16_t)WEBRTC_SPL_RSHIFT_W32(A, 16); // x_norm = AH |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 156 | |
| 157 | nshift = WEBRTC_SPL_RSHIFT_W16(sh, 1); // nshift = sh>>1 |
| 158 | nshift = -nshift; // Negate the power for later de-normalization |
| 159 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 160 | A = (int32_t)WEBRTC_SPL_LSHIFT_W32((int32_t)x_norm, 16); |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 161 | A = WEBRTC_SPL_ABS_W32(A); // A = abs(x_norm<<16) |
| 162 | A = WebRtcSpl_SqrtLocal(A); // A = sqrt(A) |
| 163 | |
| 164 | if ((-2 * nshift) == sh) |
| 165 | { // Even shift value case |
| 166 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 167 | t16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(A, 16); // t16 = AH |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 168 | |
| 169 | A = WEBRTC_SPL_MUL_16_16(k_sqrt_2, t16) * 2; // A = 1/sqrt(2)*t16 |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 170 | A = A + ((int32_t)32768); // Round off |
| 171 | A = A & ((int32_t)0x7fff0000); // Round off |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 172 | |
| 173 | A = WEBRTC_SPL_RSHIFT_W32(A, 15); // A = A>>16 |
| 174 | |
| 175 | } else |
| 176 | { |
| 177 | A = WEBRTC_SPL_RSHIFT_W32(A, 16); // A = A>>16 |
| 178 | } |
| 179 | |
pbos@webrtc.org | 1727dc7 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 180 | A = A & ((int32_t)0x0000ffff); |
| 181 | A = (int32_t)WEBRTC_SPL_SHIFT_W32(A, nshift); // De-normalize the result |
andrew@webrtc.org | a7b57da | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 182 | |
| 183 | return A; |
| 184 | } |