int main()
{
TPPLPoly poly;
{
float x = 2.0f;
float buf[] = { -x, -x, x, -x, x, x, -x, x };
make_poly(buf, 4, poly);
}
TPPLPoly hole;
{
float x = 1.0f;
// float buf[] = { -x, -x, x, -x, x, x, 0, 0, -x, x };
float buf[] = {-x,-x, -x,x, 0,0, x,x, x,-x};
make_poly(buf, 5, hole);
}
std::list< TPPLPoly > list;
list.push_back(poly);
list.push_back(hole);
std::list< TPPLPoly > triangles;
TPPLPartition pp;
if (!pp.Triangulate_EC(&list, &triangles))
{
printf("failed\n");
}
return 0;
}
void
Cylinder::triangulate(list<TPPLPoly>& tri_list) const
{
TPPLPartition pp;
list<TPPLPoly> polys;
TPPLPoly poly;
TPPLPoint pt;
double d_alpha = 0.5;
double alpha_max = 0, alpha_min = std::numeric_limits<double>::max();
for(size_t i = 0; i < contours_[0].size(); ++i)
{
double alpha = contours_[0][i](0) / r_;
if (alpha > alpha_max) alpha_max = alpha;
if (alpha < alpha_min) alpha_min = alpha;
}
std::cout << "r " << r_ << std::endl;
std::cout << "alpha " << alpha_min << "," << alpha_max << std::endl;
std::vector<std::vector<std::vector<Eigen::Vector2f> > > contours_split;
for(size_t j = 0; j < contours_.size(); j++)
{
for(double i = alpha_min + d_alpha; i <= alpha_max; i += d_alpha)
{
std::vector<Eigen::Vector2f> contour_segment;
for(size_t k = 0; k < contours_[j].size(); ++k)
{
double alpha = contours_[j][k](0) / r_;
if( alpha >= i - d_alpha - 0.25 && alpha < i + 0.25)
{
contour_segment.push_back(contours_[j][k]);
}
}
//std::cout << "c " << j << i << " has " << contour_segment.size() << " points" << std::endl;
if(contour_segment.size() < 3) continue;
poly.Init(contour_segment.size());
poly.SetHole(holes_[j]);
for( unsigned int l = 0; l < contour_segment.size(); l++)
{
pt.x = contour_segment[l](0);
pt.y = contour_segment[l](1);
poly[l] = pt;
}
if (holes_[j])
poly.SetOrientation(TPPL_CW);
else
poly.SetOrientation(TPPL_CCW);
polys.push_back(poly);
}
}
// triangulation into monotone triangles
pp.Triangulate_EC (&polys, &tri_list);
}
void GenerateTestData() {
TPPLPartition pp;
list<TPPLPoly> testpolys,result,expectedResult;
ReadPolyList("test_input.txt",&testpolys);
DrawPolyList("test_input.bmp",&testpolys);
pp.Triangulate_EC(&testpolys,&result);
//pp.Triangulate_EC(&(*testpolys.begin()),&result);
DrawPolyList("test_triangulate_EC.bmp",&result);
WritePolyList("test_triangulate_EC.txt",&result);
result.clear(); expectedResult.clear();
pp.Triangulate_OPT(&(*testpolys.begin()),&result);
DrawPolyList("test_triangulate_OPT.bmp",&result);
WritePolyList("test_triangulate_OPT.txt",&result);
result.clear(); expectedResult.clear();
pp.Triangulate_MONO(&testpolys,&result);
//pp.Triangulate_MONO(&(*testpolys.begin()),&result);
DrawPolyList("test_triangulate_MONO.bmp",&result);
WritePolyList("test_triangulate_MONO.txt",&result);
result.clear(); expectedResult.clear();
pp.ConvexPartition_HM(&testpolys,&result);
//pp.ConvexPartition_HM(&(*testpolys.begin()),&result);
DrawPolyList("test_convexpartition_HM.bmp",&result);
WritePolyList("test_convexpartition_HM.txt",&result);
result.clear(); expectedResult.clear();
pp.ConvexPartition_OPT(&(*testpolys.begin()),&result);
DrawPolyList("test_convexpartition_OPT.bmp",&result);
WritePolyList("test_convexpartition_OPT.txt",&result);
}
// Triangulate a polygon, returning vertex indices to the triangles
//
// verts - 3D vertex positions
// outerRingInds - Indices of outer boundary vertices in `verts` array;
// the j'th component of the i'th polygon vertex is
// verts[3*outerRingInds[i]+j].
// innerRingSizes - Number of vertices in each polygon hole
// innerRingInds - Indices of vertices for all holes; the first hole has
// indices innerRingInds[i] for i in [0, innerRingSize[0])
// triangleInds - Indices of triangular pieces are appended to this array.
//
// Return false if polygon couldn't be triangulated.
static bool triangulatePolygon(const std::vector<float>& verts,
const std::vector<GLuint>& outerRingInds,
const std::vector<GLuint>& innerRingSizes,
const std::vector<GLuint>& innerRingInds,
std::vector<GLuint>& triangleInds)
{
// Figure out which dimension the bounding box is smallest along, and
// discard it to reduce dimensionality to 2D.
Imath::Box3d bbox;
for (size_t i = 0; i < outerRingInds.size(); ++i)
{
GLuint j = 3*outerRingInds[i];
assert(j + 2 < verts.size());
bbox.extendBy(V3d(verts[j], verts[j+1], verts[j+2]));
}
V3d diag = bbox.size();
int xind = 0;
int yind = 1;
if (diag.z > std::min(diag.x, diag.y))
{
if (diag.x < diag.y)
xind = 2;
else
yind = 2;
}
std::list<TPPLPoly> triangles;
if (innerRingSizes.empty())
{
TPPLPoly poly;
// Simple case - no holes
//
// TODO: Use Triangulate_MONO, after figuring out why it's not always
// working?
initTPPLPoly(poly, verts, xind, yind,
outerRingInds.data(), (int)outerRingInds.size(), false);
TPPLPartition polypartition;
if (!polypartition.Triangulate_EC(&poly, &triangles))
{
g_logger.warning("Ignoring polygon which couldn't be triangulated.");
return false;
}
}
else
{
// Set up outer polygon boundary and holes.
std::list<TPPLPoly> inputPolys;
inputPolys.resize(1 + innerRingSizes.size());
auto polyIter = inputPolys.begin();
initTPPLPoly(*polyIter, verts, xind, yind,
outerRingInds.data(), (int)outerRingInds.size(), false);
++polyIter;
for (size_t i = 0, j = 0; i < innerRingSizes.size(); ++i, ++polyIter)
{
size_t count = innerRingSizes[i];
if (j + count > innerRingInds.size())
{
g_logger.warning("Ignoring polygon with inner ring %d of %d too large",
i, innerRingSizes.size());
return false;
}
initTPPLPoly(*polyIter, verts, xind, yind,
innerRingInds.data() + j, (int)count, true);
j += count;
}
// Triangulate
std::list<TPPLPoly> noHolePolys;
TPPLPartition polypartition;
polypartition.RemoveHoles(&inputPolys, &noHolePolys);
if (!polypartition.Triangulate_EC(&noHolePolys, &triangles))
{
g_logger.warning("Ignoring polygon which couldn't be triangulated.");
return false;
}
}
for (auto tri = triangles.begin(); tri != triangles.end(); ++tri)
{
triangleInds.push_back((*tri)[0].id);
triangleInds.push_back((*tri)[1].id);
triangleInds.push_back((*tri)[2].id);
}
return true;
}
void
ShapeMarker::createMarker (visualization_msgs::InteractiveMarkerControl& im_ctrl)
{
marker.id = shape_.id;
marker.header = shape_.header;
//std::cout << marker.header.frame_id << std::endl;
//marker.header.stamp = ros::Time::now() ;
marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
marker.ns = "shape visualization";
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration ();
//set color
marker.color.g = shape_.color.g;
marker.color.b = shape_.color.b;
marker.color.r = shape_.color.r;
if (arrows_ || deleted_){
marker.color.a = 0.5;
}
else
{
marker.color.a = shape_.color.a;
// marker.color.r = shape_.color.r;
}
//set scale
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 1;
/* transform shape points to 2d and store 2d point in triangle list */
TPPLPartition pp;
list<TPPLPoly> polys, tri_list;
Eigen::Vector3f v, normal, origin;
if(shape_.type== cob_3d_mapping_msgs::Shape::CYLINDER)
{
cob_3d_mapping::Cylinder c;
cob_3d_mapping::fromROSMsg (shape_, c);
c.ParamsFromShapeMsg();
// make trinagulated cylinder strip
//transform cylinder in local coordinate system
c.makeCyl2D();
c.TransformContours(c.transform_from_world_to_plane);
//c.transform2tf(c.transform_from_world_to_plane);
//TODO: WATCH OUT NO HANDLING FOR MULTY CONTOUR CYLINDERS AND HOLES
TPPLPoly poly;
TPPLPoint pt;
for(size_t j=0;j<c.contours.size();j++){
poly.Init(c.contours[j].size());
poly.SetHole (shape_.holes[j]);
for(size_t i=0;i<c.contours[j].size();++i){
pt.x=c.contours[j][i][0];
pt.y=c.contours[j][i][1];
poly[i]=pt;
}
if (shape_.holes[j])
poly.SetOrientation (TPPL_CW);
else
poly.SetOrientation (TPPL_CCW);
polys.push_back(poly);
}
// triangualtion itno monotone triangles
pp.Triangulate_EC (&polys, &tri_list);
transformation_inv_ = c.transform_from_world_to_plane.inverse();
// optional refinement step
list<TPPLPoly> refined_tri_list;
triangle_refinement(tri_list,refined_tri_list);
tri_list=refined_tri_list;
}
if(shape_.type== cob_3d_mapping_msgs::Shape::POLYGON)
{
cob_3d_mapping::Polygon p;
if (shape_.params.size () == 4)
{
cob_3d_mapping::fromROSMsg (shape_, p);
normal (0) = shape_.params[0];
normal (1) = shape_.params[1];
normal (2) = shape_.params[2];
origin (0) = shape_.centroid.x;
origin (1) = shape_.centroid.y;
origin (2) = shape_.centroid.z;
v = normal.unitOrthogonal ();
pcl::getTransformationFromTwoUnitVectorsAndOrigin (v, normal, origin, transformation_);
transformation_inv_ = transformation_.inverse ();
}
for (size_t i = 0; i < shape_.points.size (); i++)
{
pcl::PointCloud<pcl::PointXYZ> pc;
TPPLPoly poly;
pcl::fromROSMsg (shape_.points[i], pc);
poly.Init (pc.points.size ());
poly.SetHole (shape_.holes[i]);
for (size_t j = 0; j < pc.points.size (); j++)
{
poly[j] = msgToPoint2D (pc[j]);
}
if (shape_.holes[i])
poly.SetOrientation (TPPL_CW);
else
poly.SetOrientation (TPPL_CCW);
polys.push_back (poly);
}
pp.Triangulate_EC (&polys, &tri_list);
}//Polygon
if(tri_list.size()==0)
{
ROS_WARN("Could not triangulate, will not display this shape! (ID: %d)", shape_.id);
}
//ROS_INFO(" creating markers for this shape.....");
marker.points.resize (/*it->GetNumPoints ()*/tri_list.size()*3);
TPPLPoint pt;
int ctr=0;
for (std::list<TPPLPoly>::iterator it = tri_list.begin (); it != tri_list.end (); it++)
{
//draw each triangle
switch(shape_.type)
{
case(cob_3d_mapping_msgs::Shape::POLYGON):
{
for (long i = 0; i < it->GetNumPoints (); i++)
{
pt = it->GetPoint (i);
marker.points[3*ctr+i].x = pt.x;
marker.points[3*ctr+i].y = pt.y;
marker.points[3*ctr+i].z = 0;
//if(shape_.id == 39) std::cout << pt.x << "," << pt.y << std::endl;
}
//std::cout << marker.points.size() << std::endl;
}
case(cob_3d_mapping_msgs::Shape::CYLINDER):
{
for (long i = 0; i < it->GetNumPoints (); i++)
{
pt = it->GetPoint(i);
//apply rerolling of cylinder analogous to cylinder class
if(shape_.params.size()!=10){
break;
}
double alpha=pt.x/shape_.params[9];
marker.points[3*ctr+i].x = shape_.params[9]*sin(-alpha);
marker.points[3*ctr+i].y = pt.y;
marker.points[3*ctr+i].z = shape_.params[9]*cos(-alpha);
////Keep Cylinder flat - Debuging
//marker.points[i].x = pt.x;
//marker.points[i].y = pt.y;
//marker.points[i].z = 0;
}
}
}
ctr++;
}
//set pose
Eigen::Quaternionf quat (transformation_inv_.rotation ());
Eigen::Vector3f trans (transformation_inv_.translation ());
marker.pose.position.x = trans (0);
marker.pose.position.y = trans (1);
marker.pose.position.z = trans (2);
marker.pose.orientation.x = quat.x ();
marker.pose.orientation.y = quat.y ();
marker.pose.orientation.z = quat.z ();
marker.pose.orientation.w = quat.w ();
im_ctrl.markers.push_back (marker);
// if(!arrows_) {
// Added For displaying the arrows on Marker Position
marker_.pose.position.x = marker.pose.position.x ;
marker_.pose.position.y = marker.pose.position.y ;
marker_.pose.position.z = marker.pose.position.z ;
marker_.pose.orientation.x = marker.pose.orientation.x ;
marker_.pose.orientation.y = marker.pose.orientation.y ;
marker_.pose.orientation.z = marker.pose.orientation.z ;
// end
}
std::vector<std::vector<Point3d> > computeTriangulation(const Point3dVector& vertices, const std::vector<std::vector<Point3d> >& holes, double tol)
{
std::vector<std::vector<Point3d> > result;
// check input
if (vertices.size () < 3){
return result;
}
boost::optional<Vector3d> normal = getOutwardNormal(vertices);
if (!normal || normal->z() > -0.999){
return result;
}
for (const auto& hole : holes){
normal = getOutwardNormal(hole);
if (!normal || normal->z() > -0.999){
return result;
}
}
std::vector<Point3d> allPoints;
// PolyPartition does not support holes which intersect the polygon or share an edge
// if any hole is not fully contained we will use boost to remove all the holes
bool polyPartitionHoles = true;
for (const std::vector<Point3d>& hole : holes){
if (!within(hole, vertices, tol)){
// PolyPartition can't handle this
polyPartitionHoles = false;
break;
}
}
if (!polyPartitionHoles){
// use boost to do all the intersections
std::vector<std::vector<Point3d> > allFaces = subtract(vertices, holes, tol);
std::vector<std::vector<Point3d> > noHoles;
for (const std::vector<Point3d>& face : allFaces){
std::vector<std::vector<Point3d> > temp = computeTriangulation(face, noHoles);
result.insert(result.end(), temp.begin(), temp.end());
}
return result;
}
// convert input to vector of TPPLPoly
std::list<TPPLPoly> polys;
TPPLPoly outerPoly; // must be counter-clockwise, input vertices are clockwise
outerPoly.Init(vertices.size());
outerPoly.SetHole(false);
unsigned n = vertices.size();
for(unsigned i = 0; i < n; ++i){
// should all have zero z coordinate now
double z = vertices[n-i-1].z();
if (abs(z) > tol){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
return result;
}
Point3d point = getCombinedPoint(vertices[n-i-1], allPoints, tol);
outerPoly[i].x = point.x();
outerPoly[i].y = point.y();
}
outerPoly.SetOrientation(TPPL_CCW);
polys.push_back(outerPoly);
for (const std::vector<Point3d>& holeVertices : holes){
if (holeVertices.size () < 3){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Hole has fewer than 3 points, ignoring");
continue;
}
TPPLPoly innerPoly; // must be clockwise, input vertices are clockwise
innerPoly.Init(holeVertices.size());
innerPoly.SetHole(true);
//std::cout << "inner :";
for(unsigned i = 0; i < holeVertices.size(); ++i){
// should all have zero z coordinate now
double z = holeVertices[i].z();
if (abs(z) > tol){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
return result;
}
Point3d point = getCombinedPoint(holeVertices[i], allPoints, tol);
innerPoly[i].x = point.x();
innerPoly[i].y = point.y();
}
innerPoly.SetOrientation(TPPL_CW);
polys.push_back(innerPoly);
}
// do partitioning
TPPLPartition pp;
std::list<TPPLPoly> resultPolys;
int test = pp.Triangulate_EC(&polys,&resultPolys);
if (test == 0){
test = pp.Triangulate_MONO(&polys, &resultPolys);
}
if (test == 0){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Failed to partition polygon");
return result;
}
// convert back to vertices
std::list<TPPLPoly>::iterator it, itend;
//std::cout << "Start" << std::endl;
for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){
it->SetOrientation(TPPL_CW);
std::vector<Point3d> triangle;
for (long i = 0; i < it->GetNumPoints(); ++i){
TPPLPoint point = it->GetPoint(i);
triangle.push_back(Point3d(point.x, point.y, 0));
}
//std::cout << triangle << std::endl;
result.push_back(triangle);
}
//std::cout << "End" << std::endl;
return result;
}
void PlaneExt::TriangulatePlanePolygon()
{
// clear Marker and Shape messages
planeTriangles.points.clear();
planeTrianglesSRS.clear();
// for all polygons representing this plane
for (unsigned int polygon_i = 0; polygon_i < planePolygonsClipper.size(); ++polygon_i)
{
planeTrianglesSRS.push_back(cob_3d_mapping_msgs::Shape());
planeTrianglesSRS.back().type = cob_3d_mapping_msgs::Shape::POLYGON;
planeTrianglesSRS.back().params.push_back(a);
planeTrianglesSRS.back().params.push_back(b);
planeTrianglesSRS.back().params.push_back(c);
planeTrianglesSRS.back().params.push_back(d);
planeTrianglesSRS.back().holes.push_back(false);
// convert each polygon to polygonizer DS
TPPLPoly triPolygon;
triPolygon.Init(planePolygonsClipper[polygon_i].outer.size());
pcl::PointCloud<pcl::PointXYZ> current_point_cloud;
for (unsigned int i = 0; i < planePolygonsClipper[polygon_i].outer.size(); ++i)
{
triPolygon[i].x = CONVERT_FROM_LONG(planePolygonsClipper[polygon_i].outer[i].X);
triPolygon[i].y = CONVERT_FROM_LONG(planePolygonsClipper[polygon_i].outer[i].Y);
// additionaly, insert this point into shape message
pcl::PointXYZ point;
point.x = triPolygon[i].x;
point.y = triPolygon[i].y;
point.z = 0;
current_point_cloud.push_back(point);
}
// triangulate
TPPLPartition triangulation;
std::list<TPPLPoly> triangles;
triangulation.Triangulate_EC(&triPolygon, &triangles);
// create a message object
for (std::list<TPPLPoly>::iterator it = triangles.begin(); it != triangles.end(); ++it)
{
for (unsigned int j = 0; j < it->GetNumPoints(); ++j)
{
Eigen::Vector3f vec;
vec(0) = it->GetPoint(j).x;
vec(1) = it->GetPoint(j).y;
vec(2) = 0;
vec = planeTransXY.inverse() * vec;
geometry_msgs::Point p;
p.x = vec(0) + planeShift*a;
p.y = vec(1) + planeShift*b;
p.z = vec(2) + planeShift*c;
planeTriangles.points.push_back(p);
}
}
// insert polygon point cloud into Shape message
pcl::transformPointCloud(current_point_cloud, current_point_cloud, planeTransXY.inverse());
pcl::transformPointCloud(current_point_cloud, current_point_cloud, Eigen::Vector3f(a*planeShift, b*planeShift, c*planeShift), Eigen::Quaternion<float>(0,0,0,0));
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(current_point_cloud, cloud);
planeTrianglesSRS.back().points.push_back(cloud);
planeTrianglesSRS.back().centroid.x = planeShift*a;
planeTrianglesSRS.back().centroid.y = planeShift*b;
planeTrianglesSRS.back().centroid.z = planeShift*c;
planeTrianglesSRS.back().color = color;
planeTriangles.color = color;
}
// compute centroid
}
void ShapeMarker::onNewMessage( const MarkerConstPtr& old_message,
const MarkerConstPtr& new_message )
{
TPPLPartition pp;
list<TPPLPoly> polys,result;
//fill polys
for(size_t i=0; i<new_message->points.size(); i++) {
pcl::PointCloud<pcl::PointXYZ> pc;
TPPLPoly poly;
pcl::fromROSMsg(new_message->points[i],pc);
poly.Init(pc.size());
poly.SetHole(new_message->holes[i]);
for(size_t j=0; j<pc.size(); j++) {
poly[j] = MsgToPoint2D(pc[j], new_message);
}
if(new_message->holes[i])
poly.SetOrientation(TPPL_CW);
else
poly.SetOrientation(TPPL_CCW);
polys.push_back(poly);
}
pp.Triangulate_EC(&polys,&result);
polygon_->clear();
polygon_->begin(createMaterialIfNotExists(new_message->color.r,new_message->color.b,new_message->color.g,new_message->color.a), Ogre::RenderOperation::OT_TRIANGLE_LIST);
TPPLPoint p1,p2,p3,p4, p12,p23,p31;
for(std::list<TPPLPoly>::iterator it=result.begin(); it!=result.end(); it++) {
//draw each triangle
if(it->GetNumPoints()!=3) continue;
p1 = it->GetPoint(0);
p2 = it->GetPoint(1);
p3 = it->GetPoint(2);
p4.x = (p1.x+p2.x+p3.x)/3;
p4.y = (p1.y+p2.y+p3.y)/3;
p12.x = (p1.x+p2.x)/2;
p12.y = (p1.y+p2.y)/2;
p23.x = (p3.x+p2.x)/2;
p23.y = (p3.y+p2.y)/2;
p31.x = (p1.x+p3.x)/2;
p31.y = (p1.y+p3.y)/2;
triangle(new_message,polygon_,p1,p12,p4);
triangle(new_message,polygon_,p1,p31,p4);
triangle(new_message,polygon_,p3,p23,p4);
triangle(new_message,polygon_,p12,p2,p4);
triangle(new_message,polygon_,p31,p3,p4);
triangle(new_message,polygon_,p23,p2,p4);
}
polygon_->end();
vis_manager_->getSelectionManager()->removeObject(coll_);
/*coll_ = vis_manager_->getSelectionManager()->createCollisionForObject(
shape_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID(
"fake_ns", new_message->id))), coll_);*/
Ogre::Vector3 pos, scale, scale_correct;
Ogre::Quaternion orient;
//transform(new_message, pos, orient, scale);
/*if (owner_ && (new_message->scale.x * new_message->scale.y
* new_message->scale.z == 0.0f))
{
owner_->setMarkerStatus(getID(), status_levels::Warn,
"Scale of 0 in one of x/y/z");
}*/
Eigen::Vector3f origin=MsgToOrigin(new_message);
pos.x = origin(0);
pos.y = origin(1);
pos.z = origin(2);
//setPosition(pos);
return;
setOrientation( orient * Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) );
//scale_correct = Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) * scale;
//shape_->setScale(scale_correct);
}