blob: db88070d350f831309676a69214dfa6a1c449d20 [file] [log] [blame]
Lun Donga43730e2022-06-22 21:52:14 -07001/*
Lun Dong26c106a2023-10-24 09:14:25 -07002 * Copyright 2023 Google LLC
Lun Donga43730e2022-06-22 21:52:14 -07003 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
Lun Dong26c106a2023-10-24 09:14:25 -07008 * http://www.apache.org/licenses/LICENSE-2.0
Lun Donga43730e2022-06-22 21:52:14 -07009 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
Cindy Liu3c4d6272022-08-04 18:54:12 -070017#include "audio_prep/util.h"
Lun Donga43730e2022-06-22 21:52:14 -070018
Cindy Liu3c4d6272022-08-04 18:54:12 -070019#include <math.h>
Lun Donga43730e2022-06-22 21:52:14 -070020
21#ifndef M_SQRT2
22#define M_SQRT2 1.41421356237309504880
23#endif
24
25// Evenly linear spaced array
26void linspace(float* x, float start, float end, int n) {
27 float step = (end - start) / (n - 1);
28 for (int i = 0; i < n; i++) {
29 x[i] = start + i * step;
30 }
31}
32
33// Calculate the dot product of two vectors
34float dot_product(float* v, float* u, int n) {
35 float result = 0.0;
36 for (int i = 0; i < n; i++) {
37 result += v[i] * u[i];
38 }
39 return result;
40}
41
42/*---------------------------------------------------------------------------
43 * FUNCTION NAME: rfft
44 *
45 * PURPOSE: Real valued, in-place split-radix FFT
46 *
47 * INPUT:
48 * x Pointer to input and output array
49 * m 2^m = n is the Length of FFT
50 *
51 * OUTPUT Output order
52 * Re(0), Re(1), ..., Re(n/2), Im(N/2-1), ..., Im(1)
53 *
54 * RETURN VALUE
55 * none
56 *
57 * DESIGN REFERENCE:
58 * IEEE Transactions on Acoustic, Speech, and Signal Processing,
59 * Vol. ASSP-35. No. 6, June 1987, pp. 849-863.
60 *
61 * Subroutine adapted from fortran routine pp. 858-859.
62 * Note corrected printing errors on page 859:
63 * SS1 = SIN(A3) -> should be SS1 = SIN(A);
64 * CC3 = COS(3) -> should be CC3 = COS(A3)
65 *
66 *---------------------------------------------------------------------------*/
67
68void rfft(float* x, int m) {
69 int n = 1 << m;
70 int j, i, k, is, id;
71 int i0, i1, i2, i3, i4, i5, i6, i7, i8;
72 int n2, n4, n8;
73 float xt, a0, e, a, a3;
74 float t1, t2, t3, t4, t5, t6;
75 float cc1, ss1, cc3, ss3;
76 float* r0;
77
78 /* Digit reverse counter */
79
80 j = 0;
81 r0 = x;
82
83 for (i = 0; i < n - 1; i++) {
84 if (i < j) {
85 xt = x[j];
86 x[j] = *r0;
87 *r0 = xt;
88 }
89 r0++;
90
91 k = n >> 1;
92
93 while (k <= j) {
94 j = j - k;
95 k >>= 1;
96 }
97 j += k;
98 }
99
100 /* Length two butterflies */
101 is = 0;
102 id = 4;
103
104 while (is < n - 1) {
105 for (i0 = is; i0 < n; i0 += id) {
106 i1 = i0 + 1;
107 a0 = x[i0];
108 x[i0] += x[i1];
109 x[i1] = a0 - x[i1];
110 }
111
112 is = (id << 1) - 2;
113 id <<= 2;
114 }
115
116 /* L shaped butterflies */
117 n2 = 2;
118 for (k = 1; k < m; k++) {
119 n2 <<= 1;
120 n4 = n2 >> 2;
121 n8 = n2 >> 3;
122 e = (M_PI * 2) / n2;
123 is = 0;
124 id = n2 << 1;
125 while (is < n) {
126 for (i = is; i <= n - 1; i += id) {
127 i1 = i;
128 i2 = i1 + n4;
129 i3 = i2 + n4;
130 i4 = i3 + n4;
131 t1 = x[i4] + x[i3];
132 x[i4] -= x[i3];
133 x[i3] = x[i1] - t1;
134 x[i1] += t1;
135
136 if (n4 != 1) {
137 i1 += n8;
138 i2 += n8;
139 i3 += n8;
140 i4 += n8;
141 t1 = (x[i3] + x[i4]) / M_SQRT2;
142 t2 = (x[i3] - x[i4]) / M_SQRT2;
143 x[i4] = x[i2] - t1;
144 x[i3] = -x[i2] - t1;
145 x[i2] = x[i1] - t2;
146 x[i1] = x[i1] + t2;
147 }
148 }
149 is = (id << 1) - n2;
150 id <<= 2;
151 }
152
153 for (j = 1; j < n8; j++) {
154 a = j * e;
155 a3 = 3 * a;
156 cc1 = cosf(a);
157 ss1 = sinf(a);
158 cc3 = cosf(a3);
159 ss3 = sinf(a3);
160
161 is = 0;
162 id = n2 << 1;
163
164 while (is < n) {
165 for (i = is; i <= n - 1; i += id) {
166 i1 = i + j;
167 i2 = i1 + n4;
168 i3 = i2 + n4;
169 i4 = i3 + n4;
170 i5 = i + n4 - j;
171 i6 = i5 + n4;
172 i7 = i6 + n4;
173 i8 = i7 + n4;
174 t1 = x[i3] * cc1 + x[i7] * ss1;
175 t2 = x[i7] * cc1 - x[i3] * ss1;
176 t3 = x[i4] * cc3 + x[i8] * ss3;
177 t4 = x[i8] * cc3 - x[i4] * ss3;
178 t5 = t1 + t3;
179 t6 = t2 + t4;
180 t3 = t1 - t3;
181 t4 = t2 - t4;
182 t2 = x[i6] + t6;
183 x[i3] = t6 - x[i6];
184 x[i8] = t2;
185 t2 = x[i2] - t3;
186 x[i7] = -x[i2] - t3;
187 x[i4] = t2;
188 t1 = x[i1] + t5;
189 x[i6] = x[i1] - t5;
190 x[i1] = t1;
191 t1 = x[i5] + t4;
192 x[i5] = x[i5] - t4;
193 x[i2] = t1;
194 }
195 is = (id << 1) - n2;
196 id <<= 2;
197 }
198 }
199 }
200}