// Initialize polypartition TPPLPoly from a list of indices and vertices
//
// verts - 3D polygon vertex vectors
// xind,yind - Indices of 3D vectors to extract as x and y coordinates for 2D
// triangulation computation.
// inds,size - Array of indices into the `verts` list
// isHole - Value for the hole flag, and to determine the orientation
static void initTPPLPoly(TPPLPoly& poly,
const std::vector<float>& verts,
int xind, int yind,
const GLuint* inds, int size,
bool isHole)
{
// Check for explicitly closed polygons (last and first vertices equal) and
// discard the last vertex in these cases. This is a pretty stupid
// convention, but the OGC have blessed it and now we've got a bunch of
// geospatial formats (kml, WKT, GeoJSON) which require it. Sigh.
// http://gis.stackexchange.com/questions/10308/why-do-valid-polygons-repeat-the-same-start-and-end-point/10309#10309
if (inds[0] == inds[size-1] ||
(verts[3*inds[0]+0] == verts[3*inds[size-1]+0] &&
verts[3*inds[0]+1] == verts[3*inds[size-1]+1] &&
verts[3*inds[0]+2] == verts[3*inds[size-1]+2]))
{
g_logger.warning_limited("Ignoring duplicate final vertex in explicitly closed polygon");
size -= 1;
}
// Copy into polypartition data structure
poly.Init(size);
for (int i = 0; i < size; ++i)
{
poly[i].x = verts[3*inds[i]+xind];
poly[i].y = verts[3*inds[i]+yind];
poly[i].id = inds[i];
}
int orientation = poly.GetOrientation();
// Invert so that outer = ccw, holes = cw
if ((orientation == TPPL_CW) ^ isHole)
poly.Invert();
poly.SetHole(isHole);
}
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 make_poly(float* buf, int pnt_sz, TPPLPoly& poly)
{
poly.Init(pnt_sz);
for (int k(0); k < pnt_sz; k++)
{
poly[k].x = buf[2 * k];
poly[k].y = buf[2 * k + 1];
}
if (poly.GetOrientation() == TPPL_CW)
{
poly.SetHole(true);
}
}
GeneralPolygon::operator list<TPPLPoly>()
{
auto isPolygonOutside = [&](const Contour &referencePolygon, const Contour &poly)
{
for(unsigned int p=0; p < referencePolygon.size() ; ++p)
if( !_evenOddRuleAlgorithm( referencePolygon[p], poly) )
return true;
return false;
};
list<TPPLPoly> polys;
for(unsigned int c=0; c < _contours.size() ; ++c)
{
const Contour &contour = _contours[c];
TPPLPoly poly;
poly.Init(contour.size());
for(unsigned int v=0; v < contour.size() ; ++v)
{
TPPLPoint point;
point.x = contour[v].x;
point.y = contour[v].y;
poly[v] = point;
}
// BE CAREFULL!!! not really correct because the polygon could be concave then
// the center of mass could not be inside the polygon
sf::Vector2f centerOfMass(0.0f, 0.0f);
for(unsigned int i=0; i < contour.size() ; ++i)
centerOfMass += contour[i];
centerOfMass *= (1.0f/contour.size());
std::vector<Contour> outsideContours;
for(unsigned int i=0; i < _contours.size() ; ++i)
{
if( i == c )
{
outsideContours.push_back(_contours[i]);
continue;
}
if( isPolygonOutside(_contours[i], contour) )
outsideContours.push_back(_contours[i]);
}
// first test if is a hole or not
if( !_evenOddRuleAlgorithm( centerOfMass, outsideContours ) )
poly.SetHole(true);
else
poly.SetHole(false);
// if it is a hole then it must be in CW order to the algorithm to recognize
// else must be in CCW
if( poly.IsHole() )
poly.SetOrientation(TPPL_CW);// Hole orientation (needed in the algorithm)
else
poly.SetOrientation(TPPL_CCW);// Not a hole orientation (needed in the algorithm)
polys.push_back(poly);
}
return polys;
}
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 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);
}
