Ejemplo n.º 1
0
//! Return the range of numbers that might be used with this format to
//! represent a number within `x`.
Interval FloatFormat::convert (const Interval& x) const
{
	Interval ret;
	Interval tmp = x;

	if (x.hasNaN())
	{
		// If NaN might be supported, NaN is a legal return value
		if (m_hasNaN != NO)
			ret |= TCU_NAN;

		// If NaN might not be supported, any (non-NaN) value is legal,
		// _subject_ to clamping. Hence we modify tmp, not ret.
		if (m_hasNaN != YES)
			tmp = Interval::unbounded();
	}

	// Round both bounds _inwards_ to closest representable values.
	if (!tmp.empty())
		ret |= clampValue(round(tmp.lo(), true)) | clampValue(round(tmp.hi(), false));

	// If this format's precision is not exact, the (possibly out-of-bounds)
	// original value is also a possible result.
	if (!m_exactPrecision)
		ret |= x;

	return ret;
}
Ejemplo n.º 2
0
/**
Calculates the intersection of this interval with another.

@param rhs	The other interval
@return		The intersection of the two intervals
*/
Interval Interval::intersect(const Interval& rhs) const
{
	if(empty() || rhs.empty()) return Interval();

	double largerLow = std::max(m_low, rhs.m_low);
	double smallerHigh = std::min(m_high, rhs.m_high);
	return Interval(largerLow, smallerHigh);
}
Ejemplo n.º 3
0
//! Round output of an operation.
//! \param roundUnderOverflow Can +/-inf rounded to min/max representable;
//!							  should be false if any of operands was inf, true otherwise.
Interval FloatFormat::roundOut (const Interval& x, bool roundUnderOverflow) const
{
	Interval ret = x.nan();

	if (!x.empty())
		ret |= Interval(roundOut(x.lo(), false, roundUnderOverflow),
						roundOut(x.hi(), true, roundUnderOverflow));

	return ret;
}
Ejemplo n.º 4
0
std::string FloatFormat::intervalToHex (const Interval& interval) const
{
	if (interval.empty())
		return interval.hasNaN() ? "{ NaN }" : "{}";

	else if (interval.lo() == interval.hi())
		return (std::string(interval.hasNaN() ? "{ NaN, " : "{ ") +
				floatToHex(interval.lo()) + " }");
	else if (interval == Interval::unbounded(true))
		return "<any>";

	return (std::string(interval.hasNaN() ? "{ NaN } | " : "") +
			"[" + floatToHex(interval.lo()) + ", " + floatToHex(interval.hi()) + "]");
}
void ManhattanGroundTruth::ProcessWalls(const proto::FloorPlan& fp,
                                        const PosedCamera& pc) {
    // Initialize
    nocclusions = 0;
    nwalls = 0;

    // First strip out the NaN vertices
    vector<Vec2> verts;
    for (int i = 0; i < fp.vertices_size(); i++) {
        Vec2 v = asToon(fp.vertices(i));
        if (!isnan(v[0]) && !isnan(v[1])) {
            verts.push_back(v);
        }
    }

    // Compute rectifier in retina coords
    Mat3 ret_vrect = GetVerticalRectifierInRetina(pc);
    Mat3 ret_vrect_inv = LU<3>(ret_vrect).get_inverse();
    Bounds2D<> ret_vrect_bounds = Bounds2D<>::ComputeBoundingBox
                                  (pc.retina_bounds().GetPolygon().Transform(ret_vrect));
    Interval xbounds(ret_vrect_bounds.left(), ret_vrect_bounds.right());

    // Note that the floorplan vertices do _not_ wrap around
    Vec3 focal_line = makeVector(0, -1, 1e-6);

    vector<Vec3> lines;
    vector<Interval> intervals;
    for (int i = 0; i < verts.size()-1; i++) {
        Vec3 a_cam = ret_vrect * (pc.pose() * concat(verts[i], 0.0));
        Vec3 b_cam = ret_vrect * (pc.pose() * concat(verts[i+1], 0.0));
        Vec3 a_flat = makeVector(a_cam[0], a_cam[2], 1.0);  // y no longer matters, and now homog
        Vec3 b_flat = makeVector(b_cam[0], b_cam[2], 1.0);  // y no longer matters, and now homog
        bool nonempty = ClipAgainstLine(a_flat, b_flat, focal_line, -1);
        if (nonempty) {
            // project() must come after clipping
            Vec2 a0 = project(a_flat);
            Vec2 b0 = project(b_flat);
            intervals.push_back(Interval(a0[0]/a0[1], b0[0]/b0[1]));
            lines.push_back(a_flat ^ b_flat);
        }
    }

    // Compare polygons
    MatI interval_comp(verts.size()-1, verts.size()-1);
    for (int i = 0; i < intervals.size(); i++) {
        for (int j = 0; j < intervals.size(); j++) {
            if (i==j) continue;
            Interval isct = Intersect(intervals[i], intervals[j]);
            if (isct.empty()) {
                interval_comp[i][j] = i>j ? 1 : -1;
            } else {
                double m = (isct.hi + isct.lo) / 2.;
                double zi = -lines[i][2] / (lines[i][0]*m + lines[i][1]);
                double zj = -lines[j][2] / (lines[j][0]*m + lines[j][1]);
                interval_comp[i][j] = zi > zj ? 1 : -1;
            }
        }
    }

    // Generate tokens
    vector<Token> tokens;
    for (int i = 0; i < intervals.size(); i++) {
        tokens.push_back(Token(intervals[i].lo, i, true));
        tokens.push_back(Token(intervals[i].hi, i, false));
    }
    // Add tokens for left and right image bounds
    tokens.push_back(Token(xbounds.lo, -1, true));
    tokens.push_back(Token(xbounds.hi, -1, false));
    sort_all(tokens);

    // Walk from left to right
    vector<Vec2> xs;
    int prev = -1;
    bool inbounds = false;
    bool done = false;
    VecI active(intervals.size(), 0);
    TableComp comp(interval_comp);
    priority_queue<int,vector<int>,TableComp> heap(comp);
    for (int i = 0; !done; i++) {
        // Process next token
        bool bound_token = tokens[i].index == -1;
        if (bound_token && tokens[i].activate) {
            inbounds = true;
        } else if (bound_token && !tokens[i].activate) {
            done = true;
        } else if (tokens[i].activate) {
            active[tokens[i].index] = true;
            heap.push(tokens[i].index);
        } else {
            active[tokens[i].index] = false;
            while (!heap.empty() && !active[heap.top()]) {  // okay for heap to be temporarily empty
                heap.pop();
            }
        }

        double xcur = tokens[i].x;
        if ((tokens[i+1].x-xcur) > 1e-8) {
            int cur = heap.empty() ? -1 : heap.top();

            // Count walls
            if (inbounds && (cur != prev || bound_token)) {
                if (!done) {
                    nwalls++;
                    if (!bound_token && RingDist<int>(prev, cur, intervals.size()) > 1) {
                        nocclusions++;
                    }
                }

                // Reconstruct vertices
                vector<int> to_add;
                if (!xs.empty()) {
                    to_add.push_back(prev);
                }
                if (!done) {
                    to_add.push_back(cur);
                }

                // Compute vertices
                BOOST_FOREACH(int j, to_add) {
                    const Vec3& line = lines[j];
                    const Interval& intv = intervals[j];
                    Vec2 pflat = project(line ^ makeVector(-1, xcur, 0));
                    Vec3 pcam = makeVector(pflat[0], 0, pflat[1]);
                    Vec3 pworld = pc.pose_inverse() * (ret_vrect_inv * pcam);
                    xs.push_back(pworld.slice<0,2>());
                }
            }
            prev = cur;
        }
    }
Ejemplo n.º 6
0
void NavMeshGenerator::determine_links()
{
	for(EdgePlaneTable::const_iterator it=m_edgePlaneTable.begin(), iend=m_edgePlaneTable.end(); it!=iend; ++it)
	{
		// Generate (n,u,v) coordinate system for plane, where v = (0,0,1).
		const Plane& plane = it->first;
		OrthonormalCoordSystem2D coordSystem(plane);

		// Check pairs of different-facing edges to see whether we need to create any links.
		const EdgeReferences& sameFacingEdgeRefs = it->second.sameFacing;
		const EdgeReferences& oppFacingEdgeRefs = it->second.oppFacing;
		int sameFacingEdgeRefCount = static_cast<int>(sameFacingEdgeRefs.size());
		int oppFacingEdgeRefCount = static_cast<int>(oppFacingEdgeRefs.size());

		for(int j=0; j<sameFacingEdgeRefCount; ++j)
		{
			const EdgeReference& edgeJ = sameFacingEdgeRefs[j];
			NavPolygon& navPolyJ = *m_walkablePolygons[edgeJ.navPolyIndex];
			const CollisionPolygon& colPolyJ = *m_polygons[navPolyJ.collision_poly_index()];
			int mapIndexJ = colPolyJ.auxiliary_data().map_index();

			// Calculate the 2D coordinates of edgeJ in the plane.
			const Vector3d& p1J = colPolyJ.vertex(edgeJ.startVertex);
			const Vector3d& p2J = colPolyJ.vertex((edgeJ.startVertex+1) % colPolyJ.vertex_count());
			Vector2d q1J = coordSystem.from_canonical(p1J), q2J = coordSystem.from_canonical(p2J);
			Interval xIntervalJ(std::min(q1J.x,q2J.x), std::max(q1J.x,q2J.x));

			for(int k=0; k<oppFacingEdgeRefCount; ++k)
			{
				const EdgeReference& edgeK = oppFacingEdgeRefs[k];
				NavPolygon& navPolyK = *m_walkablePolygons[edgeK.navPolyIndex];
				const CollisionPolygon& colPolyK = *m_polygons[navPolyK.collision_poly_index()];
				int mapIndexK = colPolyK.auxiliary_data().map_index();

				// We only want to create links between polygons in the same map.
				if(mapIndexJ != mapIndexK) continue;

				// Calculate the 2D coordinates of edgeK in the plane.
				const Vector3d& p1K = colPolyK.vertex(edgeK.startVertex);
				const Vector3d& p2K = colPolyK.vertex((edgeK.startVertex+1) % colPolyK.vertex_count());
				Vector2d q1K = coordSystem.from_canonical(p1K), q2K = coordSystem.from_canonical(p2K);

				// Calculate the x overlap between the 2D edges. If there's no overlap,
				// then we don't need to carry on looking for a link.
				Interval xIntervalK(std::min(q1K.x,q2K.x), std::max(q1K.x,q2K.x));
				Interval xOverlap = xIntervalJ.intersect(xIntervalK);
				if(xOverlap.empty()) continue;

				// Calculate the segments for the various types of link.
				LinkSegments linkSegments = calculate_link_segments(q1J, q2J, q1K, q2K, xOverlap);

				// Add the appropriate links.
				if(linkSegments.stepDownSourceToDestSegment)
				{
					assert(linkSegments.stepUpDestToSourceSegment != NULL);

					// Add a step down link from j -> k, and a step up one from k -> j.
					Vector3d j1 = coordSystem.to_canonical(linkSegments.stepDownSourceToDestSegment->e1);
					Vector3d j2 = coordSystem.to_canonical(linkSegments.stepDownSourceToDestSegment->e2);
					Vector3d k1 = coordSystem.to_canonical(linkSegments.stepUpDestToSourceSegment->e1);
					Vector3d k2 = coordSystem.to_canonical(linkSegments.stepUpDestToSourceSegment->e2);
					add_nav_link(NavLink_Ptr(new StepDownLink(edgeJ.navPolyIndex, edgeK.navPolyIndex, j1, j2, k1, k2)));
					add_nav_link(NavLink_Ptr(new StepUpLink(edgeK.navPolyIndex, edgeJ.navPolyIndex, k1, k2, j1, j2)));
				}
				if(linkSegments.stepUpSourceToDestSegment)
				{
					assert(linkSegments.stepDownDestToSourceSegment != NULL);

					// Add a step up link from j -> k, and a step down one from k -> j.
					Vector3d j1 = coordSystem.to_canonical(linkSegments.stepUpSourceToDestSegment->e1);
					Vector3d j2 = coordSystem.to_canonical(linkSegments.stepUpSourceToDestSegment->e2);
					Vector3d k1 = coordSystem.to_canonical(linkSegments.stepDownDestToSourceSegment->e1);
					Vector3d k2 = coordSystem.to_canonical(linkSegments.stepDownDestToSourceSegment->e2);
					add_nav_link(NavLink_Ptr(new StepUpLink(edgeJ.navPolyIndex, edgeK.navPolyIndex, j1, j2, k1, k2)));
					add_nav_link(NavLink_Ptr(new StepDownLink(edgeK.navPolyIndex, edgeJ.navPolyIndex, k1, k2, j1, j2)));
				}
				if(linkSegments.walkSegment)
				{
					// Add a walk link from j -> k, and one from k -> j.
					Vector3d e1 = coordSystem.to_canonical(linkSegments.walkSegment->e1);
					Vector3d e2 = coordSystem.to_canonical(linkSegments.walkSegment->e2);
					add_nav_link(NavLink_Ptr(new WalkLink(edgeJ.navPolyIndex, edgeK.navPolyIndex, e1, e2)));
					add_nav_link(NavLink_Ptr(new WalkLink(edgeK.navPolyIndex, edgeJ.navPolyIndex, e1, e2)));
				}
			}
		}
	}
}