C++ Line2类代码示例

void LidarLine2::set(const Line2& line)
{
	Vector2d normal = line.getNormal();
	l = abs(line.getC() / normal.getLength());

	normal *= -line.getC();
	alpha = normal.getAngle2D();
}

LidarLine2::LidarLine2(const std::vector<Vector2d>& points)
{
	Line2 line = Line2::leastSquareLine(points);
	set(line);

	size_t indexEndPointA = 0, indexEndPointB = 0;
	for(size_t i = 1; i < points.size(); i++)
	{
		if(points[i].getId() < points[indexEndPointA].getId()) indexEndPointA = i;
		if(points[i].getId() > points[indexEndPointB].getId()) indexEndPointB = i;
	}

	setEndPointA(line.getClosestPoint(points[indexEndPointA]));
	setEndPointB(line.getClosestPoint(points[indexEndPointB]));
}

	std::pair<bool, float> Line2::intersects(const Line2& rhs) const
	{
		Vector2 diff = rhs.getOrigin() - getOrigin();
		Vector2 perpDir = rhs.getDirection();
		perpDir = Vector2(perpDir.y, -perpDir.x);

		float dot = getDirection().dot(perpDir);
		if (std::abs(dot) > 1.0e-4f) // Not parallel
		{
			float distance = diff.dot(perpDir) / dot;

			return std::make_pair(true, distance);
		}
		else // Parallel
			return std::make_pair(true, 0.0f);
	}

/** Tell on which side of the specified line the current bounding circle is.
 *  @param [in] line Line.
 */
Side BoundingCircle::classify(Line2 const& line)
{
    float d = line.distance(_center);
    if(d <= -_radius)
    { return Side::Back; }
    if(d >= _radius)
    { return Side::Front; }
    return Side::On;
}

bool Polygon::intersects(const Line2& line)
{
	Vector2 p0;
	Vector2 p1;
	Line2 segment;
	for (int i = 0; i < count; i++)
	{
		segment.src = points[i];
		segment.dest = points[(i + 1) % count];

		if (segment.intersects(line))
		{
			return true;
		}
	}

	return false;
}

//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Line2& rkLine0, const Line2& rkLine1,
    int& riQuantity, Real afT[2])
{
    Vector2 kDiff;
    Real fD0SqrLen;
    bool bIntersects = Find(rkLine0.Origin(),rkLine0.Direction(),
        rkLine1.Origin(),rkLine1.Direction(),kDiff,fD0SqrLen,riQuantity,afT);

    if ( bIntersects )
    {
        if ( riQuantity == 2 )
        {
            afT[0] = -Math::MAX_REAL;
            afT[1] = Math::MAX_REAL;
        }
    }

    return riQuantity != 0;
}

bool oppositeSide(Point2 p1, Point2 p2, Line2 ab)//returns whether or not p1 and p2 are on the same side of AB
{
	double & ax = p1.x;
	double & ay = p1.y;

	double & bx = p2.x;
	double & by = p2.y;

	double & x1 = ab.x;
	double & y1 = ab.y;

	double x2 = ab.getEnd().x;
	double y2 = ab.getEnd().y;

	return ((y1 - y2)*(ax - x1)+(x2 - x1)*(ay - y1))*((y1 - y2)*(bx - x1)+(x2 - x1)*(by - y1))<0;
}

//----------------------------------------------------------------------------
static int WhichSide (const Line2& rkLine, int iEdgeQuantity,
    const int* aiEdge, const Vector2* akPoint)
{
    // establish which side of line hull is on
    Real fC0;
    for (int i1 = 0, i0 = iEdgeQuantity-1; i1 < iEdgeQuantity; i0 = i1++)
    {
        fC0 = rkLine.Normal().Dot(akPoint[aiEdge[i0]]);
        if ( fC0 > rkLine.Constant()+gs_fEpsilon )  // hull on positive side
            return +1;
        if ( fC0 < rkLine.Constant()-gs_fEpsilon )  // hull on negative side
            return -1;

        fC0 = rkLine.Normal().Dot(akPoint[aiEdge[i1]]);
        if ( fC0 > rkLine.Constant()+gs_fEpsilon )  // hull on positive side
            return +1;
        if ( fC0 < rkLine.Constant()-gs_fEpsilon )  // hull on negative side
            return -1;
    }

    // hull is effectively collinear
    return 0;
}

//----------------------------------------------------------------------------
static int OnSameSide (const Line2& rkLine, int iEdgeQuantity,
    const int* aiEdge, const Vector2* akPoint)
{
    // test if all points on same side of line Dot(N,X) = c
    Real fC0;
    int iPosSide = 0, iNegSide = 0;

    for (int i1 = 0, i0 = iEdgeQuantity-1; i1 < iEdgeQuantity; i0 = i1++)
    {
        fC0 = rkLine.Normal().Dot(akPoint[aiEdge[i0]]);
        if ( fC0 > rkLine.Constant() + gs_fEpsilon )
            iPosSide++;
        else if ( fC0 < rkLine.Constant() - gs_fEpsilon )
            iNegSide++;
        
        if ( iPosSide && iNegSide )
        {
            // line splits point set
            return 0;
        }

        fC0 = rkLine.Normal().Dot(akPoint[aiEdge[i1]]);
        if ( fC0 > rkLine.Constant() + gs_fEpsilon )
            iPosSide++;
        else if ( fC0 < rkLine.Constant() - gs_fEpsilon )
            iNegSide++;
        
        if ( iPosSide && iNegSide )
        {
            // line splits point set
            return 0;
        }
    }

    return iPosSide ? +1 : -1;
}

bool intersect(Line2 ab, Line2 cd)
{
	return oppositeSide(ab, ab.getEnd(), cd) && oppositeSide(cd, cd.getEnd(), ab);
}

IntersectType clipPoly2D(const Points2D& points,
	const Line2<typename points_adaptor<Points2D>::scalar>& plane,
	std::list<ClippedPoly<typename points_adaptor<Points2D>::scalar> >& result,
	detail::ClippingContext<typename points_adaptor<Points2D>::scalar>* ctxt)
{
	typedef points_adaptor<Points2D> 					Adaptor;
	typedef typename Adaptor::scalar					T;
	typedef typename Adaptor::elem_type					point_type;
	typedef typename Adaptor::const_elem_ref			const_point_ref;
	typedef ClippedPoly<T> 								polyclip_type;
	typedef typename polyclip_type::intersection		polyclip_intersection;

	typedef std::multimap<T, unsigned int>							onplane_lookup;
	typedef boost::unordered_multimap<unsigned int, unsigned int>	edge_lookup;
	typedef boost::unordered_set<unsigned int>						visited_verts_set;
	typedef boost::unordered_map<unsigned int, unsigned int>		shared_verts_map;

	Adaptor a(points);

	unsigned int npoints = a.size();
	assert(a.size() > 2);

	static const unsigned int shared_offset = std::numeric_limits<unsigned int>::max()/2;
	bool shared_vert_check = (npoints > 4);
	visited_verts_set visited_verts;
	shared_verts_map shared_vert_remap;

	unsigned int index1, index2;
	index1 = a.index(0);
	if(shared_vert_check)
		visited_verts.insert(index1);

	std::vector<polyclip_intersection> onplanePoints;
	std::vector<unsigned int> insidePoints;

	polyclip_intersection edgeInt;
	point_type line_dir = plane.dir();
	bool anyPtOutside = false;
	bool ptInsideNext;
	bool ptInside = (ctxt)? ctxt->isPointInside(index1) : plane.isRight(a[0]);

	onplane_lookup intersectionsOut, intersectionsIn;
	edge_lookup edges(npoints);

	// do the clip. Note that onplane points are indexed as i+npoints and are adjusted after
	for(unsigned int i=0; i<npoints; ++i, ptInside=ptInsideNext, index1=index2)
	{
		unsigned int j = (i+1) % npoints;
		const_point_ref pt_i = a[i];
		const_point_ref pt_j = a[j];

		index2 = a.index(j);
		anyPtOutside |= !ptInside;
		ptInsideNext = (ctxt)? ctxt->isPointInside(index2) : plane.isRight(pt_j);

		if((j>0) && shared_vert_check && !visited_verts.insert(index2).second)
		{
			unsigned int alias_index = shared_offset + shared_vert_remap.size();
			shared_vert_remap.insert(shared_verts_map::value_type(alias_index, index2));
			index2 = alias_index;
		}

		if(ptInsideNext != ptInside)
		{
			// calc edge intersection with line
			edgeInt.m_point1 = index1;
			edgeInt.m_point2 = index2;
			plane.intersect(pt_i, pt_j, edgeInt.m_u);

			// if the edge is almost exactly parallel to the plane it is possible to
			// get a spurious value due to float-pt inaccuracy - solve the prob in double
			if((edgeInt.m_u <= 0) || (edgeInt.m_u >= 1))
			{
				if(boost::is_same<T,double>::value)
					edgeInt.m_u = std::min(std::max(edgeInt.m_u, T(0.0)), T(1.0));
				else
				{
					Line2<double> plane_d(plane);
					Imath::V2d ptd_i(pt_i);
					Imath::V2d ptd_j(pt_j);
					double u;
					plane_d.intersect(ptd_i, ptd_j, u);
					u = std::min(std::max(u, 0.0), 1.0);
					edgeInt.m_u = static_cast<T>(u);
				}
			}

			// sort intersection wrt line dir
			point_type pt_int = pt_i + (pt_j-pt_i)*edgeInt.m_u;
			T dist = pt_int.dot(line_dir);

			unsigned int vert_int = onplanePoints.size() + npoints;
			onplanePoints.push_back(edgeInt);

			if(ptInside)
				intersectionsOut.insert(typename onplane_lookup::value_type(dist, vert_int));
			else
				intersectionsIn.insert(typename onplane_lookup::value_type(dist, vert_int));

			if(ptInside)
//.........这里部分代码省略.........

arma::vec2
intersectionPoint(const Line2 & l1, const Line2 & l2)
{
  const double num1 = l1.b() * l2.c() - l2.b() * l1.c();
  const double num2 = l2.a() * l1.c() - l1.a() * l2.c();
  const double denom = l1.a() * l2.b() - l2.a() * l1.b();
  arma::vec2 pt;
  pt << num1 / denom << endr << num2 / denom;
  return pt;
}

bool operator==(Line2 const & lhs, Line2 const & rhs)
{
	return (lhs.getStart() == rhs.getStart() && lhs.getEnd() == rhs.getEnd()) || (lhs.getStart() == rhs.getEnd() && lhs.getEnd() == rhs.getStart());
}

bool Line2::operator!=(const Line2& r) const
{
  return origin() != r.origin() || direction() != r.direction();  
}

    Solution split(const Line2& line, bool up) const {
        Solution result;
        result.points = points;
        for (const auto& facet: facets) {
            if (doIntersect(facet.polygon, line)) {
                Facet facet1;
                facet1.transformation = facet.transformation;
                Facet facet2;
                Transformation2 reflection( sqr(line.b()) - sqr(line.a()), -2*line.a()*line.b(), -2*line.a()*line.c(),
                                            -2*line.a()*line.b(), sqr(line.a()) - sqr(line.b()), -2*line.b()*line.c(),
                                            sqr(line.a()) + sqr(line.b()) );
                facet2.transformation = reflection*facet.transformation;
                auto inverse = facet.transformation.inverse();
                bool fail = false;
                for (auto edge = facet.polygon.edges_begin(); edge != facet.polygon.edges_end(); ++edge) {
                    const auto len2 = edge->squared_length();
                    const auto len2new = edge->transform(facet2.transformation).squared_length();
                    if (len2 != len2new) {
                        throw runtime_error("bad transform");
                    }

                    if (line.has_on_positive_side(edge->source()) == up) {
                        facet1.polygon.push_back(edge->source());
                    } else {
                        facet2.polygon.push_back(reflection(edge->source()));
                    }
                    auto intersect = intersection(*edge, line);
                    if (intersect) {
                        Point2* p = boost::get<Point2>(&*intersect);
                        if (p) {
                            auto revPoint = inverse(*p);
                            if (!result.points.count(revPoint)) {
                                result.points[revPoint] = result.points.size();
                            }
                            facet1.polygon.push_back(*p);
                            facet2.polygon.push_back(*p);
                        } else {
                            fail = true;
                        }
                    }
                }
                auto normalize = [&](Facet& f) {
                    auto area = f.polygon.area();
                    if (area == 0) {
                        return;
                    }
                    vector<Point2> points(f.polygon.size());
                    for (size_t i = 0; i < f.polygon.size(); ++i) {
                        points[i] = f.polygon[i];
                    }
                    points.erase(unique(points.begin(), points.end()), points.end());
                    if (points.empty()) {
                        return;
                    }
                    if (area < 0) {
                        reverse(points.begin(), points.end());
                    }
                    Polygon2 p;
                    for (const auto& pnt: points) {
                        p.push_back(pnt);
                    }
                    f.polygon = p;
                    result.facets.push_back(f);
                };
                if (!fail) {
                    normalize(facet1);
                    normalize(facet2);
                } else {
                    result.facets.push_back(facet);
                }
            } else {
                result.facets.push_back(facet);
            }
        }

        for (const auto& f: result.facets) {
            result.polygon.join(f.polygon);
        }

        return result;
    }