spl_sqrt.c (4856B)
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 * This file contains the function WebRtcSpl_Sqrt(). 13 * The description header can be found in signal_processing_library.h 14 * 15 */ 16 17 #include "common_audio/signal_processing/include/signal_processing_library.h" 18 #include "rtc_base/checks.h" 19 20 int32_t WebRtcSpl_SqrtLocal(int32_t in); 21 22 int32_t WebRtcSpl_SqrtLocal(int32_t in) { 23 int16_t x_half, t16; 24 int32_t A, B, x2; 25 26 /* The following block performs: 27 y=in/2 28 x=y-2^30 29 x_half=x/2^31 30 t = 1 + (x_half) - 0.5*((x_half)^2) + 0.5*((x_half)^3) - 0.625*((x_half)^4) 31 + 0.875*((x_half)^5) 32 */ 33 34 B = in / 2; 35 36 B = B - ((int32_t)0x40000000); // B = in/2 - 1/2 37 x_half = (int16_t)(B >> 16); // x_half = x/2 = (in-1)/2 38 B = B + ((int32_t)0x40000000); // B = 1 + x/2 39 B = B + 40 ((int32_t)0x40000000); // Add 0.5 twice (since 1.0 does not exist in Q31) 41 42 x2 = ((int32_t)x_half) * ((int32_t)x_half) * 2; // A = (x/2)^2 43 A = -x2; // A = -(x/2)^2 44 B = B + (A >> 1); // B = 1 + x/2 - 0.5*(x/2)^2 45 46 A >>= 16; 47 A = A * A * 2; // A = (x/2)^4 48 t16 = (int16_t)(A >> 16); 49 B += -20480 * t16 * 2; // B = B - 0.625*A 50 // After this, B = 1 + x/2 - 0.5*(x/2)^2 - 0.625*(x/2)^4 51 52 A = x_half * t16 * 2; // A = (x/2)^5 53 t16 = (int16_t)(A >> 16); 54 B += 28672 * t16 * 2; // B = B + 0.875*A 55 // After this, B = 1 + x/2 - 0.5*(x/2)^2 - 0.625*(x/2)^4 + 0.875*(x/2)^5 56 57 t16 = (int16_t)(x2 >> 16); 58 A = x_half * t16 * 2; // A = x/2^3 59 60 B = B + (A >> 1); // B = B + 0.5*A 61 // After this, B = 1 + x/2 - 0.5*(x/2)^2 + 0.5*(x/2)^3 - 0.625*(x/2)^4 + 62 // 0.875*(x/2)^5 63 64 B = B + ((int32_t)32768); // Round off bit 65 66 return B; 67 } 68 69 int32_t WebRtcSpl_Sqrt(int32_t value) { 70 /* 71 Algorithm: 72 73 Six term Taylor Series is used here to compute the square root of a number 74 y^0.5 = (1+x)^0.5 where x = y-1 75 = 1+(x/2)-0.5*((x/2)^2+0.5*((x/2)^3-0.625*((x/2)^4+0.875*((x/2)^5) 76 0.5 <= x < 1 77 78 Example of how the algorithm works, with ut=sqrt(in), and 79 with in=73632 and ut=271 (even shift value case): 80 81 in=73632 82 y= in/131072 83 x=y-1 84 t = 1 + (x/2) - 0.5*((x/2)^2) + 0.5*((x/2)^3) - 0.625*((x/2)^4) + 85 0.875*((x/2)^5) ut=t*(1/sqrt(2))*512 86 87 or: 88 89 in=73632 90 in2=73632*2^14 91 y= in2/2^31 92 x=y-1 93 t = 1 + (x/2) - 0.5*((x/2)^2) + 0.5*((x/2)^3) - 0.625*((x/2)^4) + 94 0.875*((x/2)^5) ut=t*(1/sqrt(2)) ut2=ut*2^9 95 96 which gives: 97 98 in = 73632 99 in2 = 1206386688 100 y = 0.56176757812500 101 x = -0.43823242187500 102 t = 0.74973506527313 103 ut = 0.53014274874797 104 ut2 = 2.714330873589594e+002 105 106 or: 107 108 in=73632 109 in2=73632*2^14 110 y=in2/2 111 x=y-2^30 112 x_half=x/2^31 113 t = 1 + (x_half) - 0.5*((x_half)^2) + 0.5*((x_half)^3) - 0.625*((x_half)^4) 114 + 0.875*((x_half)^5) 115 ut=t*(1/sqrt(2)) 116 ut2=ut*2^9 117 118 which gives: 119 120 in = 73632 121 in2 = 1206386688 122 y = 603193344 123 x = -470548480 124 x_half = -0.21911621093750 125 t = 0.74973506527313 126 ut = 0.53014274874797 127 ut2 = 2.714330873589594e+002 128 129 */ 130 131 int16_t x_norm, nshift, t16, sh; 132 int32_t A; 133 134 int16_t k_sqrt_2 = 23170; // 1/sqrt2 (==5a82) 135 136 A = value; 137 138 // The convention in this function is to calculate sqrt(abs(A)). Negate the 139 // input if it is negative. 140 if (A < 0) { 141 if (A == WEBRTC_SPL_WORD32_MIN) { 142 // This number cannot be held in an int32_t after negating. 143 // Map it to the maximum positive value. 144 A = WEBRTC_SPL_WORD32_MAX; 145 } else { 146 A = -A; 147 } 148 } else if (A == 0) { 149 return 0; // sqrt(0) = 0 150 } 151 152 sh = WebRtcSpl_NormW32(A); // # shifts to normalize A 153 A = WEBRTC_SPL_LSHIFT_W32(A, sh); // Normalize A 154 if (A < (WEBRTC_SPL_WORD32_MAX - 32767)) { 155 A = A + ((int32_t)32768); // Round off bit 156 } else { 157 A = WEBRTC_SPL_WORD32_MAX; 158 } 159 160 x_norm = (int16_t)(A >> 16); // x_norm = AH 161 162 nshift = (sh / 2); 163 RTC_DCHECK_GE(nshift, 0); 164 165 A = (int32_t)WEBRTC_SPL_LSHIFT_W32((int32_t)x_norm, 16); 166 A = WEBRTC_SPL_ABS_W32(A); // A = abs(x_norm<<16) 167 A = WebRtcSpl_SqrtLocal(A); // A = sqrt(A) 168 169 if (2 * nshift == sh) { 170 // Even shift value case 171 172 t16 = (int16_t)(A >> 16); // t16 = AH 173 174 A = k_sqrt_2 * t16 * 2; // A = 1/sqrt(2)*t16 175 A = A + ((int32_t)32768); // Round off 176 A = A & ((int32_t)0x7fff0000); // Round off 177 178 A >>= 15; // A = A>>16 179 180 } else { 181 A >>= 16; // A = A>>16 182 } 183 184 A = A & ((int32_t)0x0000ffff); 185 A >>= nshift; // De-normalize the result. 186 187 return A; 188 }