bool FGTrim::RemoveState( State state ) {
FGTrimAxis* ta;
bool result=false;
mode = tCustom;
vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
while (iAxes != TrimAxes.end()) {
ta=*iAxes;
if( ta->GetStateType() == state ) {
delete ta;
iAxes = TrimAxes.erase(iAxes);
result=true;
continue;
}
iAxes++;
}
if(result) {
delete[] sub_iterations;
delete[] successful;
delete[] solution;
sub_iterations=new double[TrimAxes.size()];
successful=new double[TrimAxes.size()];
solution=new bool[TrimAxes.size()];
}
return result;
}
bool FGTrim::solve(FGTrimAxis& axis) {
double x1,x2,x3,f1,f2,f3,d,d0;
const double relax =0.9;
double eps=axis.GetSolverEps();
x1=x2=x3=0;
d=1;
bool success=false;
//initializations
if( solutionDomain != 0) {
/* if(ahi > alo) { */
x1=xlo;f1=alo;
x3=xhi;f3=ahi;
/* } else {
x1=xhi;f1=ahi;
x3=xlo;f3=alo;
} */
d0=fabs(x3-x1);
//iterations
//max_sub_iterations=axis.GetIterationLimit();
while ( (axis.InTolerance() == false )
&& (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
Nsub++;
d=(x3-x1)/d0;
x2=x1-d*d0*f1/(f3-f1);
axis.SetControl(x2);
axis.Run();
f2=axis.GetState();
if(Debug > 1) {
cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
<< ", " << x2 << ", " << x3 << endl;
cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
}
if(f1*f2 <= 0.0) {
x3=x2;
f3=f2;
f1=relax*f1;
//cout << "Solution is between x1 and x2" << endl;
}
else if(f2*f3 <= 0.0) {
x1=x2;
f1=f2;
f3=relax*f3;
//cout << "Solution is between x2 and x3" << endl;
}
//cout << i << endl;
}//end while
if(Nsub < max_sub_iterations) success=true;
}
return success;
}
void FGTrim::setDebug(FGTrimAxis& axis) {
if(debug_axis == tAll ||
axis.GetStateType() == debug_axis ) {
Debug=DebugLevel;
return;
} else {
Debug=0;
return;
}
}
bool FGTrim::EditState( State state, Control new_control ){
FGTrimAxis* ta;
bool result=false;
mode = tCustom;
vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
while (iAxes != TrimAxes.end()) {
ta=*iAxes;
if( ta->GetStateType() == state ) {
TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
delete ta;
TrimAxes.erase(iAxes+1);
result=true;
break;
}
iAxes++;
}
return result;
}
bool FGTrim::AddState( State state, Control control ) {
FGTrimAxis* ta;
bool result=true;
mode = tCustom;
vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
while (iAxes != TrimAxes.end()) {
ta=*iAxes;
if( ta->GetStateType() == state )
result=false;
iAxes++;
}
if(result) {
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
delete[] sub_iterations;
delete[] successful;
delete[] solution;
sub_iterations=new double[TrimAxes.size()];
successful=new double[TrimAxes.size()];
solution=new bool[TrimAxes.size()];
}
return result;
}
bool FGTrim::checkLimits(FGTrimAxis& axis)
{
bool solutionExists;
double current_control=axis.GetControl();
double current_accel=axis.GetState();
xlo=axis.GetControlMin();
xhi=axis.GetControlMax();
axis.SetControl(xlo);
axis.Run();
alo=axis.GetState();
axis.SetControl(xhi);
axis.Run();
ahi=axis.GetState();
if(Debug > 1)
cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
<< alo << ", " << ahi << endl;
solutionDomain=0;
solutionExists=false;
if(fabs(ahi-alo) > axis.GetTolerance()) {
if(alo*current_accel <= 0) {
solutionExists=true;
solutionDomain=-1;
xhi=current_control;
ahi=current_accel;
} else if(current_accel*ahi < 0){
solutionExists=true;
solutionDomain=1;
xlo=current_control;
alo=current_accel;
}
}
axis.SetControl(current_control);
axis.Run();
return solutionExists;
}
/*
produces an interval (xlo..xhi) on one side or the other of the current
control value in which a solution exists. This domain is, hopefully,
smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
to find the solution. This is, hopefully, more efficient than having the
solver start from scratch every time. Maybe it isn't though...
This tries to take advantage of the idea that the changes from iteration to
iteration will be small after the first one or two top-level iterations.
assumes that changing the control will a produce significant change in the
accel i.e. checkLimits() has already been called.
if a solution is found above the current control, the function returns true
and xlo is set to the current control, xhi to the interval max it found, and
solutionDomain is set to 1.
if the solution lies below the current control, then the function returns
true and xlo is set to the interval min it found and xmax to the current
control. if no solution is found, then the function returns false.
in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
no assumptions about the state of the sim after this function has run
can be made.
*/
bool FGTrim::findInterval(FGTrimAxis& axis) {
bool found=false;
double step;
double current_control=axis.GetControl();
double current_accel=axis.GetState();;
double xmin=axis.GetControlMin();
double xmax=axis.GetControlMax();
double lastxlo,lastxhi,lastalo,lastahi;
step=0.025*fabs(xmax);
xlo=xhi=current_control;
alo=ahi=current_accel;
lastxlo=xlo;lastxhi=xhi;
lastalo=alo;lastahi=ahi;
do {
Nsub++;
step*=2;
xlo-=step;
if(xlo < xmin) xlo=xmin;
xhi+=step;
if(xhi > xmax) xhi=xmax;
axis.SetControl(xlo);
axis.Run();
alo=axis.GetState();
axis.SetControl(xhi);
axis.Run();
ahi=axis.GetState();
if(fabs(ahi-alo) <= axis.GetTolerance()) continue;
if(alo*ahi <=0) { //found interval with root
found=true;
if(alo*current_accel <= 0) { //narrow interval down a bit
solutionDomain=-1;
xhi=lastxlo;
ahi=lastalo;
//xhi=current_control;
//ahi=current_accel;
} else {
solutionDomain=1;
xlo=lastxhi;
alo=lastahi;
//xlo=current_control;
//alo=current_accel;
}
}
lastxlo=xlo;lastxhi=xhi;
lastalo=alo;lastahi=ahi;
if( !found && xlo==xmin && xhi==xmax ) continue;
if(Debug > 1)
cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
<< " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
} while(!found && (Nsub <= max_sub_iterations) );
return found;
}
