src/FDM/UIUCModel/uiuc_3Dinterpolation.cpp

   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., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
  74  USA or view http://www.gnu.org/copyleft/gpl.html.
  75
  76 **********************************************************************/
  77 #include <simgear/compiler.h>    // MSVC: to disable C4244 d to f warning
  78
  79 #include "uiuc_3Dinterpolation.h"
  80
  81 void uiuc_1DtoSingle( int temp1Darray[30],
  82                       int filenumber,
  83                       int &single_value)
  84 {
  85   single_value = temp1Darray[filenumber];
  86 }
  87
  88 void uiuc_2Dto1D( double temp2Darray[30][100],
  89                   int filenumber,
  90                   double array1D[100])
  91 {
  92   int count1;
  93
  94   for (count1=0; count1<=99; count1++)
  95     {
  96       array1D[count1] = temp2Darray[filenumber][count1];
  97     }
  98 }
  99
 100 void uiuc_2Dto1D_int( int temp2Darray[30][100],
 101                       int filenumber,
 102                       int array1D[100])
 103 {
 104   int count1;
 105
 106   for (count1=0; count1<=99; count1++)
 107     {
 108       array1D[count1] = temp2Darray[filenumber][count1];
 109     }
 110 }
 111
 112 void uiuc_3Dto2D( double temp3Darray[30][100][100],
 113                   int filenumber,
 114                   double array2D[100][100])
 115 {
 116   int count1, count2;
 117
 118   for (count1=0; count1<=99; count1++)
 119     {
 120       for (count2=0; count2<=99; count2++)
 121         {
 122           array2D[count1][count2] = temp3Darray[filenumber][count1][count2];
 123         }
 124     }
 125 }
 126
 127 double uiuc_3Dinterpolation( double third_Array[30],
 128                              double full_xArray[30][100][100],
 129                              double full_yArray[30][100],
 130                              double full_zArray[30][100][100],
 131                              int full_nxArray[30][100],
 132                              int full_ny[30],
 133                              int third_max,
 134                              double third_bet,
 135                              double x_value,
 136                              double y_value)
 137 {
 138   double reduced_xArray[100][100], reduced_yArray[100];
 139   double reduced_zArray[100][100];
 140   int reduced_nxArray[100], reduced_ny;
 141
 142   double interpmin, interpmax, third_u, third_l;
 143   double interpI;
 144   int third_min;
 145   int k=1;
 146   bool third_same=false;
 147
 148   if (third_bet <= third_Array[1])
 149     {
 150       third_min = 1;
 151       third_same = true;
 152     }
 153   else if (third_bet >= third_Array[third_max])
 154     {
 155       third_min = third_max;
 156       third_same = true;
 157     }
 158   else
 159     {
 160       while (third_Array[k] <= third_bet)
 161         {
 162           k++;
 163         }
 164       third_max = k;
 165       third_min = k-1;
 166     }
 167   if (third_same)
 168     {
 169
 170       uiuc_3Dto2D(full_xArray, third_min, reduced_xArray);
 171       uiuc_2Dto1D(full_yArray, third_min, reduced_yArray);
 172       uiuc_3Dto2D(full_zArray, third_min, reduced_zArray);
 173       uiuc_2Dto1D_int(full_nxArray, third_min, reduced_nxArray);
 174       uiuc_1DtoSingle(full_ny, third_min, reduced_ny);
 175
 176       interpI = uiuc_2Dinterpolation(reduced_xArray,
 177                                      reduced_yArray,
 178                                      reduced_zArray,
 179                                      reduced_nxArray,
 180                                      reduced_ny,
 181                                      x_value,
 182                                      y_value);
 183     }
 184   else
 185     {
 186       uiuc_3Dto2D(full_xArray, third_min, reduced_xArray);
 187       uiuc_2Dto1D(full_yArray, third_min, reduced_yArray);
 188       uiuc_3Dto2D(full_zArray, third_min, reduced_zArray);
 189       uiuc_2Dto1D_int(full_nxArray, third_min, reduced_nxArray);
 190       uiuc_1DtoSingle(full_ny, third_min, reduced_ny);
 191
 192       interpmin = uiuc_2Dinterpolation(reduced_xArray,
 193                                      reduced_yArray,
 194                                      reduced_zArray,
 195                                      reduced_nxArray,
 196                                      reduced_ny,
 197                                      x_value,
 198                                      y_value);
 199
 200       uiuc_3Dto2D(full_xArray, third_max, reduced_xArray);
 201       uiuc_2Dto1D(full_yArray, third_max, reduced_yArray);
 202       uiuc_3Dto2D(full_zArray, third_max, reduced_zArray);
 203       uiuc_2Dto1D_int(full_nxArray, third_max, reduced_nxArray);
 204       uiuc_1DtoSingle(full_ny, third_max, reduced_ny);
 205
 206       interpmax = uiuc_2Dinterpolation(reduced_xArray,
 207                                      reduced_yArray,
 208                                      reduced_zArray,
 209                                      reduced_nxArray,
 210                                      reduced_ny,
 211                                      x_value,
 212                                      y_value);
 213
 214       third_u = third_Array[third_max];
 215       third_l = third_Array[third_min];
 216
 217       interpI=interpmax - (third_u-third_bet)*(interpmax-interpmin)/(third_u-third_l);
 218     }
 219
 220   return interpI;
 221
 222 }
 223
 224
 225 double uiuc_3Dinterp_quick( double z[30],
 226                             double x[100],
 227                             double y[100],
 228                             double fxyz[30][100][100],
 229                             int xmax,
 230                             int ymax,
 231                             int zmax,
 232                             double zp,
 233                             double xp,
 234                             double yp)
 235 {
 236
 237   int xnuml, xnumu, ynuml, ynumu, znuml, znumu;
 238   double xl, xu, yl, yu, zl, zu;
 239   double ptxl, ptxu, ptyl, ptyu, ptylxl, ptylxu, ptyuxl, ptyuxu;
 240   double ptzl, ptzu, ptzlxl, ptzlxu, ptzuxl, ptzuxu;
 241   double ptzlyl, ptzlyu, ptzuyl, ptzuyu;
 242   double ptzlylxl, ptzlylxu, ptzlyuxl, ptzlyuxu;
 243   double ptzuylxl, ptzuylxu, ptzuyuxl, ptzuyuxu, data_point;
 244
 245   int i=1;
 246   int j=1;
 247   int k=1;
 248
 249   bool xsame=false;
 250   bool ysame=false;
 251   bool zsame=false;
 252
 253
 254   // Find the z's
 255   if (zp <= z[1])
 256     {
 257       znuml=1;
 258       zsame=true;
 259     }
 260   else if (zp >= z[zmax])
 261     {
 262       znuml=zmax;
 263       zsame=true;
 264     }
 265   else
 266     {
 267       while (z[k] <= zp)
 268         k++;
 269       zu=z[k];
 270       zl=z[k-1];
 271       znumu=k;
 272       znuml=k-1;
 273     }
 274
 275   // Find the y's
 276   if (yp <= y[1])
 277     {
 278       ynuml=1;
 279       ysame=true;
 280     }
 281   else if (yp >= y[ymax])
 282     {
 283       ynuml=ymax;
 284       ysame=true;
 285     }
 286   else
 287     {
 288       while (y[j] <= yp)
 289         j++;
 290       yu=y[j];
 291       yl=y[j-1];
 292       ynumu=j;
 293       ynuml=j-1;
 294     }
 295
 296
 297   // Find the x's
 298   if (xp <= x[1])
 299     {
 300       xnuml=1;
 301       xsame=true;
 302     }
 303   else if (xp >= x[xmax])
 304     {
 305       xnuml=xmax;
 306       xsame=true;
 307     }
 308   else
 309     {
 310       while (x[i] <= xp)
 311         i++;
 312       xu=x[i];
 313       xl=x[i-1];
 314       xnumu=i;
 315       xnuml=i-1;
 316     }
 317
 318   if (zsame)
 319     {
 320       if (ysame && xsame)
 321         {
 322           data_point = fxyz[znuml][ynuml][xnuml];
 323         }
 324       else if (ysame)
 325         {
 326           ptxl = fxyz[znuml][ynuml][xnuml];
 327           ptxu = fxyz[znuml][ynuml][xnumu];
 328           data_point = ptxu - (xu-xp)*(ptxu-ptxl)/(xu-xl);
 329         }
 330       else if (xsame)
 331         {
 332           ptyl = fxyz[znuml][ynuml][xnuml];
 333           ptyu = fxyz[znuml][ynumu][xnuml];
 334           data_point = ptyu - (yu-yp)*(ptyu-ptyl)/(yu-yl);
 335         }
 336       else
 337         {
 338           ptylxl = fxyz[znuml][ynuml][xnuml];
 339           ptylxu = fxyz[znuml][ynuml][xnumu];
 340           ptyuxl = fxyz[znuml][ynumu][xnuml];
 341           ptyuxu = fxyz[znuml][ynumu][xnumu];
 342           ptyl = ptylxu - (xu-xp)*(ptylxu-ptylxl)/(xu-xl);
 343           ptyu = ptyuxu - (xu-xp)*(ptyuxu-ptyuxl)/(xu-xl);
 344           data_point = ptyu - (yu-yp)*(ptyu-ptyl)/(yu-yl);
 345         }
 346     }
 347   else
 348     {
 349       if (ysame && xsame)
 350         {
 351           ptzl = fxyz[znuml][ynuml][xnuml];
 352           ptzu = fxyz[znumu][ynuml][xnuml];
 353           data_point = ptzu - (zu-zp)*(ptzu-ptzl)/(zu-zl);
 354         }
 355       else if (ysame)
 356         {
 357           ptzlxl = fxyz[znuml][ynuml][xnuml];
 358           ptzlxu = fxyz[znuml][ynuml][xnumu];
 359           ptzuxl = fxyz[znumu][ynuml][xnuml];
 360           ptzuxu = fxyz[znumu][ynuml][xnumu];
 361           ptzl = ptzlxu - (xu-xp)*(ptzlxu-ptzlxl)/(xu-xl);
 362           ptzu = ptzuxu - (xu-xp)*(ptzuxu-ptzuxl)/(xu-xl);
 363           data_point = ptzu - (zu-zp)*(ptzu-ptzl)/(zu-zl);
 364         }
 365       else if (xsame)
 366         {
 367           ptzlyl = fxyz[znuml][ynuml][xnuml];
 368           ptzlyu = fxyz[znuml][ynumu][xnuml];
 369           ptzuyl = fxyz[znumu][ynuml][xnuml];
 370           ptzuyu = fxyz[znumu][ynumu][xnuml];
 371           ptzl = ptzlyu - (yu-yp)*(ptzlyu-ptzlyl)/(yu-yl);
 372           ptzu = ptzuyu - (yu-yp)*(ptzuyu-ptzuyl)/(yu-yl);
 373           data_point = ptzu - (zu-zp)*(ptzu-ptzl)/(zu-zl);
 374         }
 375       else
 376         {
 377           ptzlylxl = fxyz[znuml][ynuml][xnuml];
 378           ptzlylxu = fxyz[znuml][ynuml][xnumu];
 379           ptzlyuxl = fxyz[znuml][ynumu][xnuml];
 380           ptzlyuxu = fxyz[znuml][ynumu][xnumu];
 381           ptzuylxl = fxyz[znumu][ynuml][xnuml];
 382           ptzuylxu = fxyz[znumu][ynuml][xnumu];
 383           ptzuyuxl = fxyz[znumu][ynumu][xnuml];
 384           ptzuyuxu = fxyz[znumu][ynumu][xnumu];
 385           ptzlyl = ptzlylxu - (xu-xp)*(ptzlylxu-ptzlylxl)/(xu-xl);
 386           ptzlyu = ptzlyuxu - (xu-xp)*(ptzlyuxu-ptzlyuxl)/(xu-xl);
 387           ptzuyl = ptzuylxu - (xu-xp)*(ptzuylxu-ptzuylxl)/(xu-xl);
 388           ptzuyu = ptzuyuxu - (xu-xp)*(ptzuyuxu-ptzuyuxl)/(xu-xl);
 389           ptzl = ptzlyu - (yu-yp)*(ptzlyu-ptzlyl)/(yu-yl);
 390           ptzu = ptzuyu - (yu-yp)*(ptzuyu-ptzuyl)/(yu-yl);
 391           data_point = ptzu - (zu-zp)*(ptzu-ptzl)/(zu-zl);
 392         }
 393
 394     }
 395
 396
 397   return data_point;
 398 }