polygon.cpp

Go to the documentation of this file.

/*
 * This work is licensed under a Creative Commons 
 * Attribution-Noncommercial-Share Alike 3.0 United States License.
 * 
 *    http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 * 
 * You are free:
 * 
 *    to Share - to copy, distribute, display, and perform the work
 *    to Remix - to make derivative works
 * 
 * Under the following conditions:
 * 
 *    Attribution. You must attribute the work in the manner specified by the
 *    author or licensor (but not in any way that suggests that they endorse you
 *    or your use of the work).
 * 
 *    Noncommercial. You may not use this work for commercial purposes.
 * 
 *    Share Alike. If you alter, transform, or build upon this work, you may
 *    distribute the resulting work only under the same or similar license to
 *    this one.
 * 
 * For any reuse or distribution, you must make clear to others the license
 * terms of this work. The best way to do this is by including this header.
 * 
 * Any of the above conditions can be waived if you get permission from the
 * copyright holder.
 * 
 * Apart from the remix rights granted under this license, nothing in this
 * license impairs or restricts the author's moral rights.
 */


#include <jwsc++/config.h>
#include <jwsc/config.h>

#include <cassert>
#include <cmath>
#include <iostream>
#include <vector>

#include <inttypes.h>

#if defined JWSCXX_HAVE_OPENGL
#include <GL/gl.h>
#elif defined JWSCXX_HAVE_OPENGL_FRAMEWORK
#include <OpenGL/gl.h>
#endif

#include <jwsc/base/bits.h>
#include <jwsc/vector/vector.h>
#include <jwsc/vector/vector_math.h>
#include <jwsc/matrix/matrix.h>
#include <jwsc/matrix/matrix_math.h>

#include "jwsc++/base/exception.h"
#include "jwsc++/graphics/camera.h"
#include "jwsc++/graphics/polygon.h"

#ifdef JWSCXX_HAVE_DMALLOC
#include <dmalloc.h>
#endif


using namespace jwscxx::base;
using namespace jwscxx::graphics;


Polygon_f::Polygon_f()
{
    normal = 0;
    centroid = 0;
    normal_flipped = 0;
    id = generate_id();
}


Polygon_f::Polygon_f(uint32_t N)
{
    pts.reserve(N);
    normal = 0;
    centroid = 0;
    normal_flipped = 0;
    id = generate_id();
}


Polygon_f::Polygon_f(const Polygon_f& p)
    : pts(p.pts.capacity())
{
    normal = 0;
    centroid = 0;
    *this = p;
}


Polygon_f::Polygon_f(const char* fname) 
    throw (jwscxx::base::Arg_error, jwscxx::base::IO_error)
{
    normal = 0;
    centroid = 0;
    normal_flipped = 0;

    jwscxx::base::Readable::read(fname);
}


Polygon_f::Polygon_f(std::istream& in) 
    throw (jwscxx::base::Arg_error, jwscxx::base::IO_error)
{
    normal = 0;
    centroid = 0;
    normal_flipped = 0;

    read(in);
}


Polygon_f::~Polygon_f()
{
    free_vector_f(normal);
    free_vector_f(centroid);

    for (size_t i = 0; i < pts.size(); i++)
    {
        free_vector_f(pts[ i ]);
    }
}


Polygon_f& Polygon_f::operator= (const Polygon_f& p)
{
    using namespace std;
    using namespace jwsc;

    size_t n = pts.size();
    size_t N = p.pts.size();

    if (n > N)
    {
        for (size_t i = N; i < n; i++)
        {
            free_vector_f(pts[ i ]);
        }
        pts.erase(pts.begin() + N, pts.end());
    }
    else
    {
        pts.reserve(N);
    }

    for (size_t i = 0; i < N; i++)
    {
        if (i < n)
        {
            copy_vector_f(&(pts[ i ]), p.pts[ i ]);
        }
        else
        {
            Vector_f* pt = 0;
            copy_vector_f(&pt, p.pts[ i ]);
            pts.push_back(pt);
        }
    }

    if (p.normal)
    {
        copy_vector_f(&normal, p.normal);
    }
    if (p.centroid)
    {
        copy_vector_f(&centroid, p.centroid);
    }

    normal_flipped = p.normal_flipped;

    id = p.id;

    return *this;
}


Polygon_f* Polygon_f::clone() const
{
    return new Polygon_f(*this);
}


void Polygon_f::read(std::istream& in) throw (jwscxx::base::IO_error,
        jwscxx::base::Arg_error)
{
    using namespace jwsc;

    in.read((char*)&id, sizeof(uint32_t));
    if (in.fail() || in.eof())
    {
        throw IO_error("Could not read id");
    }
#ifndef JWSC_BIGENDIAN
    bswap_u32(&(id));
#endif

    assert(sizeof(float) == sizeof(uint32_t));

    float x, y, z, w;

    // Number of points.
    uint32_t N;

    in.read((char*)&N, sizeof(uint32_t));
    if (in.fail() || in.eof())
    {
        throw IO_error("Could not read number of points");
    }

#ifndef JWSC_BIGENDIAN
    bswap_u32(&(N));
#endif

    // Reserve space in the pts vector.
    size_t n = pts.size();

    if (n > N)
    {
        for (size_t i = N; i < n; i++)
        {
            free_vector_f(pts[ i ]);
        }
        pts.erase(pts.begin() + N, pts.end());
    }
    else
    {
        pts.reserve(N);
    }


    // Points.
    for (size_t i = 0; i < N; i++)
    {
        in.read((char*)&x, sizeof(float));
        in.read((char*)&y, sizeof(float));
        in.read((char*)&z, sizeof(float));
        in.read((char*)&w, sizeof(float));
        if (in.fail() || in.eof())
        {
            throw IO_error("Could not read points");
        }

#ifndef JWSC_BIGENDIAN
        bswap_u32((uint32_t*)&(x));
        bswap_u32((uint32_t*)&(y));
        bswap_u32((uint32_t*)&(z));
        bswap_u32((uint32_t*)&(w));
#endif

        if (i < n)
        {
            pts[ i ]->elts[0] = x;
            pts[ i ]->elts[1] = y;
            pts[ i ]->elts[2] = z;
            pts[ i ]->elts[3] = w;
        }
        else
        {
            Vector_f* pt = 0;
            create_vector_f(&pt, 4);

            pt->elts[0] = x;
            pt->elts[1] = y;
            pt->elts[2] = z;
            pt->elts[3] = w;

            pts.push_back(pt);
        }
    }


    // Normal.
    if (N >= 3)
    {
        in.read((char*)&x, sizeof(float));
        in.read((char*)&y, sizeof(float));
        in.read((char*)&z, sizeof(float));
        in.read((char*)&w, sizeof(float));
        if (in.fail() || in.eof())
        {
            throw IO_error("Could not read normal");
        }

#ifndef JWSC_BIGENDIAN
        bswap_u32((uint32_t*)&(x));
        bswap_u32((uint32_t*)&(y));
        bswap_u32((uint32_t*)&(z));
        bswap_u32((uint32_t*)&(w));
#endif

        create_vector_f(&normal, 4);
        normal->elts[0] = x;
        normal->elts[1] = y;
        normal->elts[2] = z;
        normal->elts[3] = w;
    }


    // Centroid.
    if (N > 0)
    {
        in.read((char*)&x, sizeof(float));
        in.read((char*)&y, sizeof(float));
        in.read((char*)&z, sizeof(float));
        in.read((char*)&w, sizeof(float));
        if (in.fail() || in.eof())
        {
            throw IO_error("Could not read centroid");
        }

#ifndef JWSC_BIGENDIAN
        bswap_u32((uint32_t*)&(x));
        bswap_u32((uint32_t*)&(y));
        bswap_u32((uint32_t*)&(z));
        bswap_u32((uint32_t*)&(w));
#endif

        create_vector_f(&centroid, 4);
        centroid->elts[0] = x;
        centroid->elts[1] = y;
        centroid->elts[2] = z;
        centroid->elts[3] = w;
    }

    in.read((char*)&normal_flipped, sizeof(uint8_t));
}


void Polygon_f::write(std::ostream& out) const throw (jwscxx::base::IO_error)
{
    using namespace jwsc;

    uint32_t id_cp = id;
#ifndef JWSC_BIGENDIAN
    bswap_u32(&(id_cp));
#endif
    out.write((char*)&id_cp, sizeof(uint32_t));
    if (out.fail() || out.eof())
    {
        throw IO_error("Could not write id");
    }

    assert(sizeof(float) == sizeof(uint32_t));

    float x, y, z, w;

    // Number of points.
    uint32_t N = pts.size();

#ifndef JWSC_BIGENDIAN
    bswap_u32(&(N));
#endif

    out.write((char*)&N, sizeof(uint32_t));
    if (out.fail() || out.eof())
    {
        throw IO_error("Could not write number of points");
    }


    // Points.
    for (size_t i = 0; i < pts.size(); i++)
    {
        Vector_f* pt = pts[ i ];

        x = pt->elts[0];
        y = pt->elts[1];
        z = pt->elts[2];
        w = pt->elts[3];

#ifndef JWSC_BIGENDIAN
        bswap_u32((uint32_t*)&(x));
        bswap_u32((uint32_t*)&(y));
        bswap_u32((uint32_t*)&(z));
        bswap_u32((uint32_t*)&(w));
#endif

        out.write((char*)&x, sizeof(float));
        out.write((char*)&y, sizeof(float));
        out.write((char*)&z, sizeof(float));
        out.write((char*)&w, sizeof(float));
        if (out.fail() || out.eof())
        {
            throw IO_error("Could not write points");
        }
    }


    // Normal.
    if (N >= 3)
    {
        assert(normal);

        x = normal->elts[0];
        y = normal->elts[1];
        z = normal->elts[2];
        w = normal->elts[3];

#ifndef JWSC_BIGENDIAN
        bswap_u32((uint32_t*)&(x));
        bswap_u32((uint32_t*)&(y));
        bswap_u32((uint32_t*)&(z));
        bswap_u32((uint32_t*)&(w));
#endif

        out.write((char*)&x, sizeof(float));
        out.write((char*)&y, sizeof(float));
        out.write((char*)&z, sizeof(float));
        out.write((char*)&w, sizeof(float));
        if (out.fail() || out.eof())
        {
            throw IO_error("Could not write normal");
        }
    }


    // Centroid.
    if (N > 0)
    {
        assert(centroid);

        x = centroid->elts[0];
        y = centroid->elts[1];
        z = centroid->elts[2];
        w = centroid->elts[3];

#ifndef JWSC_BIGENDIAN
        bswap_u32((uint32_t*)&(x));
        bswap_u32((uint32_t*)&(y));
        bswap_u32((uint32_t*)&(z));
        bswap_u32((uint32_t*)&(w));
#endif

        out.write((char*)&x, sizeof(float));
        out.write((char*)&y, sizeof(float));
        out.write((char*)&z, sizeof(float));
        out.write((char*)&w, sizeof(float));
        if (out.fail() || out.eof())
        {
            throw IO_error("Could not write centroid");
        }
    }

    out.write((char*)&normal_flipped, sizeof(uint8_t));
}


void Polygon_f::transform(const jwsc::Matrix_f* M) 
    throw (jwscxx::base::Arg_error)
{
    using namespace jwsc;

    if (M->num_rows != 4 && M->num_cols != 4)
    {
        throw Arg_error("Transformation matrix not homogeneous");
    }

    size_t N = pts.size();

    // Transform the points.
    for (size_t i = 0; i < N; i++)
    {
        Vector_f* pt = pts[ i ];

        multiply_matrix_and_vector_f(&pt, M, pt);
    }

    // Transform the centroid.
    multiply_matrix_and_vector_f(&centroid, M, centroid);

    update_normal();
}


void Polygon_f::wire_render() const
{
    using namespace jwsc;

    if (normal)
    {
        assert(fabs(normal->elts[3] - 1.0f) < 1.0e-8f);
        glNormal3f(normal->elts[0], normal->elts[1], normal->elts[2]);
    }

    Vector_f* pt;

    glBegin(GL_LINE_STRIP);
    for (size_t i = 0; i < pts.size(); i++)
    {
        pt = pts[ i ];

        glVertex4f(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    if (pts.size() > 0)
    {
        pt = pts[0];
        glVertex4f(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    glEnd();
}


void Polygon_f::wire_occlude_render() const
{
    using namespace jwsc;

    glDisable(GL_DEPTH_TEST);
    glEnable(GL_STENCIL_TEST);
    glClear(GL_STENCIL_BUFFER_BIT);
    glStencilFunc(GL_ALWAYS, 1, 1);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);

    GLboolean color_mask[4];
    glGetBooleanv(GL_COLOR_WRITEMASK, color_mask);
    glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);


    Vector_f* pt;

    glBegin(GL_LINE_STRIP);
    for (size_t i = 0; i < pts.size(); i++)
    {
        pt = pts[ i ];

        glVertex4f(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    if (pts.size() > 0)
    {
        pt = pts[0];
        glVertex4f(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    glEnd();


    glStencilFunc(GL_NOTEQUAL, 1, 1);
    glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);

    glEnable(GL_DEPTH_TEST);

    glBegin(GL_POLYGON);
    for (size_t i = 0; i < pts.size(); i++)
    {
        pt = pts[ i ];

        glVertex4f(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    glEnd();

    glDisable(GL_STENCIL_TEST);

    glColorMask(color_mask[0], color_mask[1], color_mask[2], color_mask[3]);
}

void Polygon_f::wire_occluded_silhouette_render(std::vector<bool> & iVector, bool &iVisible) const
{
    using namespace jwsc;
    
    assert(iVector.size() == pts.size());

    Vector_f* pt;

    glDisable(GL_DEPTH_TEST);
    glEnable(GL_STENCIL_TEST);
    glClear(GL_STENCIL_BUFFER_BIT);
    glStencilFunc(GL_ALWAYS, 1, 1);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);

    GLboolean color_mask[4];
    glGetBooleanv(GL_COLOR_WRITEMASK, color_mask);
    glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);

    glBegin(GL_LINES);
    uint32_t i = 0;
    for(std::vector<bool>::const_iterator it = iVector.begin(); it != iVector.end(); it++,i++)
    {
        if(*it)
        {
            //If it is the last edge, we need to render the last and the first point of the polygon
            int j = ( (i+1) == pts.size()) ? 0 : (i+1);
            glVertex4d(pts[i]->elts[0], pts[i]->elts[1], pts[i]->elts[2], pts[i]->elts[3]);
            glVertex4d(pts[j]->elts[0], pts[j]->elts[1], pts[j]->elts[2], pts[j]->elts[3]);
        }
    }
    glEnd();


    glStencilFunc(GL_NOTEQUAL, 1, 1);
    glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
    glEnable(GL_DEPTH_TEST);

    glBegin(GL_POLYGON);
    if(iVisible)
    {
    for (size_t i = 0; i < pts.size(); i++)
        {
            pt = pts[ i ];
            glVertex4d(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
        }
    }
    glEnd();

    glDisable(GL_STENCIL_TEST);

    glColorMask(color_mask[0], color_mask[1], color_mask[2], color_mask[3]);
}

void Polygon_f::solid_render() const
{
    using namespace jwsc;

    if (normal)
    {
        assert(fabs(normal->elts[3] - 1.0f) < 1.0e-8f);
        glNormal3f(normal->elts[0], normal->elts[1], normal->elts[2]);
    }

    glBegin(GL_POLYGON);
    for (size_t i = 0; i < pts.size(); i++)
    {
        Vector_f* pt = pts[ i ];

        glVertex4f(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    glEnd();
}


void Polygon_f::add_point(float x, float y, float z) 
    throw (jwscxx::base::Arg_error)
{
    using namespace jwsc;

    Vector_f* pt = 0;

    create_vector_f(&pt, 4);
    pt->elts[0] = x;
    pt->elts[1] = y;
    pt->elts[2] = z;
    pt->elts[3] = 1.0f;

    add_point(pt);
}


void Polygon_f::add_point(jwsc::Vector_f* pt) throw (jwscxx::base::Arg_error)
{
    using namespace jwsc;

    if (pt->num_elts != 4)
    {
        throw Arg_error("Point to add not homogeneous");
    }
    else if (fabs(pt->elts[3]) < 1.0e-10)
    {
        throw Arg_error("Homogeneous coordinate is zero");
    }

    size_t N = pts.size();

    pts.push_back(pt);

    if (N >= 3)
    {
        assert(normal);

        float dp1, dp2;

        calc_vector_dot_prod_f(&dp1, normal, pts[0]);
        calc_vector_dot_prod_f(&dp2, normal, pt);

        if (fabs(dp1 - dp2) > 1.0e-4)
        {
            throw Arg_error("Point to add not in the polygon's plane");
        }
    }
    else if (N == 2)
    {
        update_normal();
    }

    update_centroid();
}


const jwsc::Vector_f* Polygon_f::get_normal() const 
{ 
    return normal; 
}


const jwsc::Vector_f* Polygon_f::get_centroid() const 
{ 
    return centroid; 
}


size_t Polygon_f::get_num_points() const 
{ 
    return pts.size(); 
}


const jwsc::Vector_f* Polygon_f::get_point(size_t i) const 
    throw (jwscxx::base::Arg_error)
{ 
    if (i >= pts.size())
    { 
        throw jwscxx::base::Arg_error("Invalid point index"); 
    }

    return static_cast<const jwsc::Vector_f*>(pts[ i ]);
}


void Polygon_f::flip_normal() 
{ 
    normal_flipped = !normal_flipped; 
    update_normal(); 
}


void Polygon_f::project()
{
    using namespace jwsc;

    GLfloat glM[16] = {0};

    glGetFloatv(GL_MODELVIEW_MATRIX, glM);

    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glMultMatrixf(glM);
    glGetFloatv(GL_PROJECTION_MATRIX, glM);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);

    Matrix_f* M = 0;
    create_matrix_f(&M, 4, 4);
    for (size_t row = 0; row < 4; row++)
    {
        for (size_t col = 0; col < 4; col++)
        {
            M->elts[ row ][ col ] = glM[ col*4 + row  ];
        }
    }

    float width  = Camera_f::get_gl_viewport_width();
    float height = Camera_f::get_gl_viewport_height();

    for (size_t i = 0; i < pts.size(); i++)
    {
        Vector_f* pt = pts[ i ];

        multiply_matrix_and_vector_f(&pt, M, pt);

        float w = 1.0f / pt->elts[ 3 ];
        pt->elts[ 0 ] = 0.5f*width* (pt->elts[ 0 ]*w + 1.0f);
        pt->elts[ 1 ] = 0.5f*height*(pt->elts[ 1 ]*w + 1.0f);
        pt->elts[ 2 ] = 0;
        pt->elts[ 3 ] = 1.0f;
    }

    free_matrix_f(M);
}


uint8_t Polygon_f::is_visible(const Camera_f* camera) const
{
    if (!normal)
        return 0;

    const jwsc::Vector_f* prp = camera->get_prp();

    float x = prp->elts[0] - centroid->elts[0];
    float y = prp->elts[1] - centroid->elts[1];
    float z = prp->elts[2] - centroid->elts[2];

    if ((x*normal->elts[0] + y*normal->elts[1] + z*normal->elts[2]) > 0)
        return 1;
    return 0;
}

uint8_t Polygon_f::is_visible_up_to_epsilon(const Camera_d* camera, float & epsilon) const
{
    if (!normal)
        return 0;

    const jwsc::Vector_d* prp = camera->get_prp();

    float x = prp->elts[0] - centroid->elts[0];
    float y = prp->elts[1] - centroid->elts[1];
    float z = prp->elts[2] - centroid->elts[2];

    float dp = ((float)(x*normal->elts[0] + y*normal->elts[1] + z*normal->elts[2]));
    
    return ( dp > epsilon);
}


uint32_t Polygon_f::get_id() const 
{ 
    return id; 
}


void Polygon_f::set_id(uint32_t id) 
{ 
    this->id = id; 
}


void Polygon_f::update_normal()
{
    using namespace jwsc;

    size_t N = pts.size();

    if (N < 3)
        return;

    Vector_f* p1 = pts[0];
    Vector_f* p2 = pts[1];
    Vector_f* pN = pts[N-1];

    float w1 = pts[0]->elts[3];
    float w2 = pts[1]->elts[3];
    float wN = pts[N-1]->elts[3];

    assert(fabs(w1) >= 1.0e-10);
    assert(fabs(w2) >= 1.0e-10);
    assert(fabs(wN) >= 1.0e-10);

    w1 = 1.0f / w1;
    w2 = 1.0f / w2;
    wN = 1.0f / wN;

    // p2 - p1
    float v1_x = p2->elts[0]*w2 - p1->elts[0]*w1;
    float v1_y = p2->elts[1]*w2 - p1->elts[1]*w1;
    float v1_z = p2->elts[2]*w2 - p1->elts[2]*w1;

    // pN - p1
    float v2_x = pN->elts[0]*wN - p1->elts[0]*w1;
    float v2_y = pN->elts[1]*wN - p1->elts[1]*w1;
    float v2_z = pN->elts[2]*wN - p1->elts[2]*w1;

    // v1 x v2
    create_vector_f(&normal, 4);
    normal->elts[0] = v1_y*v2_z - v1_z*v2_y;
    normal->elts[1] = v1_z*v2_x - v1_x*v2_z;
    normal->elts[2] = v1_x*v2_y - v1_y*v2_x;
    normal->elts[3] = 0.0f;

    if (normal_flipped)
    {
        normal->elts[0] *= -1;
        normal->elts[1] *= -1;
        normal->elts[2] *= -1;
    }

    normalize_vector_mag_f(&normal, normal);
    normal->elts[3] = 1.0f;
}


void Polygon_f::update_centroid()
{
    using namespace jwsc;

    size_t N = pts.size();
    float area = 0;

    if (N == 0)
    {
        return;
    }
    else if (N == 1)
    {
        copy_vector_f(&centroid, pts[0]);
        return;
    }
    else if (N == 2)
    {
        float x = (pts[0]->elts[0]/pts[0]->elts[3] + 
                   pts[1]->elts[0]/pts[1]->elts[3]) * 0.5f;
        float y = (pts[0]->elts[1]/pts[0]->elts[3] + 
                   pts[1]->elts[1]/pts[1]->elts[3]) * 0.5f;
        float z = (pts[0]->elts[2]/pts[0]->elts[3] + 
                   pts[1]->elts[2]/pts[1]->elts[3]) * 0.5f;
        create_zero_vector_f(&centroid, 4);
        centroid->elts[0] = x;
        centroid->elts[1] = y;
        centroid->elts[2] = z;
        centroid->elts[3] = 1.0f;
        return;
    }

    float x3 = pts[N-1]->elts[0];
    float y3 = pts[N-1]->elts[1];
    float z3 = pts[N-1]->elts[2];

    for (size_t i = 0; i < N - 2; i++)
    {
        float x1 = pts[i]->elts[0];
        float y1 = pts[i]->elts[1];
        float z1 = pts[i]->elts[2];

        float x2 = pts[i+1]->elts[0];
        float y2 = pts[i+1]->elts[1];
        float z2 = pts[i+1]->elts[2];

        float dot = (x2 - x1)*(x3 - x1) + 
                    (y2 - y1)*(y3 - y1) + 
                    (z2 - z1)*(z3 - z1);

        float base = sqrt((x2 - x1)*(x2 - x1) + 
                          (y2 - y1)*(y2 - y1) + 
                          (z2 - z1)*(z2 - z1));

        float height;

        if (base < 1.0e-8f)
        {
            height = 0;
        }
        else 
        {
            float alpha = dot / (base*base);

            float a = x3 - x1 - alpha*(x2 - x1);
            float b = y3 - y1 - alpha*(y2 - y1);
            float c = z3 - z1 - alpha*(z2 - z1);

            height = sqrt(a*a + b*b + c*c);
        }

        float tri_area = 0.5f * base * height;

        area += tri_area;
        centroid->elts[0] += tri_area * (x1 + x2 + x3) / 3.0f;
        centroid->elts[1] += tri_area * (y1 + y2 + y3) / 3.0f;
        centroid->elts[2] += tri_area * (z1 + z2 + z3) / 3.0f;
    }

    if (area > 1.0e-8f)
    {
        centroid->elts[0] /= area;
        centroid->elts[1] /= area;
        centroid->elts[2] /= area;
        centroid->elts[3]  = 1.0f;
    }
    else
    {
        copy_vector_f(&centroid, pts[0]);
    }
}


uint32_t Polygon_f::generate_id() 
{ 
    static uint32_t next_id = 1; 
    return next_id++; 
}


Polygon_d::Polygon_d()
{
    normal = 0;
    centroid = 0;
    normal_flipped = 0;
    id = generate_id();
}


Polygon_d::Polygon_d(uint32_t N)
{
    pts.reserve(N);
    normal = 0;
    centroid = 0;
    normal_flipped = 0;
    id = generate_id();
}


Polygon_d::Polygon_d(const Polygon_d& p)
    : pts(p.pts.capacity())
{
    normal = 0;
    centroid = 0;
    *this = p;
}


Polygon_d::Polygon_d(const char* fname) 
    throw (jwscxx::base::Arg_error, jwscxx::base::IO_error)
{
    normal = 0;
    centroid = 0;
    normal_flipped = 0;

    jwscxx::base::Readable::read(fname);
}


Polygon_d::Polygon_d(std::istream& in) 
    throw (jwscxx::base::Arg_error, jwscxx::base::IO_error)
{
    normal = 0;
    centroid = 0;
    normal_flipped = 0;

    read(in);
}


Polygon_d::~Polygon_d()
{
    free_vector_d(normal);
    free_vector_d(centroid);

    for (size_t i = 0; i < pts.size(); i++)
    {
        free_vector_d(pts[ i ]);
    }
}


Polygon_d& Polygon_d::operator= (const Polygon_d& p)
{
    using namespace std;
    using namespace jwsc;

    size_t n = pts.size();
    size_t N = p.pts.size();

    if (n > N)
    {
        for (size_t i = N; i < n; i++)
        {
            free_vector_d(pts[ i ]);
        }
        pts.erase(pts.begin() + N, pts.end());
    }
    else
    {
        pts.reserve(N);
    }

    for (size_t i = 0; i < N; i++)
    {
        if (i < n)
        {
            copy_vector_d(&(pts[ i ]), p.pts[ i ]);
        }
        else
        {
            Vector_d* pt = 0;
            copy_vector_d(&pt, p.pts[ i ]);
            pts.push_back(pt);
        }
    }

    if (p.normal)
    {
        copy_vector_d(&normal, p.normal);
    }
    if (p.centroid)
    {
        copy_vector_d(&centroid, p.centroid);
    }

    normal_flipped = p.normal_flipped;

    id = p.id;

    return *this;
}


Polygon_d* Polygon_d::clone() const
{
    return new Polygon_d(*this);
}


void Polygon_d::read(std::istream& in) throw (jwscxx::base::IO_error,
        jwscxx::base::Arg_error)
{
    using namespace jwsc;

    in.read((char*)&id, sizeof(uint32_t));
    if (in.fail() || in.eof())
    {
        throw IO_error("Could not read id");
    }
#ifndef JWSC_BIGENDIAN
    bswap_u32(&(id));
#endif

    assert(sizeof(double) == sizeof(uint64_t));

    double x, y, z, w;

    // Number of points.
    uint32_t N;

    in.read((char*)&N, sizeof(uint32_t));
    if (in.fail() || in.eof())
    {
        throw IO_error("Could not read number of points");
    }

#ifndef JWSC_BIGENDIAN
    bswap_u32(&(N));
#endif

    // Reserve space in the pts vector.
    size_t n = pts.size();

    if (n > N)
    {
        for (size_t i = N; i < n; i++)
        {
            free_vector_d(pts[ i ]);
        }
        pts.erase(pts.begin() + N, pts.end());
    }
    else
    {
        pts.reserve(N);
    }


    // Points.
    for (size_t i = 0; i < N; i++)
    {
        in.read((char*)&x, sizeof(double));
        in.read((char*)&y, sizeof(double));
        in.read((char*)&z, sizeof(double));
        in.read((char*)&w, sizeof(double));
        if (in.fail() || in.eof())
        {
            throw IO_error("Could not read points");
        }

#ifndef JWSC_BIGENDIAN
        bswap_u64((uint64_t*)&(x));
        bswap_u64((uint64_t*)&(y));
        bswap_u64((uint64_t*)&(z));
        bswap_u64((uint64_t*)&(w));
#endif

        if (i < n)
        {
            pts[ i ]->elts[0] = x;
            pts[ i ]->elts[1] = y;
            pts[ i ]->elts[2] = z;
            pts[ i ]->elts[3] = w;
        }
        else
        {
            Vector_d* pt = 0;
            create_vector_d(&pt, 4);

            pt->elts[0] = x;
            pt->elts[1] = y;
            pt->elts[2] = z;
            pt->elts[3] = w;

            pts.push_back(pt);
        }
    }


    // Normal.
    if (N >= 3)
    {
        in.read((char*)&x, sizeof(double));
        in.read((char*)&y, sizeof(double));
        in.read((char*)&z, sizeof(double));
        in.read((char*)&w, sizeof(double));
        if (in.fail() || in.eof())
        {
            throw IO_error("Could not read normal");
        }

#ifndef JWSC_BIGENDIAN
        bswap_u64((uint64_t*)&(x));
        bswap_u64((uint64_t*)&(y));
        bswap_u64((uint64_t*)&(z));
        bswap_u64((uint64_t*)&(w));
#endif

        create_vector_d(&normal, 4);
        normal->elts[0] = x;
        normal->elts[1] = y;
        normal->elts[2] = z;
        normal->elts[3] = w;
    }


    // Centroid.
    if (N > 0)
    {
        in.read((char*)&x, sizeof(double));
        in.read((char*)&y, sizeof(double));
        in.read((char*)&z, sizeof(double));
        in.read((char*)&w, sizeof(double));
        if (in.fail() || in.eof())
        {
            throw IO_error("Could not read centroid");
        }

#ifndef JWSC_BIGENDIAN
        bswap_u64((uint64_t*)&(x));
        bswap_u64((uint64_t*)&(y));
        bswap_u64((uint64_t*)&(z));
        bswap_u64((uint64_t*)&(w));
#endif

        create_vector_d(&centroid, 4);
        centroid->elts[0] = x;
        centroid->elts[1] = y;
        centroid->elts[2] = z;
        centroid->elts[3] = w;
    }

    in.read((char*)&normal_flipped, sizeof(uint8_t));
}


void Polygon_d::write(std::ostream& out) const throw (jwscxx::base::IO_error)
{
    using namespace jwsc;

    uint32_t id_cp = id;
#ifndef JWSC_BIGENDIAN
    bswap_u32(&(id_cp));
#endif
    out.write((char*)&id_cp, sizeof(uint32_t));
    if (out.fail() || out.eof())
    {
        throw IO_error("Could not write id");
    }

    assert(sizeof(double) == sizeof(uint64_t));

    double x, y, z, w;

    // Number of points.
    uint32_t N = pts.size();

#ifndef JWSC_BIGENDIAN
    bswap_u32(&(N));
#endif

    out.write((char*)&N, sizeof(uint32_t));
    if (out.fail() || out.eof())
    {
        throw IO_error("Could not write number of points");
    }


    // Points.
    for (size_t i = 0; i < pts.size(); i++)
    {
        Vector_d* pt = pts[ i ];

        x = pt->elts[0];
        y = pt->elts[1];
        z = pt->elts[2];
        w = pt->elts[3];

#ifndef JWSC_BIGENDIAN
        bswap_u64((uint64_t*)&(x));
        bswap_u64((uint64_t*)&(y));
        bswap_u64((uint64_t*)&(z));
        bswap_u64((uint64_t*)&(w));
#endif

        out.write((char*)&x, sizeof(double));
        out.write((char*)&y, sizeof(double));
        out.write((char*)&z, sizeof(double));
        out.write((char*)&w, sizeof(double));
        if (out.fail() || out.eof())
        {
            throw IO_error("Could not write points");
        }
    }


    // Normal.
    if (N >= 3)
    {
        assert(normal);

        x = normal->elts[0];
        y = normal->elts[1];
        z = normal->elts[2];
        w = normal->elts[3];

#ifndef JWSC_BIGENDIAN
        bswap_u64((uint64_t*)&(x));
        bswap_u64((uint64_t*)&(y));
        bswap_u64((uint64_t*)&(z));
        bswap_u64((uint64_t*)&(w));
#endif

        out.write((char*)&x, sizeof(double));
        out.write((char*)&y, sizeof(double));
        out.write((char*)&z, sizeof(double));
        out.write((char*)&w, sizeof(double));
        if (out.fail() || out.eof())
        {
            throw IO_error("Could not write normal");
        }
    }


    // Centroid.
    if (N > 0)
    {
        assert(centroid);

        x = centroid->elts[0];
        y = centroid->elts[1];
        z = centroid->elts[2];
        w = centroid->elts[3];

#ifndef JWSC_BIGENDIAN
        bswap_u64((uint64_t*)&(x));
        bswap_u64((uint64_t*)&(y));
        bswap_u64((uint64_t*)&(z));
        bswap_u64((uint64_t*)&(w));
#endif

        out.write((char*)&x, sizeof(double));
        out.write((char*)&y, sizeof(double));
        out.write((char*)&z, sizeof(double));
        out.write((char*)&w, sizeof(double));
        if (out.fail() || out.eof())
        {
            throw IO_error("Could not write centroid");
        }
    }

    out.write((char*)&normal_flipped, sizeof(uint8_t));
}


void Polygon_d::transform(const jwsc::Matrix_d* M) 
    throw (jwscxx::base::Arg_error)
{
    using namespace jwsc;

    if (M->num_rows != 4 && M->num_cols != 4)
    {
        throw Arg_error("Transformation matrix not homogeneous");
    }

    size_t N = pts.size();

    // Transform the points.
    for (size_t i = 0; i < N; i++)
    {
        Vector_d* pt = pts[ i ];

        multiply_matrix_and_vector_d(&pt, M, pt);
    }

    // Transform the centroid.
    multiply_matrix_and_vector_d(&centroid, M, centroid);

    update_normal();
}


void Polygon_d::wire_render() const
{
    using namespace jwsc;

    if (normal)
    {
        assert(fabs(normal->elts[3] - 1.0) < 1.0e-8);
        glNormal3d(normal->elts[0], normal->elts[1], normal->elts[2]);
    }

    Vector_d* pt;

    glBegin(GL_LINE_STRIP);
    for (size_t i = 0; i < pts.size(); i++)
    {
        pt = pts[ i ];

        glVertex4d(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    if (pts.size() > 0)
    {
        pt = pts[0];
        glVertex4d(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    glEnd();
}


void Polygon_d::wire_occlude_render() const
{
    using namespace jwsc;

    glDisable(GL_DEPTH_TEST);
    glEnable(GL_STENCIL_TEST);
    glClear(GL_STENCIL_BUFFER_BIT);
    glStencilFunc(GL_ALWAYS, 1, 1);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);

    GLboolean color_mask[4];
    glGetBooleanv(GL_COLOR_WRITEMASK, color_mask);
    glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);

    Vector_d* pt;

    glBegin(GL_LINE_STRIP);
    for (size_t i = 0; i < pts.size(); i++)
    {
        pt = pts[ i ];

        glVertex4d(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    if (pts.size() > 0)
    {
        pt = pts[0];
        glVertex4d(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    glEnd();


    glStencilFunc(GL_NOTEQUAL, 1, 1);
    glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);

    glEnable(GL_DEPTH_TEST);

    glBegin(GL_POLYGON);
    for (size_t i = 0; i < pts.size(); i++)
    {
        pt = pts[ i ];

        glVertex4d(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    glEnd();

    glDisable(GL_STENCIL_TEST);

    glColorMask(color_mask[0], color_mask[1], color_mask[2], color_mask[3]);
}

void Polygon_d::wire_occluded_silhouette_render(std::vector<bool> & iVector, bool &visible) const
{
    using namespace jwsc;
    
    assert(iVector.size() == pts.size());

    Vector_d* pt;

    glDisable(GL_DEPTH_TEST);
    glEnable(GL_STENCIL_TEST);
    glClear(GL_STENCIL_BUFFER_BIT);
    glStencilFunc(GL_ALWAYS, 1, 1);
    glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);

    GLboolean color_mask[4];
    glGetBooleanv(GL_COLOR_WRITEMASK, color_mask);
    glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);

    glBegin(GL_LINES);
    uint32_t i = 0;
    for(std::vector<bool>::const_iterator it = iVector.begin(); it != iVector.end(); it++,i++)
    {
        if(*it)
        {
            //If it is the last edge, we need to render the last and the first point of the polygon
            int j = ( (i+1) == pts.size()) ? 0 : (i+1);
            glVertex4d(pts[i]->elts[0], pts[i]->elts[1], pts[i]->elts[2], pts[i]->elts[3]);
            glVertex4d(pts[j]->elts[0], pts[j]->elts[1], pts[j]->elts[2], pts[j]->elts[3]);
        }
    }
    glEnd();


    glStencilFunc(GL_NOTEQUAL, 1, 1);
    glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
    glEnable(GL_DEPTH_TEST);

    glBegin(GL_POLYGON);
    if(visible)
    {
    for (size_t i = 0; i < pts.size(); i++)
        {
            pt = pts[ i ];
            glVertex4d(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
        }
    }
    glEnd();

    glDisable(GL_STENCIL_TEST);

    glColorMask(color_mask[0], color_mask[1], color_mask[2], color_mask[3]);
}


void Polygon_d::solid_render() const
{
    using namespace jwsc;

    if (normal)
    {
        assert(fabs(normal->elts[3] - 1.0) < 1.0e-8);
        glNormal3d(normal->elts[0], normal->elts[1], normal->elts[2]);
    }

    glBegin(GL_POLYGON);
    for (size_t i = 0; i < pts.size(); i++)
    {
        Vector_d* pt = pts[ i ];

        glVertex4d(pt->elts[0], pt->elts[1], pt->elts[2], pt->elts[3]);
    }
    glEnd();
}


void Polygon_d::add_point(double x, double y, double z) 
    throw (jwscxx::base::Arg_error)
{
    using namespace jwsc;

    Vector_d* pt = 0;

    create_vector_d(&pt, 4);
    pt->elts[0] = x;
    pt->elts[1] = y;
    pt->elts[2] = z;
    pt->elts[3] = 1.0;

    add_point(pt);
}


void Polygon_d::add_point(jwsc::Vector_d* pt) throw (jwscxx::base::Arg_error)
{
    using namespace jwsc;

    if (pt->num_elts != 4)
    {
        throw Arg_error("Point to add not homogeneous");
    }
    else if (fabs(pt->elts[3]) < 1.0e-10)
    {
        throw Arg_error("Homogeneous coordinate is zero");
    }

    size_t N = pts.size();

    pts.push_back(pt);

    if (N >= 3)
    {
        assert(normal);

        double dp1, dp2;

        calc_vector_dot_prod_d(&dp1, normal, pts[0]);
        calc_vector_dot_prod_d(&dp2, normal, pt);

        if (fabs(dp1 - dp2) > 1.0e-4)
        {
            throw Arg_error("Point to add not in the polygon's plane");
        }
    }
    else if (N == 2)
    {
        update_normal();
    }

    update_centroid();
}


const jwsc::Vector_d* Polygon_d::get_normal() const 
{ 
    return normal; 
}


const jwsc::Vector_d* Polygon_d::get_centroid() const 
{ 
    return centroid; 
}


size_t Polygon_d::get_num_points() const 
{ 
    return pts.size(); 
}


const jwsc::Vector_d* Polygon_d::get_point(size_t i) const 
    throw (jwscxx::base::Arg_error)
{ 
    if (i >= pts.size())
    { 
        throw jwscxx::base::Arg_error("Invalid point index"); 
    }

    return static_cast<const jwsc::Vector_d*>(pts[ i ]);
}


void Polygon_d::flip_normal() 
{ 
    normal_flipped = !normal_flipped; 
    update_normal(); 
}


void Polygon_d::project()
{
    using namespace jwsc;

    GLdouble glM[16] = {0};

    glGetDoublev(GL_MODELVIEW_MATRIX, glM);

    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glMultMatrixd(glM);
    glGetDoublev(GL_PROJECTION_MATRIX, glM);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);

    Matrix_d* M = 0;
    create_matrix_d(&M, 4, 4);
    for (size_t row = 0; row < 4; row++)
    {
        for (size_t col = 0; col < 4; col++)
        {
            M->elts[ row ][ col ] = glM[ col*4 + row  ];
        }
    }

    double width  = Camera_d::get_gl_viewport_width();
    double height = Camera_d::get_gl_viewport_height();

    for (size_t i = 0; i < pts.size(); i++)
    {
        Vector_d* pt = pts[ i ];

        multiply_matrix_and_vector_d(&pt, M, pt);

        double w = 1.0 / pt->elts[ 3 ];
        pt->elts[ 0 ] = 0.5*width* (pt->elts[ 0 ]*w + 1.0);
        pt->elts[ 1 ] = 0.5*height*(pt->elts[ 1 ]*w + 1.0);
        pt->elts[ 2 ] = 0;
        pt->elts[ 3 ] = 1.0;
    }

    free_matrix_d(M);
}


uint8_t Polygon_d::is_visible(const Camera_d* camera) const
{
    if (!normal)
        return 0;

    const jwsc::Vector_d* prp = camera->get_prp();

    double x = prp->elts[0] - centroid->elts[0];
    double y = prp->elts[1] - centroid->elts[1];
    double z = prp->elts[2] - centroid->elts[2];

    if ((x*normal->elts[0] + y*normal->elts[1] + z*normal->elts[2]) > 0)
        return 1;
    return 0;
}

uint8_t Polygon_d::is_visible_up_to_epsilon(const Camera_d* camera, double & epsilon) const
{
    if (!normal)
        return 0;

    const jwsc::Vector_d* prp = camera->get_prp();

    double x = prp->elts[0] - centroid->elts[0];
    double y = prp->elts[1] - centroid->elts[1];
    double z = prp->elts[2] - centroid->elts[2];

    double dp = ((double)(x*normal->elts[0] + y*normal->elts[1] + z*normal->elts[2]));
    
    return (dp > epsilon);
}


uint32_t Polygon_d::get_id() const 
{ 
    return id; 
}


void Polygon_d::set_id(uint32_t id) 
{ 
    this->id = id; 
}


void Polygon_d::update_normal()
{
    using namespace jwsc;

    size_t N = pts.size();

    if (N < 3)
        return;

    Vector_d* p1 = pts[0];
    Vector_d* p2 = pts[1];
    Vector_d* pN = pts[N-1];

    double w1 = pts[0]->elts[3];
    double w2 = pts[1]->elts[3];
    double wN = pts[N-1]->elts[3];

    assert(fabs(w1) >= 1.0e-10);
    assert(fabs(w2) >= 1.0e-10);
    assert(fabs(wN) >= 1.0e-10);

    w1 = 1.0 / w1;
    w2 = 1.0 / w2;
    wN = 1.0 / wN;

    // p2 - p1
    double v1_x = p2->elts[0]*w2 - p1->elts[0]*w1;
    double v1_y = p2->elts[1]*w2 - p1->elts[1]*w1;
    double v1_z = p2->elts[2]*w2 - p1->elts[2]*w1;

    // pN - p1
    double v2_x = pN->elts[0]*wN - p1->elts[0]*w1;
    double v2_y = pN->elts[1]*wN - p1->elts[1]*w1;
    double v2_z = pN->elts[2]*wN - p1->elts[2]*w1;

    // v1 x v2
    create_vector_d(&normal, 4);
    normal->elts[0] = v1_y*v2_z - v1_z*v2_y;
    normal->elts[1] = v1_z*v2_x - v1_x*v2_z;
    normal->elts[2] = v1_x*v2_y - v1_y*v2_x;
    normal->elts[3] = 0.0;

    if (normal_flipped)
    {
        normal->elts[0] *= -1;
        normal->elts[1] *= -1;
        normal->elts[2] *= -1;
    }

    normalize_vector_mag_d(&normal, normal);
    normal->elts[3] = 1.0;
}


void Polygon_d::update_centroid()
{
    using namespace jwsc;

    size_t N = pts.size();
    double area = 0;

    if (N == 0)
    {
        return;
    }
    else if (N == 1)
    {
        copy_vector_d(&centroid, pts[0]);
        return;
    }
    else if (N == 2)
    {
        double x = (pts[0]->elts[0]/pts[0]->elts[3] + 
                    pts[1]->elts[0]/pts[1]->elts[3]) * 0.5;
        double y = (pts[0]->elts[1]/pts[0]->elts[3] + 
                    pts[1]->elts[1]/pts[1]->elts[3]) * 0.5;
        double z = (pts[0]->elts[2]/pts[0]->elts[3] + 
                    pts[1]->elts[2]/pts[1]->elts[3]) * 0.5;
        create_zero_vector_d(&centroid, 4);
        centroid->elts[0] = x;
        centroid->elts[1] = y;
        centroid->elts[2] = z;
        centroid->elts[3] = 1.0;
        return;
    }

    double x3 = pts[N-1]->elts[0];
    double y3 = pts[N-1]->elts[1];
    double z3 = pts[N-1]->elts[2];

    for (size_t i = 0; i < N - 2; i++)
    {
        double x1 = pts[i]->elts[0];
        double y1 = pts[i]->elts[1];
        double z1 = pts[i]->elts[2];

        double x2 = pts[i+1]->elts[0];
        double y2 = pts[i+1]->elts[1];
        double z2 = pts[i+1]->elts[2];

        double dot = (x2 - x1)*(x3 - x1) + 
                     (y2 - y1)*(y3 - y1) + 
                     (z2 - z1)*(z3 - z1);

        double base = sqrt((x2 - x1)*(x2 - x1) + 
                           (y2 - y1)*(y2 - y1) + 
                           (z2 - z1)*(z2 - z1));

        double height;

        if (base < 1.0e-8)
        {
            height = 0;
        }
        else 
        {
            double alpha = dot / (base*base);

            double a = x3 - x1 - alpha*(x2 - x1);
            double b = y3 - y1 - alpha*(y2 - y1);
            double c = z3 - z1 - alpha*(z2 - z1);

            height = sqrt(a*a + b*b + c*c);
        }

        double tri_area = 0.5 * base * height;

        area += tri_area;
        centroid->elts[0] += tri_area * (x1 + x2 + x3) / 3.0;
        centroid->elts[1] += tri_area * (y1 + y2 + y3) / 3.0;
        centroid->elts[2] += tri_area * (z1 + z2 + z3) / 3.0;
    }

    if (area > 1.0e-8)
    {
        centroid->elts[0] /= area;
        centroid->elts[1] /= area;
        centroid->elts[2] /= area;
        centroid->elts[3]  = 1.0;
    }
    else
    {
        copy_vector_d(&centroid, pts[0]);
    }
}


uint32_t Polygon_d::generate_id() 
{ 
    static uint32_t next_id = 1; 
    return next_id++; 
}