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