Constraints constraintsRemovingRedundantEqualities(Variables const &vars, Constraints const &constraints) { EqualityConstraintSet equalitySets(vars); Constraints cs = Constraints(constraints.size()); int csSize = 0; for (unsigned i = 0; i < constraints.size(); ++i) { Constraint *c = constraints[i]; if (c->equality) { if (!equalitySets.isRedundant(c->left, c->right, c->gap)) { // Only add non-redundant equalities equalitySets.mergeSets(c->left, c->right, c->gap); cs[csSize++] = c; } } else { // Add all non-equalities cs[csSize++] = c; } } cs.resize(csSize); return cs; }
void HAPIHapticShape::getConstraints( const Vec3 &point, Constraints &constraints, Collision::FaceType face, HAPIFloat radius ) { if( have_transform ) { unsigned int nr_constraints = constraints.size(); Vec3 local_point = toLocal( point ); Vec3 scale = inverse.getScalePart(); HAPIFloat s = max( scale.x, max( scale.y, scale.z ) ); getConstraintsOfShape( local_point, constraints, face, radius * s ); if( non_uniform_scaling ) { for( unsigned int i = nr_constraints; i < constraints.size(); ++i ) { PlaneConstraint &pc = constraints[i]; pc.normal = transform.getRotationPart() * pc.normal; pc.normal.x *= scale.x; pc.normal.y *= scale.y; pc.normal.z *= scale.z; pc.normal = inverse.getRotationPart() * pc.normal; pc.normal = transform.getRotationPart() * pc.normal; pc.normal.normalizeSafe(); pc.point = transform * pc.point; } } else { for( unsigned int i = nr_constraints; i < constraints.size(); ++i ) { PlaneConstraint &pc = constraints[i]; pc.normal = transform.getRotationPart() * pc.normal; pc.point = transform * pc.point; } } } else { getConstraintsOfShape( point, constraints, face, radius ); } }
Constraints::Constraints (Constraints &cs): std::map <std::string, ConstraintPtr > (cs) { for (Constraints::iterator iter = cs.begin (); iter != cs.end (); iter++) { Constraint *con = createConstraint (iter->first.c_str ()); // can "compare" chars, as they are const char* if (con->getName () == CONSTRAINT_TIME || con->getName () == CONSTRAINT_AIRMASS || con->getName () == CONSTRAINT_ZENITH_DIST || con->getName () == CONSTRAINT_HA || con->getName () == CONSTRAINT_LDISTANCE || con->getName () == CONSTRAINT_LALTITUDE || con->getName () == CONSTRAINT_LPHASE || con->getName () == CONSTRAINT_SDISTANCE || con->getName () == CONSTRAINT_SALTITUDE) { ((ConstraintInterval *) con)->copyIntervals (((ConstraintInterval *) iter->second->th ())); } else if (con->getName () == CONSTRAINT_MAXREPEATS) { ((ConstraintMaxRepeat *) con)->copyConstraint (((ConstraintMaxRepeat *) iter->second->th ())); } else { std::cerr << "unsuported constraint type in copy constructor " __FILE__ ":" << __LINE__ << std::endl; exit (10); } (*this)[iter->first] = ConstraintPtr (con); } }
bool hasConstraint(const ConstraintPtr& c, const Constraints& cs) { Constraints r; for (Constraints::const_iterator ci = cs.begin(); ci != cs.end(); ++ci) { if (*c == **ci) { return true; } } return false; }
Constraints removeConstraint(const ConstraintPtr& c, const Constraints& cs) { Constraints r; for (Constraints::const_iterator ci = cs.begin(); ci != cs.end(); ++ci) { if (!(*c == **ci)) { r.push_back(*ci); } } return r; }
void HapticTriangleSet::getConstraintsOfShape( const Vec3 &point, Constraints &constraints, Collision::FaceType face, HAPIFloat radius ) { if( triangles.size() > 0 ) { unsigned int size = constraints.size(); if( ( convex == CONVEX_FRONT && face == Collision::FRONT ) || ( convex == CONVEX_BACK && face == Collision::BACK ) ) { PlaneConstraint constraint; PlaneConstraint best; HAPIFloat best_sqr_dist = 0; bool first_constraint = true; // triangle set is convex so only need the closest constraint for( std::vector< Collision::Triangle >::iterator i = triangles.begin(); i != triangles.end(); ++i ) { //unsigned int i = 0; i < triangles.size(); ++i ) { Collision::Triangle &t = (*i); //*triangles[i]; if( t.getConstraint( point, &constraint, face ) ) { if( first_constraint ) { best = constraint; Vec3 v = point - best.point; best_sqr_dist = v * v; first_constraint = false; } else { Vec3 v = point - constraint.point; HAPIFloat sqr_dist = v * v; if( sqr_dist < best_sqr_dist ) { best = constraint; best_sqr_dist = sqr_dist; } } } //constraint.clear(); } if( !first_constraint ) constraints.push_back( best ); } else { // not convex, so add constraints from all triangles. for( unsigned int i = 0; i < triangles.size(); ++i ) { Collision::Triangle &t = triangles[i]; t.getConstraints( point, constraints, face, radius ); } } for( unsigned int i = size; i < constraints.size(); ++i ) { PlaneConstraint &pc = constraints[i]; pc.haptic_shape.reset(this); } } }
void HapticPrimitiveTree::getConstraintsOfShape( const Vec3 &point, Constraints &constraints, Collision::FaceType face, HAPIFloat radius ) { unsigned int size = constraints.size(); tree->getConstraints( point, constraints, face, radius ); for( unsigned int i = size; i < constraints.size(); ++i ) { PlaneConstraint &pc = constraints[i]; pc.haptic_shape.reset(this); } }
void print(Constraints constraints) { int row = constraints.size() - 1; while (!constraints.empty()) { auto constraint = constraints.top(); constraints.pop(); std::cout << "Constraints[" << row-- << "]: "; for (auto c : constraint) { std::cout << c << " "; } std::cout << std::endl; } }
void TestFPGrowth::withConstraints() { QList<QStringList> transactions; transactions.append(QStringList() << "A" << "B" << "C" << "D"); transactions.append(QStringList() << "A" << "B"); transactions.append(QStringList() << "A" << "C"); transactions.append(QStringList() << "A" << "B" << "C"); transactions.append(QStringList() << "A" << "D"); transactions.append(QStringList() << "A" << "C" << "D"); transactions.append(QStringList() << "C" << "B"); transactions.append(QStringList() << "B" << "C"); transactions.append(QStringList() << "C" << "D"); transactions.append(QStringList() << "C" << "E"); Constraints constraints; constraints.addItemConstraint("A", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY); FPNode<SupportCount>::resetLastNodeID(); ItemIDNameHash itemIDNameHash; ItemNameIDHash itemNameIDHash; ItemIDList sortedFrequentItemIDs; FPGrowth * fpgrowth = new FPGrowth(transactions, 0.4 * transactions.size(), &itemIDNameHash, &itemNameIDHash, &sortedFrequentItemIDs); fpgrowth->setConstraints(constraints); QList<FrequentItemset> frequentItemsets = fpgrowth->mineFrequentItemsets(FPGROWTH_SYNC); // Characteristics about the transactions above, and the found results // (*after* applying filtering): // * support: // - A: 6 // - B: 3 // - C: 4 // - D: 3 // - E: 0 // * minimum support = 0.4 // * number of transactions: 10 // * absolute min support: 4 // * items qualifying: A, C // * frequent itemsets: {{A}, {A, C}} // Helpful for debugging/expanding this test. // Currently, this should match: // (({A(0)}, sup: 6), ({C(2), A(0)}, sup: 4)) //qDebug() << frequentItemsets; // Verify the results. QCOMPARE(frequentItemsets, QList<FrequentItemset>() << FrequentItemset(ItemIDList() << 0 , 6) << FrequentItemset(ItemIDList() << 2 << 0, 4) ); delete fpgrowth; }
IncSolver::IncSolver(Variables const &vs, Constraints const &cs) : m(cs.size()), cs(cs), n(vs.size()), vs(vs), needsScaling(false) { for(unsigned i=0;i<n;++i) { vs[i]->in.clear(); vs[i]->out.clear(); // Set needsScaling if any variables have a scale other than 1. needsScaling |= (vs[i]->scale != 1); } for(unsigned i=0;i<m;++i) { Constraint *c=cs[i]; c->left->out.push_back(c); c->right->in.push_back(c); c->needsScaling = needsScaling; } bs=new Blocks(vs); #ifdef LIBVPSC_LOGGING printBlocks(); //COLA_ASSERT(!constraintGraphIsCyclic(n,vs)); #endif inactive=cs; for(Constraints::iterator i=inactive.begin();i!=inactive.end();++i) { (*i)->active=false; } }
void HapticPrimitiveSet::getConstraintsOfShape( const Vec3 &point, Constraints &constraints, Collision::FaceType face, HAPIFloat radius ) { if( primitives.size() > 0 ) { unsigned int size = constraints.size(); for( unsigned int i = 0; i < primitives.size(); i++ ) { Collision::GeometryPrimitive *a_primitive = primitives[i]; a_primitive->getConstraints( point, constraints, face, radius ); } for( unsigned int i = size; i < constraints.size(); i ++ ) { PlaneConstraint &pc = constraints[i]; pc.haptic_shape.reset(this); } } }
void HapticLineSet::getConstraintsOfShape( const Vec3 &point, Constraints &constraints, Collision::FaceType face, HAPIFloat radius ) { if( lines.size() > 0 ) { unsigned int size = constraints.size(); for( unsigned int i = 0; i < lines.size(); ++i ) { Collision::LineSegment &l = lines[i]; l.getConstraints( point, constraints, face, radius ); } for( unsigned int i = size; i < constraints.size(); ++i ) { PlaneConstraint &pc = constraints[i]; pc.haptic_shape.reset(this); } } }
void HapticPointSet::getConstraintsOfShape( const Vec3 &point, Constraints &constraints, Collision::FaceType face, HAPIFloat radius ) { if( points.size() > 0 ) { unsigned int size = constraints.size(); for( unsigned int i = 0; i < points.size(); i++ ) { Collision::Point &temp_p = points[i]; temp_p.getConstraints( point, constraints, face, radius ); } for( unsigned int i = size; i < constraints.size(); i ++ ) { PlaneConstraint &pc = constraints[i]; pc.haptic_shape.reset(this); } } }
void HAPIHapticShape::getConstraintsOfShape( const Vec3 &point, Constraints &constraints, Collision::FaceType face, HAPIFloat radius ) { Vec3 cp, n, tc; closestPointOnShape( point, cp, n, tc ); Vec3 v = cp - point; if( radius < 0 || v * v <= radius * radius ) constraints.push_back( PlaneConstraint( cp, n, tc, this ) ); }
/* * Returns the active path between variables u and v... not back tracking over w */ bool Block::getActivePathBetween(Constraints& path, Variable const* u, Variable const* v, Variable const *w) const { if(u==v) return true; for (Cit_const c=u->in.begin();c!=u->in.end();++c) { if (canFollowLeft(*c,w)) { if(getActivePathBetween(path, (*c)->left, v, u)) { path.push_back(*c); return true; } } } for (Cit_const c=u->out.begin();c!=u->out.end();++c) { if (canFollowRight(*c,w)) { if(getActivePathBetween(path, (*c)->right, v, u)) { path.push_back(*c); return true; } } } return false; }
bool Block::getActiveDirectedPathBetween( Constraints& path, Variable const* u, Variable const* v) const { if(u==v) return true; for (Cit_const c=u->out.begin();c!=u->out.end();++c) { if(canFollowRight(*c,NULL)) { if(getActiveDirectedPathBetween(path,(*c)->right,v)) { path.push_back(*c); return true; } } } return false; }
void guess(Queens& queens, Constraints& constraints) { auto row = queens.size(); for (int column = 0; column < SIZE; ++column) { bool good = true; if (!constraints.empty() && constraints.top().find(column) != constraints.top().cend()) { good = false; } else { for (int other_row = 0; other_row < static_cast<int>(row); ++other_row) { if (queens[other_row] == column) { good = false; break; } if ((queens[other_row] + other_row) == (column + static_cast<int>(row)) || (queens[other_row] - other_row) == (column - static_cast<int>(row))) { good = false; break; } } } if (good) { queens.push_back(column); if (queens.size() > constraints.size()) { constraints.push(std::unordered_set<int>()); } return; } } if (constraints.size() > queens.size()) { constraints.pop(); } if (row == queens.size()) { constraints.top().insert(queens.back()); queens.pop_back(); } }
/** Find a path for the specified distance estimates. * @param out [out] the solution path */ static void handleEstimate(const Graph& g, const EstimateRecord& er, bool dirIdx, ContigPath& out) { if (er.estimates[dirIdx].empty()) return; ContigNode origin(er.refID, dirIdx); ostringstream vout_ss; ostream bitBucket(NULL); ostream& vout = opt::verbose > 0 ? vout_ss : bitBucket; vout << "\n* " << get(vertex_name, g, origin) << '\n'; unsigned minNumPairs = UINT_MAX; // generate the reachable set Constraints constraints; for (Estimates::const_iterator iter = er.estimates[dirIdx].begin(); iter != er.estimates[dirIdx].end(); ++iter) { ContigNode v = iter->first; const DistanceEst& ep = iter->second; minNumPairs = min(minNumPairs, ep.numPairs); constraints.push_back(Constraint(v, ep.distance + allowedError(ep.stdDev))); } vout << "Constraints:"; printConstraints(vout, g, constraints) << '\n'; ContigPaths solutions; unsigned numVisited = 0; constrainedSearch(g, origin, constraints, solutions, numVisited); bool tooComplex = numVisited >= opt::maxCost; bool tooManySolutions = solutions.size() > opt::maxPaths; set<ContigID> repeats = findRepeats(er.refID, solutions); if (!repeats.empty()) { vout << "Repeats:"; for (set<ContigID>::const_iterator it = repeats.begin(); it != repeats.end(); ++it) vout << ' ' << get(g_contigNames, *it); vout << '\n'; } unsigned numPossiblePaths = solutions.size(); if (numPossiblePaths > 0) vout << "Paths: " << numPossiblePaths << '\n'; for (ContigPaths::iterator solIter = solutions.begin(); solIter != solutions.end();) { vout << *solIter << '\n'; // Calculate the path distance to each node and see if // it is within the estimated distance. map<ContigNode, int> distanceMap = makeDistanceMap(g, origin, *solIter); // Remove solutions whose distance estimates are not correct. unsigned validCount = 0, invalidCount = 0, ignoredCount = 0; for (Estimates::const_iterator iter = er.estimates[dirIdx].begin(); iter != er.estimates[dirIdx].end(); ++iter) { ContigNode v = iter->first; const DistanceEst& ep = iter->second; vout << get(vertex_name, g, v) << ',' << ep << '\t'; map<ContigNode, int>::iterator dmIter = distanceMap.find(v); if (dmIter == distanceMap.end()) { // This contig is a repeat. ignoredCount++; vout << "ignored\n"; continue; } // translate distance by -overlap to match // coordinate space used by the estimate int actualDistance = dmIter->second; int diff = actualDistance - ep.distance; unsigned buffer = allowedError(ep.stdDev); bool invalid = (unsigned)abs(diff) > buffer; bool repeat = repeats.count(v.contigIndex()) > 0; bool ignored = invalid && repeat; if (ignored) ignoredCount++; else if (invalid) invalidCount++; else validCount++; vout << "dist: " << actualDistance << " diff: " << diff << " buffer: " << buffer << " n: " << ep.numPairs << (ignored ? " ignored" : invalid ? " invalid" : "") << '\n'; } if (invalidCount == 0 && validCount > 0) ++solIter; else solIter = solutions.erase(solIter); } vout << "Solutions: " << solutions.size(); if (tooComplex) vout << " (too complex)"; if (tooManySolutions) vout << " (too many solutions)"; vout << '\n'; ContigPaths::iterator bestSol = solutions.end(); int minDiff = 999999; for (ContigPaths::iterator solIter = solutions.begin(); solIter != solutions.end(); ++solIter) { map<ContigNode, int> distanceMap = makeDistanceMap(g, origin, *solIter); int sumDiff = 0; for (Estimates::const_iterator iter = er.estimates[dirIdx].begin(); iter != er.estimates[dirIdx].end(); ++iter) { ContigNode v = iter->first; const DistanceEst& ep = iter->second; if (repeats.count(v.contigIndex()) > 0) continue; map<ContigNode, int>::iterator dmIter = distanceMap.find(v); assert(dmIter != distanceMap.end()); int actualDistance = dmIter->second; int diff = actualDistance - ep.distance; sumDiff += abs(diff); } if (sumDiff < minDiff) { minDiff = sumDiff; bestSol = solIter; } vout << *solIter << " length: " << calculatePathLength(g, origin, *solIter) << " sumdiff: " << sumDiff << '\n'; } /** Lock the debugging stream. */ static pthread_mutex_t coutMutex = PTHREAD_MUTEX_INITIALIZER; pthread_mutex_lock(&coutMutex); stats.totalAttempted++; g_minNumPairs = min(g_minNumPairs, minNumPairs); if (tooComplex) { stats.tooComplex++; } else if (tooManySolutions) { stats.tooManySolutions++; } else if (numPossiblePaths == 0) { stats.noPossiblePaths++; } else if (solutions.empty()) { stats.noValidPaths++; } else if (repeats.count(er.refID) > 0) { vout << "Repeat: " << get(vertex_name, g, origin) << '\n'; stats.repeat++; } else if (solutions.size() > 1) { ContigPath path = constructAmbiguousPath(g, origin, solutions); if (!path.empty()) { if (opt::extend) extend(g, path.back(), back_inserter(path)); vout << path << '\n'; if (opt::scaffold) { out.insert(out.end(), path.begin(), path.end()); g_minNumPairsUsed = min(g_minNumPairsUsed, minNumPairs); } } stats.multiEnd++; } else { assert(solutions.size() == 1); assert(bestSol != solutions.end()); ContigPath& path = *bestSol; if (opt::verbose > 1) printDistanceMap(vout, g, origin, path); if (opt::extend) extend(g, path.back(), back_inserter(path)); out.insert(out.end(), path.begin(), path.end()); stats.uniqueEnd++; g_minNumPairsUsed = min(g_minNumPairsUsed, minNumPairs); } cout << vout_ss.str(); if (!out.empty()) assert(!out.back().ambiguous()); pthread_mutex_unlock(&coutMutex); }
/** Return the consensus sequence of the specified gap. */ static ContigPath fillGap(const Graph& g, const AmbPathConstraint& apConstraint, vector<bool>& seen, ofstream& outFasta) { if (opt::verbose > 1) cerr << "\n* " << get(vertex_name, g, apConstraint.source) << ' ' << apConstraint.dist << "N " << get(vertex_name, g, apConstraint.dest) << '\n'; Constraints constraints; constraints.push_back(Constraint(apConstraint.dest, apConstraint.dist + opt::distanceError)); ContigPaths solutions; unsigned numVisited = 0; constrainedSearch(g, apConstraint.source, constraints, solutions, numVisited); bool tooComplex = numVisited >= opt::maxCost; for (ContigPaths::iterator solIt = solutions.begin(); solIt != solutions.end(); solIt++) solIt->insert(solIt->begin(), apConstraint.source); ContigPath consensus; bool tooManySolutions = solutions.size() > opt::numBranches; if (tooComplex) { stats.tooComplex++; if (opt::verbose > 1) cerr << solutions.size() << " paths (too complex)\n"; } else if (tooManySolutions) { stats.numTooManySolutions++; if (opt::verbose > 1) cerr << solutions.size() << " paths (too many)\n"; } else if (solutions.empty()) { stats.numNoSolutions++; if (opt::verbose > 1) cerr << "no paths\n"; } else if (solutions.size() == 1) { if (opt::verbose > 1) cerr << "1 path\n" << solutions.front() << '\n'; stats.numMerged++; } else { assert(solutions.size() > 1); if (opt::verbose > 2) copy(solutions.begin(), solutions.end(), ostream_iterator<ContigPath>(cerr, "\n")); else if (opt::verbose > 1) cerr << solutions.size() << " paths\n"; consensus = align(g, solutions, outFasta); if (!consensus.empty()) { stats.numMerged++; // Mark contigs that are used in a consensus. markSeen(seen, solutions, true); if (opt::verbose > 1) cerr << consensus << '\n'; } else stats.notMerged++; } return consensus; }
void Sector::collision_static_constrains(MovingObject& object) { using namespace collision; float infinity = (std::numeric_limits<float>::has_infinity ? std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max()); Constraints constraints; Vector movement = object.get_movement(); Vector pressure = Vector(0,0); Rectf& dest = object.dest; for(int i = 0; i < 2; ++i) { collision_static(&constraints, Vector(0, movement.y), dest, object); if(!constraints.has_constraints()) break; // apply calculated horizontal constraints if(constraints.get_position_bottom() < infinity) { float height = constraints.get_height (); if(height < object.get_bbox().get_height()) { // we're crushed, but ignore this for now, we'll get this again // later if we're really crushed or things will solve itself when // looking at the vertical constraints pressure.y += object.get_bbox().get_height() - height; } else { dest.p2.y = constraints.get_position_bottom() - DELTA; dest.p1.y = dest.p2.y - object.get_bbox().get_height(); } } else if(constraints.get_position_top() > -infinity) { dest.p1.y = constraints.get_position_top() + DELTA; dest.p2.y = dest.p1.y + object.get_bbox().get_height(); } } if(constraints.has_constraints()) { if(constraints.hit.bottom) { dest.move(constraints.ground_movement); } if(constraints.hit.top || constraints.hit.bottom) { constraints.hit.left = false; constraints.hit.right = false; object.collision_solid(constraints.hit); } } constraints = Constraints(); for(int i = 0; i < 2; ++i) { collision_static(&constraints, movement, dest, object); if(!constraints.has_constraints()) break; // apply calculated vertical constraints float width = constraints.get_width (); if(width < infinity) { if(width + SHIFT_DELTA < object.get_bbox().get_width()) { // we're crushed, but ignore this for now, we'll get this again // later if we're really crushed or things will solve itself when // looking at the horizontal constraints pressure.x += object.get_bbox().get_width() - width; } else { float xmid = constraints.get_x_midpoint (); dest.p1.x = xmid - object.get_bbox().get_width()/2; dest.p2.x = xmid + object.get_bbox().get_width()/2; } } else if(constraints.get_position_right() < infinity) { dest.p2.x = constraints.get_position_right() - DELTA; dest.p1.x = dest.p2.x - object.get_bbox().get_width(); } else if(constraints.get_position_left() > -infinity) { dest.p1.x = constraints.get_position_left() + DELTA; dest.p2.x = dest.p1.x + object.get_bbox().get_width(); } } if(constraints.has_constraints()) { if( constraints.hit.left || constraints.hit.right || constraints.hit.top || constraints.hit.bottom || constraints.hit.crush ) object.collision_solid(constraints.hit); } // an extra pass to make sure we're not crushed vertically if (pressure.y > 0) { constraints = Constraints(); collision_static(&constraints, movement, dest, object); if(constraints.get_position_bottom() < infinity) { float height = constraints.get_height (); if(height + SHIFT_DELTA < object.get_bbox().get_height()) { CollisionHit h; h.top = true; h.bottom = true; h.crush = pressure.y > 16; object.collision_solid(h); } } } // an extra pass to make sure we're not crushed horizontally if (pressure.x > 0) { constraints = Constraints(); collision_static(&constraints, movement, dest, object); if(constraints.get_position_right() < infinity) { float width = constraints.get_width (); if(width + SHIFT_DELTA < object.get_bbox().get_width()) { CollisionHit h; h.top = true; h.bottom = true; h.left = true; h.right = true; h.crush = pressure.x > 16; object.collision_solid(h); } } } }
/* * s e t u p A u x i l i a r y Q P */ returnValue SQProblem::setupAuxiliaryQP ( SymmetricMatrix *H_new, Matrix *A_new, const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new ) { int i; int nV = getNV( ); int nC = getNC( ); returnValue returnvalue; if ( ( getStatus( ) == QPS_NOTINITIALISED ) || ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) ) { return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED ); } status = QPS_PREPARINGAUXILIARYQP; /* I) SETUP NEW QP MATRICES AND VECTORS: */ /* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure * that old optimal solution remains feasible for new QP data. */ /* Firstly, shift by -A'*x_opt and ... */ if ( nC > 0 ) { if ( A_new == 0 ) return THROWERROR( RET_INVALID_ARGUMENTS ); for ( i=0; i<nC; ++i ) { lbA[i] = -Ax_l[i]; ubA[i] = Ax_u[i]; } /* Set constraint matrix as well as ... */ setA( A_new ); /* ... secondly, shift by +A_new'*x_opt. */ for ( i=0; i<nC; ++i ) { lbA[i] += Ax[i]; ubA[i] += Ax[i]; } /* update constraint products. */ for ( i=0; i<nC; ++i ) { Ax_u[i] = ubA[i] - Ax[i]; Ax_l[i] = Ax[i] - lbA[i]; } } /* 2) Set new Hessian matrix, determine Hessian type and * regularise new Hessian matrix if necessary. */ /* a) Setup new Hessian matrix and determine its type. */ if ( H_new != 0 ) { setH( H_new ); hessianType = HST_UNKNOWN; if ( determineHessianType( ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); /* b) Regularise new Hessian if necessary. */ if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_SEMIDEF ) || ( usingRegularisation( ) == BT_TRUE ) ) { regVal = 0.0; /* reset previous regularisation */ if ( regulariseHessian( ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); } } else { if ( H != 0 ) return THROWERROR( RET_NO_HESSIAN_SPECIFIED ); } /* 3) Setup QP gradient. */ if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); /* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */ /* 1) Make a copy of current bounds/constraints ... */ Bounds oldBounds = bounds; Constraints oldConstraints = constraints; /* we're trying to find an active set with positive definite null * space Hessian twice: * - first for the current active set including all equalities * - second after moving all inactive variables to a bound * (depending on Options). This creates an empty null space and * is guaranteed to succeed. Thus this loop will exit after n_try=1. */ int n_try; for (n_try = 0; n_try < 2; ++n_try) { if (n_try > 0) { // the current active set leaves an indefinite null space Hessian // move all inactive variables to a bound, creating an empty null space for (int ii = 0; ii < nV; ++ii) if (oldBounds.getStatus (ii) == ST_INACTIVE) oldBounds.setStatus (ii, options.initialStatusBounds); } /* ... reset them ... */ bounds.init( nV ); constraints.init( nC ); /* ... and set them up afresh. */ if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); /* 2) Setup TQ factorisation. */ if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); // check for equalities that have become bounds ... for (int ii = 0; ii < nC; ++ii) { if (oldConstraints.getType (ii) == ST_EQUALITY && constraints.getType (ii) == ST_BOUNDED) { if (oldConstraints.getStatus (ii) == ST_LOWER && y[nV+ii] < 0.0) oldConstraints.setStatus (ii, ST_UPPER); else if (oldConstraints.getStatus (ii) == ST_UPPER && y[nV+ii] > 0.0) oldConstraints.setStatus (ii, ST_LOWER); } } // ... and do the same also for the bounds! for (int ii = 0; ii < nV; ++ii) { if (oldBounds.getType(ii) == ST_EQUALITY && bounds.getType(ii) == ST_BOUNDED) { if (oldBounds.getStatus(ii) == ST_LOWER && y[ii] < 0.0) oldBounds.setStatus(ii, ST_UPPER); else if (oldBounds.getStatus(ii) == ST_UPPER && y[ii] > 0.0) oldBounds.setStatus(ii, ST_LOWER); } } /* 3) Setup old working sets afresh (updating TQ factorisation). */ if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); /* Factorise projected Hessian * this now handles all special cases (no active bounds/constraints, no nullspace) */ returnvalue = computeProjectedCholesky( ); /* leave the loop if decomposition was successful, i.e. we have * found an active set with positive definite null space Hessian */ if ( returnvalue == SUCCESSFUL_RETURN ) break; } /* adjust lb/ub if we changed the old active set in the second try */ if (n_try > 0) { // as per setupAuxiliaryQPbounds assumptions ... oh the troubles for (int ii = 0; ii < nC; ++ii) Ax_l[ii] = Ax_u[ii] = Ax[ii]; setupAuxiliaryQPbounds (&bounds, &constraints, BT_FALSE); } status = QPS_AUXILIARYQPSOLVED; return SUCCESSFUL_RETURN; }
int ParticleSystem_Interactive::collision(Particle* object, Vector movement) { using namespace collision; // calculate rectangle where the object will move float x1, x2; float y1, y2; x1 = object->pos.x; x2 = x1 + 32 + movement.x; y1 = object->pos.y; y2 = y1 + 32 + movement.y; bool water = false; // test with all tiles in this rectangle int starttilex = int(x1-1) / 32; int starttiley = int(y1-1) / 32; int max_x = int(x2+1); int max_y = int(y2+1); Rectf dest(x1, y1, x2, y2); dest.move(movement); Constraints constraints; for(std::list<TileMap*>::const_iterator i = Sector::current()->solid_tilemaps.begin(); i != Sector::current()->solid_tilemaps.end(); ++i) { TileMap* solids = *i; // FIXME Handle a nonzero tilemap offset for(int x = starttilex; x*32 < max_x; ++x) { for(int y = starttiley; y*32 < max_y; ++y) { const Tile* tile = solids->get_tile(x, y); if(!tile) continue; // skip non-solid tiles, except water if(! (tile->getAttributes() & (Tile::WATER | Tile::SOLID))) continue; Rectf rect = solids->get_tile_bbox(x, y); if(tile->is_slope ()) { // slope tile AATriangle triangle = AATriangle(rect, tile->getData()); if(rectangle_aatriangle(&constraints, dest, triangle)) { if(tile->getAttributes() & Tile::WATER) water = true; } } else { // normal rectangular tile if(intersects(dest, rect)) { if(tile->getAttributes() & Tile::WATER) water = true; set_rectangle_rectangle_constraints(&constraints, dest, rect); } } } } } // TODO don't use magic numbers here... // did we collide at all? if(!constraints.has_constraints()) return -1; const CollisionHit& hit = constraints.hit; if (water) { return 0; //collision with water tile - don't draw splash } else { if (hit.right || hit.left) { return 2; //collision from right } else { return 1; //collision from above } } return 0; }
/** Example for qpOASES main function using the QProblem class. */ int main( ) { USING_NAMESPACE_QPOASES /* Setup data of first QP. */ real_t H[4*4] = { 1.0, 0.0, 0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.5, 0.0, 0.0, 1.0 }; real_t A[3*4] = { 1.0, 1.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 1.0 }; real_t g[4] = { 1.5, 1.0, -1.0, -1.0 }; real_t lb[4] = { 0.5, -2.0, 0.0, 0.0 }; real_t ub[4] = { 1.0, 2.0, 1.0, 0.5 }; real_t lbA[3] = { -1.0, -1.0, -1.0 }; real_t ubA[3] = { 0.0, 0.25, 1.0 }; /* Setting up QProblem object. */ QProblem example( 4,3 ); Options options; example.setOptions( options ); /* Solve first QP. */ int nWSR = 10; example.init( H,g,A,lb,ub,lbA,ubA, nWSR ); /* Get and print solution of second QP. */ real_t xOpt[4]; real_t yOpt[4+3]; example.getPrimalSolution( xOpt ); example.getDualSolution( yOpt ); print( xOpt,4,"xOpt" ); print( yOpt,4+3,"yOpt" ); printf( "objVal = %e\n\n", example.getObjVal() ); /* Compute KKT tolerances */ real_t stat, feas, cmpl; getKKTResidual( 4,3, H,g,A,lb,ub,lbA,ubA, xOpt,yOpt, stat,feas,cmpl ); printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl ); QPOASES_TEST_FOR_TRUE( stat <= 1e-15 ); QPOASES_TEST_FOR_TRUE( feas <= 1e-15 ); QPOASES_TEST_FOR_TRUE( cmpl <= 1e-15 ); /* Solve first QP again (with optimal guess for working set). */ Bounds prevBounds; Constraints prevConstraints; example.getBounds( prevBounds ); example.getConstraints( prevConstraints ); nWSR = 10; example.hotstart( g,lb,ub,lbA,ubA, nWSR,0,&prevBounds,&prevConstraints ); /* Get and print solution of second QP. */ example.getPrimalSolution( xOpt ); example.getDualSolution( yOpt ); print( xOpt,4,"xOpt" ); print( yOpt,4+3,"yOpt" ); printf( "objVal = %e\n\n", example.getObjVal() ); /* Compute KKT tolerances */ getKKTResidual( 4,3, H,g,A,lb,ub,lbA,ubA, xOpt,yOpt, stat,feas,cmpl ); printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl ); QPOASES_TEST_FOR_TRUE( stat <= 1e-15 ); QPOASES_TEST_FOR_TRUE( feas <= 1e-15 ); QPOASES_TEST_FOR_TRUE( cmpl <= 1e-15 ); /* Solve first QP again (with inaccurate guess for working set). */ prevBounds.print(); prevBounds.rotate(1); //prevBounds.moveFixedToFree(0); prevBounds.print(); prevConstraints.print(); //prevConstraints.moveInactiveToActive(0,ST_LOWER); prevConstraints.moveActiveToInactive(1); prevConstraints.print(); nWSR = 10; example.hotstart( g,lb,ub,lbA,ubA, nWSR,0,&prevBounds,&prevConstraints ); /* Get and print solution of second QP. */ example.getPrimalSolution( xOpt ); example.getDualSolution( yOpt ); print( xOpt,4,"xOpt" ); print( yOpt,4+3,"yOpt" ); printf( "objVal = %e\n\n", example.getObjVal() ); /* Compute KKT tolerances */ getKKTResidual( 4,3, H,g,A,lb,ub,lbA,ubA, xOpt,yOpt, stat,feas,cmpl ); printf( "stat = %e\nfeas = %e\ncmpl = %e\n", stat,feas,cmpl ); QPOASES_TEST_FOR_TRUE( stat <= 1e-15 ); QPOASES_TEST_FOR_TRUE( feas <= 1e-15 ); QPOASES_TEST_FOR_TRUE( cmpl <= 1e-15 ); return TEST_PASSED; }
void ikTest() { cout << "--------------------------------------------" << endl; cout << "| Testing Standard Damped Least Squares IK |" << endl; cout << "--------------------------------------------" << endl; Robot parseTest("../urdf/huboplus.urdf"); parseTest.linkage("Body_RSP").name("RightArm"); parseTest.linkage("Body_LSP").name("LeftArm"); parseTest.linkage("Body_RHY").name("RightLeg"); parseTest.linkage("Body_LHY").name("LeftLeg"); string limb = "LeftArm"; VectorXd targetValues, jointValues, restValues, storedVals; targetValues.resize(parseTest.linkage(limb).nJoints()); jointValues.resize(parseTest.linkage(limb).nJoints()); restValues.resize(parseTest.linkage(limb).nJoints()); restValues.setZero(); if(limb == "RightLeg" || limb == "LeftLeg") { restValues[0] = 0; restValues[1] = 0; restValues[2] = -0.15; restValues[3] = 0.3; restValues[4] = -0.15; restValues[5] = 0; } else if(limb == "RightArm" || limb == "LeftArm") { if(limb=="RightArm") restValues[0] = -30*M_PI/180; else restValues[0] = 30*M_PI/180; restValues[1] = 0; restValues[2] = 0; restValues[3] = -30*M_PI/180; restValues[4] = 0; restValues[5] = 0; } bool totallyRandom = false; int resolution = 1000; double scatterScale = 0.05; int tests = 10000; Constraints constraints; constraints.restingValues(restValues); constraints.performNullSpaceTask = false; constraints.maxAttempts = 1; constraints.maxIterations = 50; constraints.convergenceTolerance = 0.001; // constraints.wrapToJointLimits = true; // constraints.wrapSolutionToJointLimits = true; constraints.wrapToJointLimits = false; constraints.wrapSolutionToJointLimits = false; TRANSFORM target; int wins = 0, jumps = 0; srand(time(NULL)); clock_t time; time = clock(); for(int k=0; k<tests; k++) { for(int i=0; i<parseTest.linkage(limb).nJoints(); i++) { int randomVal = rand(); targetValues(i) = ((double)(randomVal%resolution))/((double)resolution-1) *(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min()) + parseTest.linkage(limb).joint(i).min(); // cout << "vals: " << randomVal; randomVal = rand(); // cout << "\t:\t" << randomVal << endl; /////////////////////// TOTALLY RANDOM TEST /////////////////////////// if(totallyRandom) jointValues(i) = ((double)(randomVal%resolution))/((double)resolution-1) *(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min()) + parseTest.linkage(limb).joint(i).min(); ////////////////////// NOT COMPLETELY RANDOM TEST ////////////////////// else // jointValues(i) = targetValues(i) + // ((double)(2*rand()%resolution)/((double)resolution-1)-1)*scatterScale // *(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min()); jointValues(i) = targetValues(i) + ((double)(2*rand()%resolution)/((double)resolution-1)-1)*scatterScale; parseTest.linkage(limb).joint(i).value(targetValues(i)); } target = parseTest.linkage(limb).tool().respectToRobot(); parseTest.linkage(limb).values(jointValues); // cout << "Start:" << endl; // cout << parseTest.linkage(limb).tool().respectToRobot().matrix() << endl; // cout << "Target:" << endl; // cout << target.matrix() << endl; storedVals = jointValues; rk_result_t result = parseTest.dampedLeastSquaresIK_linkage(limb, jointValues, target, constraints); if( result == RK_SOLVED ) wins++; // if(false) if(!totallyRandom && (result != RK_SOLVED || (storedVals-jointValues).norm() > 1.1*(storedVals-targetValues).norm()) ) { if( result == RK_SOLVED ) cout << "SOLVED" << endl; else { Vector3d Terr = target.translation() - parseTest.linkage(limb).tool().respectToRobot().translation(); AngleAxisd aaerr; aaerr = target.rotation()*parseTest.linkage(limb).tool().respectToRobot().rotation().transpose(); Vector3d Rerr; if(fabs(aaerr.angle()) <= M_PI) Rerr = aaerr.angle()*aaerr.axis(); else Rerr = (aaerr.angle()-2*M_PI)*aaerr.axis(); cout << "FAILED (" << Terr.norm() << ", " << Rerr.norm() << ")" << endl; } cout << "Start: " << storedVals.transpose() << endl; cout << "Solve: " << jointValues.transpose() << endl; cout << "Goal : " << targetValues.transpose() << endl; cout << "Delta: " << (storedVals-targetValues).transpose() << "\t(" << (storedVals-targetValues).norm() << ")" << endl; cout << "Diff : " << (storedVals-jointValues).transpose() << "\t(" << (storedVals-jointValues).norm() << ")" << endl << endl; if( result == RK_SOLVED ) jumps++; } // cout << "End:" << endl; // cout << parseTest.linkage(limb).tool().respectToRobot().matrix() << endl; // cout << "Target Joint Values: " << targetValues.transpose() << endl; // if(result != RK_SOLVED) if(false) { cout << "Start values: "; for(int p=0; p<storedVals.size(); p++) cout << storedVals[p] << "\t\t"; cout << endl; cout << "Target values: "; for(int p=0; p<targetValues.size(); p++) cout << targetValues[p] << "\t\t"; cout << endl; cout << "Final Joint Values: "; for(int p=0; p<jointValues.size(); p++) cout << jointValues[p] << "\t\t"; cout << endl; cout << "Norm: " << ( jointValues - targetValues ).norm() << endl; cout << "Error: " << (parseTest.linkage(limb).tool().respectToRobot().translation() - target.translation()).norm() << endl; } } clock_t endTime; endTime = clock(); cout << "Time: " << (endTime - time)/((double)CLOCKS_PER_SEC*tests) << " : " << (double)CLOCKS_PER_SEC*tests/(endTime-time) << endl; cout << "Win: " << ((double)wins)/((double)tests)*100 << "%" << endl; if(!totallyRandom) cout << "Jump: " << ((double)jumps)/((double)tests)*100 << "%" << endl; }
void Sector::collision_static_constrains(MovingObject& object) { using namespace collision; float infinity = (std::numeric_limits<float>::has_infinity ? std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max()); Constraints constraints; Vector movement = object.get_movement(); Rect& dest = object.dest; float owidth = object.get_bbox().get_width(); float oheight = object.get_bbox().get_height(); for(int i = 0; i < 2; ++i) { collision_static(&constraints, Vector(0, movement.y), dest, object); if(!constraints.has_constraints()) break; // apply calculated horizontal constraints if(constraints.bottom < infinity) { float height = constraints.bottom - constraints.top; if(height < oheight) { // we're crushed, but ignore this for now, we'll get this again // later if we're really crushed or things will solve itself when // looking at the vertical constraints } dest.p2.y = constraints.bottom - DELTA; dest.p1.y = dest.p2.y - oheight; } else if(constraints.top > -infinity) { dest.p1.y = constraints.top + DELTA; dest.p2.y = dest.p1.y + oheight; } } if(constraints.has_constraints()) { if(constraints.hit.bottom) { dest.move(constraints.ground_movement); } if(constraints.hit.top || constraints.hit.bottom) { constraints.hit.left = false; constraints.hit.right = false; object.collision_solid(constraints.hit); } } constraints = Constraints(); for(int i = 0; i < 2; ++i) { collision_static(&constraints, movement, dest, object); if(!constraints.has_constraints()) break; // apply calculated vertical constraints if(constraints.right < infinity) { float width = constraints.right - constraints.left; if(width + SHIFT_DELTA < owidth) { printf("Object %p crushed horizontally... L:%f R:%f\n", &object, constraints.left, constraints.right); CollisionHit h; h.left = true; h.right = true; h.crush = true; object.collision_solid(h); } else { dest.p2.x = constraints.right - DELTA; dest.p1.x = dest.p2.x - owidth; } } else if(constraints.left > -infinity) { dest.p1.x = constraints.left + DELTA; dest.p2.x = dest.p1.x + owidth; } } if(constraints.has_constraints()) { if( constraints.hit.left || constraints.hit.right || constraints.hit.top || constraints.hit.bottom || constraints.hit.crush ) object.collision_solid(constraints.hit); } // an extra pass to make sure we're not crushed horizontally constraints = Constraints(); collision_static(&constraints, movement, dest, object); if(constraints.bottom < infinity) { float height = constraints.bottom - constraints.top; if(height + SHIFT_DELTA < oheight) { printf("Object %p crushed vertically...\n", &object); CollisionHit h; h.top = true; h.bottom = true; h.crush = true; object.collision_solid(h); } } }