Subversion Repositories gelsvn

#include "build_bbtree.h"

#include <HMesh/Manifold.h>

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

namespace
{
    const float EDGE_MIN_SQ_LENGTH = CGLA::MINUTE;

    inline bool degenerate_edge(const Manifold& m, HalfEdgeID h)
    {
                HalfEdgeWalker w = m.halfedgewalker(h);
        if(sqr_length(m.pos(w.vertex()) - m.pos(w.opp().vertex())) < 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(FaceHandle fi=m.fhandles_begin(); fi != m.fhandles_end();++fi,++i)
        {
            face_normals[i] = normal(fi);
            fi.get().touched = i;
        }

        for(HalfEdgeHandle ei = m.hhandles_begin(); ei != m.hhandles_end(); ++ei)
            ei.get().touched = -1;

        i=0;
        vector<Vec3f> edge_normals(0);
        for(HalfEdgeHandle ei = m.hhandles_begin(); ei != m.hhandles_end(); ++ei){
            if(ei.get().touched == -1){
                Vec3f n_a = face_normals[ei.face().get().touched];
                Vec3f n_b = n_a;
                if(ei.opp().face() != NULL_FHANDLE)
                    n_b = face_normals[ei.opp().face().get().touched];

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

        for(VertexHandle vi = m.vhandles_begin(); vi != m.vhandles_end(); ++vi)
            vi.get().touched = -1;

        int vertex_cluster_number=0;
        vector<Vec3f> vertex_normals(m.no_vertices());
        for(VertexHandle vi = m.vhandles_begin(); vi != m.vhandles_end(); ++vi){
            if(vi.get().touched == -1){
                int cluster_size = 0;
                Vec3f nsum(0);
                vector<VertexHandle> cluster(1,vi);
                while(!cluster.empty()){
                    cluster_size ++;
                    VertexHandle vert_handle = cluster.back();
                    cluster.pop_back();
                    vert_handle.get().touched = vertex_cluster_number;

                    vector<VertexHandle> nbrs;
                    VertexCirculator vc(vert_handle);
                    for(;!vc.end();++vc)
                        nbrs.push_back(vc.vertex());

                    IndexType N = vc.no_steps();
                    Vec3f p = vert_handle.get().pos;
                    for(IndexType j = 0; j < N; ++j){
                        Vec3f va = nbrs[(j+1)%N].get().pos - p;
                        Vec3f vb = nbrs[j].get().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].get().touched != vertex_cluster_number){
                                assert(nbrs[j].get().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(FaceHandle fi = m.fhandles_begin(); fi != m.fhandles_end();++fi,++i)
        {
            Vec3f fn = face_normals[fi.get().touched];

            FaceCirculator fc(fi);

            Vec3f v0,v1,v2;
            Vec3f vn0,vn1,vn2;
            Vec3f en0,en1,en2;

            v0  = fc.vertex().get().pos;
            vn0 = vertex_normals[fc.vertex().get().touched];
            en0 = edge_normals[fc.next_halfedge().get().touched];

            ++fc;

            v1  = fc.vertex().get().pos;
            vn1 = vertex_normals[fc.vertex().get().touched];
            en1 = edge_normals[fc.next_halfedge().get().touched];

            ++fc;

            v2  = fc.vertex().get().pos;
            vn2 = vertex_normals[fc.vertex().get().touched];
            en2 = edge_normals[fc.next_halfedge().get().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.fhandles_begin(); fi != m.fhandles_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);
    }

}