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