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
20WebRtc_Word32 WebRtcSpl_SqrtLocal(WebRtc_Word32 in);
21
22WebRtc_Word32 WebRtcSpl_SqrtLocal(WebRtc_Word32 in)
23{
24
25    WebRtc_Word16 x_half, t16;
26    WebRtc_Word32 A, B, x2;
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
39    B = B - ((WebRtc_Word32)0x40000000); // B = in/2 - 1/2
40    x_half = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(B, 16);// x_half = x/2 = (in-1)/2
41    B = B + ((WebRtc_Word32)0x40000000); // B = 1 + x/2
42    B = B + ((WebRtc_Word32)0x40000000); // Add 0.5 twice (since 1.0 does not exist in Q31)
43
44    x2 = ((WebRtc_Word32)x_half) * ((WebRtc_Word32)x_half) * 2; // A = (x/2)^2
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
50    t16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(A, 16);
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
54    t16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(A, 16);
55    A = WEBRTC_SPL_MUL_16_16(x_half, t16) * 2; // A = (x/2)^5
56    t16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(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 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(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 + ((WebRtc_Word32)32768); // Round off bit
67
68    return B;
69}
70
71WebRtc_Word32 WebRtcSpl_Sqrt(WebRtc_Word32 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    WebRtc_Word16 x_norm, nshift, t16, sh;
136    WebRtc_Word32 A;
137
138    WebRtc_Word16 k_sqrt_2 = 23170; // 1/sqrt2 (==5a82)
139
140    A = value;
141
142    if (A == 0)
143        return (WebRtc_Word32)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 + ((WebRtc_Word32)32768); // Round off bit
150    } else
151    {
152        A = WEBRTC_SPL_WORD32_MAX;
153    }
154
155    x_norm = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(A, 16); // x_norm = AH
156
157    nshift = WEBRTC_SPL_RSHIFT_W16(sh, 1); // nshift = sh>>1
158    nshift = -nshift; // Negate the power for later de-normalization
159
160    A = (WebRtc_Word32)WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)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 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(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 + ((WebRtc_Word32)32768); // Round off
171        A = A & ((WebRtc_Word32)0x7fff0000); // Round off
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
180    A = A & ((WebRtc_Word32)0x0000ffff);
181    A = (WebRtc_Word32)WEBRTC_SPL_SHIFT_W32(A, nshift); // De-normalize the result
182
183    return A;
184}
185