vector<double> BPMPDModel::getVarValues(const VarVector& vars) const {
vector<double> out(vars.size());
for (int i=0; i < vars.size(); ++i) {
int varind = vars[i].var_rep->index;
out[i] = m_soln[varind];
}
return out;
}
RotationContinuityQuadraticCost::RotationContinuityQuadraticCost(const VarVector& vars, double coeff, NeedleProblemHelperPtr helper) : Cost("RotationContinuity"), vars(vars), coeff(coeff), helper(helper)
{
for (int i = 1; i < vars.size(); ++i)
{
exprInc(expr, exprMult(exprSquare(exprSub(AffExpr(vars[i]), AffExpr(vars[i-1]))), coeff));
}
}
bool Variable::unify(const GlobalStorage *g, const Value& t, const VarVector& vars, ValueVector& vals) const
{
// TODO: constant access would be nice
for (size_t i = 0; i < vars.size(); ++i)
{
// if variable is bound
if (vars[i] == uid_)
{
// variable is not set, then set, else compare
if (vals[i].type_ == Value::UNDEF)
{
vals[i] = t;
return true;
}
else
return vals[i].equal(t);
}
}
FAIL(true);
}
inline OutVecType getVec1(const vector<double>& x, const VarVector& vars) {
OutVecType out(vars.size());
for (unsigned i=0; i < vars.size(); ++i) out[i] = x[vars[i].var_rep->index];
return out;
}
inline void setVec(DblVec& x, const VarVector& vars, const VecType& vals) {
assert(vars.size() == vals.size());
for (int i = 0; i < vars.size(); ++i) {
x[vars[i].var_rep->index] = vals[i];
}
}
DblVec getDblVec(const vector<double>& x, const VarVector& vars) {
DblVec out(vars.size());
for (unsigned i=0; i < vars.size(); ++i) out[i] = x[vars[i].var_rep->index];
return out;
}
void BPMPDModel::removeVars(const VarVector& vars) {
vector<int>inds = vars2inds(vars);
for (int i=0; i < vars.size(); ++i) vars[i].var_rep->removed = true;
}
RotationQuadraticCost::RotationQuadraticCost(const VarVector& vars, double coeff, NeedleProblemHelperPtr helper) : Cost("Rotation"), vars(vars), coeff(coeff), helper(helper) {
for (int i = 0; i < vars.size(); ++i) {
exprInc(expr, exprMult(exprSquare(vars[i]), coeff));
}
}
