1 /**********************************************************************
2
3 FILENAME: uiuc_3Dinterpolation.cpp
4
5 ----------------------------------------------------------------------
6
7 DESCRIPTION: A 3D interpolator. Does a linear interpolation between
8 two values that were found from using the 2D
9 interpolator (3Dinterpolation()), or uses 3Dinterp_quick()
10 to perform a 3D linear interpolation on "nice" data
11
12 ----------------------------------------------------------------------
13
14 STATUS: alpha version
15
16 ----------------------------------------------------------------------
17
18 REFERENCES:
19
20 ----------------------------------------------------------------------
21
22 HISTORY: 11/07/2001 initial release
23 02/18/2002 (RD) Created uiuc_3Dinterp_quick() to take
24 advantage of the "nice" format of the
25 nonlinear Twin Otter data. Performs a
26 quicker 3D interpolation. Modified
27 uiuc_3Dinterpolation() to handle new input
28 form of the data.
29 ----------------------------------------------------------------------
30
31 AUTHOR(S): Robert Deters <rdeters@uiuc.edu>
32
33 ----------------------------------------------------------------------
34
35 VARIABLES:
36
37 ----------------------------------------------------------------------
38
39 INPUTS:
40
41 ----------------------------------------------------------------------
42
43 OUTPUTS: interpI
44
45 ----------------------------------------------------------------------
46
47 CALLED BY: uiuc_coef_drag
48 uiuc_coef_lift
49 uiuc_coef_pitch
50 uiuc_coef_roll
51 uiuc_coef_sideforce
52 uiuc_coef_yaw
53
54 ----------------------------------------------------------------------
55
56 CALLS TO: 2Dinterpolation
57
58 ----------------------------------------------------------------------
59
60 COPYRIGHT: (C) 2001 by Michael Selig
61
62 This program is free software; you can redistribute it and/or
63 modify it under the terms of the GNU General Public License
64 as published by the Free Software Foundation.
65
66 This program is distributed in the hope that it will be useful,
67 but WITHOUT ANY WARRANTY; without even the implied warranty of
68 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
69 GNU General Public License for more details.
70
71 You should have received a copy of the GNU General Public License
72 along with this program; if not, write to the Free Software
73 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
74
75 **********************************************************************/
76 #include <simgear/compiler.h> // MSVC: to disable C4244 d to f warning
77
78 #include "uiuc_3Dinterpolation.h"
79
uiuc_1DtoSingle(int temp1Darray[30],int filenumber,int & single_value)80 void uiuc_1DtoSingle( int temp1Darray[30],
81 int filenumber,
82 int &single_value)
83 {
84 single_value = temp1Darray[filenumber];
85 }
86
uiuc_2Dto1D(double temp2Darray[30][100],int filenumber,double array1D[100])87 void uiuc_2Dto1D( double temp2Darray[30][100],
88 int filenumber,
89 double array1D[100])
90 {
91 int count1;
92
93 for (count1=0; count1<=99; count1++)
94 {
95 array1D[count1] = temp2Darray[filenumber][count1];
96 }
97 }
98
uiuc_2Dto1D_int(int temp2Darray[30][100],int filenumber,int array1D[100])99 void uiuc_2Dto1D_int( int temp2Darray[30][100],
100 int filenumber,
101 int array1D[100])
102 {
103 int count1;
104
105 for (count1=0; count1<=99; count1++)
106 {
107 array1D[count1] = temp2Darray[filenumber][count1];
108 }
109 }
110
uiuc_3Dto2D(double temp3Darray[30][100][100],int filenumber,double array2D[100][100])111 void uiuc_3Dto2D( double temp3Darray[30][100][100],
112 int filenumber,
113 double array2D[100][100])
114 {
115 int count1, count2;
116
117 for (count1=0; count1<=99; count1++)
118 {
119 for (count2=0; count2<=99; count2++)
120 {
121 array2D[count1][count2] = temp3Darray[filenumber][count1][count2];
122 }
123 }
124 }
125
uiuc_3Dinterpolation(double third_Array[30],double full_xArray[30][100][100],double full_yArray[30][100],double full_zArray[30][100][100],int full_nxArray[30][100],int full_ny[30],int third_max,double third_bet,double x_value,double y_value)126 double uiuc_3Dinterpolation( double third_Array[30],
127 double full_xArray[30][100][100],
128 double full_yArray[30][100],
129 double full_zArray[30][100][100],
130 int full_nxArray[30][100],
131 int full_ny[30],
132 int third_max,
133 double third_bet,
134 double x_value,
135 double y_value)
136 {
137 double reduced_xArray[100][100], reduced_yArray[100];
138 double reduced_zArray[100][100];
139 int reduced_nxArray[100], reduced_ny;
140
141 double interpmin, interpmax, third_u, third_l;
142 double interpI;
143 int third_min;
144 int k=1;
145 bool third_same=false;
146
147 if (third_bet <= third_Array[1])
148 {
149 third_min = 1;
150 third_same = true;
151 }
152 else if (third_bet >= third_Array[third_max])
153 {
154 third_min = third_max;
155 third_same = true;
156 }
157 else
158 {
159 while (third_Array[k] <= third_bet)
160 {
161 k++;
162 }
163 third_max = k;
164 third_min = k-1;
165 }
166 if (third_same)
167 {
168
169 uiuc_3Dto2D(full_xArray, third_min, reduced_xArray);
170 uiuc_2Dto1D(full_yArray, third_min, reduced_yArray);
171 uiuc_3Dto2D(full_zArray, third_min, reduced_zArray);
172 uiuc_2Dto1D_int(full_nxArray, third_min, reduced_nxArray);
173 uiuc_1DtoSingle(full_ny, third_min, reduced_ny);
174
175 interpI = uiuc_2Dinterpolation(reduced_xArray,
176 reduced_yArray,
177 reduced_zArray,
178 reduced_nxArray,
179 reduced_ny,
180 x_value,
181 y_value);
182 }
183 else
184 {
185 uiuc_3Dto2D(full_xArray, third_min, reduced_xArray);
186 uiuc_2Dto1D(full_yArray, third_min, reduced_yArray);
187 uiuc_3Dto2D(full_zArray, third_min, reduced_zArray);
188 uiuc_2Dto1D_int(full_nxArray, third_min, reduced_nxArray);
189 uiuc_1DtoSingle(full_ny, third_min, reduced_ny);
190
191 interpmin = uiuc_2Dinterpolation(reduced_xArray,
192 reduced_yArray,
193 reduced_zArray,
194 reduced_nxArray,
195 reduced_ny,
196 x_value,
197 y_value);
198
199 uiuc_3Dto2D(full_xArray, third_max, reduced_xArray);
200 uiuc_2Dto1D(full_yArray, third_max, reduced_yArray);
201 uiuc_3Dto2D(full_zArray, third_max, reduced_zArray);
202 uiuc_2Dto1D_int(full_nxArray, third_max, reduced_nxArray);
203 uiuc_1DtoSingle(full_ny, third_max, reduced_ny);
204
205 interpmax = uiuc_2Dinterpolation(reduced_xArray,
206 reduced_yArray,
207 reduced_zArray,
208 reduced_nxArray,
209 reduced_ny,
210 x_value,
211 y_value);
212
213 third_u = third_Array[third_max];
214 third_l = third_Array[third_min];
215
216 interpI=interpmax - (third_u-third_bet)*(interpmax-interpmin)/(third_u-third_l);
217 }
218
219 return interpI;
220
221 }
222
223
uiuc_3Dinterp_quick(double z[30],double x[100],double y[100],double fxyz[30][100][100],int xmax,int ymax,int zmax,double zp,double xp,double yp)224 double uiuc_3Dinterp_quick( double z[30],
225 double x[100],
226 double y[100],
227 double fxyz[30][100][100],
228 int xmax,
229 int ymax,
230 int zmax,
231 double zp,
232 double xp,
233 double yp)
234 {
235
236 int xnuml, xnumu, ynuml, ynumu, znuml, znumu;
237 double xl, xu, yl, yu, zl, zu;
238 double ptxl, ptxu, ptyl, ptyu, ptylxl, ptylxu, ptyuxl, ptyuxu;
239 double ptzl, ptzu, ptzlxl, ptzlxu, ptzuxl, ptzuxu;
240 double ptzlyl, ptzlyu, ptzuyl, ptzuyu;
241 double ptzlylxl, ptzlylxu, ptzlyuxl, ptzlyuxu;
242 double ptzuylxl, ptzuylxu, ptzuyuxl, ptzuyuxu, data_point;
243
244 int i=1;
245 int j=1;
246 int k=1;
247
248 bool xsame=false;
249 bool ysame=false;
250 bool zsame=false;
251
252
253 // Find the z's
254 if (zp <= z[1])
255 {
256 znuml=1;
257 zsame=true;
258 }
259 else if (zp >= z[zmax])
260 {
261 znuml=zmax;
262 zsame=true;
263 }
264 else
265 {
266 while (z[k] <= zp)
267 k++;
268 zu=z[k];
269 zl=z[k-1];
270 znumu=k;
271 znuml=k-1;
272 }
273
274 // Find the y's
275 if (yp <= y[1])
276 {
277 ynuml=1;
278 ysame=true;
279 }
280 else if (yp >= y[ymax])
281 {
282 ynuml=ymax;
283 ysame=true;
284 }
285 else
286 {
287 while (y[j] <= yp)
288 j++;
289 yu=y[j];
290 yl=y[j-1];
291 ynumu=j;
292 ynuml=j-1;
293 }
294
295
296 // Find the x's
297 if (xp <= x[1])
298 {
299 xnuml=1;
300 xsame=true;
301 }
302 else if (xp >= x[xmax])
303 {
304 xnuml=xmax;
305 xsame=true;
306 }
307 else
308 {
309 while (x[i] <= xp)
310 i++;
311 xu=x[i];
312 xl=x[i-1];
313 xnumu=i;
314 xnuml=i-1;
315 }
316
317 if (zsame)
318 {
319 if (ysame && xsame)
320 {
321 data_point = fxyz[znuml][ynuml][xnuml];
322 }
323 else if (ysame)
324 {
325 ptxl = fxyz[znuml][ynuml][xnuml];
326 ptxu = fxyz[znuml][ynuml][xnumu];
327 data_point = ptxu - (xu-xp)*(ptxu-ptxl)/(xu-xl);
328 }
329 else if (xsame)
330 {
331 ptyl = fxyz[znuml][ynuml][xnuml];
332 ptyu = fxyz[znuml][ynumu][xnuml];
333 data_point = ptyu - (yu-yp)*(ptyu-ptyl)/(yu-yl);
334 }
335 else
336 {
337 ptylxl = fxyz[znuml][ynuml][xnuml];
338 ptylxu = fxyz[znuml][ynuml][xnumu];
339 ptyuxl = fxyz[znuml][ynumu][xnuml];
340 ptyuxu = fxyz[znuml][ynumu][xnumu];
341 ptyl = ptylxu - (xu-xp)*(ptylxu-ptylxl)/(xu-xl);
342 ptyu = ptyuxu - (xu-xp)*(ptyuxu-ptyuxl)/(xu-xl);
343 data_point = ptyu - (yu-yp)*(ptyu-ptyl)/(yu-yl);
344 }
345 }
346 else
347 {
348 if (ysame && xsame)
349 {
350 ptzl = fxyz[znuml][ynuml][xnuml];
351 ptzu = fxyz[znumu][ynuml][xnuml];
352 data_point = ptzu - (zu-zp)*(ptzu-ptzl)/(zu-zl);
353 }
354 else if (ysame)
355 {
356 ptzlxl = fxyz[znuml][ynuml][xnuml];
357 ptzlxu = fxyz[znuml][ynuml][xnumu];
358 ptzuxl = fxyz[znumu][ynuml][xnuml];
359 ptzuxu = fxyz[znumu][ynuml][xnumu];
360 ptzl = ptzlxu - (xu-xp)*(ptzlxu-ptzlxl)/(xu-xl);
361 ptzu = ptzuxu - (xu-xp)*(ptzuxu-ptzuxl)/(xu-xl);
362 data_point = ptzu - (zu-zp)*(ptzu-ptzl)/(zu-zl);
363 }
364 else if (xsame)
365 {
366 ptzlyl = fxyz[znuml][ynuml][xnuml];
367 ptzlyu = fxyz[znuml][ynumu][xnuml];
368 ptzuyl = fxyz[znumu][ynuml][xnuml];
369 ptzuyu = fxyz[znumu][ynumu][xnuml];
370 ptzl = ptzlyu - (yu-yp)*(ptzlyu-ptzlyl)/(yu-yl);
371 ptzu = ptzuyu - (yu-yp)*(ptzuyu-ptzuyl)/(yu-yl);
372 data_point = ptzu - (zu-zp)*(ptzu-ptzl)/(zu-zl);
373 }
374 else
375 {
376 ptzlylxl = fxyz[znuml][ynuml][xnuml];
377 ptzlylxu = fxyz[znuml][ynuml][xnumu];
378 ptzlyuxl = fxyz[znuml][ynumu][xnuml];
379 ptzlyuxu = fxyz[znuml][ynumu][xnumu];
380 ptzuylxl = fxyz[znumu][ynuml][xnuml];
381 ptzuylxu = fxyz[znumu][ynuml][xnumu];
382 ptzuyuxl = fxyz[znumu][ynumu][xnuml];
383 ptzuyuxu = fxyz[znumu][ynumu][xnumu];
384 ptzlyl = ptzlylxu - (xu-xp)*(ptzlylxu-ptzlylxl)/(xu-xl);
385 ptzlyu = ptzlyuxu - (xu-xp)*(ptzlyuxu-ptzlyuxl)/(xu-xl);
386 ptzuyl = ptzuylxu - (xu-xp)*(ptzuylxu-ptzuylxl)/(xu-xl);
387 ptzuyu = ptzuyuxu - (xu-xp)*(ptzuyuxu-ptzuyuxl)/(xu-xl);
388 ptzl = ptzlyu - (yu-yp)*(ptzlyu-ptzlyl)/(yu-yl);
389 ptzu = ptzuyu - (yu-yp)*(ptzuyu-ptzuyl)/(yu-yl);
390 data_point = ptzu - (zu-zp)*(ptzu-ptzl)/(zu-zl);
391 }
392
393 }
394
395
396 return data_point;
397 }
398