source: GTP/trunk/App/Demos/Vis/FriendlyCulling/src/Polyhedron.cpp @ 3214

#include "Polyhedron.h"
#include "common.h"
#include "Polygon3.h"
#include "Plane3.h"


#ifdef _CRT_SET
        #define _CRTDBG_MAP_ALLOC
        #include <stdlib.h>
        #include <crtdbg.h>

        // redefine new operator
        #define DEBUG_NEW new(_NORMAL_BLOCK, __FILE__, __LINE__)
        #define new DEBUG_NEW
#endif


using namespace std;


namespace CHCDemoEngine 
{

static const float epsilon = 1e-6f;



/************************************************************/
/*              class Polyhedron Implementation             */
/************************************************************/


Polyhedron::Polyhedron()
{
        mBox.Initialize();
}


Polyhedron::Polyhedron(const PolygonContainer &polys)
{
        mPolygons = polys;

        mBox.Initialize();
        mBox.Include(mPolygons);
}


Polyhedron::Polyhedron(const Polyhedron &rhs)
{
        mPolygons.reserve(rhs.mPolygons.size());
        mBox = rhs.mBox;

        PolygonContainer::const_iterator it, it_end = rhs.mPolygons.end();

        for (it = rhs.mPolygons.begin(); it != it_end; ++ it)
                Add(new Polygon3(*(*it)));
}


Polyhedron *Polyhedron::CreatePolyhedron(const vector<Plane3> &planes, 
                                                                                 const AxisAlignedBox3 &sceneBox)
{
        // add the bounding box
        PolygonContainer polygons;
        sceneBox.ExtractPolygons(polygons);

        Polyhedron *oldPolyhedron = new Polyhedron(polygons);

        if (!oldPolyhedron->Valid()) cerr << "******************* not valid!! ************* " << endl;

        vector<Plane3>::const_iterator it, it_end = planes.end();

        Polyhedron *newPolyhedron = NULL;
        
        // intersect bounding box with planes
        for (it = planes.begin(); it != it_end; ++ it)
        {       
                Plane3 plane = *it;
                
                newPolyhedron = oldPolyhedron->CalcIntersection(plane);
        
                if (!newPolyhedron)
                {
                        //cerr << "polyhedron not valid or NULL!" << endl; 
                        return NULL;    
                }

                DEL_PTR(oldPolyhedron);
                oldPolyhedron = newPolyhedron;
        }

        return newPolyhedron;
}


Polyhedron::~Polyhedron()
{
        CLEAR_CONTAINER(mPolygons);
}


int Polyhedron::NumPolygons() const
{
        return (int)mPolygons.size();
}


void Polyhedron::Add(Polygon3 *p)
{
    mPolygons.push_back(p);
        mBox.Include(*p);
}


const PolygonContainer &Polyhedron::GetPolygons() const
{
        return mPolygons;
}


float Polyhedron::GetArea() const 
{ 
        float area = 0;

        for (size_t i = 0; i < mPolygons.size(); ++ i)
        {
                area += mPolygons[i]->GetArea();
        }

        return area;
}



float Polyhedron::GetVolume() const
{
        // computes volume by tetrahedralization of the geometry
        // We just compute the sum of the tetrahedron volumes
        float volume = 0;
        const float f = 1.0f / 6.0f;

        PolygonContainer::const_iterator pit, pit_end = mPolygons.end();

        // note: can take arbitrary point, e.g., the origin. However,
        // we rather take the center of mass to prevents precision errors
        const Vector3 center = CenterOfMass();

        for (pit = mPolygons.begin(); pit != pit_end; ++ pit)
        {
                Polygon3 *poly = *pit;
                const Vector3 v0 = poly->mVertices[0] - center;

                for (int i = 1; i < (int)poly->mVertices.size() - 1; ++ i)
                {
                        const Vector3 v1 = poly->mVertices[i] - center;
                        const Vector3 v2 = poly->mVertices[i + 1] - center;

                        // more robust version using abs and the center of mass
                        volume += fabs (f * (DotProd(v0, CrossProd(v1, v2))));
                }
        }

        return volume;
}


AxisAlignedBox3 Polyhedron::GetBoundingBox() const
{
        return mBox;
}


int Polyhedron::Side(const Plane3 &plane) const
{
        int boxSide = mBox.Side(plane);

        // plane does not intersect bounding box
        if (boxSide != 0) return boxSide;

        PolygonContainer::const_iterator it, it_end = mPolygons.end();

        int s = 0;

        for (it = mPolygons.begin(); it != it_end; ++ it)
        {
        const int side = (*it)->Side(plane, epsilon);

                if (side == 0) // intersects polygon => intersects
                        return 0;
                else if (side != 0)
                {
                        if (side == -s)
                                return 0; // sign changed => intersects

                        s = side;
                }
        }

        return s;
}


Vector3 Polyhedron::CenterOfMass() const
{
        int n = 0;

        Vector3 center(0,0,0);

        PolygonContainer::const_iterator pit, pit_end = mPolygons.end();

        for (pit = mPolygons.begin(); pit != pit_end; ++ pit)
        {
                Polygon3 *poly = *pit;
                
                VertexArray::const_iterator vit, vit_end = poly->mVertices.end();

                for(vit = poly->mVertices.begin(); vit != vit_end; ++ vit)
                {
                        center += *vit;
                        ++ n;
                }
        }

        return center / (float)n;
}


bool Polyhedron::Valid() const
{
        // polyhedron is degenerated
        if (mPolygons.size() < 3)
                return false;
        
        const Vector3 center = CenterOfMass();

        PolygonContainer::const_iterator pit, pit_end = mPolygons.end();

        for (pit = mPolygons.begin(); pit != pit_end; ++ pit)
        {
                Polygon3 *poly = *pit;
                Plane3 plane = poly->GetSupportingPlane();

                float dist = plane.Distance(center);
                
                if (dist > 0) return false;
        }
        return true;
}


Polyhedron *Polyhedron::CalcIntersection(const Plane3 &splitPlane) const
{       
        // first test plane intersection with geometry
        const int intersect = Side(splitPlane);
        
        if (intersect == 1)
        {
                // on the other side of the plane, the intersection is empty
                return NULL;
        }
        else if (intersect == -1)
        {
                // does not intersect, just return a copy
                return new Polyhedron(*this);
        }

        // polyhedron is intersected: create new polyhedron
        Polyhedron *clippedPolyhedron = new Polyhedron();


        //////////
        //-- create and add new polygon to close polyhedron

        // get cross section of new polygon
        Polygon3 *planePoly = mBox.CrossSection(splitPlane);

        // create new face: split polygon with all other polygons
        planePoly = SplitPolygon(planePoly);

        if (planePoly) 
        {
                // we can now savely add the new polygon
                clippedPolyhedron->Add(planePoly);
        }
        else if (0)
        {
                // something is wrong, probably some numerical error
                cerr << "no polygon: should not happen" << endl; 
                return new Polyhedron(*this); 
        }
        

        /////////////
        //-- clip polyhedron: plane splits all polygons

        for (size_t i = 0; i < mPolygons.size(); ++ i)
        {
                Polygon3 *poly = mPolygons[i];

                const int cf = mPolygons[i]->ClassifyPlane(splitPlane, epsilon);
                        
                switch (cf)
                {
                        // split case: split polygon and add it
                        case Polygon3::SPLIT:
                                {
                                        Polygon3 *poly = mPolygons[i];

                                        Polygon3 *frontPoly = new Polygon3();
                                        Polygon3 *backPoly = new Polygon3();
                                
                                        poly->Split(splitPlane, *frontPoly, *backPoly, epsilon);

                                        // we are only interested in the polytope behind the plane
                                        DEL_PTR(frontPoly);

                                        if (backPoly->Valid(epsilon))
                                                clippedPolyhedron->Add(backPoly);
                                        else
                                                DEL_PTR(backPoly);
                                }
                                
                                break;
                        case Polygon3::BACK_SIDE:
                        case Polygon3::COINCIDENT:
                                clippedPolyhedron->Add(new Polygon3(mPolygons[i]->mVertices));
                                break;
                        default:
                                // polygon is not in intersection
                                break;
                }
        }

        return clippedPolyhedron;
}


Polygon3 *Polyhedron::SplitPolygon(Polygon3 *polygon) const
{
        if (!polygon->Valid(epsilon)) DEL_PTR(polygon);

        // polygon is split by all other planes
        for (size_t i = 0; (i < mPolygons.size()) && polygon; ++ i)
        {
                const Plane3 plane = mPolygons[i]->GetSupportingPlane();

                int cf = polygon->ClassifyPlane(plane, epsilon);

                // split new polygon with all previous planes
                switch (cf)
                {
                        case Polygon3::SPLIT:
                                {
                                        Polygon3 *frontPoly = new Polygon3();
                                        Polygon3 *backPoly = new Polygon3();

                                        polygon->Split(plane, *frontPoly, *backPoly, epsilon);
                                        
                                        // don't need these anymore
                                        DEL_PTR(frontPoly);
                                        DEL_PTR(polygon);

                                        // back polygon belongs to geometry
                                        if (backPoly->Valid(epsilon))
                                                polygon = backPoly;
                                        else
                                                DEL_PTR(backPoly);
                                }
                                break;
                        case Polygon3::FRONT_SIDE:
                                //cerr << "SplitPolygon: should not come here" << endl;
                                DEL_PTR(polygon);
                break;
                        // polygon is taken as it is
                        case Polygon3::BACK_SIDE:
                        case Polygon3::COINCIDENT:
                        default:
                                break;
                }
        }

        return polygon;
}


void Polyhedron::CollectVertices(VertexArray &vertices) const
{
        PolygonContainer::const_iterator it, it_end = mPolygons.end();

        for (it = mPolygons.begin(); it != it_end; ++ it)
        {
                Polygon3 *poly = *it;

                VertexArray::const_iterator vit, vit_end = poly->mVertices.end();

                for (vit = poly->mVertices.begin(); vit != vit_end; ++ vit)
                {
                        vertices.push_back(*vit);
                }
        }
}


}