33 #ifndef _TEXTURED_TRIANGLE_H_
34 #define _TEXTURED_TRIANGLE_H_
40 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
41 #include <CGAL/Constrained_Delaunay_triangulation_2.h>
43 typedef CGAL::Exact_predicates_inexact_constructions_kernel
K1;
54 #include <pcl/segmentation/sac_segmentation.h>
55 #include <pcl/filters/project_inliers.h>
56 #include <pcl/point_cloud.h>
57 #include <pcl/point_types.h>
58 #include <pcl_ros/transforms.h>
59 #include <pcl/ModelCoefficients.h>
61 #include <visualization_msgs/Marker.h>
64 #define DONT_INTERSECT 0
65 #define DO_INTERSECT 1
68 #define SAME_SIGNS( a, b ) \
69 (((long) ((unsigned long) a ^ (unsigned long) b)) >= 0 )
110 ROS_INFO(
"v0 x=%f y=%f z=%f rgb=%f\n v1 x=%f y=%f z=%f rgb=%f\n v2 x=%f y=%f z=%f rgb=%f\n",
v[0].x,
v[0].y,
v[0].z,
v[0].rgb,
v[1].x,
v[1].y,
v[1].z,
v[1].rgb,
v[2].x,
v[2].y,
v[2].z,
v[2].rgb);
116 int test2(
double px,
double py,
double m,
double b ) {
117 if (py < m * px + b ) {
119 }
else if ( py == m * px + b ){
126 int test1(
double px,
double py,
double m,
double b,
double lx,
double ly) {
127 return (
test2(px,py, m,b) ==
test2(lx,ly,m,b));
134 int line1, line2, line3;
135 double m01 = (y1-y0)/(x1-x0);
136 double b01 = m01 * -x1 + y1;
137 double m02, m12, b02, b12;
138 m02 = (y2-y0)/(x2-x0);
139 m12 = (y2-y1)/(x2-x1);
140 b02 = m02 * -x2 + y2;
141 b12 = m12 * -x2 + y2;
147 line1 = ( (px < x0) == (x2 < x0) );
149 line1 =
test1( px, py, m01, b01,x2,y2);
153 line2 = ( (px < x2) == (x0 < x2) );
155 line2 =
test1(px,py, m12, b12,x0,y0);
159 line3 = ( (px < x0 ) == (x1 < x0) );}
else {
160 line3 =
test1(px, py, m02,b02,x1,y1);
163 if(line1==-1 && line2==-1 && line3==-1)
165 else if(line1==1 && line2==1 && line3==1)
184 double *X,
double *Y) {
186 double distAB, theCos, theSin, newX, ABpos ;
189 if (((Ax==Bx) && (Ay==By)) || ((Cx==Dx) && (Cy==Dy)))
return DONT_INTERSECT;
192 if (((Ax==Cx) && (Ay==Cy)) || ((Bx==Cx) && (By==Cy))
193 || ((Ax==Dx) && (Ay==Dy)) || ((Bx==Dx) && (By==Dy))) {
202 distAB=sqrt(Bx*Bx+By*By);
207 newX=Cx*theCos+Cy*theSin;
208 Cy =Cy*theCos-Cx*theSin; Cx=newX;
209 newX=Dx*theCos+Dy*theSin;
210 Dy =Dy*theCos-Dx*theSin; Dx=newX;
213 if ((Cy<0. && Dy<0.) || (Cy>=0. && Dy>=0.))
return DONT_INTERSECT;
216 ABpos=Dx+(Cx-Dx)*Dy/(Dy-Cy);
239 #define ADDED_TRIANGLE 1
240 #define DID_NOT_ADD_TRIANGLE 0
262 std::vector<boost::shared_ptr<class_textured_triangle> >
set;
263 pcl::PointCloud<pcl::PointXYZRGB>
pc;
int point_inside_triangle(double x0, double y0, double x1, double y1, double x2, double y2, double px, double py)
int set_transform(tf::Transform *st)
std::vector< int > pc_proveniences
class_textured_vertex v[3]
bool overlaps(class_textured_triangle *t)
~class_textured_vertex(void)
class_textured_vertex operator=(class_textured_vertex vin)
int test2(double px, double py, double m, double b)
CGAL::Exact_predicates_inexact_constructions_kernel K1
int add_triangle(class_textured_vertex v0, class_textured_vertex v1, class_textured_vertex v2, int provenience, float weight, t_add_method method)
class_textured_vertex(void)
std::vector< boost::shared_ptr< class_textured_triangle > > set
int test1(double px, double py, double m, double b, double lx, double ly)
int lineSegmentIntersection(float Ax, float Ay, float Bx, float By, float Cx, float Cy, float Dx, float Dy, double *X, double *Y)
public domain function by Darel Rex Finley, 2006. Determines the intersection point of the line segme...
pcl::PointCloud< pcl::PointXYZRGB > pc
~class_textured_triangle(void)
class_textured_triangle(void)