VariableSet GeneralConstraint::differenceVariables( 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);
}
for(VariableSet::const_iterator it=vs2.begin();
it!=vs2.end();
++it)
{
VariableSet::const_iterator it2 = vs1.find(*it);
if(it2==vs1.end()) tempSet.insert(*it);
}
return tempSet;
} // End of method 'GeneralConstraint::differenceVariables()'
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 );
}
}
}
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 );
}
