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 
18 #include "signal_processing_library.h"
19 
20 #include <assert.h>
21 
22 int32_t WebRtcSpl_SqrtLocal(int32_t in);
23 
WebRtcSpl_SqrtLocal(int32_t in)24 int32_t WebRtcSpl_SqrtLocal(int32_t in)
25 {
26 
27     int16_t x_half, t16;
28     int32_t A, B, x2;
29 
30     /* The following block performs:
31      y=in/2
32      x=y-2^30
33      x_half=x/2^31
34      t = 1 + (x_half) - 0.5*((x_half)^2) + 0.5*((x_half)^3) - 0.625*((x_half)^4)
35          + 0.875*((x_half)^5)
36      */
37 
38     B = in / 2;
39 
40     B = B - ((int32_t)0x40000000); // B = in/2 - 1/2
41     x_half = (int16_t)(B >> 16);  // x_half = x/2 = (in-1)/2
42     B = B + ((int32_t)0x40000000); // B = 1 + x/2
43     B = B + ((int32_t)0x40000000); // Add 0.5 twice (since 1.0 does not exist in Q31)
44 
45     x2 = ((int32_t)x_half) * ((int32_t)x_half) * 2; // A = (x/2)^2
46     A = -x2; // A = -(x/2)^2
47     B = B + (A >> 1); // B = 1 + x/2 - 0.5*(x/2)^2
48 
49     A >>= 16;
50     A = A * A * 2; // A = (x/2)^4
51     t16 = (int16_t)(A >> 16);
52     B = B + WEBRTC_SPL_MUL_16_16(-20480, t16) * 2; // B = B - 0.625*A
53     // After this, B = 1 + x/2 - 0.5*(x/2)^2 - 0.625*(x/2)^4
54 
55     A = WEBRTC_SPL_MUL_16_16(x_half, t16) * 2; // A = (x/2)^5
56     t16 = (int16_t)(A >> 16);
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 
60     t16 = (int16_t)(x2 >> 16);
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 
66     B = B + ((int32_t)32768); // Round off bit
67 
68     return B;
69 }
70 
WebRtcSpl_Sqrt(int32_t value)71 int32_t WebRtcSpl_Sqrt(int32_t value)
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 
135     int16_t x_norm, nshift, t16, sh;
136     int32_t A;
137 
138     int16_t k_sqrt_2 = 23170; // 1/sqrt2 (==5a82)
139 
140     A = value;
141 
142     if (A == 0)
143         return (int32_t)0; // sqrt(0) = 0
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     {
149         A = A + ((int32_t)32768); // Round off bit
150     } else
151     {
152         A = WEBRTC_SPL_WORD32_MAX;
153     }
154 
155     x_norm = (int16_t)(A >> 16);  // x_norm = AH
156 
157     nshift = (sh / 2);
158     assert(nshift >= 0);
159 
160     A = (int32_t)WEBRTC_SPL_LSHIFT_W32((int32_t)x_norm, 16);
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 
167         t16 = (int16_t)(A >> 16);  // t16 = AH
168 
169         A = WEBRTC_SPL_MUL_16_16(k_sqrt_2, t16) * 2; // A = 1/sqrt(2)*t16
170         A = A + ((int32_t)32768); // Round off
171         A = A & ((int32_t)0x7fff0000); // Round off
172 
173         A >>= 15;  // A = A>>16
174 
175     } else
176     {
177         A >>= 16;  // A = A>>16
178     }
179 
180     A = A & ((int32_t)0x0000ffff);
181     A >>= nshift;  // De-normalize the result.
182 
183     return A;
184 }
185