inline int intersect(const point &a, const point &b, point &u, point &v, double radius) {
if (point_to_line(point(0, 0), line(a, b)) + EPS > radius)
return 0;
u = line_to_circle(a, b);
v = line_to_circle(b, a);
return point_on_line(u, line(a, b)) + point_on_line(v, line(a, b));
}
void show_device_direction(float yaw,float pitch,float roll){
float theta = yaw/3*M_PI - M_PI/2;
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2 =transform_1;
theta = -roll/3*M_PI;
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitY()));
//theta = -yy/3*M_PI;
//transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
Eigen::Vector3f point_on_line(0,0,0), line_direction(0,0,1);
line_direction=transform_1*line_direction;
pcl::ModelCoefficients line_coeff;
line_coeff.values.resize (6); // We need 6 values
line_coeff.values[0] = point_on_line.x ();
line_coeff.values[1] = point_on_line.y ();
line_coeff.values[2] = point_on_line.z ();
line_coeff.values[3] = line_direction.x ();
line_coeff.values[4] = line_direction.y ();
line_coeff.values[5] = line_direction.z ();
viewer->addLine (line_coeff);
}
Eigen::ParametrizedLine<float, 3> FloorFilter::LineFromCoefficients(
const pcl::ModelCoefficients &line_coefficients) {
Eigen::ParametrizedLine<float, 3>::VectorType point_on_line(
line_coefficients.values[0], line_coefficients.values[1], line_coefficients.values[2]);
Eigen::ParametrizedLine<float, 3>::VectorType direction(
line_coefficients.values[3], line_coefficients.values[4], line_coefficients.values[5]);
return Eigen::ParametrizedLine<float, 3>(point_on_line, direction);
}
bool point_in_polygon(const Point &p, const std::vector<Point> &polygon) {
int counter = 0;
for (int i = 0; i < (int)polygon.size(); ++i) {
Point a = polygon[i], b = polygon[(i + 1) % (int)polygon.size()];
if (point_on_line(p, a, b)) {
// Point on the boundary are excluded.
return false;
}
int x = sgn(det(a, p, b));
int y = sgn(a.y - p.y);
int z = sgn(b.y - p.y);
counter += (x > 0 && y <= 0 && z > 0);
counter -= (x < 0 && z <= 0 && y > 0);
}
return counter;
}
void SubSurface::SplitSegsW( const double & w )
{
double tol = 1e-10;
int num_l_segs = m_SplitLVec.size();
int num_splits = 0;
bool reorder = false;
vector<SSLineSeg> new_lsegs;
vector<int> inds;
for ( int i = 0 ; i < num_l_segs ; i++ )
{
SSLineSeg& seg = m_SplitLVec[i];
vec3d p0 = seg.GetP0();
vec3d p1 = seg.GetP1();
double t = ( w - p0.y() ) / ( p1.y() - p0.y() );
if ( t < 1 - tol && t > 0 + tol )
{
if ( m_FirstSplit )
{
m_FirstSplit = false;
reorder = true;
}
// Split the segments
vec3d int_pnt = point_on_line( p0, p1, t );
SSLineSeg split_seg = SSLineSeg( seg );
seg.SetP1( int_pnt );
split_seg.SetP0( int_pnt );
inds.push_back( i + num_splits + 1 );
new_lsegs.push_back( split_seg );
num_splits++;
}
}
for ( int i = 0; i < ( int )inds.size() ; i++ )
{
m_SplitLVec.insert( m_SplitLVec.begin() + inds[i], new_lsegs[i] );
}
if ( reorder )
{
ReorderSplitSegs( inds[0] );
}
}
bool in_polygon(const point &p, const vector<point> &poly) {
int n = (int)poly.size();
int counter = 0;
for (int i = 0; i < n; ++i) {
point a = poly[i], b = poly[(i + 1) % n];
if (point_on_line(p, line(a, b)))
return false; // bounded excluded
int x = sign(det(p - a, b - a));
int y = sign(a.y - p.y);
int z = sign(b.y - p.y);
if (x > 0 && y <= 0 && z > 0)
counter++;
if (x < 0 && z <= 0 && y > 0)
counter--;
}
return counter != 0;
}