CompGeom.h

/****************************************************************************
 * Copyright 2006 Evan Drumwright
 * This library is distributed under the terms of the GNU General Public 
 * License (found in COPYING).
 ****************************************************************************/

#ifndef _COMPGEOM_H
#define _COMPGEOM_H

#include <pthread.h>
#include <iostream>
#include <vector>
#include <map>
#include <list>
#include <boost/shared_ptr.hpp>
#include <Physsim/Constants.h>
#include <Physsim/Triangle.h>
#include <Physsim/Vector2.h>
#include <Physsim/Polyhedron.h>

// needed for qhull
extern "C"
{
        #include <qhull/qhull.h>
        #include <qhull/qset.h>
        #include <qhull/user.h>
}

namespace Physsim {

class Polyhedron;       
        
class CompGeom
{
        public:
                enum SegSegIntersectType { SegSegNoIntersect, SegSegIntersect, SegSegVertex, SegSegEdge };
                enum SegTriIntersectType { SegTriNoIntersect, SegTriInclFace, SegTriInclVertex, SegTriInclEdge, SegTriInside, SegTriVertex, SegTriEdge, SegTriFace, SegTriEdgeOverlap, SegTriPlanarIntersect };
                enum SegPlaneIntersectType { SegPlaneInPlane, SegPlaneToSide, SegPlaneFirst, SegPlaneSecond, SegPlaneOtherIntersect };
                enum PolygonLocation { PolygonInside, PolygonOutside, PolygonOnVertex, PolygonOnEdge };
                enum SegBoxIntersectType { SegBoxNoIntersect, SegBoxFullyInside, SegBoxPartiallyInside, SegBoxSurfaceIntersect, SegBoxPointIntersect, SegBoxPassThrough };      

                static Vector3 generate_point_on_plane(const Vector3& normal, Real d);
                static SegBoxIntersectType intersect_seg_box(const Vector3& lo, const Vector3& hi, const std::pair<Vector3, Vector3>& seg, Vector3& isect1, Vector3& isect2);
                static bool is_convex_polygon(const std::list<Vector3>& polygon, const Vector3& normal);
                static std::list<TrianglePtr> triangulate_convex_polygon(const std::list<Vector3>& polygon);
                static bool intersect_noncoplanar_tris(const Triangle& t1, const Triangle& t2, Vector3& p1, Vector3& p2);
                static boost::shared_ptr<std::list<Vector3> > intersect_tris(const Triangle& t1, const Triangle& t2);
                static Real fit_plane(const std::list<Vector3>& points, Vector3& normal, Real& d);
                static SegTriIntersectType intersect_seg_tri(const Triangle& t, const std::pair<Vector3, Vector3>& seg, Vector3& isect, Vector3& isect2);
                static SegSegIntersectType intersect_segs_3D(const std::pair<Vector3, Vector3>& s1, const std::pair<Vector3, Vector3>& s2, Vector3& isect, Vector3& isect2);
                static SegPlaneIntersectType intersect_seg_plane(const Triangle& tri, const std::pair<Vector3, Vector3>& seg, Vector3& isect, unsigned int& big_dim);
                static boost::shared_ptr<std::list<Vector3> > calc_convex_hull_2D(const std::list<Vector3>& points, const VectorN& normal);
                static PolyhedronPtr calc_convex_hull_3D(const std::list<Vector3>& points);
                static boost::shared_ptr<std::list<Vector3> > intersect_coplanar_tris(const Triangle& t1, const Triangle& t2, const Vector3& normal, Real d);
                static SegSegIntersectType intersect_segs_2D(const std::pair<Vector2, Vector2>& s1, const std::pair<Vector2, Vector2>& s2, Vector2& isect, Vector2& isect2);
                static PolygonLocation polygon_location(const std::list<Vector2>& polygon, const Vector2& point);
                static bool collinear(const Vector3& a, const Vector3& b, const Vector3& c);
                static bool collinear(const Vector2& a, const Vector2& b, const Vector2& c);
                static LongReal volume(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d);
                static int volume_sign(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d);
                static Vector3 calc_centroid_2D(const std::list<Vector3>& poly, const Vector3& normal);
                static void project_plane(std::list<Vector3>& points, const Vector3& normal, Real d);
                static bool coplanar(const Triangle& t1, const Triangle& t2);
                static Real calc_area(const std::list<Vector3>& poly, const Vector3& normal);   
                static Vector3 to_3D(const Vector2& v);
                static Vector3 to_3D(const Vector2& v, Real d);
                static Vector2 to_2D(const Vector3& v);

                static void det_line_endpoints(std::list<Vector3>& points, std::pair<Vector3, Vector3>& endpoints);
                static void det_line_endpoints(std::list<Vector2>& points, std::pair<Vector2, Vector2>& endpoints);
                static bool query_intersect_tri_tri(const Triangle& t1, const Triangle& t2);

                template <class ForwardIterator>
                static bool collinear(ForwardIterator first, ForwardIterator last);

                template <class ForwardIterator>
                static PolyhedronPtr calc_convex_hull_3D(ForwardIterator first, ForwardIterator last);

                template <class ForwardIterator>
                static Vector3 calc_centroid(ForwardIterator first, ForwardIterator last);
                
        private:
                static pthread_mutex_t _qhull_mutex;
                static bool compute_intervals_isectline(const Triangle& t1, Real vv0, Real vv1, Real vv2, Real d0, Real d1, Real d2, Real d0d1, Real d0d2, Real& isect0, Real& isect1, Vector3& isectpoint0, Vector3& isectpoint1);
                static void isect2X(const Vector3& vtx0, const Vector3& vtx1, const Vector3& vtx2, Real vv0, Real vv1, Real vv2, Real d0, Real d1, Real d2, Real& isect0, Real& isect1, Vector3& isectpoint0, Vector3& isectpoint1);
                static SegSegIntersectType get_parallel_intersect_type(const std::pair<Vector2, Vector2>& s1, const std::pair<Vector2, Vector2>& s2, Vector2& isect, Vector2& isect2);
                static bool between(const Vector2& a, const Vector2& b, const Vector2& c);
                static unsigned advance(unsigned a, unsigned* aa, unsigned n, bool inside, const Vector2& v, std::list<Vector3>& poly, const Matrix3& RT, Real d);
                static SegTriIntersectType seg_tri_cross(const Triangle& t, const Vector3& q, const Vector3& r);
                static SegTriIntersectType in_tri_3D(const Triangle& t, const Vector3& p, unsigned int skip_dim);
                static SegTriIntersectType in_tri_2D(const Vector2& ta, const Vector2& tb, const Vector2& tc, const Vector2& p);
                static void find_plane_coeff(const Triangle& t, Vector3& N, unsigned int& m, Real& dot);
                static VectorN normal_vec(const Vector3& a, const Vector3& b, const Vector3& c);
                static int area_sign(const Vector2& a, const Vector2& b, const Vector2& c);
                static bool test_edge_edge(const Vector3& p, const Vector3& a, const Vector3& b, Real Ax, Real Ay, unsigned i0, unsigned i1);
                static bool test_edge_tri(const Vector3& a, const Vector3& b, const Triangle& t, unsigned i0, unsigned i1);
                static bool test_point_in_tri(const Vector3& p, const Triangle& t, unsigned i0, unsigned i1);
                static bool test_coplanar_tri_tri(const Vector3& N, const Triangle& t1, const Triangle& t2);
}; // end class

/**************************************************************
 * Template methods follow
 *************************************************************/

template <class ForwardIterator>
bool CompGeom::collinear(ForwardIterator first, ForwardIterator last)
{  
        for (ForwardIterator i = first; i != last; i++)
        {
                ForwardIterator j = i;
                j++;
                for (; j != last; j++)
                {
                        ForwardIterator k = j;
                        k++;
                        for (; k != last; k++)
                                if (!collinear(*i, *j, *k))
                                        return false;
                }
        }
        
        return true;
}


template <class ForwardIterator>
Vector3 CompGeom::calc_centroid(ForwardIterator first, ForwardIterator last)
{
        Real area_sum = 0;
        
        // compute the area of each facet and contribution from each facet
        Vector3 centroid = ZEROS_3;
        for (ForwardIterator i = first; i != last; i++)
        {
                Real area = (*i)->get_area();
                area_sum += area;
                centroid += area * (*(*i)->a() + *(*i)->b() + *(*i)->c());
        }
        
        // divide the centroid by 3*area_sum
        centroid /= (area_sum * 3);
        
        return centroid;
}


template <class ForwardIterator>
PolyhedronPtr CompGeom::calc_convex_hull_3D(ForwardIterator first, ForwardIterator last)
{
        const unsigned X = 0, Y = 1, Z = 2;
        FILE* outfile=NULL;
        FILE* errfile=stderr;
        int exit_code;
        int curlong, totlong;

        // setup qhull output (standard -- error always goes to stderr)
        #ifdef _DEBUG_COMPGEOM_
        outfile = stdout;
        #endif

        // determine how many points we are processing
        unsigned sz = 0;
        for (ForwardIterator i = first; i != last; i++)
                sz++;

        // setup constants
        const int DIM = 3;
        const int N_POINTS = sz;
        const boolT IS_MALLOC = false;
        char flags[] = "qhull ";

        // make sure there are enough points
        assert(N_POINTS >= 4);
        
        // setup the points
        coordT* qhull_points = new coordT[N_POINTS*DIM];
        unsigned j=0;
        for (ForwardIterator i = first; i != last; i++)
        {
                qhull_points[j] = (*i)[X];
                qhull_points[j+1] = (*i)[Y];
                qhull_points[j+2] = (*i)[Z];
                j += DIM;
        }
        
        #ifdef _DEBUG_COMPGEOM_
        std::cout << "computing 3D convex hull of: " << std::endl;
        for (ForwardIterator i = first; i != last; i++)
                std::cout << *i << std::endl;
        #endif

        // lock the qhull mutex -- qhull is non-reentrant
        pthread_mutex_lock(&_qhull_mutex);
        
        // execute qhull        
        exit_code = qh_new_qhull(DIM, N_POINTS, qhull_points, IS_MALLOC, flags, outfile, errfile);
        if (exit_code)
        {
                // qhull failed -- perhaps the dimensionality is 2 rather than 3?
                pthread_mutex_unlock(&_qhull_mutex);
                return PolyhedronPtr();
        }

        // construct a new mesh from the facets
        std::vector<TriangleConstPtr> mesh;

        // get all of the vertices
        std::map<vertexT*, Vector3Ptr > vertices;
        for (vertexT* vertex=qh vertex_list;vertex && vertex->next;vertex= vertex->next)
        {
                Vector3Ptr v(new Vector3);
                for (unsigned i=0; i< (unsigned) DIM; i++)
                        (*v)[i] = vertex->point[i];
                vertices[vertex] = v;
        }
        
        // get the facet information
        for (facetT* facet=qh facet_list;facet && facet->next;facet=facet->next)
        {
                if (!facet->vertices)
                        continue;

                // setup an array of vertices for the facet
                std::vector<Vector3Ptr> facet_vertices;
                
                // get all vertices in the facet
                vertexT* vertex;
                for (vertexT** vertex_pointer = (vertexT**)& ((facet->vertices)->e[0].p); (vertex = (*vertex_pointer++));)
                        facet_vertices.push_back(vertices[vertex]);

                // triangulate the facet; try to use only the original edges first
                bool colin = false;
                std::list<TrianglePtr > new_tris;
                for (unsigned i=1; i< facet_vertices.size()-1; i++)
                {
                        // check whether the the three points are collinear
                        if (collinear(*facet_vertices[i], *facet_vertices[i+1], *facet_vertices.front()))
                        {
                                colin = true;
                                new_tris.clear();
                                break;
                        }
                        new_tris.push_back(TrianglePtr(new Triangle(facet_vertices[i], facet_vertices[i+1], facet_vertices.front())));
                }       
                
                // if it is collinear, create a new point at the centroid of the points
                if (colin)
                {
                        // compute the centroid
                        Vector3Ptr centroid(new Vector3(ZEROS_3));
                        for (unsigned i=0; i< facet_vertices.size(); i++)
                                (*centroid) += *facet_vertices[i];
                        (*centroid) /= facet_vertices.size();
                        
                        // now make the new triangles -- first, add the first vertex twice for closure of the polygon
                        facet_vertices.push_back(facet_vertices.front());
                        for (unsigned i=0; i< facet_vertices.size()-1; i++)
                                new_tris.push_back(TrianglePtr(new Triangle(facet_vertices[i], facet_vertices[i+1], centroid)));
                }
                
                // get one normal
                Vector3 normal = new_tris.front()->normal();
                
                // determine whether the normal is inverted; if so, reverse ordering on all triangles
                Vector3 true_normal(facet->normal[X], facet->normal[Y], facet->normal[Z]);
                if (Vector3::dot(normal, true_normal) < 0)
                        for (std::list<TrianglePtr >::iterator i = new_tris.begin(); i != new_tris.end(); i++)
                        {
                                Vector3ConstPtr a = (*i)->a();
                                (*i)->set_a((*i)->b());
                                (*i)->set_b(a);
                        }
                        
                // add all triangles to the mesh
                while (!new_tris.empty())
                {
                        mesh.push_back(new_tris.front());
                        new_tris.pop_front();
                }
        }
        
        // free memory for the points
        delete [] qhull_points;
        
        // free qhull memory
        qh_freeqhull(!qh_ALL);
        qh_memfreeshort(&curlong, &totlong);
        assert(!curlong && !totlong);
        
        // release the qhull mutex
        pthread_mutex_unlock(&_qhull_mutex);
        
        // if the there aren't enough triangles, can't create the polyhedron
        assert(mesh.size() >= 4);
        
        // create the polyhedron
        PolyhedronPtr polyhedron(new Polyhedron(mesh));
/*
        #ifndef NDEBUG
        if (!polyhedron->is_convex())
        {
                std::ofstream out_nc("non_convex.wrl");
                out_nc << "#VRML V2.0 utf8" << std::endl;
                Polyhedron::to_VRML(out_nc, *polyhedron, Vector3(1,0,0), false); 
                out_nc.close();
                assert(false);  
        }
        #endif
*/      
        #ifdef _DEBUG_COMPGEOM_
        std::cout << "3D convex hull is:" << std::endl << *polyhedron;
        #endif

        return polyhedron;      
}

} // end namespace

#endif

---