1
1
//
2
2
// Copyright (c) 2021-2024 suzumushi
3
3
//
4
- // 2024-1-4 SPDSPparam.cpp
4
+ // 2024-3-23 SPDSPparam.cpp
5
5
//
6
6
// Licensed under Creative Commons Attribution-NonCommercial-ShareAlike 4.0 (CC BY-NC-SA 4.0).
7
7
//
8
8
// https://creativecommons.org/licenses/by-nc-sa/4.0/
9
9
//
10
10
11
11
#include " SPDSPparam.h"
12
+ #include " SOXYPad.h"
12
13
13
14
14
15
namespace suzumushi {
@@ -58,8 +59,8 @@ void SPDSPparam:: rt_param_update (struct GUI_param &gp, IParameterChanges* outP
58
59
gp.s_z = std::min (std::max (gp.s_z , edge [5 ]), edge [4 ]);
59
60
60
61
// re-calculate positions
61
- double gp_s_x_2_y_2 = pow ( gp.s_x , 2.0 ) + pow ( gp.s_y , 2.0 ) ;
62
- gp.r = sqrt (gp_s_x_2_y_2 + pow ( gp.s_z , 2.0 ) );
62
+ double gp_s_x_2_y_2 = gp.s_x * gp. s_x + gp.s_y * gp. s_y ;
63
+ gp.r = sqrt (gp_s_x_2_y_2 + gp.s_z * gp. s_z );
63
64
if (gp.r != 0.0 ) {
64
65
gp.theta = asin (gp.s_z / gp.r ) * 180.0 / pi;
65
66
gp.phi = acos (gp.s_x / sqrt (gp_s_x_2_y_2)) * 180.0 / pi;
@@ -106,20 +107,24 @@ void SPDSPparam:: rt_param_update (struct GUI_param &gp, IParameterChanges* outP
106
107
double s_y = gp.s_y ;
107
108
double s_z = gp.s_z ;
108
109
if (! gp.reset ) {
109
- double v = sqrt (pow (gp.s_x - prev_s_x, 2.0 ) + pow (gp.s_y - prev_s_y, 2.0 ) + pow (gp.s_z - prev_s_z, 2.0 ));
110
+ double d_s_x = s_x - prev_s_x;
111
+ double d_s_y = s_y - prev_s_y;
112
+ double d_s_z = s_z - prev_s_z;
113
+ double v = sqrt (d_s_x * d_s_x + d_s_y * d_s_y + d_s_z * d_s_z);
110
114
if (v > v_limit) {
111
- s_x = (gp.s_x - prev_s_x) * v_limit / v + prev_s_x;
112
- s_y = (gp.s_y - prev_s_y) * v_limit / v + prev_s_y;
113
- s_z = (gp.s_z - prev_s_z) * v_limit / v + prev_s_z;
115
+ double cof = v_limit / v;
116
+ s_x = d_s_x * cof + prev_s_x;
117
+ s_y = d_s_y * cof + prev_s_y;
118
+ s_z = d_s_z * cof + prev_s_z;
114
119
gp.param_changed = true ;
115
120
}
116
121
}
117
122
prev_s_x = s_x;
118
123
prev_s_y = s_y;
119
124
prev_s_z = s_z;
120
- double s_x_2 = pow ( s_x, 2.0 ) ;
121
- double s_y_2 = pow ( s_y, 2.0 ) ;
122
- double s_z_2 = pow ( s_z, 2.0 ) ;
125
+ double s_x_2 = s_x * s_x ;
126
+ double s_y_2 = s_y * s_y ;
127
+ double s_z_2 = s_z * s_z ;
123
128
124
129
125
130
// update of direct wave parameters
@@ -156,53 +161,65 @@ void SPDSPparam:: rt_param_update (struct GUI_param &gp, IParameterChanges* outP
156
161
157
162
158
163
// update of reflected waves parameters
159
- valarray <double > v_s_x {2.0 * edge [0 ] - s_x, 2.0 * edge [1 ] - s_x, s_x, s_x, s_x, s_x, s_x, s_x};
160
- valarray <double > v_s_y {s_y, s_y, 2.0 * edge [2 ] - s_y, 2.0 * edge [3 ] - s_y, s_y, s_y, 2.0 * edge [2 ] + s_y, 2.0 * edge [3 ] + s_y};
161
- valarray <double > v_s_z {s_z, s_z, s_z, s_z, 2.0 * edge [4 ] - s_z, 2.0 * edge [5 ] - s_z, s_z, s_z};
162
- valarray <double > v_s_x_2 {pow (v_s_x [0 ], 2.0 ), pow (v_s_x [1 ], 2.0 ), s_x_2, s_x_2, s_x_2, s_x_2, s_x_2, s_x_2};
163
- valarray <double > v_s_y_2 {s_y_2, s_y_2, pow (v_s_y [2 ], 2.0 ), pow (v_s_y [3 ], 2.0 ), s_y_2, s_y_2, pow (v_s_y [6 ], 2.0 ), pow (v_s_y [7 ], 2.0 )};
164
- valarray <double > v_s_z_2 {s_z_2, s_z_2, s_z_2, s_z_2, pow (v_s_z [4 ], 2.0 ), pow (v_s_z [5 ], 2.0 ), s_z_2, s_z_2};
165
- valarray <double > v_s_x_2_z_2 = v_s_x_2 + v_s_z_2;
166
- valarray <double > v_med_r = sqrt (v_s_x_2_z_2);
167
- valarray <double > v_r = sqrt (v_s_x_2_z_2 + v_s_y_2);
164
+ double v_s_x [] {2.0 * edge [0 ] - s_x, 2.0 * edge [1 ] - s_x, s_x, s_x, s_x, s_x, s_x, s_x};
165
+ double v_s_y [] {s_y, s_y, 2.0 * edge [2 ] - s_y, 2.0 * edge [3 ] - s_y, s_y, s_y, 2.0 * edge [2 ] + s_y, 2.0 * edge [3 ] + s_y};
166
+ double v_s_z [] {s_z, s_z, s_z, s_z, 2.0 * edge [4 ] - s_z, 2.0 * edge [5 ] - s_z, s_z, s_z};
167
+ double v_s_x_2 [] {v_s_x [0 ] * v_s_x [0 ], v_s_x [1 ] * v_s_x [1 ], s_x_2, s_x_2, s_x_2, s_x_2, s_x_2, s_x_2};
168
+ double v_s_y_2 [] {s_y_2, s_y_2, v_s_y [2 ] * v_s_y [2 ], v_s_y [3 ] * v_s_y [3 ], s_y_2, s_y_2, v_s_y [6 ] * v_s_y [6 ], v_s_y [7 ] * v_s_y [7 ]};
169
+ double v_s_z_2 [] {s_z_2, s_z_2, s_z_2, s_z_2, v_s_z [4 ] * v_s_z [4 ], v_s_z [5 ] * v_s_z [5 ], s_z_2, s_z_2};
170
+
171
+ double v_s_x_2_z_2 [8 ];
172
+ for (int i = 0 ; i < 8 ; i++)
173
+ v_s_x_2_z_2 [i] = v_s_x_2 [i] + v_s_z_2 [i];
174
+ double v_med_r [6 ];
175
+ for (int i = 0 ; i < 6 ; i++)
176
+ v_med_r [i] = sqrt (v_s_x_2_z_2 [i]);
168
177
169
178
for (int i = 0 ; i < 6 ; i++) {
170
179
if (v_med_r [i] != 0.0 )
171
- v_theta_p [i] = v_s_x [i] / v_med_r [i];
180
+ v_theta_p [i] = acos ( v_s_x [i] / v_med_r [i]) ;
172
181
else
173
- v_theta_p [i] = 1.0 ;
174
- }
175
- v_theta_p = acos (v_theta_p);
176
- for (int i = 0 ; i < 6 ; i++) {
182
+ v_theta_p [i] = 0.0 ;
177
183
if (v_s_z [i] < 0.0 )
178
184
v_theta_p [i] = - v_theta_p [i];
179
185
}
180
186
181
- valarray <double > v_next_cos_theta_o = v_s_y / v_r;
182
- valarray <double > v_near_side = a_r * v_next_cos_theta_o;
183
- valarray <double > v_far_side = acos (v_next_cos_theta_o);
184
- v_far_side = a_r * v_far_side;
185
- v_far_side = a_r * pi / 2.0 - v_far_side;
187
+ double v_r [8 ];
188
+ for (int i = 0 ; i < 8 ; i++)
189
+ v_r [i] = sqrt (v_s_x_2_z_2 [i] + v_s_y_2 [i]);
190
+ double v_next_cos_theta_o [8 ];
191
+ for (int i = 0 ; i < 8 ; i++)
192
+ v_next_cos_theta_o [i] = v_s_y [i] / v_r [i];
193
+ double v_near_side [8 ];
194
+ for (int i = 0 ; i < 8 ; i++)
195
+ v_near_side [i] = a_r * v_next_cos_theta_o [i];
196
+ double v_far_side [8 ];
197
+ for (int i = 0 ; i < 8 ; i++)
198
+ v_far_side [i] = a_r * (pi / 2.0 - acos (v_next_cos_theta_o [i]));
186
199
187
- for (int i = 0 ; i < 8 ; i++) {
200
+ for (int i = 0 ; i < 6 ; i++) {
201
+ if (v_next_cos_theta_o [i] >= 0.0 ) {
202
+ v_distance_LL [i] = v_distance_RR [i] = v_r [i] - v_near_side [i];
203
+ v_distance_LR [i] = v_distance_RL [i] = v_r [i] + v_far_side [i];
204
+ } else {
205
+ v_distance_LL [i] = v_distance_RR [i] = v_r [i] - v_far_side [i];
206
+ v_distance_LR [i] = v_distance_RL [i] = v_r [i] + v_near_side [i];
207
+ }
208
+ }
209
+ for (int i = 6 ; i < 8 ; i++) {
188
210
if (v_next_cos_theta_o [i] >= 0.0 ) {
189
- v_distance_LL [i] = v_r [i] - v_near_side [i];
190
- v_distance_LR [i] = v_r [i] + v_far_side [i];
211
+ v_distance_RL [i - 4 ] = v_r [i] - v_near_side [i];
212
+ v_distance_RR [i - 4 ] = v_r [i] + v_far_side [i];
191
213
} else {
192
- v_distance_LL [i] = v_r [i] - v_far_side [i];
193
- v_distance_LR [i] = v_r [i] + v_near_side [i];
214
+ v_distance_RL [i - 4 ] = v_r [i] - v_far_side [i];
215
+ v_distance_RR [i - 4 ] = v_r [i] + v_near_side [i];
194
216
}
195
217
}
196
- v_distance_RL = v_distance_LR [std::slice (0 , 6 , 1 )];
197
- v_distance_RL [std::slice (2 , 2 , 1 )] = v_distance_LL [std::slice (6 , 2 , 1 )];
198
- v_distance_RR = v_distance_LL [std::slice (0 , 6 , 1 )];
199
- v_distance_RR [std::slice (2 , 2 , 1 )] = v_distance_LR [std::slice (6 , 2 , 1 )];
200
218
201
- valarray <double > v_next_decay = pow (v_r, d_att);
202
- for (int i = 0 ; i < 8 ; i++)
203
- v_next_decay [i] = std::max (v_next_decay [i], min_dist);
204
219
double ref = rangeParameter:: dB_to_ratio (gp.reflectance );
205
- v_next_decay = ref / v_next_decay;
220
+ double v_next_decay [8 ];
221
+ for (int i = 0 ; i < 8 ; i++)
222
+ v_next_decay [i] = ref / std::max (pow (v_r [i], d_att), min_dist);
206
223
207
224
// update of parameter for crosstalk canceller
208
225
sin_phiL = sin (gp.phiL * pi / 180.0 );
@@ -212,21 +229,25 @@ void SPDSPparam:: rt_param_update (struct GUI_param &gp, IParameterChanges* outP
212
229
cos_theta_o = next_cos_theta_o;
213
230
decay_L = next_decay_L;
214
231
decay_R = next_decay_R;
215
- v_cos_theta_o_L = v_cos_theta_o_R = v_next_cos_theta_o [std::slice (0 , 6 , 1 )];
216
- v_cos_theta_o_R = - v_cos_theta_o_R;
217
- v_cos_theta_o_R [std::slice (2 , 2 , 1 )] = v_next_cos_theta_o [std::slice (6 , 2 , 1 )];
218
- v_decay_L = v_decay_R = v_next_decay [std::slice (0 , 6 , 1 )];
219
- v_decay_R [std::slice (2 , 2 , 1 )] = v_next_decay [std::slice (6 , 2 , 1 )];
232
+ for (int i = 0 ; i < 6 ; i++) {
233
+ v_cos_theta_o_L [i] = v_next_cos_theta_o [i];
234
+ v_cos_theta_o_R [i] = - v_next_cos_theta_o [i];
235
+ v_decay_L [i] = v_decay_R [i] = v_next_decay [i];
236
+ }
237
+ v_cos_theta_o_R [2 ] = v_next_cos_theta_o [6 ];
238
+ v_cos_theta_o_R [3 ] = v_next_cos_theta_o [7 ];
239
+ v_decay_R [2 ] = v_next_decay [6 ];
240
+ v_decay_R [3 ] = v_next_decay [7 ];
220
241
} else {
221
242
delta_cos_theta_o = (next_cos_theta_o - cos_theta_o) / frame_len;
222
243
delta_decay_L = (next_decay_L - decay_L) / frame_len;
223
244
delta_decay_R = (next_decay_R - decay_R) / frame_len;
224
- v_delta_cos_theta_o_L = v_next_cos_theta_o [std::slice (0 , 6 , 1 )];
225
- v_delta_cos_theta_o_L = v_delta_cos_theta_o_R = (v_delta_cos_theta_o_L - v_cos_theta_o_L) / frame_len;
245
+ for (int i = 0 ; i < 6 ; i++) {
246
+ v_delta_cos_theta_o_L [i] = v_delta_cos_theta_o_R [i] = (v_next_cos_theta_o [i] - v_cos_theta_o_L [i]) / frame_len;
247
+ v_delta_decay_L [i] = v_delta_decay_R [i] = (v_next_decay [i] - v_decay_L [i]) / frame_len;
248
+ }
226
249
v_delta_cos_theta_o_R [2 ] = (v_next_cos_theta_o [6 ] - v_cos_theta_o_R [2 ]) / frame_len;
227
250
v_delta_cos_theta_o_R [3 ] = (v_next_cos_theta_o [7 ] - v_cos_theta_o_R [3 ]) / frame_len;
228
- v_delta_decay_L = v_next_decay [std::slice (0 , 6 , 1 )];
229
- v_delta_decay_L = v_delta_decay_R = (v_delta_decay_L - v_decay_L) / frame_len;
230
251
v_delta_decay_R [2 ] = (v_next_decay [6 ] -v_decay_R [2 ]) / frame_len;
231
252
v_delta_decay_R [3 ] = (v_next_decay [7 ] -v_decay_R [3 ]) / frame_len;
232
253
}
@@ -240,7 +261,7 @@ void SPDSPparam:: nrt_param_update (struct GUI_param &gp, IParameterChanges* out
240
261
inv_cT = sampleRate / gp.c ;
241
262
v_limit = max_speed * frame_len / inv_cT;
242
263
a_r = gp.a / 1'000.0 ; // [mm] to [m]
243
- a_2 = pow ( a_r, 2.0 ) ;
264
+ a_2 = a_r * a_r ;
244
265
245
266
// parameters range check
246
267
if (gp.r_x >= max_side_len / 2.0 + min_dist)
@@ -288,10 +309,12 @@ void SPDSPparam:: param_smoothing ()
288
309
cos_theta_o += delta_cos_theta_o;
289
310
decay_L += delta_decay_L;
290
311
decay_R += delta_decay_R;
291
- v_cos_theta_o_L += v_delta_cos_theta_o_L;
292
- v_cos_theta_o_R += v_delta_cos_theta_o_R;
293
- v_decay_L += v_delta_decay_L;
294
- v_decay_R += v_delta_decay_R;
312
+ for (int i = 0 ; i < 6 ; i++) {
313
+ v_cos_theta_o_L [i] += v_delta_cos_theta_o_L [i];
314
+ v_cos_theta_o_R [i] += v_delta_cos_theta_o_R [i];
315
+ v_decay_L [i] += v_delta_decay_L [i];
316
+ v_decay_R [i] += v_delta_decay_R [i];
317
+ }
295
318
}
296
319
}
297
320
0 commit comments