source: GTP/trunk/Lib/Geom/shared/GTGeometry/src/libs/SimplificationMethod.cpp @ 1526

#include <stdlib.h>
#include "SimplificationMethod.h"
#include <iostream>
#include <fstream>

using namespace std;
using namespace Geometry;
using   namespace       simplif;

#include <gfx/geom/ProxGrid.h>
#include "quadrics.h"

//---------------------------------------------------------------------------
//      Constructor
//---------------------------------------------------------------------------
SimplificationMethod::SimplificationMethod(const Mesh *m)
{
        objmesh                                                 =       m;
        mGeoMesh                                                =       NULL;
        first_index_submesh     =       new unsigned int[objmesh->mSubMeshCount];
        indexMeshLeaves                 =       -1;

        //      Create simplification sequence.
        msimpseq        =       new MeshSimplificationSequence();
}

//---------------------------------------------------------------------------
//      Destructor
//---------------------------------------------------------------------------
SimplificationMethod::~SimplificationMethod()
{
        delete heap;
        delete [] first_index_submesh;
}

//---------------------------------------------------------------------------
//      Recalculate the cost of the vertices of an edge
//---------------------------------------------------------------------------
void SimplificationMethod::compute_pair_info(simplif::pair_info *auxpair)
{
        simplif::Vertex *v0 = auxpair->v0;
        simplif::Vertex *v1 = auxpair->v1;

        simplif::vert_info& v0_info = vertex_info(v0);
        simplif::vert_info& v1_info = vertex_info(v1);

        simplif::Mat4 Q                 =       v0_info.Q + v1_info.Q;
        simplif::real norm      =       v0_info.norm + v1_info.norm;

        auxpair->cost = simplif::quadrix_pair_target(Q, v0, v1, auxpair->candidate);

        if ( simplif::will_weight_by_area )
        {
                auxpair->cost /= norm;
        }

        if ( simplif::will_preserve_mesh_quality )
        {
                auxpair->cost += pair_mesh_penalty(M0, v0, v1, auxpair->candidate);
        }

        //
        // NOTICE:  In the heap we use the negative cost.  That's because
        //          the heap is implemented as a MAX heap.
        //
        if ( auxpair->isInHeap() )
        {
                heap->update(auxpair, (float)-auxpair->cost);
        }
        else
        {
                heap->insert(auxpair, (float)-auxpair->cost);
        }
}

//---------------------------------------------------------------------------
//      Reasign the new vertex to the adequate face
//---------------------------------------------------------------------------
int SimplificationMethod::predict_face( simplif::Face& F,
                                                                                                                                                                simplif::Vertex *v1,
                                                                                                                                                                simplif::Vertex *v2,
                                                                                                                                                                simplif::Vec3& vnew,
                                                                                                                                                                simplif::Vec3& f1,
                                                                                                                                                                simplif::Vec3& f2,
                                                                                                                                                                simplif::Vec3& f3)
{
        int nmapped = 0;

        if( F.vertex(0) == v1 || F.vertex(0) == v2 )
        {
                f1 = vnew;  nmapped++;
        }
        else
        {
                f1 = *F.vertex(0);
        }

        if( F.vertex(1) == v1 || F.vertex(1) == v2 )
        {
                f2 = vnew;  nmapped++;
        }
        else
        {
                f2 = *F.vertex(1);
        }

        if( F.vertex(2) == v1 || F.vertex(2) == v2 )
        {
                f3 = vnew;  nmapped++;
        }
        else
        {
                f3 = *F.vertex(2);
        }

        return nmapped;
}

#define MESH_INVERSION_PENALTY 1e9

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
simplif::real SimplificationMethod::pair_mesh_penalty(simplif::Model& M, simplif::Vertex *v1,
                                                                                                                                                 simplif::Vertex *v2,
                                                                                                                                                 simplif::Vec3& vnew)
{
        static simplif::face_buffer changed;

        changed.reset();

        M.contractionRegion(v1, v2, changed);

        // real Nsum = 0;
        simplif::real Nmin = 0;

        for (int i      =       0; i < changed.length(); i++)
        {
                simplif::Face& F = *changed(i);
                simplif::Vec3 f1, f2, f3;

                int nmapped = predict_face(F, v1, v2, vnew, f1, f2, f3);

                // Only consider non-degenerate faces
                if( nmapped < 2 )
                {
                        simplif::Plane Pnew(f1, f2, f3);

                        simplif::real delta =  Pnew.normal() * F.plane().normal();

                        if( Nmin > delta )
                        {
                                Nmin = delta;
                        }
                }
        }

        //return (-Nmin) * MESH_INVERSION_PENALTY;
        if( Nmin < 0.0 )
        {
                return MESH_INVERSION_PENALTY;
        }
        else
        {
                return 0.0;
        }
}

//---------------------------------------------------------------------------
//      Returns true if the givens vertices are a valid pair
//---------------------------------------------------------------------------
bool SimplificationMethod::check_for_pair(simplif::Vertex *v0,
                                                                                                                                                                        simplif::Vertex *v1)
{
        const simplif::pair_buffer& pairs = vertex_info(v0).pairs;

        for(int i=0; i<pairs.length(); i++)
        {
                if( pairs(i)->v0==v1 || pairs(i)->v1==v1 )
                {
                        return true;
                }
        }

        return false;
}

//---------------------------------------------------------------------------
//      Create a new pair with two given vertices
//---------------------------------------------------------------------------
simplif::pair_info *SimplificationMethod::new_pair(     simplif::Vertex *v0,
                                                                                                                                                                                                                simplif::Vertex *v1)
{
        simplif::vert_info& v0_info = vertex_info(v0);
        simplif::vert_info& v1_info = vertex_info(v1);

        simplif::pair_info *pair = new simplif::pair_info(v0,v1);

        v0_info.pairs.add(pair);
        v1_info.pairs.add(pair);

        return pair;
}

//---------------------------------------------------------------------------
//      Remove a given pair
//---------------------------------------------------------------------------
void SimplificationMethod::delete_pair(simplif::pair_info *auxpair)
{
        simplif::vert_info& v0_info = vertex_info(auxpair->v0);
        simplif::vert_info& v1_info = vertex_info(auxpair->v1);

        v0_info.pairs.remove(v0_info.pairs.find(auxpair));
        v1_info.pairs.remove(v1_info.pairs.find(auxpair));

        if ( auxpair->isInHeap() )
        {
                heap->kill(auxpair->getHeapPos());
        }

        delete auxpair;
}

//---------------------------------------------------------------------------
//      Contract a given pair
//---------------------------------------------------------------------------
void SimplificationMethod::do_contract( simplif::Model& m,
                                                                                                                                                                simplif::pair_info *auxpair)
{
        simplif::Vertex *v0                                     = auxpair->v0;
        simplif::Vertex *v1                                     = auxpair->v1;
        simplif::vert_info& v0_info = vertex_info(v0);
        simplif::vert_info& v1_info = vertex_info(v1);
        simplif::Vec3 vnew                                      = auxpair->candidate;

        int i;
        
        // V0 must be always the alive vertex
        // V1 must be always the removed vertex
        if (vnew == (*v0))
        {
                //      Simplification step.
                simplifstep.mV0 = v0->vID;
                simplifstep.mV1 = v1->vID;
        }
        else
        {
                //      Simplification step.
                simplifstep.mV0 = v1->vID;
                simplifstep.mV1 = v0->vID;
                v0->vID = v1->vID;
        }

        // Make v0 be the new vertex
        v0_info.Q               += v1_info.Q;
        v0_info.norm    += v1_info.norm;

        simplif::Vec3 p(vnew);
/*      simplif::Vec3 v0antes(v0->operator[](simplif::X),
                                                  v0->operator[](simplif::Y),
                                                  v0->operator[](simplif::Z));*/

        //      Perform the actual contraction.
        static simplif::face_buffer changed;

        changed.reset();
        m.contract(v0, v1, vnew, changed);

        std::cout << "Simp seq: " << simplifstep.mV1 << " --> " << simplifstep.mV0 << std::endl;

/*      simplif::Vec3 vdesp = p - v0antes;

        //      Stores the displacement in the simplification step.
        simplifstep.x   =       (float)vdesp[X];
        simplifstep.y = (float)vdesp[Y];
        simplifstep.z = (float)vdesp[Z];*/

        //      Stores the displacement in the simplification step.
        simplifstep.x = (float)0.0f;
        simplifstep.y = (float)0.0f;
        simplifstep.z = (float)0.0f;

        //      Number of triangles that are removed in this simplification step.
        int _numDelTris = 0; // 1 or 2 triangles can be removed.

        for (i = 0;     i < changed.length();   i++)
        {
                if (!changed(i)->isValid())
                {
                        _numDelTris++;
                }
        }

        int del_index   =       0;

        //      Clears faces list.
        simplifstep.mModfaces.clear();

        //      mModfaces y mT0, mT1 of simplifstep stores 
        //      the triangles id's -> geomesh has not triangles id's.
        for (i  =       0;      i < changed.length();   i++)
        {
                simplif::Face *auxface  =       changed(i);

                if (auxface->isValid())
                {
                        //      Modified triangles.
                        simplifstep.mModfaces.push_back(auxface->validID());
                }
                else
                {
                        //      Removed triangles.
                        if (_numDelTris == 1)
                        {
                                simplifstep.mT0 =       auxface->validID();
                                simplifstep.mT1 =       auxface->validID();
                        }
                        else
                        {
                                if(del_index == 0)
                                {
                                        simplifstep.mT0 =       auxface->validID();
                                        del_index++;
                                }
                                else
                                {
                                        simplifstep.mT1 =       auxface->validID();
                                }
                        }
                }
        }

        //      Simplification sequence that make reference to the 
        //      simplification method sequence.
        //decim_data.push_back(simplifstep);

        //      Adds new simplification step.
        msimpseq->mSteps.push_back(simplifstep);

#ifdef SUPPORT_VCOLOR
        //      If the vertices are colored, color the new vertex
        //      using the average of the old colors.
        v0->props->color += v1->props->color;
        v0->props->color /= 2;
#endif

        //      Remove the pair that we just contracted.
        delete_pair(auxpair);

        //      Recalculate pairs associated with v0.
        for (i  =       0;      i < v0_info.pairs.length();     i++)
        {
                simplif::pair_info *p = v0_info.pairs(i);
                compute_pair_info(p);
        }

        //      Process pairs associated with now dead vertex.
        //      Collect condemned pairs for execution.
        static simplif::pair_buffer condemned(6);

        condemned.reset();

        for (i  =       0;      i < v1_info.pairs.length(); i++)
        {
                simplif::pair_info *p = v1_info.pairs(i);

                simplif::Vertex *u;

                if( p->v0 == v1 )
                {
                        u = p->v1;
                }
                else
                {
                        if( p->v1 == v1)
                        {
                                u = p->v0;
                        }
                        else
                        {
                                std::cerr << "YOW!  This is a bogus pair." << endl;
                        }
                }

                if ( !check_for_pair(u, v0) )
                {
                        p->v0   =       v0;
                        p->v1   =       u;

                        v0_info.pairs.add(p);
                        compute_pair_info(p);
                }
                else
                {
                        condemned.add(p);
                }
        }

        for (i  =       0;      i < condemned.length(); i++)
        {
                // Do you have any last requests?
                delete_pair(condemned(i));
        }

        //      Safety precaution.
        v1_info.pairs.reset();
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
bool SimplificationMethod::decimate_quadric(simplif::Vertex *v,
                                                                                                                                                                                simplif::Mat4& Q)
{
        if( vinfo.length() > 0 )
        {
                Q = vinfo(v->uniqID).Q;
                return true;
        }
        else
        {
                return false;
        }
}

//---------------------------------------------------------------------------
//      Extract a pair from the heap, concract it and remove it form the heap
//---------------------------------------------------------------------------
void SimplificationMethod::decimate_contract(simplif::Model& m)
{
        simplif::heap_node      *top;
        simplif::pair_info      *pair;
        simplif::Vertex         *v0;
        simplif::Vertex         *v1;
        simplif::Vec3           candidate;

        for (;;)
        {
                top     =       heap->extract();
                
                if (!top)
                {
                        return;
                }
                
                pair    =       (simplif::pair_info *)top->obj;

                //      Remove all the vertices of the removed edges.
                bool    sharededge      =       false;
                
                for (int        i       =       0;      i < pair->v0->edgeUses().length();      i++)
                {
                        simplif::Edge   *econ   =       pair->v0->edgeUses()(i);
                        
                        if (pair->v1->vID == econ->org()->vID
                                        ||
                                        pair->v1->vID == econ->dest()->vID)
                        {
                                sharededge      =       true;
                                break;
                        }
                }

                if (pair->isValid() && sharededge)
                {
                        break;
                }

                //      Deletes repetitions of pair in heap.
                delete_pair(pair);
        }

        //      Copy pair.
        v0      =       new Vertex(     pair->v0->operator[](0),
                                                                                pair->v0->operator[](1),
                                                                                pair->v0->operator[](2));
        v0->uniqID = pair->v0->uniqID;
        
        v1      =       new Vertex(     pair->v1->operator[](0),
                                                                                pair->v1->operator[](1),
                                                                                pair->v1->operator[](2));

        v1->uniqID = pair->v1->uniqID;

        candidate       =       pair->candidate;

        //      First contract.
        simplifstep.obligatory  =       0;

        //      Debug.
        cout    <<      "Contracting..."        <<      endl;

        //      Contract main pair.
        do_contract(m, pair);

        //      Twin contract and lonely vertex contract.
        simplifstep.obligatory  =       1;

        //      Attempt to maintain valid vertex information.
        M0.validVertCount--;

        //      Find an edge of the same coordinates and different vertex identifiers.
        removeTwinEdges(m,v0,v1,candidate);

        //      Move vertex of non twin edges.
        contractLonelyVertices(m,v0,v1,candidate);

        //      Free Memory.
        delete  v0;
        delete  v1;
}

//---------------------------------------------------------------------------
//      Contract lonely vertex that has the same coords that the contracted one.
//---------------------------------------------------------------------------
void    SimplificationMethod::contractLonelyVertices(   simplif::Model  &m,     simplif::Vertex *v0,
                                                                                                                                                        simplif::Vertex *v1,
                                                                                                                                                        simplif::Vec3   candidate)
{
        bool                            lonely_vertex_found;
        simplif::Vertex         *vert;
        simplif::Vertex         *new_vert;
        simplif::Vertex         *lonely_vert;
        simplif::Edge           *edge;
        simplif::heap_node      *top;
        simplif::pair_info      *pair;
        simplif::vert_info      new_vert_info;
        Geometry::GeoVertex     vertex_added;
        simplif::Vertex         *take_bone_from_vert;

        //      Gets the vertex that is not the candidate.
        if ((*v0) == candidate)
        {
                vert = v1;
                take_bone_from_vert = v0;
        }
        else if ((*v1) == candidate)
        {
                vert = v0;
                take_bone_from_vert = v1;
        }

        //      For all vertex.
        for (int        i = 0; i < heap->getSize();     i++)
        {
                //      Initialize lonely vertex flag.
                lonely_vertex_found     =       false;

                //      Gets the current pair.
                top             =       heap->getElement(i);
                pair    =       (simplif::pair_info *)top->obj;

                if (pair->v0->isValid() && (*vert) == (*pair->v0))
                {
                        lonely_vert     =       pair->v0;
                        lonely_vertex_found     =       true;
                }
                else if (pair->v1->isValid() && (*vert) == (*pair->v1))
                {
                        lonely_vert     =       pair->v1;
                        lonely_vertex_found     =       true;
                }

                //      If a lonely vertex is found. Creates new vertex and contract.
                if (lonely_vertex_found)
                {
                        new_vert        =       m.newVertex(candidate[X],candidate[Y],candidate[Z]);    
                        edge            =       m.newEdge(new_vert,lonely_vert);

            // We assume here the there are the same number of vertices and texture coordinates and normals

                        // assign a texture coordinate for the vbertex
                        simplif::Vec2 newtexcoord = m.texcoord(lonely_vert->validID());
                        m.in_TexCoord(newtexcoord);

                        // assign a normal coordinate for the vbertex
                        simplif::Vec3 newnormal = m.normal(lonely_vert->validID());
                        m.in_Normal(newnormal);

                        //      Adds new vertex information to simplification sequence.
                        vertex_added.id                 =       new_vert->vID;
                        vertex_added.bonefrom   =       take_bone_from_vert->vID;
                        vertex_added.position   =       Vector3(candidate[X],candidate[Y],candidate[Z]);

                        simplif::Vec2 tc = m.texcoord(new_vert->validID());
                        vertex_added.texcoord   =       Vector2(tc[X],tc[Y]);

                        simplif::Vec3 vn = m.normal(new_vert->validID());
                        vertex_added.normal             =       Vector3(vn[X],vn[Y],vn[Z]);

                        msimpseq->mNewVertices.push_back(vertex_added);

                        pair                    =       new_pair(new_vert,lonely_vert);
                        pair->candidate =       candidate;

                        pair->notInHeap();
/*                      vinfo(new_vert->validID()).Q            =       vinfo(lonely_vert->validID()).Q;
                        vinfo(new_vert->validID()).norm =       vinfo(lonely_vert->validID()).norm;*/

                        //      Debug.
                        cout    <<      "Contracting new pair..."       <<      endl;

                        //      Contract twin pair.
                        do_contract(m, pair);
                        M0.validVertCount--;
                }
        }
}

//---------------------------------------------------------------------------
//      Find a twin edge of the given one.
//---------------------------------------------------------------------------
void SimplificationMethod::removeTwinEdges(     simplif::Model  &m,
                                                                                                                                                                                simplif::Vertex *v0,
                                                                                                                                                                                simplif::Vertex *v1,
                                                                                                                                                                                simplif::Vec3           candidate)
{
        bool                                                            twin_found;
        simplif::heap_node      *top;
        simplif::pair_info      *pair;
        simplif::Vertex                 *aux_vert;

        //      Find a twin edge in heap.
        for (int        i       =       0;      i < heap->getSize(); i++)
        {
                //      Initialize twin flag.
                twin_found      =       false;

                //      Gets the current pair.
                top             =       heap->getElement(i);
                pair    =       (simplif::pair_info *)top->obj;

                if (pair->v0->isValid()==false || pair->v1->isValid()==false)
                        continue; 

                if (v0->operator[](0) == pair->v0->operator[](0)
                                &&
                                v0->operator[](1) == pair->v0->operator[](1)
                                &&
                                v0->operator[](2) == pair->v0->operator[](2))
                {
                        if (v1->operator[](0) == pair->v1->operator[](0)
                                        &&
                                        v1->operator[](1) == pair->v1->operator[](1)
                                        &&
                                        v1->operator[](2) == pair->v1->operator[](2))
                        {
                                //      Debug.
                                cout    <<      "Twin contracted (NO Swap)..."  <<      endl;

                                //      Active twin flag.
                                twin_found      = true;
                        }
                }
                else
                {
                        //      If there is a twin edge in reverse order.
                        if (v0->operator[](0) == pair->v1->operator[](0)
                                        &&
                                        v0->operator[](1) == pair->v1->operator[](1)
                                        &&
                                        v0->operator[](2) == pair->v1->operator[](2))
                        {
                                if (v1->operator[](0) == pair->v0->operator[](0)
                                                &&
                                                v1->operator[](1) == pair->v0->operator[](1)
                                                &&
                                                v1->operator[](2) == pair->v0->operator[](2))
                                {
                                        //      Debug.
                                        cout    <<      "Twin contracted (Swap)..."     <<      endl;

                                        //      Swap.
                                        aux_vert        =       pair->v0;
                                        pair->v0        =       pair->v1;
                                        pair->v1        =       aux_vert;

                                        //      Active twin flag.
                                        twin_found      =       true;
                                }
                        }
                }

                //      If a twin edge has been found.
                if (twin_found)
                {
                        //      Extract twin edge from heap.
                        top             =       heap->extract(i);
                        pair    =       (simplif::pair_info *)top->obj;

                        //      Copy candidate.
                        pair->candidate =       candidate;

                        //      Contract twin pair.
                        do_contract(m, pair);
                        i--;
                        M0.validVertCount--;
                }
        }
}

//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
simplif::real SimplificationMethod::decimate_error(simplif::Vertex *v)
{
        simplif::vert_info& info = vertex_info(v);

        simplif::real err = simplif::quadrix_evaluate_vertex(*v, info.Q);

    if( simplif::will_weight_by_area )
                {
                        err /= info.norm;
                }

    return err;
}

//---------------------------------------------------------------------------
//      Extract the minimum cost of the the valid nodes (not simplified)
//---------------------------------------------------------------------------
simplif::real SimplificationMethod::decimate_min_error()
{
        simplif::heap_node *top;
        simplif::pair_info *pair;

        for(;;)
        {
                top = heap->top();
                
                if( !top )
                {
                        return -1.0;
                }

                pair = (simplif::pair_info *)top->obj;

                if( pair->isValid() )
                {
                        break;
                }

                top = heap->extract();
                delete_pair(pair);
        }

        return pair->cost;
}

//---------------------------------------------------------------------------
//      Returns the maximum error by vertex
//---------------------------------------------------------------------------
simplif::real SimplificationMethod::decimate_max_error(simplif::Model& m)
{
        simplif::real max_err = 0;

        for(int i=0; i<m.vertCount(); i++)
                if( m.vertex(i)->isValid() )
                {
                        max_err = MAX(max_err, decimate_error(m.vertex(i)));
                }

        return max_err;
}

//---------------------------------------------------------------------------
//      Initializations
//---------------------------------------------------------------------------
void SimplificationMethod::decimate_init(simplif::Model& m, simplif::real limit)
{
        int i,j;

        //      Reserve memory for vert_info array.
        vinfo.init(m.vertCount()*10);

        if (simplif::will_use_vertex_constraint)
        {
                //      For each vertex, calculate contraints.
                for     (i      =       0;      i < m.vertCount();      i++)
                {
                        simplif::Vertex *v      =       m.vertex(i);
                        
                        if (v->isValid())
                        {
                                vertex_info(v).Q = simplif::quadrix_vertex_constraint(*v);
                        }
                }
        }

        //      Sets the fill value of the vinfo buffer.
        //vinfo.setFill(m.vertCount());

        //      For each face, recalculate constraints.
        for (i  =       0;      i < m.faceCount();      i++)
        {
                if (m.face(i)->isValid())
                {
                        if (simplif::will_use_plane_constraint)
                        {
                                simplif::Mat4 Q = simplif::quadrix_plane_constraint(*m.face(i));
                                simplif::real norm = 0.0;

                                if (simplif::will_weight_by_area)
                                {
                                        norm    =               m.face(i)->area();
                                        Q                       *=      norm;
                                }

                                for (j  =       0;      j < 3;  j++)
                                {
                                        vertex_info(m.face(i)->vertex(j)).Q                     +=      Q;
                                        vertex_info(m.face(i)->vertex(j)).norm  +=      norm;
                                }
                        }
                }
        }

        if (simplif::will_constrain_boundaries)
        {
                //      For each edge, recalculate constraints.
                for (i  =       0;      i < m.edgeCount();      i++)
                {
                        if (m.edge(i)->isValid() && simplif::check_for_discontinuity(m.edge(i)))
                        {
                                simplif::Mat4 B = simplif::quadrix_discontinuity_constraint(m.edge(i));
                                simplif::real norm = 0.0;

                                if (simplif::will_weight_by_area)
                                {
                                        norm    =               simplif::norm2(*m.edge(i)->org() - *m.edge(i)->dest());
                                        B                       *=      norm;
                                }

                                B *= simplif::boundary_constraint_weight;
                                vertex_info(m.edge(i)->org()).Q += B;
                                vertex_info(m.edge(i)->org()).norm += norm;
                                vertex_info(m.edge(i)->dest()).Q += B;
                                vertex_info(m.edge(i)->dest()).norm += norm;
                        }
                }
        }

        //      Create heap.
        heap = new simplif::Heap(m.validEdgeCount);

        int pair_count = 0;

        //      For each edge, add pairs to heap.
        for (i  =       0;      i < m.edgeCount();      i++)
        {
                if (m.edge(i)->isValid())
                {
                        simplif::pair_info *pair = new_pair(m.edge(i)->org(), m.edge(i)->dest());
                        //pointers_to_remove.push_back(pair);
                        compute_pair_info(pair);
                        pair_count++;
                }
        }

        if (limit < 0)
        {
                limit   =       m.bounds.radius * 0.05;
        }
        
        proximity_limit =       limit * limit;
        
        if (proximity_limit > 0)
        {
                simplif::ProxGrid grid(m.bounds.min, m.bounds.max, limit);
                
                //      Add points to grid.
                for (i  =       0;      i < m.vertCount();      i++)
                {
                        grid.addPoint(m.vertex(i));
                }

                simplif::buffer<simplif::Vec3 *> nearby(32);
                
                //      For each vertex.
                for (i  =       0;      i < m.vertCount();      i++)
                {
                        nearby.reset();
                        grid.proximalPoints(m.vertex(i), nearby);

                        for (j  =       0;      j < nearby.length();    j++)
                        {
                                simplif::Vertex *v1 = m.vertex(i);
                                simplif::Vertex *v2 = (simplif::Vertex *)nearby(j);

                                if (v1->isValid() && v2->isValid())
                                {
#ifdef SAFETY
                                        assert(pair_is_valid(v1,v2));
#endif
                                        if (!check_for_pair(v1,v2))
                                        {
                                                simplif::pair_info *pair = new_pair(v1,v2);
                                                compute_pair_info(pair);
                                                pair_count++;
                                        }
                                }
                        }
                }
        }
}

//---------------------------------------------------------------------------
//      Initilizations
//---------------------------------------------------------------------------
void SimplificationMethod::simplifmethod_init(void)
{
        // Change mesh structure.
        generateSimplifModel();
                
        M0.bounds.complete();

        initialVertCount = M0.vertCount();
        initialEdgeCount = M0.edgeCount();
        initialFaceCount = M0.faceCount();

        // Get rid of degenerate faces.
        for (int        i       =       0;      i < M0.faceCount();     i++)
        {
                if (!M0.face(i)->plane().isValid())
                {
                        M0.killFace(M0.face(i));
                }
        }

        M0.removeDegeneracy(M0.allFaces());

        // Get rid of unused vertices.
        for (int        i       =       0;      i < M0.vertCount();     i++)
        {
                if (M0.vertex(i)->edgeUses().length() == 0)
                {
                        M0.vertex(i)->kill();
                }
        }
}

//---------------------------------------------------------------------------
//      Do the contractions till the required LOD (percentage option)
//---------------------------------------------------------------------------
void SimplificationMethod::simplifmethod_run(int finalfaces,TIPOFUNC upb)
{
        decimate_init(M0, simplif::pair_selection_tolerance);
        unsigned        int     contador        =       0;
        float                                   percent; // simplification percentage.

        percent =       (float)(2.0 * 100.0) / (M0.validFaceCount - finalfaces);

        //      Update progress bar.
        if (upb)
        {
                upb(0.0);
        }

        /*
        for (   std::vector<MeshSimplificationSequence::Step>::iterator
                                it      =       decim_data.begin();
                                it != decim_data.end();
                                it++)
        {
                it->mModfaces.clear();
        }
        
        decim_data.clear();
        */

        //      Debug.
        cout    <<      "M0.validFaceCount: "   <<      M0.validFaceCount       <<      endl;
        cout    <<      "finalfaces: "                          <<      finalfaces                              <<      endl;
        
        while ( M0.validFaceCount > finalfaces/*simplif::face_target*/
                                        && M0.validFaceCount > 1
                                        && decimate_min_error() < simplif::error_tolerance )
        {
                decimate_contract(M0);

                //      Update progress bar.
                if (upb)
                {
                        upb(percent);
                }
        }

        //      Update progress bar.
        if (upb)
        {
                upb(100.0);
        }
}

//---------------------------------------------------------------------------
//      Do the contractions till the required LOD (number of vertices option)
//---------------------------------------------------------------------------
void SimplificationMethod::simplifmethod_runv(int numvertices,TIPOFUNC upb)
{
        decimate_init(M0, simplif::pair_selection_tolerance);
        unsigned        int     contador        =       0;
        int                                             previousValidVertCount;
        float                                   percent; // Simplification percentage.

        previousValidVertCount  =       0;

        percent =       (float)(2.0 * 100.0)
                                                /
                                                (float)((M0.validFaceCount - numvertices / 3.0));

        //      Update progress bar.
        if (upb)
        {
                upb(0.0);
        }

        /*
        for (   std::vector<MeshSimplificationSequence::Step>::iterator
                                it      =       decim_data.begin();
                                it != decim_data.end();
                                it++)
        {
                it->mModfaces.clear();
        }
        
        decim_data.clear();
        */

        //      Debug.
        cout    <<      "M0.validVertCount:     "       <<      M0.validVertCount       <<      endl;
        cout    <<      "numvertices:   "       <<      numvertices     <<      endl;

        while(  M0.validVertCount > numvertices /*simplif::face_target*/
                        &&
                        decimate_min_error() < simplif::error_tolerance
                        &&
                        previousValidVertCount  !=      M0.validVertCount)
        {
                previousValidVertCount  =       M0.validVertCount;

                decimate_contract(M0);

                //      Update progress bar.
                if (upb)
                {
                        upb(percent);
                }
        }

        //      Update progress bar.
        if (upb)
        {
                upb(100.0);
        }
}

//---------------------------------------------------------------------------
// Class to create a map without repeated vertices
//---------------------------------------------------------------------------
class _float3_
{
        public:

                float x,y,z;

                _float3_(float x=0.0f, float y=0.0f, float z=0.0f)
                {
                        this->x = x; this->y = y; this->z = z;
                }

                _float3_(const _float3_ &f)
                {
                        x=f.x; y=f.y; z=f.z;
                }

                _float3_ & operator=(const _float3_ &f)
                {
                        x=f.x; y=f.y; z=f.z; return *this;
                }

                bool operator<(const _float3_ &f) const 
                { 
                        if (x<f.x) return true;
                        if (x>f.x) return false;
                        if (y<f.y) return true;
                        if (y>f.y) return false;
                        if (z<f.z) return true;
                        if (z>f.z) return false;
                        return false;
                }
};

//---------------------------------------------------------------------------
//      Generate simplification model.
//---------------------------------------------------------------------------
void    SimplificationMethod::generateSimplifModel(void)
{
        int     face_index      =       0;

        //      For each submesh.
        bool added_vertices = false;
        for (size_t     i =     0;      i       <       objmesh->mSubMeshCount; i++)
        {
                //      If is not a tree leaves submesh.
                if (i != indexMeshLeaves)
                {
                        //      For all the vertices of each submesh.
                        for (   size_t  j       =       0;
                                        j < objmesh->mSubMesh[i].mVertexBuffer->mVertexCount;
                                        j++)
                        {
                                //      If the vertex is not in vertices_map, then is added.
                                M0.in_Vertex(simplif::Vec3(
                                                        objmesh->mSubMesh[i].mVertexBuffer->mPosition[j].x,
                                                        objmesh->mSubMesh[i].mVertexBuffer->mPosition[j].y,
                                                        objmesh->mSubMesh[i].mVertexBuffer->mPosition[j].z));

                                M0.in_Normal(simplif::Vec3(
                                                        objmesh->mSubMesh[i].mVertexBuffer->mNormal[j].x,
                                                        objmesh->mSubMesh[i].mVertexBuffer->mNormal[j].y,
                                                        objmesh->mSubMesh[i].mVertexBuffer->mNormal[j].z));

                                M0.in_TexCoord(simplif::Vec2(
                                                        objmesh->mSubMesh[i].mVertexBuffer->mTexCoords[j].x,
                                                        objmesh->mSubMesh[i].mVertexBuffer->mTexCoords[j].y));

                                added_vertices = true;
                        }
                }       
                
                //      If is shared vertex buffer object.
                if (objmesh->mSubMesh[i].mSharedVertexBuffer && added_vertices)
                {
                        printf("Shared break");
                        break;
                }
        }

        //      For each submesh.
        for (size_t     i = 0;  i < objmesh->mSubMeshCount;     i++)
        {
                if (i != indexMeshLeaves)
                {
                        for (Index      j       =       0;      j < objmesh->mSubMesh[i].mIndexCount;   j       +=      3)
                        {
                                //      +1 is required because the first index in the
                                //      simplification method is 1
                                //      Index of the vertex in vertices_map.
                                Index index0    =               objmesh->mSubMesh[i].mIndex[j];
                                
                                Index index1    =               objmesh->mSubMesh[i].mIndex[j + 1];
                                
                                Index index2    =               objmesh->mSubMesh[i].mIndex[j + 2];
                                
                                //      Create a triangle with its indices in vertices_map.
                                face_index      =       M0.miin_Face(
                                                index0,
                                                index1,
                                                index2);

                                //      igeo allows to identify the submesh that contains the triangle.
                                M0.face(face_index)->igeo       =       (int)i;
                        }
                }
        }
        number_of_triangles     =       face_index;
}

//---------------------------------------------------------------------------
//      simplifmethod_init is executed to do some initalizations
//      simplifmethod_run is executed to do the decimation
//      Then, the simpified model is created and the decimation 
//      information is returned.
//---------------------------------------------------------------------------
MeshSimplificationSequence *SimplificationMethod::Decimate(     float lod,
                                                                                                                                                                                                                                                int simpliftype,
                                                                                                                                                                                                                                                TIPOFUNC upb)
{
        simplifmethod_init();

        if (simpliftype == 0)
        {
                //      Percentage option.
                simplifmethod_run((int)(number_of_triangles*lod),upb);
        }
        else
        {
                //      Number of vertices option.
                simplifmethod_runv((int)lod,upb);
        }

        //      Store unique vertices by submesh (without repetitions).
        typedef std::map<int,int> MAPAINDIND;

        //      Store all the indices by submesh (with repetitions).
        typedef std::vector<int> REPINDLIST;

        MAPAINDIND *unique_verts_inds_by_geo =
                                                                                                                new MAPAINDIND[objmesh->mSubMeshCount];

        REPINDLIST *ver_inds_rep_by_geo = new REPINDLIST[objmesh->mSubMeshCount];

        //      Index counter by submesh.
        int *inextvert = new int[objmesh->mSubMeshCount];

        for (unsigned int       i = 0;  i < objmesh->mSubMeshCount;     i++)
        {
                inextvert[i] = 0;
        }

        //      Construct the model.
        std::cout << "M0.faceCount(): " << M0.faceCount() << std::endl;
        for (int        i = 0;  i < M0.faceCount();     i++)
        {
                if (M0.face(i)->isValid())
                {
                        simplif::Face *auxface  =       M0.face(i);

                        //      Determine to which submesh pertains the triangle.
                        int igeo = auxface->igeo;

                        //      Insert to each submesh its triangles.
                        std::map<int,int>::iterator findit =
                                unique_verts_inds_by_geo[igeo].find(auxface->vertex(0)->validID());
                        
                        if (findit == unique_verts_inds_by_geo[igeo].end())
                        {
                                //      If it is not added... add and map it.
                                unique_verts_inds_by_geo[igeo][auxface->vertex(0)->validID()] =
                                                                                                                                                                                                                                                inextvert[igeo];
                                
                                inextvert[igeo]++;
                        }

                        findit =
                                unique_verts_inds_by_geo[igeo].find(auxface->vertex(1)->validID());
                        
                        if (findit == unique_verts_inds_by_geo[igeo].end())
                        {
                                //      If it is not added... add and map it.
                                unique_verts_inds_by_geo[igeo][auxface->vertex(1)->validID()] =
                                                                                                                                                                                                                                                inextvert[igeo];
                                inextvert[igeo]++;
                        }
                        
                        findit = 
                                unique_verts_inds_by_geo[igeo].find(auxface->vertex(2)->validID());
                        
                        if (findit == unique_verts_inds_by_geo[igeo].end())
                        {
                                //      If it is not added... add and map it.
                                unique_verts_inds_by_geo[igeo][auxface->vertex(2)->validID()] = 
                                                                                                                                                                                                                                                inextvert[igeo];
                                inextvert[igeo]++;
                        }

                        //      Total number of indices by submesh.
                        ver_inds_rep_by_geo[igeo].push_back(auxface->vertex(0)->validID());
                        ver_inds_rep_by_geo[igeo].push_back(auxface->vertex(1)->validID());
                        ver_inds_rep_by_geo[igeo].push_back(auxface->vertex(2)->validID());
                }
        }

        //      Output simplified mesh.
        mGeoMesh                                                                =       new Mesh();
        mGeoMesh->mVertexBuffer =       new     VertexBuffer();

        mGeoMesh->mSubMeshCount = objmesh->mSubMeshCount;

        //      Memory allocation for the submeshes.
        mGeoMesh->mSubMesh = new SubMesh[objmesh->mSubMeshCount];

        //      Fill up bounding box settings.
        mGeoMesh->mMeshBounds.maxX                                      =       objmesh->mMeshBounds.maxX;
        mGeoMesh->mMeshBounds.maxY                                      =       objmesh->mMeshBounds.maxY;
        mGeoMesh->mMeshBounds.maxZ                                      =       objmesh->mMeshBounds.maxZ;
        mGeoMesh->mMeshBounds.minX                                      =       objmesh->mMeshBounds.minX;
        mGeoMesh->mMeshBounds.minY                                      =       objmesh->mMeshBounds.minY;
        mGeoMesh->mMeshBounds.minZ                                      =       objmesh->mMeshBounds.minZ;
        mGeoMesh->mMeshBounds.radius                            =       objmesh->mMeshBounds.radius;
        mGeoMesh->mMeshBounds.scaleFactor               =       objmesh->mMeshBounds.scaleFactor;

        //      Copy skeleton name.
        if (objmesh->hasSkeleton)
        {
                mGeoMesh->hasSkeleton   =       true;

                strcpy(mGeoMesh->mSkeletonName,objmesh->mSkeletonName);
        }

        //      Copy mesh bones.
        if (!objmesh->mBones.empty())
        {
                for (unsigned int j = 0; j < objmesh->mBones.size(); j++)
                {
                        mGeoMesh->mBones.push_back(objmesh->mBones[j]);
                }
        }

        bool copiedShared = false;

        //      For each submesh.
        for (size_t     i = 0;  i < objmesh->mSubMeshCount;     i++)
        {
                mGeoMesh->mSubMesh[i].mStripCount       =       0;
                mGeoMesh->mSubMesh[i].mStrip                    =       NULL;

                mGeoMesh->
                        mSubMesh[i].mSharedVertexBuffer =       false;

                strcpy( mGeoMesh->mSubMesh[i].mMaterialName,
                                objmesh->mSubMesh[i].mMaterialName);

                //      Copy submesh bones.
                if (!objmesh->mSubMesh[i].mBones.empty())
                {
                        for (unsigned int j = 0; j < objmesh->mSubMesh[i].mBones.size(); j++)
                        {
                                mGeoMesh->mSubMesh[i].mBones.push_back(objmesh->
                                                                                                                                                                mSubMesh[i].mBones[j]);
                        }
                }

                if (i != indexMeshLeaves)
                {
                        //      Indices vectors.
                        mGeoMesh->mSubMesh[i].mIndexCount       =       ver_inds_rep_by_geo[i].size();
                        
                        mGeoMesh->mSubMesh[i].mIndex                    =       new Index[mGeoMesh->
                                                                                                                                                                                mSubMesh[i].mIndexCount];

                        //      Store the indices.
                        for (unsigned int       j = 0;  j < mGeoMesh->mSubMesh[i].mIndexCount;  j++)
                        {
                                //      Obtain the indices that point at VertexBuffer.
                                mGeoMesh->mSubMesh[i].mIndex[j] =
                                        unique_verts_inds_by_geo[i].
                                                                operator [](ver_inds_rep_by_geo[i].
                                                                operator [](j));
                        }

                        //      Copy the vertices.
                        if (mGeoMesh->mSubMesh[i].mSharedVertexBuffer)
                        {
                                //      Shared vertices.
                                if (!copiedShared)
                                {
                                        mGeoMesh->mVertexBuffer = new VertexBuffer();
                                        copiedShared                                            = true;
                                }

                                mGeoMesh->mSubMesh[i].mVertexBuffer = mGeoMesh->mVertexBuffer;
                        }
                        else
                        {
                                //      There's no shared vertices.
                                mGeoMesh->mSubMesh[i].mVertexBuffer     =       new VertexBuffer();
                        }

                        mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount       =
                                                                                                                                        unique_verts_inds_by_geo[i].size();
                        
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexInfo        =
                                                                                        objmesh->mSubMesh[i].mVertexBuffer->mVertexInfo;

                        //      Allocate memory for position, normal and texutre coordinates.
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mPosition  =
                                                new Vector3[mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount];
                        
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mNormal            =
                                                new Vector3[mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount];
                        
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mTexCoords =
                                                new Vector2[mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount];

                        for (   MAPAINDIND::iterator mapit = unique_verts_inds_by_geo[i].begin();
                                                mapit != unique_verts_inds_by_geo[i].end();
                                                mapit++)
                        {
                                //      Key and value.
                                int isrc = mapit->first;
                                int idst = mapit->second;

                                //      Vertex coordinate.
                                //      Construction of the submeshes.
                                Vector3 v3(     (Real)M0.vertex(isrc)->operator [](0),
                                                                                (Real)M0.vertex(isrc)->operator [](1),
                                                                                (Real)M0.vertex(isrc)->operator [](2));
                                
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mPosition[idst]    =       v3;

                                //      Normal coordinates.
                                Vector3 v3n(    (Real)M0.normal(isrc)(0),
                                                                                        (Real)M0.normal(isrc)(1),
                                                                                        (Real)M0.normal(isrc)(2));
                                
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mNormal[idst]      =       v3n;

                                //      Texture coordinates.
                                Vector2 v2(     (Real)M0.texcoord(isrc)(0),
                                                                                (Real)M0.texcoord(isrc)(1));
                                
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mTexCoords[idst]   =       v2;
                        }
                }
                else
                {
                        //      Leaves mesh.
                        mGeoMesh->mSubMesh[i].mIndexCount       =       objmesh->mSubMesh[i].mIndexCount;
                        
                        mGeoMesh->mSubMesh[i].mIndex    =               new Index[mGeoMesh->mSubMesh[i].mIndexCount];

                        //      Copy the leaves submesh indexes.
                        for (unsigned int       j       =       0;      j < mGeoMesh->mSubMesh[i].mIndexCount;  j++)
                        {       
                                mGeoMesh->mSubMesh[i].mIndex[j] =       objmesh->mSubMesh[i].mIndex[j];
                        }

                        //      Copy the leaves submesh vertices.
                        mGeoMesh->mSubMesh[i].mVertexBuffer     =       new VertexBuffer();
                        
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount       =
                                                                                                                objmesh->mSubMesh[i].mVertexBuffer->mVertexCount;
                        
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexInfo        =
                                                                                                                objmesh->mSubMesh[i].mVertexBuffer->mVertexInfo;

                        //      Allocate memory for position, normal and texture coordinates.
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mPosition  =
                                                        new Vector3[mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount];
                        
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mNormal    =
                                                        new Vector3[mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount];
                        
                        mGeoMesh->mSubMesh[i].mVertexBuffer->mTexCoords =
                                                        new Vector2[mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount];

                        for (   unsigned int j = 0;
                                        j < mGeoMesh->mSubMesh[i].mVertexBuffer->mVertexCount;
                                        j++)
                        {
                                //      Position.
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mPosition[j].x     =
                                                                        objmesh->mSubMesh[i].mVertexBuffer->mPosition[j].x;
                                
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mPosition[j].y     =
                                                                        objmesh->mSubMesh[i].mVertexBuffer->mPosition[j].y;
                                
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mPosition[j].z     =
                                                                        objmesh->mSubMesh[i].mVertexBuffer->mPosition[j].z;

                                //      Normals.
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mNormal[j].x       =
                                                                        objmesh->mSubMesh[i].mVertexBuffer->mNormal[j].x;
                                
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mNormal[j].y       =
                                                                        objmesh->mSubMesh[i].mVertexBuffer->mNormal[j].y;
                                
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mNormal[j].z       =
                                                                        objmesh->mSubMesh[i].mVertexBuffer->mNormal[j].z;

                                //      Textures.
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mTexCoords[j].x    =
                                                                        objmesh->mSubMesh[i].mVertexBuffer->mTexCoords[j].x;
                                
                                mGeoMesh->mSubMesh[i].mVertexBuffer->mTexCoords[j].y    =
                                                                        objmesh->mSubMesh[i].mVertexBuffer->mTexCoords[j].y;
                        }
                }
        }

        delete[] unique_verts_inds_by_geo;
        delete[] ver_inds_rep_by_geo;
        delete[] inextvert;

        //      Store the simplification steps in MeshSimplificationSequence.
        int     acum    =       0;

        for (size_t i   =       0; i < objmesh->mSubMeshCount; i++)
        {
                if (objmesh->mSubMesh[i].mSharedVertexBuffer)
                {
                        first_index_submesh[i]  =       0;
                }
                else
                {
                        first_index_submesh[i]  =       acum;
                }

                acum    +=      (int)objmesh->mSubMesh[i].mVertexBuffer->mVertexCount;
        }

        vector<int>     v0;
        vector<int>     v1;
        vector<int>     submesh0;
        vector<int>     submesh1;

        return msimpseq;
}

//---------------------------------------------------------------------------
//      Set submesh leaves.
//---------------------------------------------------------------------------
void SimplificationMethod::setMeshLeaves(int meshLeaves)
{
        indexMeshLeaves =       meshLeaves;
}

//      Gets mesh simplified.
Mesh    *       SimplificationMethod::GetMesh()
{
        return  mGeoMesh;
}