コード例 #1
0
ファイル: constraint.cpp プロジェクト: SiteView/NNMQT
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;
}
コード例 #2
0
ファイル: HAPIHapticShape.cpp プロジェクト: rpavlik/hapi
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 );
  }
}
コード例 #3
0
ファイル: constraints.cpp プロジェクト: welterde/rts2
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);
	}
}
コード例 #4
0
ファイル: typepreds.C プロジェクト: lipei0330/hobbes
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;
}
コード例 #5
0
ファイル: typepreds.C プロジェクト: lipei0330/hobbes
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;
}
コード例 #6
0
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);
    }
  }
}
コード例 #7
0
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);
  }
}
コード例 #8
0
ファイル: sort-main.cpp プロジェクト: prismskylabs/cpp4py-wip
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;
    }
}
コード例 #9
0
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;
}
コード例 #10
0
ファイル: vpsc.cpp プロジェクト: P-N-L/adaptagrams
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;
    }
}
コード例 #11
0
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);
    }
  }
}
コード例 #12
0
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);
    }
  }
}
コード例 #13
0
ファイル: HapticPointSet.cpp プロジェクト: rpavlik/hapi
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);
    }
  }
}
コード例 #14
0
ファイル: HAPIHapticShape.cpp プロジェクト: rpavlik/hapi
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 ) );
}
コード例 #15
0
ファイル: vpsc.cpp プロジェクト: P-N-L/adaptagrams
/*
 * 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;
}
コード例 #16
0
ファイル: vpsc.cpp プロジェクト: P-N-L/adaptagrams
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;
}
コード例 #17
0
ファイル: sort-main.cpp プロジェクト: prismskylabs/cpp4py-wip
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();
    }
}
コード例 #18
0
ファイル: SimpleGraph.cpp プロジェクト: genome-vendor/abyss
/** 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);
}
コード例 #19
0
ファイル: PathConsensus.cpp プロジェクト: Hensonmw/abyss
/** 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;
}
コード例 #20
0
ファイル: sector.cpp プロジェクト: hongtaox/supertux
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);
      }
    }
  }
}
コード例 #21
0
ファイル: SQProblem.cpp プロジェクト: bmagyar/OpenSoT
/*
 *	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;
}
コード例 #22
0
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;
}
コード例 #23
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;
}
コード例 #24
0
ファイル: ikUnitTest.cpp プロジェクト: a-price/robotKin
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;



}
コード例 #25
0
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);
    }
  }
}