35 #ifndef PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
36 #define PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
38 #include <pcl/filters/plane_clipper3D.h>
40 template<
typename Po
intT>
42 : plane_params_ (plane_params)
46 template<
typename Po
intT>
51 template<
typename Po
intT>
void
54 plane_params_ = plane_params;
57 template<
typename Po
intT>
const Eigen::Vector4f&
69 template<
typename Po
intT>
float
72 return (plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z + plane_params_[3]);
75 template<
typename Po
intT>
bool
78 return ((plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z ) >= -plane_params_[3]);
84 template<
typename Po
intT>
bool
87 float dist1 = getDistance (point1);
88 float dist2 = getDistance (point2);
90 if (dist1 * dist2 > 0)
93 float lambda = dist2 / (dist2 - dist1);
97 intersection.x = (point1.x - point2.x) * lambda + point2.x;
98 intersection.y = (point1.y - point2.y) * lambda + point2.y;
99 intersection.z = (point1.z - point2.z) * lambda + point2.z;
103 point2 = intersection;
105 point1 = intersection;
113 template<
typename Po
intT>
void
116 clipped_polygon.clear ();
117 clipped_polygon.reserve (polygon.size ());
120 if (polygon.size () < 3)
122 if (polygon.size () == 1)
125 if (clipPoint3D (polygon [0]))
126 clipped_polygon.push_back (polygon [0]);
128 else if (polygon.size () == 2)
130 clipped_polygon.push_back (polygon [0]);
131 clipped_polygon.push_back (polygon [1]);
132 if (!clipLineSegment3D (clipped_polygon [0], clipped_polygon [1]))
133 clipped_polygon.clear ();
138 float previous_distance = getDistance (polygon [0]);
140 if (previous_distance > 0)
141 clipped_polygon.push_back (polygon [0]);
143 typename std::vector<PointT>::const_iterator prev_it = polygon.begin ();
145 for (
typename std::vector<PointT>::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++)
148 float distance = getDistance (*pIt);
149 if (distance * previous_distance < 0)
151 float lambda = distance / (distance - previous_distance);
154 intersection.x = (prev_it->x - pIt->x) * lambda + pIt->x;
155 intersection.y = (prev_it->y - pIt->y) * lambda + pIt->y;
156 intersection.z = (prev_it->z - pIt->z) * lambda + pIt->z;
158 clipped_polygon.push_back (intersection);
161 clipped_polygon.push_back (*pIt);
163 previous_distance = distance;
170 template<
typename Po
intT>
void
173 std::vector<PointT> clipped;
174 clipPlanarPolygon3D (polygon, clipped);
179 template<
typename Po
intT>
void
182 if (indices.empty ())
184 clipped.reserve (cloud_in.
size ());
223 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
224 if (clipPoint3D (cloud_in[*iIt]))
225 clipped.push_back (*iIt);
228 #endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP