Vec2f VRIntersect_computeTexel(VRIntersection& ins, NodeRecPtr node) { if (!ins.hit) return Vec2f(0,0); if (node == 0) return Vec2f(0,0); GeometryRefPtr geo = dynamic_cast<Geometry*>( node->getCore() ); if (geo == 0) return Vec2f(0,0); auto texcoords = geo->getTexCoords(); if (texcoords == 0) return Vec2f(0,0); TriangleIterator iter = geo->beginTriangles(); iter.seek( ins.triangle ); Matrix m = node->getToWorld(); m.invert(); Pnt3f local_pnt; m.mult(ins.point, local_pnt); Pnt3f p0 = iter.getPosition(0); Pnt3f p1 = iter.getPosition(1); Pnt3f p2 = iter.getPosition(2); Vec3f cr = (p1 - p0).cross(p2 - p0); Vec3f n = cr; n.normalize(); float areaABC = n.dot(cr); float areaPBC = n.dot((p1 - local_pnt).cross(p2 - local_pnt)); float areaPCA = n.dot((p2 - local_pnt).cross(p0 - local_pnt)); float a = areaPBC / areaABC; float b = areaPCA / areaABC; float c = 1.0f - a - b; return iter.getTexCoords(0) * a + iter.getTexCoords(1) * b + iter.getTexCoords(2) * c; }
/*! The IntersectAction callback for Geometry. It computes if the ray used in the IntersectAction \a action hits this object and if that is the case, which triangle is hit. \param[in] action IntersectAction performing the intersect test. \return Action result code, \see OSG::Action. \note This method is registered with the IntersectAction and automatically called from there, you probably never have to call it manually. */ Action::ResultE Geometry::intersect(Action * action) { IntersectAction *ia = dynamic_cast<IntersectAction*>(action); ia->getActNode()->updateVolume(); const BoxVolume &bv = ia->getActNode()->getVolume(); if(bv.isValid() && !bv.intersect(ia->getLine())) { return Action::Skip; //bv missed -> can not hit children } TriangleIterator it = this->beginTriangles(); TriangleIterator end = this->endTriangles (); Real32 t; Vec3f norm; Line ia_line(ia->getLine()); for(; it != end; ++it) { if(ia_line.intersect(it.getPosition(0), it.getPosition(1), it.getPosition(2), t, &norm)) { ia->setHit(t, ia->getActNode(), it.getIndex(), norm, -1); } } // If we need to test lines, iterate over lines and test for // lines that are within width distance from the line if(ia->getTestLines()) { Real32 range_sq = ia->getTestLineWidth(); range_sq = range_sq * range_sq; LineIterator it = this->beginLines(); LineIterator end = this->endLines (); Pnt3f pt1, pt2; OSG::Vec3f norm; // Find closest points and if they are within the range, then add a hit for(; it != end; ++it) { Line cur_line(it.getPosition(0), it.getPosition(1)); ia_line.getClosestPoints(cur_line, pt1, pt2); Real32 dist_sq( pt1.dist2(pt2) ); if (dist_sq <= range_sq) { t = ia_line.getPosition().dist(pt1); ia->setHit(t, ia->getActNode(), -1, norm, it.getIndex()); } } } return Action::Continue; }
Action::ResultE geometryEnter(CNodePtr& node, Action * action) { IntersectAction * ia = dynamic_cast<IntersectAction*>(action); NodePtr n( node ); Geometry* core = dynamic_cast<Geometry*>(get_pointer(n->getCore())); TriangleIterator it; Real32 t; Vec3f norm; for ( it = core->beginTriangles(); it != core->endTriangles(); ++it ) { if ( ia->getLine().intersect( it.getPosition(0), it.getPosition(1), it.getPosition(2), t, &norm ) ) { ia->setHit( t, NodePtr(node), it.getIndex(), norm ); } } return Action::Continue; }
void renderScene(void) { //open file fstream file; stringstream filename; filename << "ProjectionData_"<< time(0)<<".dat"; file.open(filename.str().c_str(),ios::out); for(int q(100);q>=50;--q) { file << "FRAME NO. "<<q<<endl; for(int fu(0);fu<2;++fu){ /*if(_m[0]<1) _m[0] += 0.1; else _m[0] = 0.99;*/ /* if(_m[4]<1) _m[4] += 0.10; else _m[4] = 0.99; if(_m[8]<1) _m[8] += 0.10; else _m[8] = 0.99;*/ int window_w = 1280; int window_h = 1024; cout << "render scene... "<<endl; TriangleIterator ti; glEnable(GL_DEPTH_TEST); glEnable(GL_CULL_FACE); glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glBegin(GL_TRIANGLES); //map<int, vector<Pnt3f> > _COLORS; map<int, int> _COLORS; for(ti = TEST->beginTriangles();ti != TEST->endTriangles();++ti) { int rgb_index = 0; int r(0),g(0),b(0); while(_COLORS[rgb_index] != NULL) { // create random color r = (int)rand() % 255; g = (int)rand() % 255; b = (int)rand() % 255; // calculate index rgb_index = 256 * g + 256 * 256 * r + b; //cout << rgb_index << endl; } // SAVE ALL _USED_ COLORS vector<Pnt3f> v; v.push_back(ti.getPosition(0)); v.push_back(ti.getPosition(1)); v.push_back(ti.getPosition(2)); _COLORS[rgb_index] = ti.getIndex(); //cout << "r " << r << " g "<<g << " b "<<b<<endl; // get points from triangle Pnt3f p1 = ti.getPosition(0); Pnt3f p2 = ti.getPosition(1); Pnt3f p3 = ti.getPosition(2); //set color and add vertices //glColor3f(r,g,b); glColor3ub(r,g,b); glVertex3f(p1[0],p1[1],p1[2]); glVertex3f(p2[0],p2[1],p2[2]); glVertex3f(p3[0],p3[1],p3[2]); } glEnd(); glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(1.0, 1.0); glBegin(GL_TRIANGLES); // set background color glColor3f(0,0,0); for(ti = TEST->beginTriangles();ti != TEST->endTriangles();++ti) { Pnt3f p1 = ti.getPosition(0); Pnt3f p2 = ti.getPosition(1); Pnt3f p3 = ti.getPosition(2); glVertex3f(p1[0],p1[1],p1[2]); glVertex3f(p2[0],p2[1],p2[2]); glVertex3f(p3[0],p3[1],p3[2]); } glEnd(); glDisable(GL_POLYGON_OFFSET_FILL); glDisable(GL_DEPTH_TEST); //glutSwapBuffers(); map<int,std::vector<int> > color_map; // window size int size = window_w*window_h*3; // read pixels GLubyte *pixels = new GLubyte[size]; glReadPixels(0 , 0 , window_w , window_h , GL_RGB , GL_UNSIGNED_BYTE , pixels); // init RGB and count&debug values int red,green,blue; int count(0); //iterate through pixels for(int u(0);u < size;u=u+3){ // get pixels red = pixels[u]; green = pixels[u+1]; blue = pixels[u+2]; // calc unique index int index = 256 * green + 256 * 256 * red + blue; // ignore black if(index == 0 ) continue; // fill RGB vector vector<int> ct; ct.push_back(red); ct.push_back(green); ct.push_back(blue); // put in map color_map[index] = ct; } cout << "Colors seen in frame: "<< color_map.size()<<endl; map<int,vector<int> >::iterator mip; // for all _visible_ triangles int h(0); FaceIterator fit = TEST->beginFaces(); float thresh = 0.95; int count_lines_drawn(0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glPolygonMode(GL_FRONT_AND_BACK, GL_LINES); vector<Pnt3f> cp; for(mip = color_map.begin();mip != color_map.end();mip++){ int face_index = _COLORS[mip->first]; fit.seek(face_index); int a = fit.getPositionIndex(0); int b = fit.getPositionIndex(1); int c = fit.getPositionIndex(2); FaceIterator nit; glBegin(GL_LINES); glColor3f(1.0,0,0); for(nit = TEST->beginFaces();nit != TEST->endFaces();++nit) { if(fit.getIndex() == nit.getIndex()) continue; int a2 = nit.getPositionIndex(0); int b2 = nit.getPositionIndex(1); int c2 = nit.getPositionIndex(2); // a-b if(a == a2 || a == b2 || a == c2) if(b == a2 || b == b2 || b == c2){ if(fit.getNormal(0).dot(nit.getNormal(0)) < thresh){ count_lines_drawn++; //glVertex3f(fit.getPosition(0)[0],fit.getPosition(0)[1],fit.getPosition(0)[2]); //glVertex3f(fit.getPosition(1)[0],fit.getPosition(1)[1],fit.getPosition(1)[2]); _LINES.push_back(fit.getPosition(0)); _LINES.push_back(fit.getPosition(1)); //createControlPoints(_LINES.size()-2,fit.getPosition(0),fit.getPosition(1)); } h++; } // a-c if(a == a2 || a == b2 || a == c2) if(c == a2 || c == b2 || c == c2){ if(fit.getNormal(0).dot(nit.getNormal(0)) < thresh){ count_lines_drawn++; //glVertex3f(fit.getPosition(0)[0],fit.getPosition(0)[1],fit.getPosition(0)[2]); //glVertex3f(fit.getPosition(2)[0],fit.getPosition(2)[1],fit.getPosition(2)[2]); _LINES.push_back(fit.getPosition(0)); _LINES.push_back(fit.getPosition(2)); //createControlPoints(_LINES.size()-2,fit.getPosition(0),fit.getPosition(2)); } h++; } // c-b if(c == a2 || c == b2 || c == c2) if(b == a2 || b == b2 || b == c2){ if(fit.getNormal(0).dot(nit.getNormal(0)) < thresh){ count_lines_drawn++; //glVertex3f(fit.getPosition(1)[0],fit.getPosition(1)[1],fit.getPosition(1)[2]); //glVertex3f(fit.getPosition(2)[0],fit.getPosition(2)[1],fit.getPosition(2)[2]); _LINES.push_back(fit.getPosition(1)); _LINES.push_back(fit.getPosition(2)); //createControlPoints(_LINES.size()-2,fit.getPosition(1),fit.getPosition(2)); } h++; } } glEnd(); } //glutSwapBuffers(); // DRAW _CONTROLPOINTS_ //sorted lines by length in _LINES //sortLines(); // create CP's now _HITPOINTS.clear(); _CONTROLPOINTS.clear(); _CP2D.clear(); _LINES2D.clear(); /*for(int i(0);i < _LINES.size()/10;i=i+2){ createControlPoints(i,_LINES[i],_LINES[i+1]); }*/ /* glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glBegin(GL_POINTS); glColor3f(0,1,0); // for each line for(int i(0);i < _CONTROLPOINTS.size(); ++i){ // get CP-vector vector<Pnt3f> cps = _CONTROLPOINTS[i]; // for every single controlpoint for(int j(0);j<cps.size();++j) glVertex3f(cps[i][0],cps[i][1],cps[i][2]); } glEnd(); glutSwapBuffers(); */ cout << "Lines over Threshold :: "<< _CONTROLPOINTS.size() <<endl; cout << "Shared Edges :: " << h <<endl; cout << "Lines drawn :: " << count_lines_drawn << endl; cout << "Lines :: "<< _LINES.size()<<endl; //open cv stringstream stream; stream << "pics/undist/resize/"; stream << q; stream << ".bmp"; /*"pics/1_1.bmp"*/ cout << "/*** PICTURE "<< stream.str().c_str() << " ***/"<<endl; cv::Mat init_Image2 = cvLoadImage(stream.str().c_str()); cv::flip(init_Image2,init_Image2,0); // overwrite canny with ground truth //cv::Mat init_Image2 = cvLoadImage("ground_truth/canny/30c.bmp"); cv::Mat gray; cv::Mat init_Image;// = init_Image2.clone(); cv::cvtColor(init_Image2,gray,CV_RGB2GRAY); cv::Mat gaus; cv::GaussianBlur(gray,gaus,cv::Size(3,3),1); ////cv::Sobel(gray,init_Image,init_Image.type(),1,0,3); cv::Canny(gaus,init_Image,5,10,3,true); vector<vector<cv::Point> > contours; vector<vector<cv::Point> > new_contours; cv::findContours(init_Image,contours,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE,cv::Point()); cv::Mat new_contour = cv::Mat(1024,1280,CV_8UC3); // for all contours for(int c(0);c<contours.size();++c){ int size = (contours[c]).size(); //cout << "size = "<< size <<endl; cv::Point start = (contours[c])[0]; cv::Point end = (contours[c])[size]; float length = sqrt((start.x-end.x)*(start.x-end.x)+(start.y-end.y)*(start.y-end.y)); //check contours size if(contours[c].size()>20 /* && (size-10 < length < size+10)*/){ for(int c1(0);c1<contours[c].size();++c1){ cv::Point tmp = (contours[c])[c1]; cv::line(new_contour,tmp,tmp,CV_RGB(255,255,255)); } } //cout << start.x << " " << start.y<<endl; //cout << end.x << " " << end.y<<endl; //if(length > 20) //cout << "::"<<length<<endl; } //exit(0); cv::Mat cont; cv::cvtColor(new_contour,cont,CV_RGB2GRAY); init_Image = cont.clone(); //cv::cvtColor(init_Image,init_Image2,CV_GRAY2RGB); //init_Image = init_Image2.clone(); cv::imshow("gray",init_Image); // window size size = window_w*window_h*3; // read pixels GLubyte *pixels2 = new GLubyte[size]; glReadPixels(0 , 0 , window_w , window_h , GL_RGB , GL_UNSIGNED_BYTE , pixels2); cv::Mat ogl = cv::Mat(window_h,window_w,CV_8UC3); ogl.data = pixels2; cv::flip(ogl,ogl,0); //cv::imwrite("test45.bmp",ogl); CvSize size2; size2.height=1024; size2.width=1280; cv::Mat result = cv::Mat(size2,CV_8UC3);//= (cv::Mat)cvCreateImage(size2,8,3); //MY 2D-POINTS GLdouble modelview[16], projection2[16]; GLint viewport[4]; glGetDoublev(GL_MODELVIEW_MATRIX, modelview); glGetDoublev(GL_PROJECTION_MATRIX, projection2); glGetIntegerv(GL_VIEWPORT, viewport); /*double tx, ty, tz; for(int i(0); i < _LINES.size(); ++i) { Pnt3f tmp = _LINES[i]; cout << "3d-point:" << tmp<<endl; gluProject(tmp[0], tmp[1], tmp[2], modelview, projection2, viewport, &tx, &ty, &tz); //cout <<"x: " <<tx << "y: "<<ty<<endl; Pnt3d res; res[0] = tx; res[1] = ty; res[2] = tz; cout <<"2d-point"<<res<<endl; _LINES2D.push_back(res); }*/ int thres = (int)_LINES.size();//(int)(_LINES.size()/2)*0.15; sortLines3D(thres); _HITPOINTS = map<int,vector<Pnt2f> >(); double tx1, ty1, tz1; double tx2, ty2, tz2; // draw 2D-Lines //define threshold for CP creation //cout << ">>>>>> "<<thres<<endl; cv::flip(init_Image,init_Image,0); int ok(0); for(int i(0);i<thres;i=i+2){ ok = 0; // get 3D Line endings Pnt3d p1 = _LINES[i]; Pnt3d p2 = _LINES[i+1]; if(p1.dist(p2)<0.3){ continue; } // get 2d points of line endings gluProject(p1[0], p1[1], p1[2], modelview, projection2, viewport, &tx1, &ty1, &tz1); gluProject(p2[0], p2[1], p2[2], modelview, projection2, viewport, &tx2, &ty2, &tz2); // create Normals Pnt2f lineStart, lineEnd; lineStart[0]=tx1;lineStart[1]=ty1; lineEnd[0]=tx2;lineEnd[1]=ty2; Pnt2f lineNormal = lineStart - lineEnd; float l = sqrt(lineNormal[0]*lineNormal[0]+lineNormal[1]*lineNormal[1]); Pnt2f normal1,normal2; //normalized Normals 1 & 2 normal1[0]=-lineNormal[1]/l;normal1[1]=lineNormal[0]/l; normal2[0]=lineNormal[1]/l;normal2[1]=-lineNormal[0]/l; // draw line //cv::line(result,cv::Point(tx1,ty1),cv::Point(tx2,ty2),CV_RGB(255,0,0),1); //creating 2d controlpoints //float t = createControlPoints2D(i,p1,p2); //if(i <= thres){ //vector<Pnt2d> cp = _CP2D[i]; vector<Pnt3f> cp = _CONTROLPOINTS[i]; Pnt2f old_hit = Pnt2f(-1,0); for(int j(0);j<cp.size();++j){ //get single 3D-ControlPoint Pnt3d tmp = cp[j]; //cout <<"i: "<<i<<" j: "<< j<<endl; // fixpoint check! if(i==0){ if(_FIX[0] == -1){ double x,y,z; gluProject(tmp[0], tmp[1], tmp[2], modelview, projection2, viewport, &x,&y,&z); //Pnt2d tmp2d; _FIX[0] = x; _FIX[1] = y; // cout << "::: "<< (_HITPOINTS[0]).size()<<endl; } (_HITPOINTS[0]).push_back(_FIX); cv::circle(result,cv::Point(_FIX[0],_FIX[1]),3,CV_RGB(255,255,0),2); continue; } // project to 2D double x,y,z; gluProject(tmp[0], tmp[1], tmp[2], modelview, projection2, viewport, &x,&y,&z); Pnt2d tmp2d; tmp2d[0] = x; tmp2d[1] = y; // CONTROLPOINTS DRAWING //cv::circle(result,cv::Point(x,y),2,CV_RGB(100,100,100),2); //Pnt2f n1 = _NORMALS[i]; //Pnt2f n2 = _NORMALS[i+1]; Pnt2f hit = checkNormal(tmp2d,normal1,init_Image); Pnt2f hit1 = checkNormal(tmp2d,normal2,init_Image); //cout << "hit" << hit <<endl; bool outlier = isOutlier(tmp2d, hit,normal1,old_hit); bool outlier2= isOutlier(tmp2d,hit1,normal1,old_hit); old_hit = hit; //drawing if(outlier && outlier2){ //ok++; (_HITPOINTS[i]).push_back(Pnt2f(-1,0)); //cv::circle(result,cv::Point(hit[0],hit[1]),2,CV_RGB(0,255,255),3); //cv::circle(result,cv::Point(hit1[0],hit1[1]),2,CV_RGB(0,255,255),3); continue; } else { ok++; if(!outlier && !outlier2){ if(tmp2d.dist(hit)<tmp2d.dist(hit1)){ //cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(hit[0],hit[1]),CV_RGB(0,255,0),1); (_HITPOINTS[i]).push_back(hit); old_hit = hit; //cv::circle(result,cv::Point(hit1[0],hit1[1]),2,CV_RGB(0,255,255),3); } else{ //cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(hit1[0],hit1[1]),CV_RGB(0,255,0),1); (_HITPOINTS[i]).push_back(hit1); old_hit = hit1; //cv::circle(result,cv::Point(hit[0],hit[1]),2,CV_RGB(0,255,255),3); } } else if(!outlier){ (_HITPOINTS[i]).push_back(hit); //cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(hit[0],hit[1]),CV_RGB(0,255,0),1); old_hit = hit; //cv::circle(result,cv::Point(hit1[0],hit1[1]),2,CV_RGB(0,255,255),3); } else { (_HITPOINTS[i]).push_back(hit1); //cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(hit1[0],hit1[1]),CV_RGB(0,255,0),1); old_hit = hit1; //cv::circle(result,cv::Point(hit[0],hit[1]),2,CV_RGB(0,255,255),3); } //cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(tmp2d[0]+normal1[0],tmp2d[1]+normal1[1]),CV_RGB(0,255,0)); //cv::line(result,cv::Point(tmp2d[0],tmp2d[1]),cv::Point(tmp2d[0]+normal2[0],tmp2d[1]+normal2[1]),CV_RGB(0,255,0)); } } /*vector<Pnt2f> t_vec = _HITPOINTS[i]; if(t_vec.size() < 5){ _CONTROLPOINTS[i].clear(); _HITPOINTS[i].clear(); }*/ cout << i<<"::: "<< (_HITPOINTS[i]).size()<<endl; int realHP(0); for(int g(0);g<_HITPOINTS[i].size();++g){ if(_HITPOINTS[i][g] != -1) realHP++; } if((int)_CONTROLPOINTS[i].size()*0.2 > realHP) { _CONTROLPOINTS[i].clear(); _HITPOINTS[i].clear(); } //cout << "REAL HITPOINTS IN LINE "<< i << " -> "<<realHP<<endl; //cout << ":::"<<ok<<endl; // clear if not enough hitpoints on line if(ok < 3 && i != 0){ //(_HITPOINTS[i]).clear(); //cout << "clear hitlist"<< endl; } } int countCP = 0; //int thres = (int)(_LINES.size()/2)*0.15; for(int i(0);i<thres;i=i+2){ //cout << "line : "<<i<<endl; vector<Pnt3f> tmp = _CONTROLPOINTS[i]; vector<Pnt2f> tmp2 = _HITPOINTS[i]; for(int j(0);j<tmp.size();++j){ countCP++; Pnt2f hitp = tmp2[j]; //cv::circle(result,cv::Point(hitp[0],hitp[1]),2,CV_RGB(255,255,0)); //cout << "Point "<< tmp2[j]<<endl; // cout << "3D-Point "<< tmp[j]<<endl; } //exit(1); } cout << "####CONTROLPOINTS>> "<< countCP<<endl; countCP = 0; cout << "#### LEV - MAR ###"<<endl; for(int v(0);v<12;++v){ cout << " " << _m[v]; if(v%3 == 2) cout <<endl; } cout << "================"<<endl; int ret; //for(int go(0);go <=25 ; ++go) ret = dlevmar_dif(mapping,_m,NULL,12,_CP,1000,NULL,NULL,NULL,NULL,NULL); //ret = dlevmar_dif(mapping,_m,NULL,12,_CP,1000,NULL,NULL,NULL,NULL,NULL); for(int v(0);v<12;++v){ cout << " " << _m[v]; //file << " " << projection2[v]; if(v%3 == 2){ cout <<endl; //file <<endl; } } file << _m[0] << "," << _m[1] << "," << _m[2] << endl; file << _m[3] << "," << _m[4] << "," << _m[5] << endl; file << _m[6] << "," << _m[7] << "," << _m[8]<< endl; file << _m[9] << "," << _m[10] << "," << _m[11]<< endl; file << endl; cout <<endl; cout <<"iterated for: "<<ret<<endl; cout << "#### LEV - MAR - ENDE###"<<endl; //cout << "== HIT POINTS USED " << _HITPOINTS.size()<<endl; drawModel(result,0,0,255); //convert gray canny image back to rgb //cv::cvtColor(init_Image,init_Image2,CV_GRAY2RGB); // generated error image //cv::add(original,result,result); cv::flip(init_Image2,init_Image2,0); cv::add(init_Image2,result,result); // flip like hell cv::flip(result,result,0); // save image cv::imshow("showing image",result); stringstream ss; ss << time(0) << ".bmp"; cv::imwrite(ss.str(),result); _CONTROLPOINTS.clear(); _HITPOINTS.clear(); _LINES.clear(); } file << "=====" <<endl; } file.close(); }
// Converts geometry to a polyhedron && applies the geometry node's world transform to the polyhedron. // OpenSG geometry data isn't transformed itself but has an associated transform core. Both are unified for CGAL. CGAL::Polyhedron* CSGGeometry::toPolyhedron(GeometryRecPtr geometry, Matrix worldTransform, bool& success) { TriangleIterator it; auto gpos = geometry->getPositions(); // fix flat triangles (all three points aligned) for (it = TriangleIterator(geometry); !it.isAtEnd() ;++it) { vector<Pnt3f> p(3); vector<Vec3f> v(3); Vec3i vi = Vec3i(it.getPositionIndex(0), it.getPositionIndex(1), it.getPositionIndex(2)); for (int i=0; i<3; i++) p[i] = it.getPosition(i); v[0] = p[2]-p[1]; v[1] = p[2]-p[0]; v[2] = p[1]-p[0]; float A = (v[2].cross(v[1])).length(); if (A < 1e-16) { // small area, flat triangle? cout << "small area " << A << endl; for (int i=0; i<3; i++) cout << " pi " << p[i] << " vi " << vi[i] << " L " << v[i].squareLength() << endl; if (v[0].squareLength() < 1e-8) continue; // check if two points close, then ignore if (v[1].squareLength() < 1e-8) continue; if (v[2].squareLength() < 1e-8) continue; int im = 0; for (int i=1; i<3; i++) if (v[i].squareLength() > v[im].squareLength()) im = i; int j = (im+1)%3; cout << "set p[" << j << "] = " << p[j] << " with index i[" << im << "] = " << vi[im] << endl; gpos->setValue(p[j], vi[im]); for (int i=0; i<3; i++) p[i] = it.getPosition(i); cout << " result: " << endl; for (int i=0; i<3; i++) cout << " pi " << p[i] << endl; } } vector<CGAL::Point> positions; vector<size_t> indices; vector<Vec3f> pos; vector<int> inds; size_t curIndex = 0; // Convert triangles to cgal indices and vertices for (it = TriangleIterator(geometry); !it.isAtEnd() ;++it) { vector<size_t> IDs(3); for (int i=0; i<3; i++) IDs[i] = isKnownPoint( it.getPosition(i) ); for (int i=0; i<3; i++) { if (IDs[i] == numeric_limits<size_t>::max()) { Vec3f p = Vec3f(it.getPosition(i)); pos.push_back(p); positions.push_back( CGAL::Point(p[0], p[1], p[2]) ); IDs[i] = curIndex; //cout << "add point " << curIndex << " " << osgPos << endl; size_t *curIndexPtr = new size_t; *curIndexPtr = curIndex; oct->add(OcPoint(p[0], p[1], p[2]), curIndexPtr); curIndex++; } } //cout << "add triangle " << IDs[0] << " " << IDs[1] << " " << IDs[2] << endl; if (IDs[0] == IDs[1] || IDs[0] == IDs[2] || IDs[1] == IDs[2]) continue; // ignore flat triangles for (int i=0; i<3; i++) indices.push_back(IDs[i]); for (int i=0; i<3; i++) inds.push_back(IDs[i]); } // Cleanup for (void* o : oct->getData()) delete (size_t*)o; delete oct; oct = new Octree(THRESHOLD); // Construct the polyhedron from raw data success = true; CGAL::Polyhedron *result = new CGAL::Polyhedron(); PolyhedronBuilder<CGAL::HalfedgeDS> builder(positions, indices); result->delegate(builder); if (!result->is_closed()) { success = false; cout << "Error: The polyhedron is not a closed mesh!" << endl; create(GL_TRIANGLES, pos, pos, inds); setWorldMatrix(worldTransform); createSharedIndex(mesh); calcVertexNormals(mesh, 0.523598775598 /*30 deg in rad*/); getMaterial()->setFrontBackModes(GL_FILL, GL_NONE); } // Transform the polyhedron with the geometry's world transform matrix applyTransform(result, worldTransform); return result; }