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)); } }