src/FDM/JSBSim/math/FGFunction.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3 Module: FGFunction.cpp
   4 Author: Jon Berndt
   5 Date started: 8/25/2004
   6 Purpose: Stores various parameter types for functions
   7
   8  ------------- Copyright (C) 2004  Jon S. Berndt (jon@jsbsim.org) -------------
   9
  10  This program is free software; you can redistribute it and/or modify it under
  11  the terms of the GNU Lesser General Public License as published by the Free Software
  12  Foundation; either version 2 of the License, or (at your option) any later
  13  version.
  14
  15  This program is distributed in the hope that it will be useful, but WITHOUT
  16  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  17  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  18  details.
  19
  20  You should have received a copy of the GNU Lesser General Public License along with
  21  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  22  Place - Suite 330, Boston, MA  02111-1307, USA.
  23
  24  Further information about the GNU Lesser General Public License can also be found on
  25  the world wide web at http://www.gnu.org.
  26
  27 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  28 INCLUDES
  29 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  30
  31 #include <sstream>
  32 #include <iomanip>
  33 #include <cstdlib>
  34 #include <cmath>
  35 #include "FGFunction.h"
  36 #include "FGTable.h"
  37 #include "FGPropertyValue.h"
  38 #include "FGRealValue.h"
  39 #include "input_output/FGXMLElement.h"
  40 #include "input_output/FGPropertyManager.h"
  41
  42 using namespace std;
  43
  44 namespace JSBSim {
  45
  46 static const char *IdSrc = "$Id: FGFunction.cpp,v 1.42 2011/09/07 02:36:04 jberndt Exp $";
  47 static const char *IdHdr = ID_FUNCTION;
  48
  49 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  50 CLASS IMPLEMENTATION
  51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  52
  53 const std::string FGFunction::property_string = "property";
  54 const std::string FGFunction::value_string = "value";
  55 const std::string FGFunction::table_string = "table";
  56 const std::string FGFunction::p_string = "p";
  57 const std::string FGFunction::v_string = "v";
  58 const std::string FGFunction::t_string = "t";
  59
  60 const std::string FGFunction::function_string = "function";
  61 const std::string FGFunction::description_string = "description";
  62 const std::string FGFunction::sum_string = "sum";
  63 const std::string FGFunction::difference_string = "difference";
  64 const std::string FGFunction::product_string = "product";
  65 const std::string FGFunction::quotient_string = "quotient";
  66 const std::string FGFunction::pow_string = "pow";
  67 const std::string FGFunction::exp_string = "exp";
  68 const std::string FGFunction::log2_string = "log2";
  69 const std::string FGFunction::ln_string = "ln";
  70 const std::string FGFunction::log10_string = "log10";
  71 const std::string FGFunction::abs_string = "abs";
  72 const std::string FGFunction::sign_string = "sign";
  73 const std::string FGFunction::sin_string = "sin";
  74 const std::string FGFunction::cos_string = "cos";
  75 const std::string FGFunction::tan_string = "tan";
  76 const std::string FGFunction::asin_string = "asin";
  77 const std::string FGFunction::acos_string = "acos";
  78 const std::string FGFunction::atan_string = "atan";
  79 const std::string FGFunction::atan2_string = "atan2";
  80 const std::string FGFunction::min_string = "min";
  81 const std::string FGFunction::max_string = "max";
  82 const std::string FGFunction::avg_string = "avg";
  83 const std::string FGFunction::fraction_string = "fraction";
  84 const std::string FGFunction::mod_string = "mod";
  85 const std::string FGFunction::random_string = "random";
  86 const std::string FGFunction::integer_string = "integer";
  87 const std::string FGFunction::rotation_alpha_local_string = "rotation_alpha_local";
  88 const std::string FGFunction::rotation_beta_local_string = "rotation_beta_local";
  89 const std::string FGFunction::rotation_gamma_local_string = "rotation_gamma_local";
  90 const std::string FGFunction::rotation_bf_to_wf_string = "rotation_bf_to_wf";
  91 const std::string FGFunction::rotation_wf_to_bf_string = "rotation_wf_to_bf";
  92
  93 const std::string FGFunction::lessthan_string = "lt";
  94 const std::string FGFunction::lessequal_string = "le";
  95 const std::string FGFunction::greatthan_string = "gt";
  96 const std::string FGFunction::greatequal_string = "ge";
  97 const std::string FGFunction::equal_string = "eq";
  98 const std::string FGFunction::notequal_string = "nq";
  99 const std::string FGFunction::and_string = "and";
 100 const std::string FGFunction::or_string = "or";
 101 const std::string FGFunction::not_string = "not";
 102 const std::string FGFunction::ifthen_string = "ifthen";
 103 const std::string FGFunction::switch_string = "switch";
 104
 105 FGFunction::FGFunction(FGPropertyManager* propMan, Element* el, const string& prefix)
 106                                       : PropertyManager(propMan), Prefix(prefix)
 107 {
 108   Element* element;
 109   string operation, property_name;
 110   cached = false;
 111   cachedValue = -HUGE_VAL;
 112   invlog2val = 1.0/log10(2.0);
 113
 114   Name = el->GetAttributeValue("name");
 115   operation = el->GetName();
 116
 117   if (operation == function_string) {
 118     Type = eTopLevel;
 119   } else if (operation == product_string) {
 120     Type = eProduct;
 121   } else if (operation == difference_string) {
 122     Type = eDifference;
 123   } else if (operation == sum_string) {
 124     Type = eSum;
 125   } else if (operation == quotient_string) {
 126     Type = eQuotient;
 127   } else if (operation == pow_string) {
 128     Type = ePow;
 129   } else if (operation == log2_string) {
 130     Type = eLog2;
 131   } else if (operation == ln_string) {
 132     Type = eLn;
 133   } else if (operation == log10_string) {
 134     Type = eLog10;
 135   } else if (operation == abs_string) {
 136     Type = eAbs;
 137   } else if (operation == sign_string) {
 138     Type = eSign;
 139   } else if (operation == sin_string) {
 140     Type = eSin;
 141   } else if (operation == exp_string) {
 142     Type = eExp;
 143   } else if (operation == cos_string) {
 144     Type = eCos;
 145   } else if (operation == tan_string) {
 146     Type = eTan;
 147   } else if (operation == asin_string) {
 148     Type = eASin;
 149   } else if (operation == acos_string) {
 150     Type = eACos;
 151   } else if (operation == atan_string) {
 152     Type = eATan;
 153   } else if (operation == atan2_string) {
 154     Type = eATan2;
 155   } else if (operation == min_string) {
 156     Type = eMin;
 157   } else if (operation == max_string) {
 158     Type = eMax;
 159   } else if (operation == avg_string) {
 160     Type = eAvg;
 161   } else if (operation == fraction_string) {
 162     Type = eFrac;
 163   } else if (operation == integer_string) {
 164     Type = eInteger;
 165   } else if (operation == mod_string) {
 166     Type = eMod;
 167   } else if (operation == random_string) {
 168     Type = eRandom;
 169   } else if (operation == rotation_alpha_local_string) {
 170     Type = eRotation_alpha_local;
 171   } else if (operation == rotation_beta_local_string) {
 172     Type = eRotation_beta_local;
 173   } else if (operation == rotation_gamma_local_string) {
 174     Type = eRotation_gamma_local;
 175   } else if (operation == rotation_bf_to_wf_string) {
 176     Type = eRotation_bf_to_wf;
 177   } else if (operation == rotation_wf_to_bf_string) {
 178     Type = eRotation_wf_to_bf;
 179   } else if (operation == lessthan_string) {
 180     Type = eLT;
 181   } else if (operation == lessequal_string) {
 182     Type = eLE;
 183   } else if (operation == greatthan_string) {
 184     Type = eGT;
 185   } else if (operation == greatequal_string) {
 186     Type = eGE;
 187   } else if (operation == equal_string) {
 188     Type = eEQ;
 189   } else if (operation == notequal_string) {
 190     Type = eNE;
 191   } else if (operation == and_string) {
 192     Type = eAND;
 193   } else if (operation == or_string) {
 194     Type = eOR;
 195   } else if (operation == not_string) {
 196     Type = eNOT;
 197   } else if (operation == ifthen_string) {
 198     Type = eIfThen;
 199   } else if (operation == switch_string) {
 200     Type = eSwitch;
 201   } else if (operation != description_string) {
 202     cerr << "Bad operation " << operation << " detected in configuration file" << endl;
 203   }
 204
 205   element = el->GetElement();
 206   if (!element) {
 207     cerr << fgred << highint << endl;
 208     cerr << "  No element was specified as an argument to the \"" << operation << "\" operation" << endl;
 209     cerr << "  This can happen when, for instance, a cos operation is specified and a " << endl;
 210     cerr << "  property name is given explicitly, but is not placed within a" << endl;
 211     cerr << "  <property></property> element tag pair." << endl;
 212     cerr << reset;
 213     exit(-2);
 214   }
 215
 216   while (element) {
 217     operation = element->GetName();
 218
 219     // data types
 220     if (operation == property_string || operation == p_string) {
 221       property_name = element->GetDataLine();
 222       if (property_name.find("#") != string::npos) {
 223         if (is_number(Prefix)) {
 224           property_name = replace(property_name,"#",Prefix);
 225         }
 226       }
 227       FGPropertyManager* newNode = 0L;
 228       if (PropertyManager->HasNode(property_name)) {
 229         newNode = PropertyManager->GetNode(property_name);
 230         Parameters.push_back(new FGPropertyValue( newNode ));
 231       } else {
 232         cerr << fgcyan << "Warning: The property " + property_name + " is initially undefined."
 233              << reset << endl;
 234         Parameters.push_back(new FGPropertyValue( property_name,
 235                                                   PropertyManager ));
 236       }
 237     } else if (operation == value_string || operation == v_string) {
 238       Parameters.push_back(new FGRealValue(element->GetDataAsNumber()));
 239     } else if (operation == table_string || operation == t_string) {
 240       Parameters.push_back(new FGTable(PropertyManager, element));
 241     // operations
 242     } else if (operation == product_string ||
 243                operation == difference_string ||
 244                operation == sum_string ||
 245                operation == quotient_string ||
 246                operation == pow_string ||
 247                operation == exp_string ||
 248                operation == log2_string ||
 249                operation == ln_string ||
 250                operation == log10_string ||
 251                operation == abs_string ||
 252                operation == sign_string ||
 253                operation == sin_string ||
 254                operation == cos_string ||
 255                operation == tan_string ||
 256                operation == asin_string ||
 257                operation == acos_string ||
 258                operation == atan_string ||
 259                operation == atan2_string ||
 260                operation == min_string ||
 261                operation == max_string ||
 262                operation == fraction_string ||
 263                operation == integer_string ||
 264                operation == mod_string ||
 265                operation == random_string ||
 266                operation == avg_string ||
 267                operation == rotation_alpha_local_string||
 268                operation == rotation_beta_local_string||
 269                operation == rotation_gamma_local_string||
 270                operation == rotation_bf_to_wf_string||
 271                operation == rotation_wf_to_bf_string ||
 272                operation == lessthan_string ||
 273                operation == lessequal_string ||
 274                operation == greatthan_string ||
 275                operation == greatequal_string ||
 276                operation == equal_string ||
 277                operation == notequal_string ||
 278                operation == and_string ||
 279                operation == or_string ||
 280                operation == not_string ||
 281                operation == ifthen_string ||
 282                operation == switch_string)
 283     {
 284       Parameters.push_back(new FGFunction(PropertyManager, element, Prefix));
 285     } else if (operation != description_string) {
 286       cerr << "Bad operation " << operation << " detected in configuration file" << endl;
 287     }
 288     element = el->GetNextElement();
 289   }
 290
 291   bind(); // Allow any function to save its value
 292
 293   Debug(0);
 294 }
 295
 296 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 297
 298 FGFunction::~FGFunction(void)
 299 {
 300   for (unsigned int i=0; i<Parameters.size(); i++) delete Parameters[i];
 301 }
 302
 303 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 304
 305 void FGFunction::cacheValue(bool cache)
 306 {
 307   cached = false; // Must set cached to false prior to calling GetValue(), else
 308                   // it will _never_ calculate the value;
 309   if (cache) {
 310     cachedValue = GetValue();
 311     cached = true;
 312   }
 313 }
 314
 315 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 316
 317 unsigned int FGFunction::GetBinary(double val) const
 318 {
 319   val = fabs(val);
 320   if (val < 1E-9) return 0;
 321   else if (val-1 < 1E-9) return 1;
 322   else {
 323     throw("Malformed conditional check in function definition.");
 324   }
 325 }
 326
 327 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 328
 329 double FGFunction::GetValue(void) const
 330 {
 331   unsigned int i;
 332   double scratch;
 333   double temp=0;
 334
 335   if (cached) return cachedValue;
 336
 337   temp = Parameters[0]->GetValue();
 338
 339   switch (Type) {
 340   case eTopLevel:
 341     break;
 342   case eProduct:
 343     for (i=1;i<Parameters.size();i++) {
 344       temp *= Parameters[i]->GetValue();
 345     }
 346     break;
 347   case eDifference:
 348     for (i=1;i<Parameters.size();i++) {
 349       temp -= Parameters[i]->GetValue();
 350     }
 351     break;
 352   case eSum:
 353     for (i=1;i<Parameters.size();i++) {
 354       temp += Parameters[i]->GetValue();
 355     }
 356     break;
 357   case eQuotient:
 358     if (Parameters[1]->GetValue() != 0.0)
 359       temp /= Parameters[1]->GetValue();
 360     else
 361       temp = HUGE_VAL;
 362     break;
 363   case ePow:
 364     temp = pow(temp,Parameters[1]->GetValue());
 365     break;
 366   case eExp:
 367     temp = exp(temp);
 368     break;
 369   case eLog2:
 370     if (temp > 0.00) temp = log10(temp)*invlog2val;
 371     else temp = -HUGE_VAL;
 372     break;
 373   case eLn:
 374     if (temp > 0.00) temp = log(temp);
 375     else temp = -HUGE_VAL;
 376     break;
 377   case eLog10:
 378     if (temp > 0.00) temp = log10(temp);
 379     else temp = -HUGE_VAL;
 380     break;
 381   case eAbs:
 382     temp = fabs(temp);
 383     break;
 384   case eSign:
 385     temp =  temp < 0 ? -1:1; // 0.0 counts as positive.
 386     break;
 387   case eSin:
 388     temp = sin(temp);
 389     break;
 390   case eCos:
 391     temp = cos(temp);
 392     break;
 393   case eTan:
 394     temp = tan(temp);
 395     break;
 396   case eACos:
 397     temp = acos(temp);
 398     break;
 399   case eASin:
 400     temp = asin(temp);
 401     break;
 402   case eATan:
 403     temp = atan(temp);
 404     break;
 405   case eATan2:
 406     temp = atan2(temp, Parameters[1]->GetValue());
 407     break;
 408   case eMod:
 409     temp = ((int)temp) % ((int) Parameters[1]->GetValue());
 410     break;
 411   case eMin:
 412     for (i=1;i<Parameters.size();i++) {
 413       if (Parameters[i]->GetValue() < temp) temp = Parameters[i]->GetValue();
 414     }
 415     break;
 416   case eMax:
 417     for (i=1;i<Parameters.size();i++) {
 418       if (Parameters[i]->GetValue() > temp) temp = Parameters[i]->GetValue();
 419     }
 420     break;
 421   case eAvg:
 422     for (i=1;i<Parameters.size();i++) {
 423       temp += Parameters[i]->GetValue();
 424     }
 425     temp /= Parameters.size();
 426     break;
 427   case eFrac:
 428     temp = modf(temp, &scratch);
 429     break;
 430   case eInteger:
 431     modf(temp, &scratch);
 432     temp = scratch;
 433     break;
 434   case eRandom:
 435     temp = GaussianRandomNumber();
 436     break;
 437   case eLT:
 438     temp = (temp < Parameters[1]->GetValue())?1:0;
 439     break;
 440   case eLE:
 441     temp = (temp <= Parameters[1]->GetValue())?1:0;
 442     break;
 443   case eGT:
 444     temp = (temp > Parameters[1]->GetValue())?1:0;
 445     break;
 446   case eGE:
 447     temp = (temp >= Parameters[1]->GetValue())?1:0;
 448     break;
 449   case eEQ:
 450     temp = (temp == Parameters[1]->GetValue())?1:0;
 451     break;
 452   case eNE:
 453     temp = (temp != Parameters[1]->GetValue())?1:0;
 454     break;
 455   case eAND:
 456     {
 457       bool flag = (GetBinary(temp) != 0u);
 458       for (i=1; i<Parameters.size() && flag; i++) {
 459         flag = (GetBinary(Parameters[i]->GetValue()) != 0);
 460       }
 461       temp = flag ? 1 : 0;
 462     }
 463     break;
 464   case eOR:
 465     {
 466       bool flag = (GetBinary(temp) != 0);
 467       for (i=1; i<Parameters.size() && !flag; i++) {
 468         flag = (GetBinary(Parameters[i]->GetValue()) != 0);
 469       }
 470       temp = flag ? 1 : 0;
 471     }
 472     break;
 473   case eNOT:
 474     temp = (GetBinary(temp) != 0) ? 0 : 1;
 475     break;
 476   case eIfThen:
 477     {
 478       i = Parameters.size();
 479       if (i != 3) {
 480         if (GetBinary(temp) == 1) {
 481           temp = Parameters[1]->GetValue();
 482         } else {
 483           temp = Parameters[2]->GetValue();
 484         }
 485       } else {
 486         throw("Malformed if/then function statement");
 487       }
 488     }
 489     break;
 490   case eSwitch:
 491     {
 492       unsigned int n = Parameters.size()-1;
 493       i = int(temp+0.5);
 494       if (i >= 0u && i < n) {
 495         temp = Parameters[i+1]->GetValue();
 496       } else {
 497         throw(string("The switch function index selected a value above the range of supplied values"
 498                      " - not enough values were supplied."));
 499       }
 500     }
 501     break;
 502   case eRotation_alpha_local:
 503     if (Parameters.size()==6) // calculates local angle of attack for skydiver body component
 504         //Euler angles from the intermediate body frame to the local body frame must be from a z-y-x axis rotation order
 505     {
 506         double alpha = Parameters[0]->GetValue()*degtorad;
 507         double beta = Parameters[1]->GetValue()*degtorad;
 508         double gamma = Parameters[2]->GetValue()*degtorad;
 509         double phi = Parameters[3]->GetValue()*degtorad;
 510         double theta = Parameters[4]->GetValue()*degtorad;
 511         double psi = Parameters[5]->GetValue()*degtorad;
 512         double cphi2 = cos(-phi/2), ctht2 = cos(-theta/2), cpsi2 = cos(-psi/2);
 513         double sphi2 = sin(-phi/2), stht2 = sin(-theta/2), spsi2 = sin(-psi/2);
 514         double calpha2 = cos(-alpha/2), salpha2 = sin(-alpha/2);
 515         double cbeta2 = cos(beta/2), sbeta2 = sin(beta/2);
 516         double cgamma2 = cos(-gamma/2), sgamma2 = sin(-gamma/2);
 517         //calculate local body frame to the intermediate body frame rotation quaternion
 518         double At = cphi2*ctht2*cpsi2 - sphi2*stht2*spsi2;
 519         double Ax = cphi2*stht2*spsi2 + sphi2*ctht2*cpsi2;
 520         double Ay = cphi2*stht2*cpsi2 - sphi2*ctht2*spsi2;
 521         double Az = cphi2*ctht2*spsi2 + sphi2*stht2*cpsi2;
 522         //calculate the intermediate body frame to global wind frame rotation quaternion
 523         double Bt = calpha2*cbeta2*cgamma2 - salpha2*sbeta2*sgamma2;
 524         double Bx = calpha2*cbeta2*sgamma2 + salpha2*sbeta2*cgamma2;
 525         double By = calpha2*sbeta2*sgamma2 + salpha2*cbeta2*cgamma2;
 526         double Bz = calpha2*sbeta2*cgamma2 - salpha2*cbeta2*sgamma2;
 527         //multiply quaternions
 528         double Ct = At*Bt - Ax*Bx - Ay*By - Az*Bz;
 529         double Cx = At*Bx + Ax*Bt + Ay*Bz - Az*By;
 530         double Cy = At*By - Ax*Bz + Ay*Bt + Az*Bx;
 531         double Cz = At*Bz + Ax*By - Ay*Bx + Az*Bt;
 532         //calculate alpha_local
 533         temp = -atan2(2*(Cy*Ct-Cx*Cz),(Ct*Ct+Cx*Cx-Cy*Cy-Cz*Cz));
 534         temp *= radtodeg;
 535     } else {
 536       temp = 1;
 537     }
 538     break;
 539   case eRotation_beta_local:
 540     if (Parameters.size()==6) // calculates local angle of sideslip for skydiver body component
 541         //Euler angles from the intermediate body frame to the local body frame must be from a z-y-x axis rotation order
 542     {
 543         double alpha = Parameters[0]->GetValue()*degtorad; //angle of attack of intermediate body frame
 544         double beta = Parameters[1]->GetValue()*degtorad;  //sideslip angle of intermediate body frame
 545         double gamma = Parameters[2]->GetValue()*degtorad; //roll angle of intermediate body frame
 546         double phi = Parameters[3]->GetValue()*degtorad;   //x-axis Euler angle from the intermediate body frame to the local body frame
 547         double theta = Parameters[4]->GetValue()*degtorad; //y-axis Euler angle from the intermediate body frame to the local body frame
 548         double psi = Parameters[5]->GetValue()*degtorad;   //z-axis Euler angle from the intermediate body frame to the local body frame
 549         double cphi2 = cos(-phi/2), ctht2 = cos(-theta/2), cpsi2 = cos(-psi/2);
 550         double sphi2 = sin(-phi/2), stht2 = sin(-theta/2), spsi2 = sin(-psi/2);
 551         double calpha2 = cos(-alpha/2), salpha2 = sin(-alpha/2);
 552         double cbeta2 = cos(beta/2), sbeta2 = sin(beta/2);
 553         double cgamma2 = cos(-gamma/2), sgamma2 = sin(-gamma/2);
 554         //calculate local body frame to the intermediate body frame rotation quaternion
 555         double At = cphi2*ctht2*cpsi2 - sphi2*stht2*spsi2;
 556         double Ax = cphi2*stht2*spsi2 + sphi2*ctht2*cpsi2;
 557         double Ay = cphi2*stht2*cpsi2 - sphi2*ctht2*spsi2;
 558         double Az = cphi2*ctht2*spsi2 + sphi2*stht2*cpsi2;
 559         //calculate the intermediate body frame to global wind frame rotation quaternion
 560         double Bt = calpha2*cbeta2*cgamma2 - salpha2*sbeta2*sgamma2;
 561         double Bx = calpha2*cbeta2*sgamma2 + salpha2*sbeta2*cgamma2;
 562         double By = calpha2*sbeta2*sgamma2 + salpha2*cbeta2*cgamma2;
 563         double Bz = calpha2*sbeta2*cgamma2 - salpha2*cbeta2*sgamma2;
 564         //multiply quaternions
 565         double Ct = At*Bt - Ax*Bx - Ay*By - Az*Bz;
 566         double Cx = At*Bx + Ax*Bt + Ay*Bz - Az*By;
 567         double Cy = At*By - Ax*Bz + Ay*Bt + Az*Bx;
 568         double Cz = At*Bz + Ax*By - Ay*Bx + Az*Bt;
 569         //calculate beta_local
 570         temp = asin(2*(Cx*Cy+Cz*Ct));
 571         temp *= radtodeg;
 572     }
 573     else //
 574     {temp = 1;}
 575     break;
 576   case eRotation_gamma_local:
 577     if (Parameters.size()==6) // calculates local angle of attack for skydiver body component
 578         //Euler angles from the intermediate body frame to the local body frame must be from a z-y-x axis rotation order
 579         {
 580         double alpha = Parameters[0]->GetValue()*degtorad; //angle of attack of intermediate body frame
 581         double beta = Parameters[1]->GetValue()*degtorad;  //sideslip angle of intermediate body frame
 582         double gamma = Parameters[2]->GetValue()*degtorad; //roll angle of intermediate body frame
 583         double phi = Parameters[3]->GetValue()*degtorad;   //x-axis Euler angle from the intermediate body frame to the local body frame
 584         double theta = Parameters[4]->GetValue()*degtorad; //y-axis Euler angle from the intermediate body frame to the local body frame
 585         double psi = Parameters[5]->GetValue()*degtorad;   //z-axis Euler angle from the intermediate body frame to the local body frame
 586         double cphi2 = cos(-phi/2), ctht2 = cos(-theta/2), cpsi2 = cos(-psi/2);
 587         double sphi2 = sin(-phi/2), stht2 = sin(-theta/2), spsi2 = sin(-psi/2);
 588         double calpha2 = cos(-alpha/2), salpha2 = sin(-alpha/2);
 589         double cbeta2 = cos(beta/2), sbeta2 = sin(beta/2);
 590         double cgamma2 = cos(-gamma/2), sgamma2 = sin(-gamma/2);
 591         //calculate local body frame to the intermediate body frame rotation quaternion
 592         double At = cphi2*ctht2*cpsi2 - sphi2*stht2*spsi2;
 593         double Ax = cphi2*stht2*spsi2 + sphi2*ctht2*cpsi2;
 594         double Ay = cphi2*stht2*cpsi2 - sphi2*ctht2*spsi2;
 595         double Az = cphi2*ctht2*spsi2 + sphi2*stht2*cpsi2;
 596         //calculate the intermediate body frame to global wind frame rotation quaternion
 597         double Bt = calpha2*cbeta2*cgamma2 - salpha2*sbeta2*sgamma2;
 598         double Bx = calpha2*cbeta2*sgamma2 + salpha2*sbeta2*cgamma2;
 599         double By = calpha2*sbeta2*sgamma2 + salpha2*cbeta2*cgamma2;
 600         double Bz = calpha2*sbeta2*cgamma2 - salpha2*cbeta2*sgamma2;
 601         //multiply quaternions
 602         double Ct = At*Bt - Ax*Bx - Ay*By - Az*Bz;
 603         double Cx = At*Bx + Ax*Bt + Ay*Bz - Az*By;
 604         double Cy = At*By - Ax*Bz + Ay*Bt + Az*Bx;
 605         double Cz = At*Bz + Ax*By - Ay*Bx + Az*Bt;
 606         //calculate local roll anlge
 607         temp = -atan2(2*(Cx*Ct-Cz*Cy),(Ct*Ct-Cx*Cx+Cy*Cy-Cz*Cz));
 608         temp *= radtodeg;
 609     }
 610     else //
 611     {temp = 1;}
 612     break;
 613   case eRotation_bf_to_wf:
 614     if (Parameters.size()==7) // transforms the input vector from a body frame to a wind frame.  The origin of the vector remains the same.
 615     {
 616         double rx = Parameters[0]->GetValue();             //x component of input vector
 617         double ry = Parameters[1]->GetValue();             //y component of input vector
 618         double rz = Parameters[2]->GetValue();             //z component of input vector
 619         double alpha = Parameters[3]->GetValue()*degtorad; //angle of attack of the body frame
 620         double beta = Parameters[4]->GetValue()*degtorad;  //sideslip angle of the body frame
 621         double gamma = Parameters[5]->GetValue()*degtorad; //roll angle of the body frame
 622         double index = Parameters[6]->GetValue();
 623         double calpha2 = cos(-alpha/2), salpha2 = sin(-alpha/2);
 624         double cbeta2 = cos(beta/2), sbeta2 = sin(beta/2);
 625         double cgamma2 = cos(-gamma/2), sgamma2 = sin(-gamma/2);
 626         //calculate the body frame to wind frame quaternion
 627         double qt = calpha2*cbeta2*cgamma2 - salpha2*sbeta2*sgamma2;
 628         double qx = calpha2*cbeta2*sgamma2 + salpha2*sbeta2*cgamma2;
 629         double qy = calpha2*sbeta2*sgamma2 + salpha2*cbeta2*cgamma2;
 630         double qz = calpha2*sbeta2*cgamma2 - salpha2*cbeta2*sgamma2;
 631         //calculate the quaternion conjugate
 632         double qstart = qt;
 633         double qstarx = -qx;
 634         double qstary = -qy;
 635         double qstarz = -qz;
 636         //multiply quaternions v*q
 637         double vqt = -rx*qx - ry*qy - rz*qz;
 638         double vqx =  rx*qt + ry*qz - rz*qy;
 639         double vqy = -rx*qz + ry*qt + rz*qx;
 640         double vqz =  rx*qy - ry*qx + rz*qt;
 641         //multiply quaternions qstar*vq
 642         double Cx = qstart*vqx + qstarx*vqt + qstary*vqz - qstarz*vqy;
 643         double Cy = qstart*vqy - qstarx*vqz + qstary*vqt + qstarz*vqx;
 644         double Cz = qstart*vqz + qstarx*vqy - qstary*vqx + qstarz*vqt;
 645
 646         if (index == 1)     temp = Cx;
 647         else if (index ==2) temp = Cy;
 648         else                temp = Cz;
 649     }
 650     else //
 651     {temp = 1;}
 652     break;
 653   case eRotation_wf_to_bf:
 654     if (Parameters.size()==7) // transforms the input vector from q wind frame to a body frame.  The origin of the vector remains the same.
 655     {
 656         double rx = Parameters[0]->GetValue();             //x component of input vector
 657         double ry = Parameters[1]->GetValue();             //y component of input vector
 658         double rz = Parameters[2]->GetValue();             //z component of input vector
 659         double alpha = Parameters[3]->GetValue()*degtorad; //angle of attack of the body frame
 660         double beta = Parameters[4]->GetValue()*degtorad;  //sideslip angle of the body frame
 661         double gamma = Parameters[5]->GetValue()*degtorad; //roll angle of the body frame
 662         double index = Parameters[6]->GetValue();
 663         double calpha2 = cos(alpha/2), salpha2 = sin(alpha/2);
 664         double cbeta2 = cos(-beta/2), sbeta2 = sin(-beta/2);
 665         double cgamma2 = cos(gamma/2), sgamma2 = sin(gamma/2);
 666         //calculate the wind frame to body frame quaternion
 667         double qt =  cgamma2*cbeta2*calpha2 + sgamma2*sbeta2*salpha2;
 668         double qx = -cgamma2*sbeta2*salpha2 + sgamma2*cbeta2*calpha2;
 669         double qy =  cgamma2*cbeta2*salpha2 - sgamma2*sbeta2*calpha2;
 670         double qz =  cgamma2*sbeta2*calpha2 + sgamma2*cbeta2*salpha2;
 671         //calculate the quaternion conjugate
 672         double qstart =  qt;
 673         double qstarx = -qx;
 674         double qstary = -qy;
 675         double qstarz = -qz;
 676         //multiply quaternions v*q
 677         double vqt = -rx*qx - ry*qy - rz*qz;
 678         double vqx =  rx*qt + ry*qz - rz*qy;
 679         double vqy = -rx*qz + ry*qt + rz*qx;
 680         double vqz =  rx*qy - ry*qx + rz*qt;
 681         //multiply quaternions qstar*vq
 682         double Cx = qstart*vqx + qstarx*vqt + qstary*vqz - qstarz*vqy;
 683         double Cy = qstart*vqy - qstarx*vqz + qstary*vqt + qstarz*vqx;
 684         double Cz = qstart*vqz + qstarx*vqy - qstary*vqx + qstarz*vqt;
 685
 686         if (index == 1)     temp = Cx;
 687         else if (index ==2) temp = Cy;
 688         else                temp = Cz;
 689     }
 690     else //
 691     {temp = 1;}
 692     break;
 693   default:
 694     cerr << "Unknown function operation type" << endl;
 695     break;
 696   }
 697
 698   return temp;
 699 }
 700
 701 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 702
 703 string FGFunction::GetValueAsString(void) const
 704 {
 705   ostringstream buffer;
 706
 707   buffer << setw(9) << setprecision(6) << GetValue();
 708   return buffer.str();
 709 }
 710
 711 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 712
 713 void FGFunction::bind(void)
 714 {
 715   if ( !Name.empty() ) {
 716     string tmp;
 717     if (Prefix.empty())
 718       tmp  = PropertyManager->mkPropertyName(Name, false);
 719     else {
 720       if (is_number(Prefix)) {
 721         if (Name.find("#") != string::npos) { // if "#" is found
 722           Name = replace(Name,"#",Prefix);
 723           tmp  = PropertyManager->mkPropertyName(Name, false);
 724         } else {
 725           cerr << "Malformed function name with number: " << Prefix
 726             << " and property name: " << Name
 727             << " but no \"#\" sign for substitution." << endl;
 728         }
 729       } else {
 730         tmp  = PropertyManager->mkPropertyName(Prefix + "/" + Name, false);
 731       }
 732     }
 733
 734     PropertyManager->Tie( tmp, this, &FGFunction::GetValue);
 735   }
 736 }
 737
 738 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 739 //    The bitmasked value choices are as follows:
 740 //    unset: In this case (the default) JSBSim would only print
 741 //       out the normally expected messages, essentially echoing
 742 //       the config files as they are read. If the environment
 743 //       variable is not set, debug_lvl is set to 1 internally
 744 //    0: This requests JSBSim not to output any messages
 745 //       whatsoever.
 746 //    1: This value explicity requests the normal JSBSim
 747 //       startup messages
 748 //    2: This value asks for a message to be printed out when
 749 //       a class is instantiated
 750 //    4: When this value is set, a message is displayed when a
 751 //       FGModel object executes its Run() method
 752 //    8: When this value is set, various runtime state variables
 753 //       are printed out periodically
 754 //    16: When set various parameters are sanity checked and
 755 //       a message is printed out when they go out of bounds
 756
 757 void FGFunction::Debug(int from)
 758 {
 759   if (debug_lvl <= 0) return;
 760
 761   if (debug_lvl & 1) { // Standard console startup message output
 762     if (from == 0) { // Constructor
 763       if (Type == eTopLevel)
 764         cout << "    Function: " << Name << endl;
 765     }
 766   }
 767   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 768     if (from == 0) cout << "Instantiated: FGFunction" << endl;
 769     if (from == 1) cout << "Destroyed:    FGFunction" << endl;
 770   }
 771   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 772   }
 773   if (debug_lvl & 8 ) { // Runtime state variables
 774   }
 775   if (debug_lvl & 16) { // Sanity checking
 776   }
 777   if (debug_lvl & 64) {
 778     if (from == 0) { // Constructor
 779       cout << IdSrc << endl;
 780       cout << IdHdr << endl;
 781     }
 782   }
 783 }
 784
 785 }