// Method to set multi-constraints
ConstraintSystem& ConstraintSystem::setConstraint(const VariableSet& varSet,
const Vector<double>& prefit)
{
// Fist, we check the size of inputs
const int size = varSet.size();
if( prefit.size()!=size )
{
Exception e("The input size doesn't match.");
GPSTK_THROW(e);
}
clearConstraint();
int i(0);
for(VariableSet::const_iterator it = varSet.begin();
it != varSet.end();
++it)
{
Constraint constraint;
constraint.header.prefit = prefit[i];
constraint.body[*it] = 1.0;
addConstraint(constraint);
i++;
}
return (*this);
} // End of method 'ConstraintSystem::setConstraint()'
// Remove a single constraint
ConstraintSystem& ConstraintSystem::removeConstraint(
const Constraint& constraint )
{
ConstraintList backupList;
for(ConstraintList::iterator it= constraintList.begin();
it!=constraintList.end();
++it)
{
bool isEqual(true);
if(it->header.prefit!=constraint.header.prefit) isEqual=false;
if(it->header.variance!=constraint.header.variance) isEqual=false;
if(it->body != constraint.body) isEqual = false;
if( !isEqual ) backupList.push_back(*it);
}
clearConstraint();
for(ConstraintList::iterator it= backupList.begin();
it!=backupList.end();
++it)
{
addConstraint(*it);
}
return (*this);
} // End of method 'ConstraintSystem::removeConstraint()'
/******************************************************************************
Takes a pointer to the SnapConstraint object. A SnapConstraint can either
persist or be autodeleted when cleared. The library will immediately begin
rendering this constraint.
******************************************************************************/
void SnapConstraintsAPI::setConstraint(SnapConstraint *pConstraint)
{
if (m_pConstraint != pConstraint)
{
clearConstraint();
}
m_pConstraint = pConstraint;
if (m_pConstraint)
{
m_pConstraint->setIsDone(false);
m_pConstraint->onStartConstraint();
}
}
// Method to set multi-constraints
ConstraintSystem& ConstraintSystem::setConstraint(const VariableSet& varSet,
const Vector<double>& prefit,
const Matrix<double>& design)
{
// Fist, we check the size of inputs
const int size = varSet.size();
if( (prefit.size()!=size) || design.rows()!=size || design.cols()!=size)
{
Exception e("The input size doesn't match.");
GPSTK_THROW(e);
}
clearConstraint();
std::vector<Variable> vars;
for(VariableSet::const_iterator it = varSet.begin();
it != varSet.end();
++it)
{
vars.push_back(*it);
}
for(size_t i=0; i<vars.size(); i++)
{
VariableDataMap dataMap;
for(int k=0;k<size;k++)
{
if(design[i][k]!=0.0) dataMap[ vars[k] ] = design[i][k];
}
Constraint constraint;
constraint.header.prefit = prefit[i];
constraint.body = dataMap;
addConstraint(constraint);
}
return (*this);
} // End of method 'ConstraintSystem::setConstraint()'
/******************************************************************************
Call this method every servoloop tick to update the constrained position of
the device. This facility does not actually render a force. It is the
responsibility of the caller to then use the constrained proxy location to
render a force based on the current device position.
******************************************************************************/
bool SnapConstraintsAPI::updateConstraint(const hduVector3Dd &devicePt)
{
bool bConstrained = false;
if (m_pConstraint)
{
bConstrained = m_pConstraint->applyConstraint(devicePt, m_proxyPt);
if (m_pConstraint->isDone())
{
clearConstraint();
}
}
if (!bConstrained)
{
m_proxyPt = devicePt;
}
return bConstrained;
}
