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 Mgc::FindIntersection (const Line2& rkLine, const Segment2& rkSegment,
int& riQuantity, Real afT[2])
{
Vector2 kDiff;
Real fD0SqrLen;
bool bIntersects = Find(rkLine.Origin(),rkLine.Direction(),
rkSegment.Origin(),rkSegment.Direction(),kDiff,fD0SqrLen,riQuantity,
afT);
if ( bIntersects )
{
if ( riQuantity == 1 )
{
if ( afT[1] < 0.0f || afT[1] > 1.0f )
{
// lines intersect, but segment does not intersect line
riQuantity = 0;
}
}
else
{
// segment is contained by line, adjust intersection interval
Real fDot = rkLine.Direction().Dot(rkSegment.Direction());
Real fInvLen = 1.0f/fD0SqrLen;
if ( fDot > 0.0f )
{
afT[0] = (kDiff.Dot(rkLine.Direction()))*fInvLen;
afT[1] = afT[0] + fDot*fInvLen;
}
else
{
afT[1] = (kDiff.Dot(rkLine.Direction()))*fInvLen;
afT[0] = afT[1] + fDot*fInvLen;
}
}
}
return riQuantity != 0;
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Line2& rkLine, const Ray2& rkRay,
int& riQuantity, Real afT[2])
{
Vector2 kDiff;
Real fD0SqrLen;
bool bIntersects = Find(rkLine.Origin(),rkLine.Direction(),
rkRay.Origin(),rkRay.Direction(),kDiff,fD0SqrLen,riQuantity,afT);
if ( bIntersects )
{
if ( riQuantity == 1 )
{
if ( afT[1] < 0.0f )
{
// lines intersect, but ray does not intersect line
riQuantity = 0;
}
}
else
{
// ray is contained by line, adjust intersection interval
if ( rkLine.Direction().Dot(rkRay.Direction()) > 0.0f )
{
afT[0] = (kDiff.Dot(rkLine.Direction()))/fD0SqrLen;
afT[1] = Math::MAX_REAL;
}
else
{
afT[0] = -Math::MAX_REAL;
afT[1] = (kDiff.Dot(rkLine.Direction()))/fD0SqrLen;
}
}
}
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 Mgc::SeparatePointSets2D (int iQuantity0, const Vector2* akVertex0,
int iQuantity1, const Vector2* akVertex1, Line2& rkSeprLine)
{
// construct convex hull of point set 0
ConvexHull2D kHull0(iQuantity0,akVertex0);
kHull0.ByDivideAndConquer();
int iEdgeQuantity0 = kHull0.GetQuantity();
const int* aiEdge0 = kHull0.GetIndices();
// construct convex hull of point set 1
ConvexHull2D kHull1(iQuantity1,akVertex1);
kHull1.ByDivideAndConquer();
int iEdgeQuantity1 = kHull1.GetQuantity();
const int* aiEdge1 = kHull1.GetIndices();
// test edges of hull 0 for possible separation of points
int j0, j1, iI0, iI1, iSide0, iSide1;
Vector2 kDiff;
for (j1 = 0, j0 = iEdgeQuantity0-1; j1 < iEdgeQuantity0; j0 = j1++)
{
// lookup edge (assert: iI0 != iI1 )
iI0 = aiEdge0[j0];
iI1 = aiEdge0[j1];
// compute potential separating line (assert: (xNor,yNor) != (0,0))
kDiff = akVertex0[iI1] - akVertex0[iI0];
rkSeprLine.Normal() = kDiff.Cross();
rkSeprLine.Constant() = rkSeprLine.Normal().Dot(akVertex0[iI0]);
// determine if hull 1 is on same side of line
iSide1 = OnSameSide(rkSeprLine,iEdgeQuantity1,aiEdge1,akVertex1);
if ( iSide1 )
{
// determine which side of line hull 0 lies
iSide0 = WhichSide(rkSeprLine,iEdgeQuantity0,aiEdge0,akVertex0);
if ( iSide0*iSide1 <= 0 ) // line separates hulls
return true;
}
}
// test edges of hull 1 for possible separation of points
for (j1 = 0, j0 = iEdgeQuantity1-1; j1 < iEdgeQuantity1; j0 = j1++)
{
// lookup edge (assert: iI0 != iI1 )
iI0 = aiEdge1[j0];
iI1 = aiEdge1[j1];
// compute perpendicular to edge (assert: (xNor,yNor) != (0,0))
kDiff = akVertex1[iI1] - akVertex1[iI0];
rkSeprLine.Normal() = kDiff.Cross();
rkSeprLine.Constant() = rkSeprLine.Normal().Dot(akVertex1[iI0]);
// determine if hull 0 is on same side of line
iSide0 = OnSameSide(rkSeprLine,iEdgeQuantity0,aiEdge0,akVertex0);
if ( iSide0 )
{
// determine which side of line hull 1 lies
iSide1 = WhichSide(rkSeprLine,iEdgeQuantity1,aiEdge1,akVertex1);
if ( iSide0*iSide1 <= 0 ) // line separates hulls
return true;
}
}
return false;
}
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)
{
unsigned int vert1 = insidePoints.size();
unsigned int vert2 = vert_int;
insidePoints.push_back(index1);
edges.insert(edge_lookup::value_type(vert1, vert2));
}
else
{
unsigned int vert1 = vert_int;
unsigned int vert2 = (j==0)? 0 : insidePoints.size();
edges.insert(edge_lookup::value_type(vert1, vert2));
}
}
else if(ptInside)
{
unsigned int vert1 = insidePoints.size();
unsigned int vert2 = (j==0)? 0 : vert1+1;
insidePoints.push_back(index1);
edges.insert(edge_lookup::value_type(vert1, vert2));
}
}
if(insidePoints.empty())
return INTERSECT_OUTSIDE;
if(!anyPtOutside)
{
// poly is completely inside
result.push_back(polyclip_type());
polyclip_type& pclip = result.back();
pclip.makeInside(points);
if(ctxt)
pclip.m_polyId = ctxt->m_currentPolyId;
return INTERSECT_INSIDE;
}
assert(!intersectionsOut.empty());
assert(intersectionsOut.size() == intersectionsIn.size());
// turn each pair of intersections into an edge
while(!intersectionsOut.empty())
{
typename onplane_lookup::iterator itOut = intersectionsOut.begin();
typename onplane_lookup::iterator itIn = intersectionsIn.begin();
edges.insert(edge_lookup::value_type(itOut->second, itIn->second));
intersectionsOut.erase(itOut);
intersectionsIn.erase(itIn);
}
// find each closed loop within edges and return as poly
while(!edges.empty())
{
result.push_back(polyclip_type());
polyclip_type& pclip = result.back();
pclip.m_vertices.reserve(edges.size());
if(ctxt)
pclip.m_polyId = -std::abs(ctxt->m_currentPolyId);
edge_lookup::iterator it = edges.begin();
unsigned int first_vert = it->first;
while(it != edges.end())
{
unsigned int vert = it->first;
if(vert >= npoints)
{
pclip.m_vertices.push_back(pclip.m_onplanePoints.size() + npoints);
pclip.m_onplanePoints.push_back(onplanePoints[vert - npoints]);
}
else
{
pclip.m_vertices.push_back(pclip.m_insidePoints.size());
pclip.m_insidePoints.push_back(insidePoints[vert]);
}
unsigned int next_vert = it->second;
edges.erase(it);
it = edges.find(next_vert);
}
// remap shared verts, if any
if(shared_vert_check && !shared_vert_remap.empty())
{
for(unsigned int i=0; i<pclip.m_insidePoints.size(); ++i)
{
unsigned int vert = pclip.m_insidePoints[i];
if(vert >= shared_offset)
{
assert(shared_vert_remap.find(vert) != shared_vert_remap.end());
pclip.m_insidePoints[i] = shared_vert_remap.find(vert)->second;
}
}
for(unsigned int i=0; i<pclip.m_onplanePoints.size(); ++i)
{
polyclip_intersection& is = pclip.m_onplanePoints[i];
if(is.m_point1 >= shared_offset)
{
assert(shared_vert_remap.find(is.m_point1) != shared_vert_remap.end());
is.m_point1 = shared_vert_remap.find(is.m_point1)->second;
}
if(is.m_point2 >= shared_offset)
{
assert(shared_vert_remap.find(is.m_point2) != shared_vert_remap.end());
is.m_point2 = shared_vert_remap.find(is.m_point2)->second;
}
}
}
// remap verts so that onplane verts are correctly offset by inside pt count
assert(pclip.m_vertices.size() > 2);
unsigned int adjust = npoints - pclip.m_insidePoints.size();
for(unsigned int i=0; i<pclip.m_vertices.size(); ++i)
{
if(pclip.m_vertices[i] >= npoints)
pclip.m_vertices[i] -= adjust;
}
}
return INTERSECT_INTERSECTS;
}
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 Polygon2d::intersects(const Line2& l) const
{
// FIXME! We need to do something with the bounds.
return Polygon2d::intersects(l.origin(), l.direction(), LineSegment);
}
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();
}
// Operators
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;
}
// The main function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
bool segmentsIntersect(Line2 l1, Line2 l2)
{
// Find the four orientations needed for general and
// special cases
int o1 = orientation(l1.getStart(), l1.getEnd(), l2.getStart());
int o2 = orientation(l1.getStart(), l1.getEnd(), l2.getEnd());
int o3 = orientation(l2.getStart(), l2.getEnd(), l1.getStart());
int o4 = orientation(l2.getStart(), l2.getEnd(), l1.getEnd());
// General case
if (o1 != o2 && o3 != o4)
return true;
// Special Cases
// p1, q1 and p2 are colinear and p2 lies on segment p1q1
if (o1 == 0 && onSegment(l1.getStart(), l2.getStart(), l1.getEnd())) return true;
// p1, q1 and p2 are colinear and q2 lies on segment p1q1
if (o2 == 0 && onSegment(l1.getStart(), l2.getEnd(), l1.getEnd())) return true;
// p2, q2 and p1 are colinear and p1 lies on segment p2q2
if (o3 == 0 && onSegment(l2.getStart(), l1.getStart(), l2.getEnd())) return true;
// p2, q2 and q1 are colinear and q1 lies on segment p2q2
if (o4 == 0 && onSegment(l2.getStart(), l1.getEnd(), l2.getEnd())) return true;
return false; // Doesn't fall in any of the above cases
}
