Matrix<double> GeneralConstraint::getCovariance( const VariableSet& varSet )
{
Matrix<double> covariance(varSet.size(),varSet.size(),0.0);
int i(0);
for(VariableSet::const_iterator iti=varSet.begin();
iti!=varSet.end();
++iti)
{
int j(0);
for(VariableSet::const_iterator itj=varSet.begin();
itj!=varSet.end();
++itj)
{
covariance[i][j] = solver.getCovariance(*iti,*itj);
j++;
}
i++;
}
return covariance;
} // End of method 'GeneralConstraint::getCovariance(...'
VariableSet GeneralConstraint::intersectionVariables(const VariableSet& vs1,
const VariableSet& vs2 )
{
VariableSet tempSet;
for(VariableSet::const_iterator it=vs1.begin();
it!=vs1.end();
++it)
{
VariableSet::const_iterator it2 = vs2.find(*it);
if(it2!=vs2.end()) tempSet.insert(*it);
}
return tempSet;
} // End of method 'GeneralConstraint::intersectionVariables()'
OptionalMessageList UndeclaredVariableCheck::visitLambdaDefinition(const LambdaDefinitionAddress& lambdaDef) {
OptionalMessageList res;
VariableSet recFunctions;
for_each(lambdaDef.getAddressedNode()->getDefinitions(), [&recFunctions](const LambdaBindingPtr& cur) {
recFunctions.insert(cur->getVariable());
});
for_each(lambdaDef->getDefinitions(), [&](const LambdaBindingAddress& cur) {
// assemble set of defined variables
VariableSet declared;
// add recursive function variables
declared.insert(recFunctions.begin(), recFunctions.end());
// add parameters
auto paramList = cur.getAddressedNode()->getLambda()->getParameterList();
declared.insert(paramList.begin(), paramList.end());
// run check on body ...
VarDeclarationCheck check(declared);
// trigger check
addAll(res, conductCheck(check, cur->getLambda()));
});
return res;
}
void GeneralConstraint::stackVariables( VariableList& varList,
const VariableSet& varSet )
{
for(VariableSet::const_iterator it= varSet.begin();
it!=varSet.end();
++it)
{
varList.push_back(*it);
}
} // End of method 'GeneralConstraint::stackVariables()'
VariableSet GeneralConstraint::unionVariables( const VariableSet& vs1,
const VariableSet& vs2 )
{
VariableSet tempSet(vs1);
for(VariableSet::const_iterator it=vs2.begin();
it!=vs2.end();
++it)
{
tempSet.insert(*it);
}
return tempSet;
} // End of method 'GeneralConstraint::unionVariables()'
VariableSet GeneralConstraint::getVariables( const SatID& sat,
const TypeID& type )
{
VariableSet vset;
VariableSet varSet = getVariables(sat);
for(VariableSet::iterator itv=varSet.begin();
itv!=varSet.end();
++itv)
{
if( (itv->getType()==type) ) vset.insert(*itv);
}
return vset;
} // End of method 'GeneralConstraint::getVariables(const SatID& sat,...)'
Vector<double> GeneralConstraint::getSolution( const VariableSet& varSet )
{
Vector<double> solution(varSet.size(),0.0);
int i(0);
for(VariableSet::const_iterator it=varSet.begin();
it!=varSet.end();
++it)
{
solution[i] = solver.getSolution(*it);
i++;
}
return solution;
} // End of method 'GeneralConstraint::getSolution(...'
VariableSet GeneralConstraint::getVariables( const SatID& sat,
const TypeIDSet& typeSet )
{
VariableSet vset;
VariableSet varSet = getVariables(sat);
for(VariableSet::iterator itv=varSet.begin();
itv!=varSet.end();
++itv)
{
TypeIDSet::const_iterator it = typeSet.find(itv->getType());
if( (it!=typeSet.end()) ) vset.insert(*itv);
}
return vset;
} // End of method 'GeneralConstraint::getVariables(...'
inline std::string asString(const VariableSet& vset)
{
std::ostringstream oss;
for( VariableSet::const_iterator it = vset.begin();
it != vset.end();
++it )
{
oss << it->getType() << " "
<< it->getSource() << " "
<< it->getSatellite() << " "
<< it->getTypeIndexed() << " "
<< it->getSourceIndexed() << " "
<< it->getSatIndexed()<< std::endl;
}
return oss.str();
}
VariableSet GeneralConstraint::getVariables( const SourceID& source,
const SatIDSet& satSet,
const TypeID& type )
{
VariableSet vset;
VariableSet varSet = getVariables(source,type);
for(VariableSet::iterator itv=varSet.begin();
itv!=varSet.end();
++itv)
{
SatIDSet::const_iterator it = satSet.find(itv->getSatellite());
if( it != satSet.end() ) vset.insert(*itv);
}
return vset;
} // End of method 'GeneralConstraint::getVariables(...'
void merge( VariableSet &vs, const VariableSet &other )
{
if ( vs.empty() )
vs = other;
else
{
for ( auto i: other )
{
auto cur = vs.find( i.first );
if ( cur == vs.end() )
{
vs.emplace( std::make_pair( i.first, i.second ) );
}
else
cur->second.merge( i.second );
}
}
}
VariableSet GeneralConstraint::getVariables( const SourceIDSet& sourceSet,
const TypeID& type )
{
VariableSet vset;
VariableSet varSet = getVariables(sourceSet);
for(VariableSet::iterator itv=varSet.begin();
itv!=varSet.end();
++itv)
{
if( (itv->getType()==type) && itv->getSourceIndexed() )
{
vset.insert(*itv);
}
}
return vset;
} // End of method 'GeneralConstraint::getVariables(...'
Variable GeneralConstraint::getVariable( const SourceID& source,
const SatID& sat,
const TypeID& type )
{
VariableSet vset;
VariableSet varSet = getVariables(source,type);
for(VariableSet::iterator itv=varSet.begin();
itv!=varSet.end();
++itv)
{
if( itv->getType()==type ) return (*itv);
}
Exception e("The desirable variable not exist int the solver.");
GPSTK_THROW(e);
return Variable();
} // End of method 'GeneralConstraint::getVariables(...'
GeneralConstraint& GeneralConstraint::changeState(
const VariableList& varList,
const Matrix<double>& convertMat)
{
VariableSet allVariable = getCurrentUnknowns();
// check input
int varNum(0);
for(VariableList::const_iterator it = varList.begin();
it!=varList.end();
++it)
{
if(allVariable.find(*it)==allVariable.end())
{
Exception e("The variable doesn't exist in the solver.");
GPSTK_THROW(e);
}
varNum++;
}
if(varNum!=convertMat.rows() || varNum!=convertMat.cols())
{
Exception e("The size of input doesn't match.");
GPSTK_THROW(e);
}
const int numOfVar(varNum);
Vector<double> vectorOfSolution(numOfVar,0.0);
Matrix<double> matrixOfCovariance(numOfVar,numOfVar,0.0);
int i = 0;
for(VariableList::const_iterator iti = varList.begin();
iti != varList.end();
++iti)
{
vectorOfSolution(i) = solver.getSolution(*iti);
VariableList tempList(varList);
int j(0);
for(VariableList::iterator itj = tempList.begin();
itj!=tempList.end();
++itj)
{
matrixOfCovariance(i,j) = solver.getCovariance(*iti,*itj);
j++;
}
i++;
}
Vector<double> solution = convertMat*vectorOfSolution;
Matrix<double> covariance = convertMat*matrixOfCovariance
*transpose(convertMat);
i = 0;
for(VariableList::const_iterator iti=varList.begin();
iti!=varList.end();
++iti)
{
setSolution(*iti,solution(i));
std::list<Variable> tempList(varList);
int j(0);
for(std::list<Variable>::iterator itj = tempList.begin();
itj!=tempList.end();
++itj)
{
setCovariance(*iti,*itj,covariance(i,j));
j++;
}
i++;
}
return (*this);
} // Ebd if method 'GeneralConstraint::changeState()'
void Stepper::unregisterProcess( Process* aProcess )
{
ProcessVector::iterator ip( std::find( theProcessVector.begin(),
theProcessVector.end(),
aProcess ) );
if( ip == theProcessVector.end() )
{
THROW_EXCEPTION_INSIDE( NotFound,
asString() + ": Failed to dissociate [" +
aProcess->asString() + "] (no such Process is "
"associated to this stepper)" );
}
typedef std::set< Variable* > VariableSet;
VariableSet aVarSet;
Process::VariableReferenceVector const& aVarRefVector(
aProcess->getVariableReferenceVector() );
std::transform( aVarRefVector.begin(), aVarRefVector.end(),
inserter( aVarSet, aVarSet.begin() ),
std::mem_fun_ref( &VariableReference::getVariable ) );
VariableVector aNewVector;
VariableVector::size_type aReadWriteVariableOffset,
aReadOnlyVariableOffset;
for( VariableVector::iterator i( theVariableVector.begin() ),
e( theVariableVector.begin()
+ theReadWriteVariableOffset );
i != e; ++i )
{
VariableSet::iterator j( aVarSet.find( *i ) );
if ( j != aVarSet.end() )
aVarSet.erase( j );
else
aNewVector.push_back( *i );
}
aReadWriteVariableOffset = aNewVector.size();
for( VariableVector::iterator i( theVariableVector.begin()
+ theReadWriteVariableOffset ),
e( theVariableVector.begin()
+ theReadOnlyVariableOffset );
i != e; ++i )
{
VariableSet::iterator j( aVarSet.find( *i ) );
if ( j != aVarSet.end() )
aVarSet.erase( j );
else
aNewVector.push_back( *i );
}
aReadOnlyVariableOffset = aNewVector.size();
for( VariableVector::iterator i( theVariableVector.begin()
+ theReadOnlyVariableOffset ),
e( theVariableVector.end() );
i != e; ++i )
{
VariableSet::iterator j( aVarSet.find( *i ) );
if ( j != aVarSet.end() )
aVarSet.erase( j );
else
aNewVector.push_back( *i );
}
theVariableVector.swap( aNewVector );
theReadWriteVariableOffset = aReadWriteVariableOffset;
theReadOnlyVariableOffset = aReadOnlyVariableOffset;
theProcessVector.erase( ip );
}
