Subversion Repositories gelsvn

#include "build_bbtree.h"
#include "HMesh/VertexCirculator.h"
#include "HMesh/FaceCirculator.h"


using namespace CGLA;
using namespace std;
using namespace HMesh;

namespace
{
        const float EDGE_MIN_SQ_LENGTH = 1.0e-10f;
        
        inline bool degenerate_edge(HalfEdgeIter he)
        {
                if(sqr_length(he->vert->pos-he->opp->vert->pos)<1e-8)
                        return true;
                return false;
        }

}

namespace Geometry
{

template<class BBTree>
void build_tree_robust(Manifold& m, BBTree& tree)
{
#ifdef COMPUTE_SIGN
        vector<Vec3f> face_normals(m.no_faces());
        
        int i=0;
        for(FaceIter fi=m.faces_begin(); fi != m.faces_end();++fi,++i)
                {
                        face_normals[i] = normal(fi);
                        fi->touched = i;
                }
        
        for(HalfEdgeIter ei=m.halfedges_begin(); ei != m.halfedges_end();++ei)
                ei->touched = -1;
        
        i=0;
        vector<Vec3f> edge_normals(0);
        for(HalfEdgeIter ei=m.halfedges_begin(); ei != m.halfedges_end();++ei)
                {
                        if(ei->touched == -1)
                                {
                                        Vec3f n_a = face_normals[ei->face->touched];
                                        Vec3f n_b = n_a;
                                        if(ei->opp->face != NULL_FACE_ITER)
                                                n_b = face_normals[ei->opp->face->touched];

                                        edge_normals.push_back(normalize(n_a+n_b));
                                        ei->touched = i;
                                        ei->opp->touched = i;
                                        ++i;
                                }
                }

        for(VertexIter vi=m.vertices_begin(); vi != m.vertices_end(); ++vi)
                vi->touched = -1;

        int vertex_cluster_number=0;
        vector<Vec3f> vertex_normals(m.no_vertices());
        for(VertexIter vi=m.vertices_begin(); vi != m.vertices_end(); ++vi)
                {
                        if(vi->touched == -1)
                                {
                                        int cluster_size=0;
                                        Vec3f nsum(0);
                                        vector<VertexIter> cluster(1,vi);
                                        while(!cluster.empty())
                                                {
                                                        cluster_size ++;
                                                        VertexIter vert_iter = cluster.back();
                                                        cluster.pop_back();
                                                        vert_iter->touched = vertex_cluster_number;

                                                        vector<VertexIter> nbrs;
                                                        VertexCirculator vc(vert_iter);
                                                        for(;!vc.end();++vc)
                                                                nbrs.push_back(vc.get_vertex());
                                        
                                                        int N = vc.no_steps();
                                                        Vec3f p = vert_iter->pos;
                                                        for(int j=0;j<N;++j)
                                                                {
                                                                        Vec3f va = nbrs[(j+1)%N]->pos - p;
                                                                        Vec3f vb = nbrs[j]->pos - p;
                                                                        if(sqr_length(vb) > EDGE_MIN_SQ_LENGTH)
                                                                                {
                                                                                        if(sqr_length(va) > EDGE_MIN_SQ_LENGTH)
                                                                                                {
                                                                                                        va.normalize();
                                                                                                        vb.normalize();
                                                                                                        float a = acos(dot(va,vb));
                                                                                                        nsum += a*normalize(cross(va,vb));
                                                                                                }
                                                                                }
                                                                        else
                                                                                {
                                                                                        if(nbrs[j]->touched != vertex_cluster_number)
                                                                                                {
                                                                                                        assert(nbrs[j]->touched == -1);
                                                                                                        cluster.push_back(nbrs[j]);
                                                                                                }
                                                                                }
                                                                }
                                                }
                                        vertex_normals[vertex_cluster_number] = normalize(nsum);
                                        ++vertex_cluster_number;
                                        if(cluster_size > 1) cout << "Cluster Size " 
                                                                                                                                                << cluster_size << " normal " 
                                                                                                                                                << normalize(nsum) << endl;
                                }
                }
        
        vector<Triangle> triangle_vec;

        i=0;
        for(FaceIter fi=m.faces_begin(); fi != m.faces_end();++fi,++i)
                {
                        Vec3f fn = face_normals[fi->touched];

                        FaceCirculator fc(fi);

                        Vec3f v0,v1,v2;
                        Vec3f vn0,vn1,vn2;
                        Vec3f en0,en1,en2;
                        
                        v0  = fc.get_vertex()->pos;
                        vn0 = vertex_normals[fc.get_vertex()->touched];
                        en0 = edge_normals[fc.get_halfedge()->touched];
                        
                        ++fc;

                        v1  = fc.get_vertex()->pos;
                        vn1 = vertex_normals[fc.get_vertex()->touched];
                        en1 = edge_normals[fc.get_halfedge()->touched];
                        
                        ++fc;

                        v2  = fc.get_vertex()->pos;
                        vn2 = vertex_normals[fc.get_vertex()->touched];
                        en2 = edge_normals[fc.get_halfedge()->touched];
                        
                        if(sqr_length(v0-v1)>EDGE_MIN_SQ_LENGTH &&
                                 sqr_length(v1-v2)>EDGE_MIN_SQ_LENGTH &&
                                 sqr_length(v2-v0)>EDGE_MIN_SQ_LENGTH)
                                        triangle_vec.push_back(Triangle(v0,v1,v2,vn0,vn1,vn2,en0,en1,en2));
                        //                      else
                        //                              cout << "Killing degenerate triangle" << endl;
                }
#else
        vector<Triangle> triangle_vec;
        int i=0;
        for(FaceIter fi=m.faces_begin(); fi != m.faces_end();++fi,++i)
                {
                        FaceCirculator fc(fi);
                        Vec3f v0,v1,v2;
                        Vec3f dmy(0);
                        v0  = fc.get_vertex()->get_pos();
                        ++fc;
                        v1  = fc.get_vertex()->get_pos();
                        ++fc;
                        v2  = fc.get_vertex()->get_pos();
                        if(sqr_length(v0-v1)>EDGE_MIN_SQ_LENGTH &&
                                 sqr_length(v1-v2)>EDGE_MIN_SQ_LENGTH &&
                                 sqr_length(v2-v0)>EDGE_MIN_SQ_LENGTH)
                                        triangle_vec.push_back(Triangle(v0,v1,v2,dmy,dmy,dmy,dmy,dmy,dmy));
                }
#endif

        tree.build(triangle_vec);
}

void build_OBBTree(HMesh::Manifold& m, OBBTree& tree)
{
        build_tree_robust<OBBTree>(m, tree);
}

void build_AABBTree(HMesh::Manifold& m, AABBTree& tree)
{
        build_tree_robust<AABBTree>(m, tree);
}

}