34 #include "simgear/misc/strutils.hxx"
35 #include "FGFDMExec.h"
36 #include "FGFunction.h"
38 #include "FGRealValue.h"
39 #include "input_output/FGXMLElement.h"
40 #include "math/FGFunctionValue.h"
51 const double invlog2val = 1.0/log10(2.0);
52 constexpr
unsigned int MaxArgs = 9999;
61 : runtime_error(msg), Parameters(p), element(el) {}
62 size_t NumberOfArguments(
void)
const {
return Parameters.size(); }
63 FGParameter* FirstParameter(
void)
const {
return *(Parameters.cbegin()); }
64 const Element* GetElement(
void)
const {
return element; }
67 const vector<FGParameter_ptr> Parameters;
73 template<
typename func_t,
unsigned int Nmin>
79 FGFunction::OddEven odd_even=FGFunction::OddEven::Either)
82 Load(el, v, fdmex, prefix);
83 CheckMinArguments(el, Nmin);
84 CheckMaxArguments(el, Nmax);
85 CheckOddOrEvenArguments(el, odd_even);
88 double GetValue(
void)
const override {
89 return cached ? cachedValue : f(Parameters);
93 void bind(
Element* el,
const string& Prefix)
override {
94 string nName = CreateOutputNode(el, Prefix);
106 template<
typename func_t>
110 aFunc(
const func_t& _f, std::shared_ptr<FGPropertyManager> pm,
Element* el,
111 const string& Prefix)
115 ostringstream buffer;
116 buffer << el->
ReadFrom() << fgred << highint
117 <<
"<" << el->
GetName() <<
"> should have no arguments." << reset
125 double GetValue(
void)
const override {
126 double result = cached ? cachedValue : f();
127 if (pNode) pNode->setDoubleValue(result);
132 bool IsConstant(
void)
const override {
140 void bind(
Element* el,
const string& Prefix)
override {
141 CreateOutputNode(el, Prefix);
143 if (pNode) pNode->setDoubleValue(f());
152 bool GetBinary(
double val,
const string &ctxMsg)
155 if (val < 1E-9)
return false;
156 else if (val-1 < 1E-9)
return true;
158 cerr << ctxMsg << FGJSBBase::fgred << FGJSBBase::highint
159 <<
"Malformed conditional check in function definition."
160 << FGJSBBase::reset << endl;
161 throw(
"Fatal Error.");
169 FGFunction* make_MathFn(
double(*math_fn)(
double), FGFDMExec* fdmex, Element* el,
170 const string& prefix, FGPropertyValue* v)
172 auto f = [math_fn](
const std::vector<FGParameter_ptr> &p)->
double {
173 return math_fn(p[0]->GetValue());
175 return new aFunc<decltype(f), 1>(f, fdmex, el, prefix, v);
183 template<
typename func_t>
184 FGParameter_ptr VarArgsFn(
const func_t& _f, FGFDMExec* fdmex, Element* el,
185 const string& prefix, FGPropertyValue* v)
188 return new aFunc<func_t, 2>(_f, fdmex, el, prefix, v, MaxArgs);
190 catch(WrongNumberOfArguments& e) {
191 if ((e.GetElement() == el) && (e.NumberOfArguments() == 1)) {
192 cerr << el->ReadFrom() << FGJSBBase::fgred
193 <<
"<" << el->GetName()
194 <<
"> only has one argument which makes it a no-op." << endl
195 <<
"Its argument will be evaluated but <" << el->GetName()
196 <<
"> will not be applied to the result." << FGJSBBase::reset << endl;
197 return e.FirstParameter();
210 Load(el, var, fdmex, prefix);
211 CheckMinArguments(el, 1);
212 CheckMaxArguments(el, 1);
216 if (!sCopyTo.empty()) {
217 if (sCopyTo.find(
"#") != string::npos) {
218 if (is_number(prefix))
219 sCopyTo = replace(sCopyTo,
"#",prefix);
222 <<
"Illegal use of the special character '#'" <<
reset << endl
223 <<
"The 'copyto' argument in function " << Name <<
" is ignored."
229 pCopyTo = PropertyManager->GetNode(sCopyTo);
232 <<
"Property \"" << sCopyTo
233 <<
"\" must be previously defined in function " << Name <<
reset
234 <<
"The 'copyto' argument is ignored." << endl;
240 void FGFunction::CheckMinArguments(
Element* el,
unsigned int _min)
242 if (Parameters.size() < _min) {
243 ostringstream buffer;
245 <<
"<" << el->
GetName() <<
"> should have at least " << _min
246 <<
" argument(s)." <<
reset << endl;
253 void FGFunction::CheckMaxArguments(Element* el,
unsigned int _max)
255 if (Parameters.size() > _max) {
256 ostringstream buffer;
258 <<
"<" << el->GetName() <<
"> should have no more than " << _max
259 <<
" argument(s)." <<
reset << endl;
260 throw WrongNumberOfArguments(buffer.str(), Parameters, el);
266 void FGFunction::CheckOddOrEvenArguments(Element* el, OddEven odd_even)
271 if (Parameters.size() % 2 == 1) {
273 <<
"<" << el->GetName() <<
"> must have an even number of arguments."
275 throw(
"Fatal Error");
279 if (Parameters.size() % 2 == 0) {
281 <<
"<" << el->GetName() <<
"> must have an odd number of arguments."
283 throw(
"Fatal Error");
293 shared_ptr<RandomNumberGenerator> makeRandomGenerator(Element *el, FGFDMExec* fdmex)
295 string seed_attr = el->GetAttributeValue(
"seed");
296 if (seed_attr.empty())
297 return fdmex->GetRandomGenerator();
298 else if (seed_attr ==
"time_now")
299 return make_shared<RandomNumberGenerator>();
301 unsigned int seed = atoi(seed_attr.c_str());
302 return make_shared<RandomNumberGenerator>(seed);
308 void FGFunction::Load(Element* el, FGPropertyValue* var, FGFDMExec* fdmex,
309 const string& Prefix)
311 Name = el->GetAttributeValue(
"name");
312 Element* element = el->GetElement();
314 auto sum = [](
const decltype(Parameters)& Parameters)->
double {
317 for (
auto p: Parameters)
318 temp += p->GetValue();
324 string operation = element->GetName();
327 if (operation ==
"property" || operation ==
"p") {
328 string property_name = element->GetDataLine();
330 if (var && simgear::strutils::strip(property_name) ==
"#")
331 Parameters.push_back(var);
333 if (property_name.find(
"#") != string::npos) {
334 if (is_number(Prefix)) {
335 property_name = replace(property_name,
"#",Prefix);
338 cerr << element->ReadFrom()
339 <<
fgred <<
"Illegal use of the special character '#'"
341 throw(
"Fatal Error.");
345 if (element->HasAttribute(
"apply")) {
346 string function_str = element->GetAttributeValue(
"apply");
347 auto f = fdmex->GetTemplateFunc(function_str);
349 Parameters.push_back(
new FGFunctionValue(property_name,
350 PropertyManager, f, element));
352 cerr << element->ReadFrom()
354 << function_str <<
" has been defined. This property will "
355 <<
"not be logged. You should check your configuration file."
360 Parameters.push_back(
new FGPropertyValue(property_name,
361 PropertyManager, element));
363 }
else if (operation ==
"value" || operation ==
"v") {
364 Parameters.push_back(
new FGRealValue(element->GetDataAsNumber()));
365 }
else if (operation ==
"pi") {
366 Parameters.push_back(
new FGRealValue(M_PI));
367 }
else if (operation ==
"table" || operation ==
"t") {
368 string call_type = element->GetAttributeValue(
"type");
369 if (call_type ==
"internal") {
370 std::cerr << el->ReadFrom()
371 <<
"An internal table cannot be nested within a function."
373 throw BaseException(
"An internal table cannot be nested within a function.");
375 Parameters.push_back(
new FGTable(PropertyManager, element, Prefix));
377 }
else if (operation ==
"product") {
378 auto f = [](
const decltype(Parameters)& Parameters)->
double {
381 for (
auto p: Parameters)
382 temp *= p->GetValue();
386 Parameters.push_back(VarArgsFn<decltype(f)>(f, fdmex, element, Prefix, var));
387 }
else if (operation ==
"sum") {
388 Parameters.push_back(VarArgsFn<decltype(sum)>(sum, fdmex, element, Prefix, var));
389 }
else if (operation ==
"avg") {
390 auto avg = [&](
const decltype(Parameters)& p)->
double {
391 return sum(p) / p.size();
393 Parameters.push_back(VarArgsFn<decltype(avg)>(avg, fdmex, element, Prefix, var));
394 }
else if (operation ==
"difference") {
395 auto f = [](
const decltype(Parameters)& Parameters)->
double {
396 double temp = Parameters[0]->GetValue();
398 for (
auto p = Parameters.begin()+1; p != Parameters.end(); ++p)
399 temp -= (*p)->GetValue();
403 Parameters.push_back(VarArgsFn<decltype(f)>(f, fdmex, element, Prefix, var));
404 }
else if (operation ==
"min") {
405 auto f = [](
const decltype(Parameters)& Parameters)->
double {
406 double _min = HUGE_VAL;
408 for (
auto p : Parameters) {
409 double x = p->GetValue();
416 Parameters.push_back(VarArgsFn<decltype(f)>(f, fdmex, element, Prefix, var));
417 }
else if (operation ==
"max") {
418 auto f = [](
const decltype(Parameters)& Parameters)->
double {
419 double _max = -HUGE_VAL;
421 for (
auto p : Parameters) {
422 double x = p->GetValue();
429 Parameters.push_back(VarArgsFn<decltype(f)>(f, fdmex, element, Prefix, var));
430 }
else if (operation ==
"and") {
431 string ctxMsg = element->ReadFrom();
432 auto f = [ctxMsg](
const decltype(Parameters)& Parameters)->
double {
433 for (
auto p : Parameters) {
434 if (!GetBinary(p->GetValue(), ctxMsg))
440 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix,
442 }
else if (operation ==
"or") {
443 string ctxMsg = element->ReadFrom();
444 auto f = [ctxMsg](
const decltype(Parameters)& Parameters)->
double {
445 for (
auto p : Parameters) {
446 if (GetBinary(p->GetValue(), ctxMsg))
452 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix,
454 }
else if (operation ==
"quotient") {
455 auto f = [](
const decltype(Parameters)& p)->
double {
456 double y = p[1]->GetValue();
457 return y != 0.0 ? p[0]->GetValue()/y : HUGE_VAL;
459 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
460 }
else if (operation ==
"pow") {
461 auto f = [](
const decltype(Parameters)& p)->
double {
464 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
465 }
else if (operation ==
"toradians") {
466 auto f = [](
const decltype(Parameters)& p)->
double {
467 return p[0]->GetValue()*M_PI/180.;
469 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
470 }
else if (operation ==
"todegrees") {
471 auto f = [](
const decltype(Parameters)& p)->
double {
472 return p[0]->GetValue()*180./M_PI;
474 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
475 }
else if (operation ==
"sqrt") {
476 auto f = [](
const decltype(Parameters)& p)->
double {
477 double x = p[0]->GetValue();
478 return x >= 0.0 ? sqrt(x) : -HUGE_VAL;
480 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
481 }
else if (operation ==
"log2") {
482 auto f = [](
const decltype(Parameters)& p)->
double {
483 double x = p[0]->GetValue();
484 return x > 0.0 ? log10(x)*invlog2val : -HUGE_VAL;
486 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
487 }
else if (operation ==
"ln") {
488 auto f = [](
const decltype(Parameters)& p)->
double {
489 double x = p[0]->GetValue();
490 return x > 0.0 ? log(x) : -HUGE_VAL;
492 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
493 }
else if (operation ==
"log10") {
494 auto f = [](
const decltype(Parameters)& p)->
double {
495 double x = p[0]->GetValue();
496 return x > 0.0 ? log10(x) : -HUGE_VAL;
498 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
499 }
else if (operation ==
"sign") {
500 auto f = [](
const decltype(Parameters)& p)->
double {
501 return p[0]->GetValue() < 0.0 ? -1 : 1;
503 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
504 }
else if (operation ==
"exp") {
505 Parameters.push_back(make_MathFn(exp, fdmex, element, Prefix, var));
506 }
else if (operation ==
"abs") {
507 Parameters.push_back(make_MathFn(fabs, fdmex, element, Prefix, var));
508 }
else if (operation ==
"sin") {
509 Parameters.push_back(make_MathFn(sin, fdmex, element, Prefix, var));
510 }
else if (operation ==
"cos") {
511 Parameters.push_back(make_MathFn(cos, fdmex, element, Prefix, var));
512 }
else if (operation ==
"tan") {
513 Parameters.push_back(make_MathFn(tan, fdmex, element, Prefix, var));
514 }
else if (operation ==
"asin") {
515 Parameters.push_back(make_MathFn(asin, fdmex, element, Prefix, var));
516 }
else if (operation ==
"acos") {
517 Parameters.push_back(make_MathFn(acos, fdmex, element, Prefix, var));
518 }
else if (operation ==
"atan") {
519 Parameters.push_back(make_MathFn(atan, fdmex, element, Prefix, var));
520 }
else if (operation ==
"floor") {
521 Parameters.push_back(make_MathFn(floor, fdmex, element, Prefix, var));
522 }
else if (operation ==
"ceil") {
523 Parameters.push_back(make_MathFn(ceil, fdmex, element, Prefix, var));
524 }
else if (operation ==
"fmod") {
525 auto f = [](
const decltype(Parameters)& p)->
double {
526 double y = p[1]->GetValue();
527 return y != 0.0 ? fmod(p[0]->
GetValue(), y) : HUGE_VAL;
529 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
530 }
else if (operation ==
"atan2") {
531 auto f = [](
const decltype(Parameters)& p)->
double {
534 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
535 }
else if (operation ==
"mod") {
536 auto f = [](
const decltype(Parameters)& p)->
double {
537 return static_cast<int>(p[0]->GetValue()) %
static_cast<int>(p[1]->
GetValue());
539 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
540 }
else if (operation ==
"fraction") {
541 auto f = [](
const decltype(Parameters)& p)->
double {
543 return modf(p[0]->
GetValue(), &scratch);
545 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
546 }
else if (operation ==
"integer") {
547 auto f = [](
const decltype(Parameters)& p)->
double {
552 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
553 }
else if (operation ==
"lt") {
554 auto f = [](
const decltype(Parameters)& p)->
double {
555 return p[0]->GetValue() < p[1]->GetValue() ? 1.0 : 0.0;
557 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
558 }
else if (operation ==
"le") {
559 auto f = [](
const decltype(Parameters)& p)->
double {
560 return p[0]->GetValue() <= p[1]->GetValue() ? 1.0 : 0.0;
562 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
563 }
else if (operation ==
"gt") {
564 auto f = [](
const decltype(Parameters)& p)->
double {
565 return p[0]->GetValue() > p[1]->GetValue() ? 1.0 : 0.0;
567 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
568 }
else if (operation ==
"ge") {
569 auto f = [](
const decltype(Parameters)& p)->
double {
570 return p[0]->GetValue() >= p[1]->GetValue() ? 1.0 : 0.0;
572 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
573 }
else if (operation ==
"eq") {
574 auto f = [](
const decltype(Parameters)& p)->
double {
575 return p[0]->GetValue() == p[1]->GetValue() ? 1.0 : 0.0;
577 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
578 }
else if (operation ==
"nq") {
579 auto f = [](
const decltype(Parameters)& p)->
double {
580 return p[0]->GetValue() != p[1]->GetValue() ? 1.0 : 0.0;
582 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix, var));
583 }
else if (operation ==
"not") {
584 string ctxMsg = element->ReadFrom();
585 auto f = [ctxMsg](
const decltype(Parameters)& p)->
double {
586 return GetBinary(p[0]->
GetValue(), ctxMsg) ? 0.0 : 1.0;
588 Parameters.push_back(
new aFunc<decltype(f), 1>(f, fdmex, element, Prefix, var));
589 }
else if (operation ==
"ifthen") {
590 string ctxMsg = element->ReadFrom();
591 auto f = [ctxMsg](
const decltype(Parameters)& p)->
double {
592 if (GetBinary(p[0]->
GetValue(), ctxMsg))
595 return p[2]->GetValue();
597 Parameters.push_back(
new aFunc<decltype(f), 3>(f, fdmex, element, Prefix, var));
598 }
else if (operation ==
"random") {
601 string mean_attr = element->GetAttributeValue(
"mean");
602 string stddev_attr = element->GetAttributeValue(
"stddev");
603 if (!mean_attr.empty()) {
604 if (is_number(trim(mean_attr)))
605 mean = atof_locale_c(mean_attr);
607 cerr << element->ReadFrom()
608 <<
"Expecting a number, but got: " << mean_attr <<endl;
609 throw BaseException(
"Invalid number");
612 if (!stddev_attr.empty()) {
613 if (is_number(trim(stddev_attr)))
614 stddev = atof_locale_c(stddev_attr);
616 cerr << element->ReadFrom()
617 <<
"Expecting a number, but got: " << stddev_attr <<endl;
618 throw BaseException(
"Invalid number");
621 auto generator(makeRandomGenerator(element, fdmex));
622 auto f = [generator, mean, stddev]()->
double {
623 double value = generator->GetNormalRandomNumber();
624 return value*stddev + mean;
626 Parameters.push_back(
new aFunc<decltype(f), 0>(f, PropertyManager, element,
628 }
else if (operation ==
"urandom") {
631 string lower_attr = element->GetAttributeValue(
"lower");
632 string upper_attr = element->GetAttributeValue(
"upper");
633 if (!lower_attr.empty()) {
634 if (is_number(trim(lower_attr)))
635 lower = atof_locale_c(lower_attr);
637 cerr << element->ReadFrom()
638 <<
"Expecting a number, but got: " << lower_attr <<endl;
639 throw BaseException(
"Invalid number");
642 if (!upper_attr.empty()) {
643 if (is_number(trim(upper_attr)))
644 upper = atof_locale_c(upper_attr);
646 cerr << element->ReadFrom()
647 <<
"Expecting a number, but got: " << upper_attr <<endl;
648 throw BaseException(
"Invalid number");
651 auto generator(makeRandomGenerator(element, fdmex));
652 double a = 0.5*(upper-lower);
653 double b = 0.5*(upper+lower);
654 auto f = [generator, a, b]()->
double {
655 double value = generator->GetUniformRandomNumber();
658 Parameters.push_back(
new aFunc<decltype(f), 0>(f, PropertyManager, element,
660 }
else if (operation ==
"switch") {
661 string ctxMsg = element->ReadFrom();
662 auto f = [ctxMsg](
const decltype(Parameters)& p)->
double {
663 double temp = p[0]->GetValue();
666 <<
"The switch function index (" << temp
667 <<
") is negative." <<
reset << endl;
668 throw(
"Fatal error");
670 size_t n = p.size()-1;
671 size_t i =
static_cast<size_t>(temp+0.5);
674 return p[i+1]->GetValue();
677 <<
"The switch function index (" << temp
678 <<
") selected a value above the range of supplied values"
679 <<
"[0:" << n-1 <<
"]"
680 <<
" - not enough values were supplied." <<
reset << endl;
681 throw(
"Fatal error");
684 Parameters.push_back(
new aFunc<decltype(f), 2>(f, fdmex, element, Prefix,
686 }
else if (operation ==
"interpolate1d") {
687 auto f = [](
const decltype(Parameters)& p)->
double {
691 double x = p[0]->GetValue();
692 double xmin = p[1]->GetValue();
693 double ymin = p[2]->GetValue();
694 if (x <= xmin)
return ymin;
696 double xmax = p[n-2]->GetValue();
697 double ymax = p[n-1]->GetValue();
698 if (x >= xmax)
return ymax;
701 size_t nmax = (n-3)/2;
702 while (nmax-nmin > 1) {
703 size_t m = (nmax-nmin)/2+nmin;
704 double xm = p[2*m+1]->GetValue();
705 double ym = p[2*m+2]->GetValue();
719 return ymin + (x-xmin)*(ymax-ymin)/(xmax-xmin);
721 Parameters.push_back(
new aFunc<decltype(f), 5>(f, fdmex, element, Prefix,
722 var, MaxArgs, OddEven::Odd));
723 }
else if (operation ==
"rotation_alpha_local") {
727 auto f = [](
const decltype(Parameters)& p)->
double {
728 double alpha = p[0]->GetValue()*degtorad;
729 double beta = p[1]->GetValue()*degtorad;
730 double phi = p[3]->GetValue()*degtorad;
731 double theta = p[4]->GetValue()*degtorad;
732 double psi = p[5]->GetValue()*degtorad;
734 FGQuaternion qTb2l(phi, theta, psi);
735 double cos_beta = cos(beta);
736 FGColumnVector3 wind_body(cos(alpha)*cos_beta, sin(beta),
737 sin(alpha)*cos_beta);
738 FGColumnVector3 wind_local = qTb2l.GetT()*wind_body;
740 if (fabs(fabs(wind_local(eY)) - 1.0) < 1E-9)
743 return atan2(wind_local(eZ), wind_local(eX))*radtodeg;
745 Parameters.push_back(
new aFunc<decltype(f), 6>(f, fdmex, element, Prefix, var));
746 }
else if (operation ==
"rotation_beta_local") {
750 auto f = [](
const decltype(Parameters)& p)->
double {
751 double alpha = p[0]->GetValue()*degtorad;
752 double beta = p[1]->GetValue()*degtorad;
753 double phi = p[3]->GetValue()*degtorad;
754 double theta = p[4]->GetValue()*degtorad;
755 double psi = p[5]->GetValue()*degtorad;
756 FGQuaternion qTb2l(phi, theta, psi);
757 double cos_beta = cos(beta);
758 FGColumnVector3 wind_body(cos(alpha)*cos_beta, sin(beta),
759 sin(alpha)*cos_beta);
760 FGColumnVector3 wind_local = qTb2l.GetT()*wind_body;
762 if (fabs(fabs(wind_local(eY)) - 1.0) < 1E-9)
763 return wind_local(eY) > 0.0 ? 0.5*M_PI : -0.5*M_PI;
765 double alpha_local = atan2(wind_local(eZ), wind_local(eX));
766 double cosa = cos(alpha_local);
767 double sina = sin(alpha_local);
770 if (fabs(cosa) > fabs(sina))
771 cosb = wind_local(eX) / cosa;
773 cosb = wind_local(eZ) / sina;
775 return atan2(wind_local(eY), cosb)*radtodeg;
777 Parameters.push_back(
new aFunc<decltype(f), 6>(f, fdmex, element, Prefix, var));
778 }
else if (operation ==
"rotation_gamma_local") {
782 auto f = [](
const decltype(Parameters)& p)->
double {
783 double alpha = p[0]->GetValue()*degtorad;
784 double beta = p[1]->GetValue()*degtorad;
785 double gamma = p[2]->GetValue()*degtorad;
786 double phi = p[3]->GetValue()*degtorad;
787 double theta = p[4]->GetValue()*degtorad;
788 double psi = p[5]->GetValue()*degtorad;
789 double cos_alpha = cos(alpha), sin_alpha = sin(alpha);
790 double cos_beta = cos(beta), sin_beta = sin(beta);
791 double cos_gamma = cos(gamma), sin_gamma = sin(gamma);
792 FGQuaternion qTb2l(phi, theta, psi);
793 FGColumnVector3 wind_body_X(cos_alpha*cos_beta, sin_beta,
795 FGColumnVector3 wind_body_Y(-sin_alpha*sin_gamma-sin_beta*cos_alpha*cos_gamma,
797 -sin_alpha*sin_beta*cos_gamma+sin_gamma*cos_alpha);
798 FGColumnVector3 wind_local_X = qTb2l.GetT()*wind_body_X;
799 FGColumnVector3 wind_local_Y = qTb2l.GetT()*wind_body_Y;
800 double cosacosb = wind_local_X(eX);
801 double sinb = wind_local_X(eY);
802 double sinacosb = wind_local_X(eZ);
805 if (fabs(sinb) < 1E-9) {
806 cosc = wind_local_Y(eY);
808 if (fabs(cosacosb) > fabs(sinacosb))
809 sinc = wind_local_Y(eZ) / cosacosb;
811 sinc = -wind_local_Y(eX) / sinacosb;
813 else if (fabs(fabs(sinb)-1.0) < 1E-9) {
814 sinc = wind_local_Y(eZ);
815 cosc = -wind_local_Y(eX);
818 sinc = cosacosb*wind_local_Y(eZ)-sinacosb*wind_local_Y(eX);
819 cosc = (-sinacosb*wind_local_Y(eZ)-cosacosb*wind_local_Y(eX))/sinb;
822 return atan2(sinc, cosc)*radtodeg;
824 Parameters.push_back(
new aFunc<decltype(f), 6>(f, fdmex, element, Prefix, var));
825 }
else if (operation ==
"rotation_bf_to_wf") {
828 string ctxMsg = element->ReadFrom();
829 auto f = [ctxMsg](
const decltype(Parameters)& p)->
double {
830 double rx = p[0]->GetValue();
831 double ry = p[1]->GetValue();
832 double rz = p[2]->GetValue();
833 double alpha = p[3]->GetValue()*degtorad;
834 double beta = p[4]->GetValue()*degtorad;
835 double gamma = p[5]->GetValue()*degtorad;
836 int idx =
static_cast<int>(p[6]->GetValue());
838 if ((idx < 1) || (idx > 3)) {
840 <<
"The index must be one of the integer value 1, 2 or 3."
842 throw(
"Fatal error");
845 FGQuaternion qa(eY, -alpha), qb(eZ, beta), qc(eX, -gamma);
846 FGMatrix33 mT = (qa*qb*qc).GetT();
847 FGColumnVector3 r0(rx, ry, rz);
848 FGColumnVector3 r = mT*r0;
852 Parameters.push_back(
new aFunc<decltype(f), 7>(f, fdmex, element, Prefix, var));
853 }
else if (operation ==
"rotation_wf_to_bf") {
856 string ctxMsg = element->ReadFrom();
857 auto f = [ctxMsg](
const decltype(Parameters)& p)->
double {
858 double rx = p[0]->GetValue();
859 double ry = p[1]->GetValue();
860 double rz = p[2]->GetValue();
861 double alpha = p[3]->GetValue()*degtorad;
862 double beta = p[4]->GetValue()*degtorad;
863 double gamma = p[5]->GetValue()*degtorad;
864 int idx =
static_cast<int>(p[6]->GetValue());
866 if ((idx < 1) || (idx > 3)) {
868 <<
"The index must be one of the integer value 1, 2 or 3."
870 throw(
"Fatal error");
873 FGQuaternion qa(eY, -alpha), qb(eZ, beta), qc(eX, -gamma);
874 FGMatrix33 mT = (qa*qb*qc).GetT();
875 FGColumnVector3 r0(rx, ry, rz);
877 FGColumnVector3 r = mT*r0;
881 Parameters.push_back(
new aFunc<decltype(f), 7>(f, fdmex, element, Prefix, var));
882 }
else if (operation !=
"description") {
884 <<
"Bad operation <" << operation
885 <<
"> detected in configuration file" <<
reset << endl;
890 if (!Parameters.empty()){
893 if (p && p->IsConstant()) {
894 double constant = p->GetValue();
895 FGPropertyNode_ptr node = p->pNode;
896 string pName = p->GetName();
898 Parameters.pop_back();
899 Parameters.push_back(
new FGRealValue(constant));
902 <<
"<" << operation <<
"> is applied on constant parameters."
903 << endl <<
"It will be replaced by its result ("
907 node->setDoubleValue(constant);
908 node->setAttribute(SGPropertyNode::WRITE,
false);
910 cout <<
" and the property " << pName
911 <<
" will be unbound and made read only.";
913 cout <<
reset << endl << endl;
916 element = el->GetNextElement();
928 if (pNode && pNode->isTied())
929 PropertyManager->Untie(pNode);
938 for (
auto p: Parameters) {
939 if (!p->IsConstant())
962 if (cached)
return cachedValue;
964 double val = Parameters[0]->GetValue();
966 if (pCopyTo) pCopyTo->setDoubleValue(val);
975 ostringstream buffer;
977 buffer << setw(9) << setprecision(6) <<
GetValue();
983 string FGFunction::CreateOutputNode(
Element* el,
const string& Prefix)
987 if ( !Name.empty() ) {
989 nName = PropertyManager->mkPropertyName(Name,
false);
991 if (is_number(Prefix)) {
992 if (Name.find(
"#") != string::npos) {
993 Name = replace(Name,
"#",Prefix);
994 nName = PropertyManager->mkPropertyName(Name,
false);
997 <<
"Malformed function name with number: " << Prefix
998 <<
" and property name: " << Name
999 <<
" but no \"#\" sign for substitution." << endl;
1002 nName = PropertyManager->mkPropertyName(Prefix +
"/" + Name,
false);
1006 pNode = PropertyManager->GetNode(nName,
true);
1007 if (pNode->isTied()) {
1009 <<
"Property " << nName <<
" has already been successfully bound (late)." << endl;
1010 throw(
"Failed to bind the property to an existing already tied node.");
1019 void FGFunction::bind(Element* el,
const string& Prefix)
1021 string nName = CreateOutputNode(el, Prefix);
1046 void FGFunction::Debug(
int from)
1048 if (debug_lvl <= 0)
return;
1050 if (debug_lvl & 1) {
1053 cout <<
" Function: " << Name << endl;
1056 if (debug_lvl & 2 ) {
1057 if (from == 0) cout <<
"Instantiated: FGFunction" << endl;
1058 if (from == 1) cout <<
"Destroyed: FGFunction" << endl;
1060 if (debug_lvl & 4 ) {
1062 if (debug_lvl & 8 ) {
1064 if (debug_lvl & 16) {
1066 if (debug_lvl & 64) {
std::string GetAttributeValue(const std::string &key)
Retrieves an attribute.
unsigned int GetNumElements(void)
Returns the number of child elements for this element.
std::string ReadFrom(void) const
Return a string that contains a description of the location where the current XML element was read fr...
const std::string & GetName(void) const
Retrieves the element name.
Encapsulates the JSBSim simulation executive.
std::shared_ptr< FGPropertyManager > GetPropertyManager(void) const
Returns a pointer to the property manager object.
Represents a mathematical function.
FGFunction()
Default constructor.
double GetValue(void) const override
Retrieves the value of the function object.
bool IsConstant(void) const override
Does the function always return the same result (i.e.
void cacheValue(bool shouldCache)
Specifies whether to cache the value of the function, so it is calculated only once per frame.
std::string GetValueAsString(void) const
The value that the function evaluates to, as a string.
~FGFunction(void) override
Destructor Make sure the function is untied before destruction.
static char fggreen[6]
green text
static char fgred[6]
red text
static char reset[5]
resets text properties
static char highint[5]
highlights text
Represents various types of parameters.
Represents a property value which can use late binding.