void CPUSkinningAlgorithm::intersectGeometry(IntersectAction *iact,
SkinnedGeometry *skinGeo,
CPUSkinningDataAttachment *data )
{
const GeoVectorProperty *pos =
data->getProperties(Geometry::PositionsIndex);
TriangleIterator triIt = skinGeo->beginTriangles();
TriangleIterator triEnd = skinGeo->endTriangles ();
const Line &line = iact->getLine();
Real32 hitT = 0.f;
Vec3f hitNorm;
for(; triIt != triEnd; ++triIt)
{
if(line.intersect(
pos->getValue<Pnt3f>(triIt.getPositionIndex(0)),
pos->getValue<Pnt3f>(triIt.getPositionIndex(1)),
pos->getValue<Pnt3f>(triIt.getPositionIndex(2)), hitT, &hitNorm))
{
iact->setHit(hitT, iact->getActNode(),
triIt.getIndex(), hitNorm, -1);
}
}
}
/** Counts the number of virtual triangles of a DrawCall., i.e. the number of triangles you would retrieve by iterating over the iterator returned by triangleIterator(). */
u32 countTriangles() const
{
u32 count = 0;
for( TriangleIterator it = triangleIterator(); it.hasNext(); it.next() )
++count;
return count;
}
/*! 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;
}
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;
}
void VRAdjacencyGraph::compNeighbors() {
vertex_neighbor_params.clear();
vertex_neighbors.clear();
auto sgeo = geo.lock();
if (!sgeo) return;
map< int, map<int, int> > tmpdict;
for (TriangleIterator it = TriangleIterator(sgeo->getMesh()->geo); !it.isAtEnd(); ++it) {
int i0 = it.getPositionIndex(0);
int i1 = it.getPositionIndex(1);
int i2 = it.getPositionIndex(2);
auto reg = [&](int j0, int j1, int j2) {
if ( tmpdict.count(j0) == 0 ) tmpdict[j0] = map<int, int>();
if ( tmpdict[j0].count(j1) == 0 ) tmpdict[j0][j1] = 0;
if ( tmpdict[j0].count(j2) == 0 ) tmpdict[j0][j2] = 0;
tmpdict[j0][j1] += 1;
tmpdict[j0][j2] += 1;
};
reg(i0,i1,i2);
reg(i1,i0,i2);
reg(i2,i0,i1);
}
int N = sgeo->getMesh()->geo->getPositions()->size();
vertex_neighbor_params.resize(2*N);
for (auto i : tmpdict) vertex_neighbor_params[2*i.first+1] = i.second.size();
int kN = 0;
for (int i = 0; i < N; i++) {
vertex_neighbor_params[2*i] = kN;
kN += vertex_neighbor_params[2*i+1];
}
vertex_neighbors.resize(kN);
for (auto i : tmpdict) {
int o = vertex_neighbor_params[2*i.first];
int k = 0;
for (auto j : i.second) { vertex_neighbors[o+k] = j.first; k++; }
}
}
//-----------------------------------------------------------------------------
void RayIntersector::intersectGeometry(Actor* act, Geometry* geom)
{
ArrayAbstract* posarr = geom->vertexArray() ? geom->vertexArray() : geom->vertexAttribArray(vl::VA_Position) ? geom->vertexAttribArray(vl::VA_Position)->data() : NULL;
if (posarr)
{
fmat4 matrix = act->transform() ? (fmat4)act->transform()->worldMatrix() : fmat4();
for(int i=0; i<geom->drawCalls()->size(); ++i)
{
DrawCall* prim = geom->drawCalls()->at(i);
int itri = 0;
for(TriangleIterator trit = prim->triangleIterator(); trit.hasNext(); trit.next(), ++itri)
{
int ia = trit.a();
int ib = trit.b();
int ic = trit.c();
fvec3 a = posarr->getAsVec3(ia);
fvec3 b = posarr->getAsVec3(ib);
fvec3 c = posarr->getAsVec3(ic);
if (act->transform())
{
a = matrix * a;
b = matrix * b;
c = matrix * c;
}
intersectTriangle(a, b, c, ia, ib, ic, act, geom, prim, itri);
}
}
}
}
void VRAdjacencyGraph::compTriLoockup() {
edge_triangle_loockup.clear();
auto sgeo = geo.lock();
if (!sgeo) return;
for (TriangleIterator it = TriangleIterator(sgeo->getMesh()->geo); !it.isAtEnd(); ++it) {
triangle t;
t.v1 = it.getPositionIndex(0);
t.v2 = it.getPositionIndex(1);
t.v3 = it.getPositionIndex(2);
auto reg = [&](int j0, int j1) {
if (edge_triangle_loockup.count(j0) == 0) edge_triangle_loockup[j0] = map<int, vector<triangle> >();
if (edge_triangle_loockup[j0].count(j1) == 0) edge_triangle_loockup[j0][j1] = vector<triangle>();
edge_triangle_loockup[j0][j1].push_back(t);
};
reg(t.v1,t.v2); reg(t.v2,t.v1);
reg(t.v1,t.v3); reg(t.v3,t.v1);
reg(t.v2,t.v3); reg(t.v3,t.v2);
}
}
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 OBJSceneFileType::write(Node * const node,
std::ostream &os,
UInt32 &pIndex,
UInt32 &nIndex,
UInt32 &tIndex) const
{
UInt32 i,pCount=0,nCount=0,tCount=0;
Geometry *g = dynamic_cast<Geometry *>(node->getCore());
if(g != NULL)
{
// HACK separate it in several geometry nodes.
os << "g Geometry" << std::endl;
os << "usemtl Geometry" << std::endl;
Matrix mat = node->getToWorld();
// write vertices
if(g->getPositions())
{
pCount = g->getPositions()->size32();
for(i=0 ; i< pCount ; ++i)
{
Pnt3f v;
g->getPositions()->getValue(v, i);
mat.mult(v, v);
os << "v " << v[0] << " " << v[1] << " " << v[2] << std::endl;
}
}
// write normals
if(g->getNormals())
{
nCount = g->getNormals()->size32();
for(i=0 ; i< nCount ; ++i)
{
Vec3f v;
g->getNormals()->getValue(v, i);
mat.mult(v, v);
os << "vn " << v[0] << " " << v[1] << " " << v[2] << std::endl;
}
}
// texture coords
if(g->getTexCoords())
{
tCount = g->getTexCoords()->size32();
for(i=0 ; i< tCount ; ++i)
{
Vec2f v;
g->getTexCoords()->getValue(v, i);
os << "vt " << v[0] << " " << v[1] << std::endl;
}
}
// write indices
TriangleIterator f;
for(f=g->beginTriangles() ; f!=g->endTriangles() ; ++f)
{
os << "f";
for(i=0 ; i<3 ; ++i)
{
os << " " << f.getPositionIndex(i) + pIndex;
if(nCount || tCount)
{
os << "/";
if(tCount)
os << f.getTexCoordsIndex(i) + tIndex;
if(nCount)
os << "/" << f.getNormalIndex(i) + nIndex;
}
}
os << std::endl;
}
pIndex += pCount;
tIndex += tCount;
nIndex += nCount;
}
for(MFUnrecChildNodePtr::const_iterator nI =node->getMFChildren()->begin();
nI!=node->getMFChildren()->end();
++nI)
{
write((*nI),os,pIndex,nIndex,tIndex);
}
}
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;
}
bool DVRClipGeometry::buildTriangledGeometry(void)
{
// create triangles
for(TriangleIterator triangleIt = geometry->beginTriangles();
triangleIt != geometry->endTriangles ();
++triangleIt)
{
DVRTriangle newTriangle;
for(UInt32 i = 0; i < 3; i++)
{
Int32 vertexIndex = triangleIt.getPositionIndex(i);
vertexIndex = insertVertex(vertexIndex);
newTriangle.vertices[i] = vertexIndex;
_mfVertices[vertexIndex].adjacentTriangles.push_back(
_mfTriangles.size());
newTriangle.cutPnt [i] = 0.0;
newTriangle.cutPoint[i] = 0.0;
}
// compute normal and check orientation
newTriangle.normal =
(_mfVertices[newTriangle.vertices[0]].pos -
_mfVertices[newTriangle.vertices[1]].pos).cross(
_mfVertices[newTriangle.vertices[0]].pos -
_mfVertices[newTriangle.vertices[2]].pos);
newTriangle.normal.normalize();
if(newTriangle.normal.dot(triangleIt.getNormal(0)) < 0.0)
newTriangle.normal.negate();
_mfTriangles.push_back(newTriangle);
}
// find neighbouring triangles
for(UInt32 i = 0; i < _mfTriangles.size(); i++)
{
Int32 *vertices = _mfTriangles[i].vertices;
for(UInt32 l = 0; l < 3; l++)
{
const DVRVertex &v0 = _mfVertices[vertices[l]];
const DVRVertex &v1 = _mfVertices[vertices[(l + 1) % 3]];
for(UInt32 j = 0; j < v0.adjacentTriangles.size(); j++)
{
for(UInt32 k = 0; k < v1.adjacentTriangles.size(); k++)
{
if(v0.adjacentTriangles[j] == v1.adjacentTriangles[k] &&
v0.adjacentTriangles[j] != i &&
v1.adjacentTriangles[k] != i )
{
if(_mfTriangles[i].neighbours[l] != -1)
{
SLOG << "Error: Could not build clip geometry"
<< std::endl
<< " one triangle edge shared by more "
<< "than two triangles"
<< std::endl;
return false;
}
_mfTriangles[i].neighbours[l] =
v1.adjacentTriangles[k];
}
}
}
}
}
// check for non-closed geometry
UInt32 checkCount = 0;
for(UInt32 i = 0; i < _mfTriangles.size(); i++)
{
for(UInt32 j = 0; j < 3; j++)
{
if(_mfTriangles[i].neighbours[j] == -1)
{
checkCount++;
}
}
}
if(checkCount > 0)
{
SLOG << "Error: Could not build clip geometry"
<< std::endl
<< checkCount
<< "open edges found!"
<< std::endl;
return false;
}
return true;
}
