// triobject.cpp -- terrain object made of triangles
//
// Written by Frederic Bouvier, started June 2002.
//
// Copyright (C) 2002  Frederic Bouvier - fredb@users.sourceforge.net
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id: triobject.cpp,v 1.23 2005/06/25 16:31:14 fredb2 Exp $

#ifdef _MSC_VER
#pragma warning ( disable : 4786 4800 4503 )
#endif

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#include <algorithm>
#include <set>
//#include <limits>
#include <iostream>
#include <fstream>
#include <iomanip>

#include <simgear/math/polar3d.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/io/sg_binobj.hxx>
#include <simgear/misc/texcoord.hxx>

#include <FL/gl.h>
#include "triobject.hpp"
#include "tile.hpp"
#include "preferences.hpp"
#include "sdutil.hpp"

extern FGSD_Preferences generalPreferences;

static const double FGSD_EPSILON = 0.000005;
static unsigned char colors[8][3] = {
   { 255, 0, 0 },
   { 128, 0, 0 },
   { 0, 128, 0 },
   { 128, 128, 0 },
   { 255, 128, 0 },
   { 0, 255, 0 },
   { 128, 255, 0 },
   { 255, 255, 0 }
};

FGSD_TriangleObject::MaterialMap FGSD_TriangleObject::_materialMap;
FGSD_TriangleObject::MaterialList FGSD_TriangleObject::_materialList;
std::vector<std::string> FGSD_TriangleObject::_objectName;
std::map<std::string,size_t> FGSD_TriangleObject::_objectNameMap;
std::vector<std::vector<FGSD_TriangleObject *> > FGSD_TriangleObject::_objectSpan;
FGSD_TriangleObject::Point_set FGSD_TriangleObject::_pointSet;
std::vector<FGSD_TriangleObject::Polyhedron *> FGSD_TriangleObject::_outSurfaceList;
std::vector<size_t> FGSD_TriangleObject::_outObjectList;

FGSD_TriangleObject::FGSD_TriangleObject( FGSD_Tile *__tile )
   :  FGSD_BaseObject( __tile, FGSD_BaseObject::TriangleObject )
   ,  _vminx(  1000.0 )
   ,  _vmaxx( -1000.0 )
   ,  _vminy(  1000.0 )
   ,  _vmaxy( -1000.0 )
{
   SGBucket b = __tile->getBucket();
   double w = b.get_width();
   double h = b.get_height();
   _minx = b.get_chunk_lon() + b.get_x() * w;
   _maxx = _minx + w;
   _miny = b.get_chunk_lat() + b.get_y() * h;
   _maxy = _miny + h;
}

FGSD_TriangleObject::~FGSD_TriangleObject()
{
}

void FGSD_TriangleObject::LoadObject::operator()( HalfedgeDS& hds )
{
   CGAL::Polyhedron_incremental_builder_3<HalfedgeDS> b( hds, true );
   typedef HalfedgeDS::Vertex Vertex;
   typedef Vertex::Point Point;

   size_t i, nb, nbFaces = 0;
   const group_list &tris_v = object.get_tris_v();
   nb = tris_v.size();
   for ( i = 0; i < nb; i++ )
      nbFaces += tris_v[i].size() / 3;

   const group_list &strips_v = object.get_strips_v();
   nb = strips_v.size();
   for ( i = 0; i < nb; i++ )
      nbFaces += strips_v[i].size() - 2;

   const group_list &fans_v = object.get_fans_v();
   nb = fans_v.size();
   for ( i = 0; i < nb; i++ )
      nbFaces += fans_v[i].size() - 2;

   const point_list &wgs84_nodes = object.get_wgs84_nodes();
   const point_list &texcoords = object.get_texcoords();

   std::vector<ErrorCondition> errors;

   bool error;
   do {
      error = false;
      b.begin_surface( wgs84_nodes.size(), nbFaces );

      Point3D gbs_center = object.get_gbs_center();
      double gbs_radius = object.get_gbs_radius();

      nb = wgs84_nodes.size();
      for ( i = 0; i < nb; i++ ) {
         Point3D cart = wgs84_nodes[ i ];
         cart += gbs_center;

         Point3D polar = sgCartToPolar3d( cart );
         Point3D geod;
         geod.setlon( polar.lon() * 180 / SG_PI );
         double lat, elev, sea_level;
         sgGeocToGeod( polar.lat(), polar.radius(), &lat, &elev, &sea_level );
         geod.setlat( lat * 180 / SG_PI );
         geod.setelev( elev );

         if ( tri._vminx > geod.x() ) tri._vminx = geod.x();
         if ( tri._vmaxx < geod.x() ) tri._vmaxx = geod.x();
         if ( tri._vminy > geod.y() ) tri._vminy = geod.y();
         if ( tri._vmaxy < geod.y() ) tri._vmaxy = geod.y();

         b.add_vertex( Point( geod.x(), geod.y(), geod.z() ) );
      }

      const string_list &tri_materials = object.get_tri_materials();
      const group_list &tris_tc = object.get_tris_tc();
      nb = tris_v.size();
      for ( i = 0; i < nb && !error; i++ ) {
         size_t mat = tri.findMaterial( tri_materials[i] );
         size_t n = tris_v[i].size();
         for ( size_t j = 0; j < n && !error; j += 3 ) {
            bool skip = false;
            for ( size_t k = 0; k < errors.size() && !skip; k++ ) {
               skip = ( errors[k].type == ErrorCondition::Tris && errors[k].i1 == i && errors[k].i2 == j );
            }

            if ( !skip ) {
               size_t i1 = tris_v[i][j],
                      i2 = tris_v[i][j+1],
                      i3 = tris_v[i][j+2];
               if ( i1 != i2 && i2 != i3 && i3 != i1 ) {
                  b.begin_facet();
                  b.add_vertex_to_facet( i1 );
                  b.add_vertex_to_facet( i2 );
                  b.add_vertex_to_facet( i3 );
                  Polyhedron::Halfedge_handle h = b.end_facet();
                  if ( h != Polyhedron::Halfedge_handle() ) {
                     Polyhedron::Facet_handle f = h->facet();
                     f->material = mat;
                     f->object = iObj;
                     for ( size_t k = 0; k < 3; k++ ) {
                        Point3D p = texcoords[ tris_tc[i][j+k] ];
                        f->tex_u[k] = p.x();
                        f->tex_v[k] = p.y();
                     }
                  } else {
                     ErrorCondition e;
                     e.type = ErrorCondition::Tris;
                     e.i1 = i;
                     e.i2 = j;
                     errors.push_back( e );
                     error = true;
                     if ( iObj != (size_t)-1 ) {
                        std::cerr << "Object : " << _objectName[ iObj ] << std::endl;
                     } else {
                        std::cerr << "Terrain" << std::endl;
                     }
                  }
               }
            }
         }
      }

      const string_list &fan_materials = object.get_fan_materials();
      const group_list &fans_tc = object.get_fans_tc();
      nb = fans_v.size();
      for ( i = 0; i < nb && !error; i++ ) {
         size_t mat = tri.findMaterial( fan_materials[i] );
         size_t n = fans_v[i].size();
         size_t p0 = fans_v[i][0];
         size_t p1 = fans_v[i][1],
                i1 = 1;
         for ( size_t j = 2; j < n && !error; j++ ) {
            bool skip = false;
            for ( size_t k = 0; k < errors.size() && !skip; k++ ) {
               skip = ( errors[k].type == ErrorCondition::Fans && errors[k].i1 == i && errors[k].i2 == j );
            }

            size_t p2 = fans_v[i][j],
                   i2 = j;

            if ( !skip ) {
               b.begin_facet();
               b.add_vertex_to_facet( p0 );
               b.add_vertex_to_facet( p1 );
               b.add_vertex_to_facet( p2 );
               Polyhedron::Halfedge_handle h = b.end_facet();
               if ( h != Polyhedron::Halfedge_handle() ) {
                  Polyhedron::Facet_handle f = h->facet();
                  f->material = mat;
                  f->object = iObj;

                  Point3D p = texcoords[ fans_tc[i][0] ];
                  f->tex_u[0] = p.x();
                  f->tex_v[0] = p.y();
                  p = texcoords[ fans_tc[i][i1] ];
                  f->tex_u[1] = p.x();
                  f->tex_v[1] = p.y();
                  p = texcoords[ fans_tc[i][i2] ];
                  f->tex_u[2] = p.x();
                  f->tex_v[2] = p.y();
               } else {
                  ErrorCondition e;
                  e.type = ErrorCondition::Fans;
                  e.i1 = i;
                  e.i2 = j;
                  errors.push_back( e );
                  error = true;
                  if ( iObj != (size_t)-1 ) {
                     std::cerr << "Object : " << _objectName[ iObj ] << std::endl;
                  } else {
                     std::cerr << "Terrain" << std::endl;
                  }
               }
            }

            p1 = p2;
            i1 = i2;
         }
      }

      b.end_surface();

      if ( b.error() ) {
         b.rollback();
      }
   } while ( error );

   b.remove_unconnected_vertices();
}

bool FGSD_TriangleObject::load( const char *path, bool isTerrain )
{
   _vminx =  1000.0;
   _vmaxx = -1000.0;
   _vminy =  1000.0;
   _vmaxy = -1000.0;
   bool ret = false;

   SGBinObject object;
   if ( object.read_bin( path ) ) {
      std::string name( path );
      size_t pos;

      while ( ( pos = name.find( '/' ) ) != std::string::npos ) {
         name.erase( 0, pos + 1 );
      }

      pos = name.find( '.' );
      if ( pos != std::string::npos ) {
         name.erase( pos );
      }

      size_t iObject = (size_t)-1;
      if ( !isTerrain ) {
         iObject = _objectName.size();
         _objectNameMap[ name ] = iObject;
         _objectName.push_back( name );
         _objects.push_back( iObject );
         std::vector<FGSD_TriangleObject *> tv;
         tv.push_back( this );
         _objectSpan.push_back( tv );
/*
         if ( name == "KSFO" ) {
            int a = 0;
         }
*/
      } else {
         _name = name;
      }

      Polyhedron p;
      LoadObject lo( *this, object, iObject );
      p.delegate( lo );

      if ( isTerrain ) {
         _minx = _vminx;
         _maxx = _vmaxx;
         _miny = _vminy;
         _maxy = _vmaxy;
      }

      addPolyhedron( p, isTerrain, iObject, _vminx, _vmaxx, _vminy, _vmaxy );

      PointFeatureList &ptList = _ptsFeatures[ iObject ];
      string_list const& pt_materials = object.get_pt_materials();
      group_list const& pts_v = object.get_pts_v();
      group_list const& pts_n = object.get_pts_n();
      const point_list &wgs84_nodes = object.get_wgs84_nodes();
      const point_list &normals = object.get_normals();
      for ( size_t i = 0; i < pts_v.size(); ++i ) {
         PointFeature pf;
         pf.material = pt_materials[i];
         for ( size_t j = 0; j < pts_v[i].size(); j++ ) {
            pf.points.push_back( wgs84_nodes[ pts_v[i][j] ] );
            pf.normals.push_back( normals[ pts_n[i][j] ] );
         }
         ptList.push_back( pf );
      }
      ret = true;
   }
   return ret;
}

bool FGSD_TriangleObject::addPolyhedron( Polyhedron &poly, bool terrain, size_t o, double minx, double maxx, double miny, double maxy )
{
   if ( maxx <= _minx || minx >= _maxx || maxy <= _miny || miny >= _maxy ) {
      return false;
   }

//if ( !terrain ) return;
   Polyhedron *inner = &poly;
   bool alloc = false;
   Polyhedron *tmp1 = 0; bool alloc1 = false;
   Polyhedron *tmp2 = 0; bool alloc2 = false;
   Polyhedron *tmp3 = 0; bool alloc3 = false;
   if ( !terrain ) {
      cutPolyhedron( poly, _minx, false );
      Polyhedron *p = new Polyhedron;
      { SplitPolyhedron sp( poly, _minx, false, false ); p->delegate( sp ); }
      if ( !p->empty() ) {
         _outSurfaceList.push_back( p );
         _outObjectList.push_back( o );
         tmp1 = new Polyhedron;
         alloc1 = true;
         { SplitPolyhedron sp( poly, _minx, false, true ); tmp1->delegate( sp ); }
      } else {
         delete p;
         tmp1 = &poly;
         alloc1 = false;
      }

      cutPolyhedron( *tmp1, _maxx, false );
      p = new Polyhedron;
      { SplitPolyhedron sp( *tmp1, _maxx, false, true ); p->delegate( sp ); }
      if ( !p->empty() ) {
         _outSurfaceList.push_back( p );
         _outObjectList.push_back( o );
         tmp2 = new Polyhedron;
         alloc2 = true;
         { SplitPolyhedron sp( *tmp1, _maxx, false, false ); tmp2->delegate( sp ); }
      } else {
         delete p;
         tmp2 = tmp1;
         alloc2 = false;
      }

      cutPolyhedron( *tmp2, _miny, true );
      p = new Polyhedron;
      { SplitPolyhedron sp( *tmp2, _miny, true, false ); p->delegate( sp ); }
      if ( !p->empty() ) {
         _outSurfaceList.push_back( p );
         _outObjectList.push_back( o );
         tmp3 = new Polyhedron;
         alloc3 = true;
         { SplitPolyhedron sp( *tmp2, _miny, true, true ); tmp3->delegate( sp ); }
      } else {
         delete p;
         tmp3 = tmp2;
         alloc3 = false;
      }

      cutPolyhedron( *tmp3, _maxy, true );
      p = new Polyhedron;
      { SplitPolyhedron sp( *tmp3, _maxy, true, true ); p->delegate( sp ); }
      inner = 0;
      if ( !p->empty() ) {
         _outSurfaceList.push_back( p );
         _outObjectList.push_back( o );
         inner = new Polyhedron;
         alloc = true;
         { SplitPolyhedron sp( *tmp3, _maxy, true, false ); inner->delegate( sp ); }
      } else {
         delete p;
         inner = tmp3;
         alloc = false;
      }
   }

   if ( !inner->empty() ) {
      for ( Polyhedron::Vertex_iterator vi = inner->vertices_begin(); vi != inner->vertices_end(); ++vi ) {
         CDTplus::Vertex_handle v = _surface.insert( K::Point_2( vi->point().x(), vi->point().y() ) );
         v->elev = vi->point().z();
         v->interpolate_z = false;
         vi->v = v;

         Point_set::Vertex_handle vps = _pointSet.insert( K::Point_2( vi->point().x(), vi->point().y() ) );
         vps->v = v;
      }

      for ( Polyhedron::Edge_iterator ei = inner->edges_begin(); ei != inner->edges_end(); ++ei ) {
         if ( ( !ei->is_border() && !ei->opposite()->is_border() &&
                  ei->facet()->material != ei->opposite()->facet()->material ) ||
               ( terrain && ( ei->is_border() || ei->opposite()->is_border() ) ) ) {
            bool same_dir;
            CDTplus::Vertex_handle v0,
                                   v1 = ei->prev()->vertex()->v,
                                   v2 = ei->vertex()->v;
            _surface.insert_constraint( v1, v2 );
            CDTplus::Vertices_in_constraint vic = _surface.vertices_in_constraint_begin( v1, v2 );
            while ( vic != _surface.vertices_in_constraint_end( v1, v2 ) ) {
               CDTplus::Vertex_handle vh = *vic;

               if ( v0 != CDTplus::Vertex_handle() ) {
                  CDTplus::Face_handle fr;
                  int i;
                  if ( _surface.is_edge( v0, vh, fr, i ) ) {
                     if ( same_dir ) {
                        if ( ei->opposite()->facet() != 0 ) {
                           fr->material = ei->opposite()->facet()->material;
                           fr->object = ei->opposite()->facet()->object;
                        }
                        if ( fr->neighbor( i ) != 0 && ei->facet() != 0 ) {
                           fr->neighbor( i )->material = ei->facet()->material;
                           fr->neighbor( i )->object = ei->facet()->object;
                        }
                     } else {
                        if ( ei->facet() != 0 ) {
                           fr->material = ei->facet()->material;
                           fr->object = ei->facet()->object;
                        }
                        if ( fr->neighbor( i ) != 0 && ei->opposite()->facet() != 0 ) {
                           fr->neighbor( i )->material = ei->opposite()->facet()->material;
                           fr->neighbor( i )->object = ei->opposite()->facet()->object;
                        }
                     }
                  }
               } else if ( vh == v1 ) {
                  same_dir = true;
               } else {
                  same_dir = false;
               }

               v0 = vh;
               ++vic;
            }
         }
      }
   }

   if ( alloc ) {
      delete inner;
   }
   if ( alloc3 ) {
      delete tmp3;
   }
   if ( alloc2 ) {
      delete tmp2;
   }
   if ( alloc1 ) {
      delete tmp1;
   }

   return true;
}

bool FGSD_TriangleObject::setMaterialThruEdge( CDTplus::Face_handle f, int i )
{
   bool success = false;
   if ( !f->is_constrained( i ) && f->neighbor( i ) != 0 && f->neighbor( i )->material != (size_t)-1 ) {
      f->material = f->neighbor( i )->material;
      f->object = f->neighbor( i )->object;
      success = true;
   }
   return success;
}

void FGSD_TriangleObject::cutPolyhedron( Polyhedron &p, double value, bool horizontally )
{
   typedef std::list<Polyhedron::Halfedge_handle> CutableEdges;
   typedef std::map<Polyhedron::Facet_handle,Polyhedron::Halfedge_handle,CompareHandle<Polyhedron::Facet_handle> > FacetMap;
   FacetMap facets;
   CutableEdges cutables;
   for ( Polyhedron::Edge_iterator ei = p.edges_begin(); ei != p.edges_end(); ++ei ) {
      Polyhedron::Point_3 &p1 = ei->vertex()->point(),
                          &p2 = ei->opposite()->vertex()->point();

      if ( ( horizontally && ( ( p1.y() < value && p2.y() > value ) || ( p1.y() > value && p2.y() < value ) ) ) ||
           ( !horizontally && ( ( p1.x() < value && p2.x() > value ) || ( p1.x() > value && p2.x() < value ) ) ) ) {
         cutables.push_back( ei );
      }
   }

   for ( CutableEdges::iterator ci = cutables.begin(); ci != cutables.end(); ++ci ) {
      Polyhedron::Point_3 &p1 = (*ci)->vertex()->point(),
                          &p2 = (*ci)->opposite()->vertex()->point();

      Polyhedron::Facet_handle f1 = (*ci)->facet(),
                               f2 = (*ci)->opposite()->facet();
      Polyhedron::Halfedge_handle prev1 = (*ci)->prev(),
                                  prev2 = (*ci)->opposite()->prev();
      p.split_edge( *ci );
      double alpha, x, y;
      if ( horizontally ) {
         alpha = ( value - p2.y() ) / ( p1.y() - p2.y() );
         x = alpha * ( p1.x() - p2.x() ) + p2.x();
         y = value;
      } else {
         alpha = ( value - p2.x() ) / ( p1.x() - p2.x() );
         y = alpha * ( p1.y() - p2.y() ) + p2.y();
         x = value;
      }
      double z = alpha * ( p1.z() - p2.z() ) + p2.z();
      prev1->next()->vertex()->point() = Polyhedron::Point_3( x, y, z );

      if ( !prev1->is_border() ) {
         FacetMap::iterator it = facets.find( f1 );
         if ( it == facets.end() ) {
            facets[ f1 ] = prev1->next();
         } else {
            p.split_facet( it->second, prev1->next() );
            facets.erase( it );
         }
      }

      if ( !prev2->is_border() ) {
         FacetMap::iterator it = facets.find( f2 );
         if ( it == facets.end() ) {
            facets[ f2 ] = prev2->next();
         } else {
            p.split_facet( it->second, prev2->next() );
            facets.erase( it );
         }
      }
   }
   for ( FacetMap::iterator it = facets.begin(); it != facets.end(); ++it ) {
      Polyhedron::Halfedge_handle h1 = it->second,
                                  h2 = h1->next(),
                                  h3 = h2->next(),
                                  h4 = h3->next(),
                                  h5 = h4->next();
      if ( h4 != h1 ) {
         p.split_facet( h1, h3 );
      }
   }
//   assert( facets.size() == 0 );
}

void FGSD_TriangleObject::SplitPolyhedron::operator()( HalfedgeDS& hds )
{
   // Prepare mapping vertex_handle <-> index and facet list
   size_t index = 0;
   typedef std::map<Polyhedron::Vertex_handle,size_t,CompareHandle<Polyhedron::Vertex_handle> > VertexIndexMap;
   VertexIndexMap v2iMap;
   typedef std::vector<Polyhedron::Vertex_handle> VertexVector;
   VertexVector vertVect;
   typedef std::list<Polyhedron::Halfedge_around_facet_circulator> FacetList;
   FacetList facetList;
   for ( Polyhedron::Facet_iterator fi = orig.facets_begin(); fi != orig.facets_end(); ++fi ) {
      Polyhedron::Halfedge_around_facet_circulator h = fi->facet_begin();
      bool facet_ok =
            ( horizontal &&
              ( ( greater &&
                  ( h->vertex()->point().y() > value ||
                    ( h->vertex()->point().y() == value &&
                      ( h->next()->vertex()->point().y() > value ||
                        ( h->next()->vertex()->point().y() == value &&
                          h->next()->next()->vertex()->point().y() > value ) ) ) ) ) ||
                ( !greater &&
                  ( h->vertex()->point().y() < value ||
                    ( h->vertex()->point().y() == value &&
                      ( h->next()->vertex()->point().y() < value ||
                        ( h->next()->vertex()->point().y() == value &&
                          h->next()->next()->vertex()->point().y() < value ) ) ) ) ) ) ) ||
            ( !horizontal &&
              ( ( greater &&
                  ( h->vertex()->point().x() > value ||
                    ( h->vertex()->point().x() == value &&
                      ( h->next()->vertex()->point().x() > value ||
                        ( h->next()->vertex()->point().x() == value &&
                          h->next()->next()->vertex()->point().x() > value ) ) ) ) ) ||
                ( !greater &&
                  ( h->vertex()->point().x() < value ||
                    ( h->vertex()->point().x() == value &&
                      ( h->next()->vertex()->point().x() < value ||
                        ( h->next()->vertex()->point().x() == value &&
                          h->next()->next()->vertex()->point().x() < value ) ) ) ) ) ) );

      if ( facet_ok ) {
         facetList.push_back( h );
         Polyhedron::Halfedge_around_facet_circulator hc = h,
                                                      done( hc );
         do {
            Polyhedron::Vertex_handle v = hc->vertex();
            VertexIndexMap::iterator it = v2iMap.find( v );
            if ( it == v2iMap.end() ) {
               v2iMap[ v ] = index++;
               vertVect.push_back( v );
            }
         } while ( ++hc != done );
      }
   }

   CGAL::Polyhedron_incremental_builder_3<HalfedgeDS> b( hds, true );
   b.begin_surface( index, facetList.size() );

   size_t i;
   for ( i = 0; i < index; i++ ) {
      b.add_vertex( vertVect[ i ]->point() );
   }

   for ( FacetList::iterator fli = facetList.begin(); fli != facetList.end(); ++fli ) {
      Polyhedron::Halfedge_around_facet_circulator hc = *fli,
                                                   done( hc );
      b.begin_facet();
      do {
         b.add_vertex_to_facet( v2iMap[ hc->vertex() ] );
      } while ( ++hc != done );
      Polyhedron::Halfedge_handle h = b.end_facet();
      Polyhedron::Facet_handle f0 = done->facet(),
                               f1 = h->facet();
      f1->material = f0->material;
      f1->object = f0->object;
   }

   b.end_surface();
}

struct FlippedFacePair {
   FlippedFacePair( void *a, void *b ) { if ( a < b ) { f1 = a; f2 = b; } else { f1 = b; f2 = a; } }
   bool operator<( const FlippedFacePair &r ) const { return f1 < r.f1 || ( f1 == r.f1 && f2 < r.f2 ); }
   void *f1;
   void *f2;
};

void FGSD_TriangleObject::postLoad()
{
   bool changed;
   std::set<FlippedFacePair> alreadyFlipped;
   do {
      changed = false;
      for ( CDTplus::Finite_faces_iterator fi = _surface.finite_faces_begin();
            !changed && fi != _surface.finite_faces_end(); ++fi ) {
         CDTplus::Point &p0 = fi->vertex( 0 )->point(),
                                    &p1 = fi->vertex( 1 )->point(),
                                    &p2 = fi->vertex( 2 )->point();
         K::Vector_2 v0( p1, p2 ),
                     v1( p2, p0 ),
                     v2( p0, p1 );
         double a = v0.x() * v1.y() - v0.y() * v1.x();
         double b = sqrt( v0.squared_length() ) * sqrt( v1.squared_length() );
         double q = a / b;
         if ( q < 1e-3 ) {
            double l0 = v0.squared_length(),
                     l1 = v1.squared_length(),
                     l2 = v2.squared_length();
            int imin = 0; double minl = l0;
            int imax = 0; double maxl = l0;
            if ( minl > l1 ) { imin = 1; minl = l1; }
            if ( maxl < l1 ) { imax = 1; maxl = l1; }
            if ( minl > l2 ) { imin = 2; minl = l2; }
            if ( maxl < l2 ) { imax = 2; maxl = l2; }
            if ( minl == 0 || maxl / minl > 1000000 ) {
               // collapse edge
               _surface.tds().collapse_edge( fi, imin );
               changed = true;
            } else {
               // flip edge if possible ( quad is convex )
               int m = fi->mirror_index( imax );
               CDTplus::Face_handle n = fi->neighbor( imax );

               bool reject_flip = fi->neighbor( CDTplus::cw( imax ) ) == n->neighbor( CDTplus::ccw( m ) ) ||
                                    fi->neighbor( CDTplus::ccw( imax ) ) == n->neighbor( CDTplus::cw( m ) );

               std::set<FlippedFacePair>::iterator ffi = alreadyFlipped.find( FlippedFacePair( fi.operator->(), n.operator->() ) );
               reject_flip = reject_flip || ffi != alreadyFlipped.end();
               if ( !reject_flip ) {
                  bool do_flip = false;
                  if ( _surface.is_infinite(n) ) {
                     do_flip = true;
                  } else {
                     do_flip = orientation( fi->vertex(imax)->point(),
                                             fi->vertex(CDTplus::cw(imax))->point(),
                                             fi->mirror_vertex(imax)->point() ) == CGAL::RIGHT_TURN &&
                                 orientation( fi->vertex(imax)->point(),
                                             fi->vertex(CDTplus::ccw(imax))->point(),
                                             fi->mirror_vertex(imax)->point() ) ==  CGAL::LEFT_TURN;
                  }
                  if ( do_flip ) {
                     if ( fi->is_constrained( imax ) ) {
                        fi->set_constraint( imax, false );
                        n->set_constraint( m, false );

                        int m1 = fi->mirror_index( CDTplus::cw( imax ) );
                        int m2 = fi->mirror_index( CDTplus::ccw( imax ) );
                        CDTplus::Face_handle n1 = fi->neighbor( CDTplus::cw( imax ) );
                        CDTplus::Face_handle n2 = fi->neighbor( CDTplus::ccw( imax ) );

                        fi->set_constraint( CDTplus::cw( imax ), true );
                        fi->set_constraint( CDTplus::ccw( imax ), true );
                        n1->set_constraint( m1, true );
                        n2->set_constraint( m2, true );
                     }
                     _surface.tds().flip( fi, imax );
                     alreadyFlipped.insert( FlippedFacePair( fi.operator->(), n.operator->() ) );
                     changed = true;
                  }
               }
            }
         }
      }
   } while ( changed );



   std::list<CDTplus::Face_handle> emptyFaces;
   CDTplus::Face_iterator fi;
   for ( fi = _surface.finite_faces_begin(); fi != _surface.finite_faces_end(); ++fi ) {
      if ( fi->material == ( size_t)-1 ) {
         emptyFaces.push_back( fi );
      }
   }

   std::list<CDTplus::Face_handle>::iterator efi = emptyFaces.begin();
   bool no_change = false;
   while ( emptyFaces.size() && ( efi != emptyFaces.begin() || !no_change ) ) {
      if ( efi == emptyFaces.begin() ) {
         no_change = true;
      }
      CDTplus::Face_handle f = *efi;
      bool success = setMaterialThruEdge( f, 0 ) ||
                     setMaterialThruEdge( f, 1 ) ||
                     setMaterialThruEdge( f, 2 );

      if ( success ) {
         emptyFaces.erase( efi++ );
         no_change = false;
      } else {
         ++efi;
      }

      if ( efi == emptyFaces.end() ) {
         efi = emptyFaces.begin();
      }
   }

   for ( fi = _surface.finite_faces_begin(); fi != _surface.finite_faces_end(); ++fi ) {
      CDTplus::Face_handle f = fi;

      CDTplus::Vertex_handle v1 = f->vertex( 0 ),
                             v2 = f->vertex( 1 ),
                             v3 = f->vertex( 2 );

      Point3D p1 = sgGeodToCart( Point3D( v1->point().x() * SG_DEGREES_TO_RADIANS, v1->point().y() * SG_DEGREES_TO_RADIANS, v1->elev ) );
      Point3D p2 = sgGeodToCart( Point3D( v2->point().x() * SG_DEGREES_TO_RADIANS, v2->point().y() * SG_DEGREES_TO_RADIANS, v2->elev ) );
      Point3D p3 = sgGeodToCart( Point3D( v3->point().x() * SG_DEGREES_TO_RADIANS, v3->point().y() * SG_DEGREES_TO_RADIANS, v3->elev ) );

      Point3D p12 = p2 - p1;
      Point3D p23 = p3 - p2;

      double normal[ 3 ];
      normal[0] = p12.y() * p23.z() - p12.z() * p23.y();
      normal[1] = p12.z() * p23.x() - p12.x() * p23.z();
      normal[2] = p12.x() * p23.y() - p12.y() * p23.x();

      double l = sqrt( normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2] );
      f->normal = _Vb::Point_3( normal[0] / l, normal[1] / l, normal[2] / l );
   }

   CDTplus::Vertex_iterator vi;
   for ( vi = _surface.finite_vertices_begin(); vi != _surface.finite_vertices_end(); ++vi ) {
      double x = 0.0, y = 0.0, z = 0.0;
      size_t nb = 0;
      CDTplus::Face_circulator ifc = _surface.incident_faces( vi ),
                               done( ifc );
      do {
         if ( !_surface.is_infinite( ifc ) ) {
            x += ifc->normal.x();
            y += ifc->normal.y();
            z += ifc->normal.z();
            nb += 1;
         }
      } while ( ++ifc != done );

      if ( nb > 0 ) {
         vi->normal = _Vb::Point_3( x / nb, y / nb, z / nb );
      }
   }
}

void FGSD_TriangleObject::draw( double alpha, bool lines, DrawType drawType, double minx, double maxx, double miny, double maxy, double zLevel )
{
   double lon = ( maxx + minx ) / 2.0, lat = ( maxy + miny ) / 2.0;
   if ( drawType == Shadowed ) {
      lon -= 45;
      lat += 45;
   }

   double cosl = cos( lat * SG_DEGREES_TO_RADIANS );
   Point3D sun( cosl * cos( lon * SG_DEGREES_TO_RADIANS ),
                  cosl * sin( lon * SG_DEGREES_TO_RADIANS ),
                  sin( lat * SG_DEGREES_TO_RADIANS ) );

   glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );

   glBegin( GL_TRIANGLES );
      for ( CDTplus::Finite_faces_iterator fi = _surface.finite_faces_begin(); fi != _surface.finite_faces_end(); ++fi ) {
         CDTplus::Face_handle f = fi;
         if ( f->material != (size_t)-1 ) {
            std::string &material = _materialList[ f->material ];
            MaterialColorMap::iterator m = _materialColorMap.find( material );
            if ( m != _materialColorMap.end() ) {
               Color color = m->second;
               if ( drawType == Shadowed ) {
                  Color c;
                  double cosn;
                  Point3D n;
#define AMBIANT_FACTOR  0.01

                  CDTplus::Vertex_handle v = f->vertex( 0 );
                  n = Point3D( v->normal[0], v->normal[1], v->normal[2] );
                  cosn = n.x() * sun.x() + n.y() * sun.y() + n.z() * sun.z();
                  c = ( ( 1 - AMBIANT_FACTOR ) * cosn + AMBIANT_FACTOR ) * color;
                  glColor4d( c.red, c.green, c.blue, alpha );
                  glVertex2d( v->point().x(), v->point().y() );

                  v = f->vertex( 1 );
                  n = Point3D( v->normal[0], v->normal[1], v->normal[2] );
                  cosn = n.x() * sun.x() + n.y() * sun.y() + n.z() * sun.z();
                  c = ( ( 1 - AMBIANT_FACTOR ) * cosn + AMBIANT_FACTOR ) * color;
                  glColor4d( c.red, c.green, c.blue, alpha );
                  glVertex2d( v->point().x(), v->point().y() );

                  v = f->vertex( 2 );
                  n = Point3D( v->normal[0], v->normal[1], v->normal[2] );
                  cosn = n.x() * sun.x() + n.y() * sun.y() + n.z() * sun.z();
                  c = ( ( 1 - AMBIANT_FACTOR ) * cosn + AMBIANT_FACTOR ) * color;
                  glColor4d( c.red, c.green, c.blue, alpha );
                  glVertex2d( v->point().x(), v->point().y() );

               } else if ( drawType == SlopeGradient ) {

                  Point3D n = Point3D( f->normal[0], f->normal[1], f->normal[2] );
                  double cosn = n.x() * sun.x() + n.y() * sun.y() + n.z() * sun.z();

                  glColor4d( 1.0, cosn, cosn, alpha );

                  CDTplus::Vertex_handle v = f->vertex( 0 );
                  glVertex2d( v->point().x(), v->point().y() );
                  v = f->vertex( 1 );
                  glVertex2d( v->point().x(), v->point().y() );
                  v = f->vertex( 2 );
                  glVertex2d( v->point().x(), v->point().y() );

               } else {

                  glColor4d( color.red, color.green, color.blue, alpha );

                  for ( int i = 0; i < 3; i++ ) {
                     CDTplus::Vertex_handle v = f->vertex( i );
                     CDTplus::Point &p = v->point();
                     glVertex2d( p.x(), p.y() );
                  }
               }
            }
         }
      }
   glEnd();

   if ( lines ) {
      glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
      glColor4d( 0, 0, 0, alpha );
      glLineWidth( 1.0 );
      glBegin( GL_LINES );
         glColor4d( 0, 0, 0, alpha );
         CDTplus::Finite_edges_iterator ei;
         for ( ei = _surface.finite_edges_begin(); ei != _surface.finite_edges_end(); ++ei ) {
            CDTplus::Face_handle f = ei->first;
            int ind = ei->second;

            if ( !f->is_constrained( ind ) || !generalPreferences.showConstraints() ) {
               CDTplus::Vertex_handle v1 = f->vertex( CDTplus::ccw( ind ) );
               CDTplus::Point &p1 = v1->point();
               glVertex2d( p1.x(), p1.y() );
               CDTplus::Vertex_handle v2 = f->vertex( CDTplus::cw( ind ) );
               CDTplus::Point &p2 = v2->point();
               glVertex2d( p2.x(), p2.y() );
            }
         }
         if ( generalPreferences.showConstraints() ) {
            glColor4d( 1.0, 0, 0, alpha );
            for ( ei = _surface.finite_edges_begin(); ei != _surface.finite_edges_end(); ++ei ) {
               CDTplus::Face_handle f = ei->first;
               int ind = ei->second;

               if ( f->is_constrained( ind ) ) {
                  CDTplus::Vertex_handle v1 = f->vertex( CDTplus::ccw( ind ) );
                  CDTplus::Point &p1 = v1->point();
                  glVertex2d( p1.x(), p1.y() );
                  CDTplus::Vertex_handle v2 = f->vertex( CDTplus::cw( ind ) );
                  CDTplus::Point &p2 = v2->point();
                  glVertex2d( p2.x(), p2.y() );
               }
            }
         }
      glEnd();

      if ( generalPreferences.showVertices() ) {
         glColor4d( 0, 0, 1.0, alpha );
         glPointSize( 3.0 );
         glBegin( GL_POINTS );
            for ( CDTplus::Finite_vertices_iterator vi = _surface.finite_vertices_begin(); vi != _surface.finite_vertices_end(); ++vi ) {
               CDTplus::Point &p = vi->point();
               glVertex2d( p.x(), p.y() );
            }
         glEnd();
      }

      if ( generalPreferences.showInfiniteEdges() ) {
         double vdx = maxx - minx;
         double vdy = maxy - miny;
         double aspect = vdx / vdy;
         double h2 = vdy / 2,
                w2 = vdx / 2;

         CDTplus::Edge_circulator ec = _surface.incident_edges( _surface.infinite_vertex() ),
                           done( ec );
         if ( ec != 0 ) {
            glColor4d( 1.0, 1.0, 1.0, alpha );
            glLineWidth( 1.0 );
            glBegin( GL_LINES );
            do {
               CDTplus::Face_handle f = ec->first;
               int ind = ec->second;
               CDTplus::Vertex_handle v = f->vertex( CDTplus::ccw( ind ) );
               if ( v == _surface.infinite_vertex() ) {
                  v = f->vertex( CDTplus::cw( ind ) );
               }
               CDTplus::Point &p = v->point();

               if ( p.x() <= minx || p.x() >= maxx || p.y() <= miny || p.y() >= maxy )
                  continue;

               bool ok = true;
               double x2, y2;
               double dx = p.x() - lon;
               double dy = p.y() - lat;
               if ( dx >= 0 && dy >= 0 ) {
                  if ( dy != 0 ) {
                     double s = dx / dy;
                     if ( s > aspect ) {
                        x2 = lon + w2;
                        y2 = p.y() + ( w2 - dx ) / s;
                     } else {
                        x2 = p.x() + ( h2 - dy ) * s;
                        y2 = lat + h2;
                     }
                  } else if ( dx != 0 ) {
                     x2 = lon + w2;
                     y2 = lat;
                  } else {
                     ok = false;
                  }
               } else if ( dx >= 0 ) {
                  double s = dx / dy;
                  if ( -s > aspect ) {
                     x2 = lon + w2;
                     y2 = p.y() + ( w2 - dx ) / s;
                  } else {
                     x2 = p.x() - ( h2 + dy ) * s;
                     y2 = lat - h2;
                  }
               } else if ( dy >= 0 ) {
                  if ( dy != 0 ) {
                  double s = dx / dy;
                  if ( -s > aspect ) {
                     x2 = lon - w2;
                     y2 = p.y() - ( w2 + dx ) / s;
                  } else {
                     x2 = p.x() + ( h2 - dy ) * s;
                     y2 = lat + h2;
                  }
                  } else {
                     x2 = lon - w2;
                     y2 = lat;
                  }
               } else {
                  double s = dx / dy;
                  if ( s > aspect ) {
                     x2 = lon - w2;
                     y2 = p.y() - ( w2 + dx ) / s;
                  } else {
                     x2 = p.x() - ( h2 + dy ) * s;
                     y2 = lat - h2;
                  }
               }

               if ( ok ) {
                  glVertex2d( p.x(), p.y() );
                  glVertex2d( x2, y2 );
               }

            } while ( ++ec != done );
            glEnd();
         }
      }
   }
}

bool FGSD_TriangleObject::propertiesUnderMouse( double lon, double lat, double &height, std::string &material, std::string &airportID ) const
{
   bool ret = false;
   CDTplus::Face_handle f = _surface.locate( CDTplus::Point( lon, lat ), _lastLocate );
   const_cast<FGSD_TriangleObject *>( this )->_lastLocate = f;

   if ( _lastLocate != CDTplus::Face_handle() ) {
      CDTplus::Vertex_handle v1 = _lastLocate->vertex( 0 );
      CDTplus::Point &p1 = v1->point();
      CDTplus::Vertex_handle v2 = _lastLocate->vertex( 1 );
      CDTplus::Point &p2 = v2->point();
      CDTplus::Vertex_handle v3 = _lastLocate->vertex( 2 );
      CDTplus::Point &p3 = v3->point();

      double denom = p1.y() * p2.x() - p1.x() * p2.y() - p1.y() * p3.x() + p1.x() * p3.y()
                        + ( p2.y() - p3.y() ) * lon - ( p2.x() - p3.x() ) * lat;
      if ( denom != 0 ) {
         double n = -(-p1.y() * p2.x() + p1.x() * p2.y() + p1.y() * p3.x() - p2.y() * p3.x()
                        - p1.x() * p3.y() + p2.x() * p3.y()) / denom;
         double m = -(-p1.y() * p2.x() + p1.x() * p2.y() + ( p1.y() - p2.y() ) * lon
                        - ( p1.x() - p2.x() ) * lat) / denom;

         if ( m >= 0 && m <= 1 && n >= 1 ) {
            // Point ( lon, lat ) inside

            double z = v2->elev + ( v3->elev - v2->elev ) * m;
            height   = v1->elev + (      z   - v1->elev ) / n;

            if ( _lastLocate->material != (size_t)-1 ) {
               material = _materialList[ _lastLocate->material ];
            }
            if ( _lastLocate->object != (size_t)-1 && _lastLocate->object < _objectName.size() ) {
               airportID = _objectName[ _lastLocate->object ];
            }

            ret = true;
         }
      }
   }

   return ret;
}

bool FGSD_TriangleObject::objectUnderMouse( double lon, double lat, double zLevel )
{
   bool ret = false;
   _lastLocate = _surface.locate( CDTplus::Point( lon, lat ), _lastLocate );

   if ( _lastLocate != CDTplus::Face_handle() ) {
      double xscale = 0.0002 / zLevel;
      double yscale = xscale * cos( lat * SG_PI / 180 );

      double minx = lon - xscale; if ( minx < _minx ) minx = _minx;
      double miny = lat - yscale; if ( miny < _miny ) miny = _miny;
      double maxx = lon + xscale; if ( maxx > _maxx ) maxx = _maxx;
      double maxy = lat + yscale; if ( maxy > _maxy ) maxy = _maxy;

      CDTplus::Point &p1 = _lastLocate->vertex( 0 )->point();
      double z1 = _lastLocate->vertex( 0 )->elev;
      CDTplus::Point &p2 = _lastLocate->vertex( 1 )->point();
      double z2 = _lastLocate->vertex( 1 )->elev;
      CDTplus::Point &p3 = _lastLocate->vertex( 2 )->point();
      double z3 = _lastLocate->vertex( 2 )->elev;
      if ( minx <= p1.x() && p1.x() < maxx && miny <= p1.y() && p1.y() < maxy ) {
         Point3D p( p1.x(), p1.y(), z1 );
         _curPt = _lastLocate->vertex( 0 );
         ret = true;
      } else if ( minx <= p2.x() && p2.x() < maxx && miny <= p2.y() && p2.y() < maxy ) {
         Point3D p( p2.x(), p2.y(), z2 );
         _curPt = _lastLocate->vertex( 1 );
         ret = true;
      } else if ( minx <= p3.x() && p3.x() < maxx && miny <= p3.y() && p3.y() < maxy ) {
         Point3D p( p3.x(), p3.y(), z3 );
         _curPt = _lastLocate->vertex( 2 );
         ret = true;
      }
   }

   return ret;
}

bool FGSD_TriangleObject::visible( double minx, double maxx, double miny, double maxy ) const
{
   bool ret = _maxx > minx && _minx < maxx && _maxy > miny && _miny < maxy;
   return ret;
}

bool FGSD_TriangleObject::visible( double lon, double lat ) const
{
   return lon >= _minx && lon < _maxx && lat >= _miny && lat < _maxy;
}

std::string FGSD_TriangleObject::getName() const
{
   return _name;
}

void *FGSD_TriangleObject::convertTo( FGSD_BaseObject::ObjectType __type )
{
   return __type == FGSD_BaseObject::TriangleObject ? this : 0;
}

size_t FGSD_TriangleObject::findMaterial( const std::string &mat )
{
   size_t m;
   MaterialMap::iterator it = _materialMap.find( mat );
   if ( it == _materialMap.end() ) {
      m = _materialList.size();
      _materialList.push_back( mat );
      _materialMap[ mat ] = m;
   } else {
      m = it->second;
   }
   return m;
}

void FGSD_TriangleObject::changeCurrentPointPosition( double __lon, double __lat )
{
   if ( _curPt != CDTplus::Vertex_handle() ) {
/*      unsigned char lock = _locks[ _curPt ];
      if ( !( lock & ( Xmin | Xmax ) ) && __lon >= _minx && __lon < _maxx )
         p.setx( __lon );
      if ( !( lock & ( Ymin | Ymax ) ) && __lat >= _miny && __lat < _maxy )
         p.sety( __lat );
*/
      CDTplus::Point &pv = _curPt->point();
      Point3D p( pv.x(), pv.y(), _curPt->elev );
      if ( !_xlock ) {
         p.setx( __lon );
      }
      if ( !_ylock ) {
         p.sety( __lat );
      }
      if ( pointInsideBoundingPoly( p ) ) {
         /*
         CDTplus::Vertex_handle v = _surface.insert( CDTplus::Point( __lon, __lat ) );
         double z = _curPt->elev;
         bool interpolate_z = _curPt->interpolate_z;
         replaceVertex( _curPt, v );
         v->elev = z;
         v->interpolate_z = interpolate_z;
         _curPt = v;
         */
         _curPt->point() = CDTplus::Point( __lon, __lat );
//         _lastLocate = CDTplus::Face_handle();
#if 0
         PointKey pk0( pv->x, pv->y );
         PointMap::iterator it = _vertexMap.find( pk0 );
         if ( it != _vertexMap.end() ) {
            _vertexMap.erase( it );

            gts_point_set( pv, p.x(), p.y(), p.z() );
            FGSD_vertex_compute_normal( v, _surface, 1 );
/*
            if ( _minx > p.x() ) _minx = p.x();
            if ( _maxx < p.x() ) _maxx = p.x();
            if ( _miny > p.y() ) _miny = p.y();
            if ( _maxy < p.y() ) _maxy = p.y();
*/
            PointKey pk( p.x(), p.y() );
            _vertexMap[ pk ] = _curPt;
         }
#endif
      }

      _modified = true;
      }
}

void FGSD_TriangleObject::replaceVertex( CDTplus::Vertex_handle va, CDTplus::Vertex_handle vb )
{
   do {
      CDataList constraints;
      CDTplus::Vertex_circulator vc = _surface.incident_vertices( va ),
                                 done( vc );
      do {
         CDTplus::Face_handle f;
         int i;
         if ( _surface.is_edge( va, vc, f, i ) && f->is_constrained( i ) ) {
            constraints.push_back( CData( vc, f->material, f->object ) );
            f->set_constraint( i, false );
         }
      } while ( ++vc != done );

      _surface.remove( va );

      for ( CDataList::iterator it = constraints.begin(); it != constraints.end(); ++it ) {
         _surface.insert_constraint( vb, it->v );
         CDTplus::Face_handle f;
         int i;
         if ( _surface.is_edge( vb, it->v, f, i ) ) {
            f->material = it->m;
            f->object = it->o;
         }
      }
   } while ( 0 );

   do {
      CDTplus::Face_circulator fc = _surface.incident_faces( vb ),
                               start( fc );
      int done = 0;
      while ( true ) {
         if ( fc == start ) {
            done ++;
         }
         if ( done == 2 ) {
            break;
         }

         int i = fc->index( vb ),
               i0 = CDTplus::ccw( i ),
               i1 = CDTplus::cw( i );
         if ( fc->material == size_t( -1 ) ) {
            if ( !fc->is_constrained( i0 ) && fc->neighbor( i0 )->material != size_t( -1 ) ) {
               fc->material = fc->neighbor( i0 )->material;
               done = 0;
            } else if ( !fc->is_constrained( i1 ) && fc->neighbor( i1 )->material != size_t( -1 ) ) {
               fc->material = fc->neighbor( i1 )->material;
               done = 0;
            }
         } else if ( !fc->is_constrained( i ) && fc->neighbor( i )->material == size_t( -1 ) ) {
            fc->neighbor( i )->material = fc->material;
         }

         ++fc;
      }
   } while ( 0 );
}

FGSD_TriangleObject::CDTplus::Vertex_handle FGSD_TriangleObject::replaceVertex( CDTplus::Vertex_handle va, const CDTplus::Point &p )
{
   CDTplus::Vertex_handle vb;
   do {
      CDataList constraints;
      CDTplus::Vertex_circulator vc = _surface.incident_vertices( va ),
                                 done( vc );
      do {
         CDTplus::Face_handle f;
         int i;
         if ( _surface.is_edge( va, vc, f, i ) && f->is_constrained( i ) ) {
            constraints.push_back( CData( vc, f->material, f->object ) );
            f->set_constraint( i, false ); 
         }
      } while ( ++vc != done );

      _surface.remove( va );
      vb = _surface.insert( p );

      for ( CDataList::iterator it = constraints.begin(); it != constraints.end(); ++it ) {
         _surface.insert_constraint( vb, it->v );
         CDTplus::Face_handle f;
         int i;
         if ( _surface.is_edge( vb, it->v, f, i ) ) {
            f->material = it->m;
            f->object = it->o;
         }
      }
   } while ( 0 );

   do {
      CDTplus::Face_circulator fc = _surface.incident_faces( vb ),
                               start( fc );
      int done = 0;
      while ( true ) {
         if ( fc == start ) {
            done ++;
         }
         if ( done == 2 ) {
            break;
         }

         if ( fc->material == size_t( -1 ) ) {
            int i = fc->index( vb ),
                i0 = CDTplus::ccw( i ),
                i1 = CDTplus::cw( i );
            if ( !fc->is_constrained( i0 ) && fc->neighbor( i0 )->material != size_t( -1 ) ) {
               fc->material = fc->neighbor( i0 )->material;
               done = 0;
            } else if ( !fc->is_constrained( i1 ) && fc->neighbor( i1 )->material != size_t( -1 ) ) {
               fc->material = fc->neighbor( i1 )->material;
               done = 0;
            }
         }
         ++fc;
      }
   } while ( 0 );

   return vb;
}

bool FGSD_TriangleObject::pointInsideBoundingPoly( Point3D p0 ) const
{
   bool ret = true;

   CDTplus::Vertex_circulator vc = _surface.incident_vertices( _curPt ),
                              v0 = vc++,
                              done = vc;
   do {
      CDTplus::Point &p1 = v0->point(),
                     &p2 = vc->point();

      double v1x = p1.x() - p0.x(),
             v1y = p1.y() - p0.y(),
             v2x = p2.x() - p0.x(),
             v2y = p2.y() - p0.y();

      double o = v1x * v2y - v1y * v2x;
      if ( o < 0 ) {
         ret = false;
         break;
      }

      v0 = vc;
   } while( ++vc != done );

   return ret;
}

bool FGSD_TriangleObject::xlock( bool &__min )
{
   _xlock = false;
   return _xlock;
}

bool FGSD_TriangleObject::ylock( bool &__min )
{
   _ylock = false;
   return _ylock;
}

bool FGSD_TriangleObject::interior() const
{
   return false;
}

void FGSD_TriangleObject::changeCurrentPointHeight( double __height )
{
   if ( _curPt != CDTplus::Vertex_handle() ) {
      _curPt->elev = __height;
      _curPt->interpolate_z = false;
      _modified = true;
   }
}

bool FGSD_TriangleObject::getPointPosition( size_t __index, double &__lon, double &__lat ) const
{
   bool ret = false;
   return ret;
}

Point3D FGSD_TriangleObject::current() const
{
   Point3D ret( -1000, 0, -9999 );
   if ( _curPt != CDTplus::Vertex_handle() ) {
      CDTplus::Point &p = _curPt->point();
      ret.setx( p.x() );
      ret.sety( p.y() );
      ret.setz( _curPt->elev );
   }
   return ret;
}

void FGSD_TriangleObject::setCurrent( const Point3D &p )
{
   CDTplus::Locate_type lt;
   int li;
   CDTplus::Face_handle f = _surface.locate( CDTplus::Point( p.x(), p.y() ), lt, li, _lastLocate );

   if ( f != CDTplus::Face_handle() ) {
      if ( lt == CDTplus::VERTEX ) {
         _curPt = f->vertex( li );
      } else {
         CDTplus::Point &p1 = f->vertex( 0 )->point(),
                        &p2 = f->vertex( 1 )->point(),
                        &p3 = f->vertex( 2 )->point();
         double d1 = ( p1.x() - p.x() ) * ( p1.x() - p.x() ) + ( p1.y() - p.y() ) * ( p1.y() - p.y() );
         double d2 = ( p2.x() - p.x() ) * ( p2.x() - p.x() ) + ( p2.y() - p.y() ) * ( p2.y() - p.y() );
         double d3 = ( p3.x() - p.x() ) * ( p3.x() - p.x() ) + ( p3.y() - p.y() ) * ( p3.y() - p.y() );
         if ( d1 <= d2 && d1 <= d3 ) {
            _curPt = f->vertex( 0 );
         } else if ( d2 <= d1 && d2 <= d3 ) {
            _curPt = f->vertex( 1 );
         } else {
            _curPt = f->vertex( 2 );
         }
      }
   }
}

void FGSD_TriangleObject::getAirportList( std::vector<std::string> &__aptList ) const
{
   for ( size_t i = 0; i < _objects.size(); i++ ) {
      __aptList.push_back( _objectName[ _objects[ i ] ] );
   }
}

bool FGSD_TriangleObject::save( const std::string &path, bool objectBase, bool object )
{
   if ( objectBase ) {
      std::string terrainName = _tile->getName() + ".btg";
      saveObject( path, terrainName.c_str(), (size_t)-1 );
   }

   if ( object ) {
      for ( size_t i = 0; i < _objects.size(); i++ ) {
         std::string objectName = _objectName[ _objects[ i ] ] + ".btg";
         saveObject( path, objectName.c_str(), _objects[ i ] );
      }
   }

   return true;
}

bool FGSD_TriangleObject::saveAscii( const std::string &path, bool objectBase, bool object )
{
   if ( objectBase ) {
      std::string terrainName = _tile->getName() + ".asc";
      saveObjectAscii( path, terrainName.c_str(), (size_t)-1 );
   }

   if ( object ) {
      for ( size_t i = 0; i < _objects.size(); i++ ) {
         std::string objectName = _objectName[ _objects[ i ] ] + ".asc";
         saveObjectAscii( path, objectName.c_str(), _objects[ i ] );
      }
   }

   return true;
}


bool FGSD_TriangleObject::saveObject( const std::string &path, const char *name, size_t iObj )
{
   typedef std::list<CDTplus::Face_handle> FaceList;
   typedef std::map<size_t, FaceList> FaceListMap;
   FaceListMap flist;
   typedef std::map<CDTplus::Vertex_handle, size_t> VertexMap;
   VertexMap vmap;
   typedef std::vector<CDTplus::Vertex_handle> VertexVector;
   VertexVector vvect;
   size_t vcnt = 0;

   std::vector<FGSD_TriangleObject *> dummySpanList;
   std::vector<FGSD_TriangleObject *> *spanList = 0;
   if ( iObj == (size_t)-1 ) {
      dummySpanList.push_back( this );
      spanList = &dummySpanList;
   } else {
      spanList = &_objectSpan[ iObj ];
   }

   size_t si;
   for ( si = 0; si < spanList->size(); si++ ) {
      FGSD_TriangleObject *o = (*spanList)[ si ];
      for ( CDTplus::Finite_faces_iterator fi = o->_surface.finite_faces_begin();
               fi != o->_surface.finite_faces_end(); ++fi ) {
         CDTplus::Face_handle f = fi;
         if ( f->object == iObj ) {
            CDTplus::Vertex_handle v1 = f->vertex( 0 ),
                                   v2 = f->vertex( 1 ),
                                   v3 = f->vertex( 2 );
            size_t v1i, v2i, v3i;
            VertexMap::iterator vmi = vmap.find( v1 );
            if ( vmi == vmap.end() ) {
               v1i = vcnt++;
               vmap[ v1 ] = v1i;
               vvect.push_back( v1 );
            } else {
               v1i = vmi->second;
            }
            vmi = vmap.find( v2 );
            if ( vmi == vmap.end() ) {
               v2i = vcnt++;
               vmap[ v2 ] = v2i;
               vvect.push_back( v2 );
            } else {
               v2i = vmi->second;
            }
            vmi = vmap.find( v3 );
            if ( vmi == vmap.end() ) {
               v3i = vcnt++;
               vmap[ v3 ] = v3i;
               vvect.push_back( v3 );
            } else {
               v3i = vmi->second;
            }
            flist[ f->material ].push_back( f );
         }
      }
   }

   SGBinObject object;
   point_list wgs84_nodes, geod_nodes, normals;
   wgs84_nodes.resize( vvect.size() );
   geod_nodes.resize( vvect.size() );
   normals.resize( vvect.size() );

   point_list texcoords;
   for ( size_t i = 0; i < vvect.size(); i++ ) {
      CDTplus::Vertex_handle v = vvect[ i ];
      Point3D geod( v->point().x(), v->point().y(), v->elev );
      geod_nodes[ i ] = geod;
      geod.setlon( geod.lon() * SG_DEGREES_TO_RADIANS );
      geod.setlat( geod.lat() * SG_DEGREES_TO_RADIANS );
      wgs84_nodes[ i ] = sgGeodToCart( geod );
      normals[ i ] = Point3D( v->normal[0], v->normal[1], v->normal[2] );
   }

   group_list tris_v;
   group_list tris_tc;
   string_list tri_materials;
   SGBucket b = _tile->getBucket();

   size_t mcnt = 0;
   for ( FaceListMap::iterator it = flist.begin(); it != flist.end(); ++it ) {
      FaceList &fl = it->second;
      size_t imat = it->first;
      if ( imat == (size_t)-1 )
         continue;
      std::string mat = _materialList[ imat ];
      for ( FaceList::iterator ii = fl.begin(); ii != fl.end(); ++ii ) {
         tri_materials.push_back( mat );
         tris_v.push_back( int_list() );
         CDTplus::Face_handle f = *ii;
         CDTplus::Vertex_handle v1 = f->vertex( 0 ),
                                v2 = f->vertex( 1 ),
                                v3 = f->vertex( 2 );
         tris_v[ mcnt ].push_back( vmap.find( v1 )->second );
         tris_v[ mcnt ].push_back( vmap.find( v2 )->second );
         tris_v[ mcnt ].push_back( vmap.find( v3 )->second );

         /*if ( mat.substr( 0, 3 ) == "pa_" || mat.substr( 0, 3 ) == "pc_" ) {
         } else*/ {
            point_list tp_list = sgCalcTexCoords( b, geod_nodes, tris_v[ mcnt ] );
            int_list ti_list;
            for ( int k = 0; k < (int)tp_list.size(); ++k ) {
               ti_list.push_back( texcoords.size() );
               texcoords.push_back( tp_list[ k ] );
            }
            tris_tc.push_back( ti_list );
         }

         mcnt += 1;
      }
   }

   Point3D gbs_center;
   double gbs_radius = calc_gbs( wgs84_nodes, gbs_center );

   object.set_gbs_center( gbs_center );
   object.set_gbs_radius( gbs_radius );

   object.set_wgs84_nodes( wgs84_nodes );
   object.set_normals( normals );
   object.set_texcoords( texcoords );

   object.set_tris_v( tris_v );
   object.set_tris_tc( tris_tc );
   object.set_tri_materials( tri_materials );

   object.write_bin( path, name, b );

   return true;
}

/*
 * Function to allow multiple tiles to have the same offset UTM when exporting
 * Ascii.
 */
static
bool check_local_offset( const std::string &path, const char *name, Point3D &ref )
{
   // Check if file exists
   string offset = path + "/local_offset";
   string logname = path + "/" + name + ".offset";

   std::ifstream in(offset.c_str());
   std::ofstream log(logname.c_str());

   log << ref << endl;        // give the user the option of changing local_offset
   log << "\n# offset from :" << logname << endl;
   log << "# copy this to ./local_offset and recreate scenery to use this offset.";
   log << endl;
   log.close();

   if( in.good() ) {
      // in exists, read it into ref and return true
      in >> ref;
      //std::cout << "local offset found: " << ref << endl;
      in.close();
      return true;
   } else {
      // path/local_offset does *not* exist.  save ref to loname
      std::ofstream out(offset.c_str());
      if(out.good()) {
         out << ref << endl;
         /*        out << ref.x() << endl;
         out << ref.y() << endl;
         out << ref.z() << endl;*/
         out.close();
      } else {
         std::cerr << "check_local_offset: file open error." << endl;
         return false;
      }
      return false;
   }
}

bool FGSD_TriangleObject::saveObjectAscii( const std::string &path, const char *name, size_t iObj )
{
   typedef std::list<CDTplus::Face_handle> FaceList;
   typedef std::map<size_t, FaceList> FaceListMap;
   FaceListMap flist;
   typedef std::map<CDTplus::Vertex_handle, size_t> VertexMap;
   VertexMap vmap;
   typedef std::vector<CDTplus::Vertex_handle> VertexVector;
   VertexVector vvect;
   size_t vcnt = 0;

   std::vector<FGSD_TriangleObject *> dummySpanList;
   std::vector<FGSD_TriangleObject *> *spanList = 0;
   if ( iObj == (size_t)-1 ) {
      dummySpanList.push_back( this );
      spanList = &dummySpanList;
   } else {
      spanList = &_objectSpan[ iObj ];
   }

   size_t si;
   for ( si = 0; si < spanList->size(); si++ ) {
      FGSD_TriangleObject *o = (*spanList)[ si ];
      for ( CDTplus::Finite_faces_iterator fi = o->_surface.finite_faces_begin();
               fi != o->_surface.finite_faces_end(); ++fi ) {
         CDTplus::Face_handle f = fi;
         if ( f->object == iObj ) {
            CDTplus::Vertex_handle v1 = f->vertex( 0 ),
                                 v2 = f->vertex( 1 ),
                                 v3 = f->vertex( 2 );
            size_t v1i, v2i, v3i;
            VertexMap::iterator vmi = vmap.find( v1 );
            if ( vmi == vmap.end() ) {
               v1i = vcnt++;
               vmap[ v1 ] = v1i;
               vvect.push_back( v1 );
            } else {
               v1i = vmi->second;
            }
            vmi = vmap.find( v2 );
            if ( vmi == vmap.end() ) {
               v2i = vcnt++;
               vmap[ v2 ] = v2i;
               vvect.push_back( v2 );
            } else {
               v2i = vmi->second;
            }
            vmi = vmap.find( v3 );
            if ( vmi == vmap.end() ) {
               v3i = vcnt++;
               vmap[ v3 ] = v3i;
               vvect.push_back( v3 );
            } else {
               v3i = vmi->second;
            }
            flist[ f->material ].push_back( f );
         }
      }
   }

   SGBinObject object;
   point_list utm_nodes, wgs84_nodes, geod_nodes, normals;
   wgs84_nodes.resize( vvect.size() );
   utm_nodes.resize( vvect.size() );
   geod_nodes.resize( vvect.size() );
   normals.resize( vvect.size() );

   point_list texcoords;

// Do utm_nodes for Ascii vertex's
   for ( size_t i = 0; i < vvect.size(); i++ ) {
      CDTplus::Vertex_handle v = vvect[ i ];
      Point3D geod( v->point().x(), v->point().y(), v->elev );

      geod_nodes[ i ] = geod;
      double utm_x, utm_y;
      int zone;
      if ( FGSD_Util::getUTMPosition( FGSD_Util::D_WGS84, utm_x, utm_y, zone, geod.lat(), geod.lon() ) != true ) {
         std::cout << "getUTMPosition() failed." << endl;
      }
      geod.setlon( utm_x );
      geod.setlat( utm_y );
      geod.setz( v->elev );
      utm_nodes[ i ] = geod;

      normals[ i ] = Point3D( v->normal[0], v->normal[1], v->normal[2] );
   }

// Do gbs_center calculation.
   Point3D utm_max(0.0,0.0,0.0), utm_min(0.0,0.0,0.0), utm_center;
   for ( size_t i = 0; i < vvect.size(); i++ ) {
      Point3D utmp = Point3D( utm_nodes[i].x(), utm_nodes[i].y(), 0.0 );
      // Initialize min/max to within the tile or it won't work...
      if(utm_max.x()==0.0) {
         utm_max.setx( utmp.x() );
         utm_max.sety( utmp.y() );
         utm_min.setx( utmp.x() );
         utm_min.sety( utmp.y() );
      }
                        
      if( utmp.x() > utm_max.x() ) utm_max.setx( utmp.x() );
      if( utmp.y() > utm_max.y() ) utm_max.sety( utmp.y() );

      if( utmp.x() < utm_min.x() ) utm_min.setx( utmp.x() );
      if( utmp.y() < utm_min.y() ) utm_min.sety( utmp.y() );
   }
// utm_center = utm_min + 0.5*(utm_max - utm_min);
   utm_center.setx(utm_min.x() + 0.5*(utm_max.x() - utm_min.x()) );
   utm_center.sety(utm_min.y() + 0.5*(utm_max.y() - utm_min.y()) );
   utm_center.setz(0.0);

   group_list tris_v;
   group_list tris_tc;
   string_list tri_materials;
   SGBucket b = _tile->getBucket();

   size_t mcnt = 0;
   for ( FaceListMap::iterator it = flist.begin(); it != flist.end(); ++it ) {
      FaceList &fl = it->second;
      size_t imat = it->first;
      if ( imat == (size_t)-1 )
         continue;
      std::string mat = _materialList[ imat ];
      for ( FaceList::iterator ii = fl.begin(); ii != fl.end(); ++ii ) {
         tri_materials.push_back( mat );
         tris_v.push_back( int_list() );
         CDTplus::Face_handle f = *ii;
         CDTplus::Vertex_handle v1 = f->vertex( 0 ),
                                v2 = f->vertex( 1 ),
                                v3 = f->vertex( 2 );
         tris_v[ mcnt ].push_back( vmap.find( v1 )->second );
         tris_v[ mcnt ].push_back( vmap.find( v2 )->second );
         tris_v[ mcnt ].push_back( vmap.find( v3 )->second );

         point_list tp_list = sgCalcTexCoords( b, geod_nodes, tris_v[ mcnt ] );
         int_list ti_list;
         for ( int k = 0; k < (int)tp_list.size(); ++k ) {
            ti_list.push_back( texcoords.size() );
            texcoords.push_back( tp_list[ k ] );
         }
         tris_tc.push_back( ti_list );

         mcnt += 1;
      }
   }

   Point3D gbs_center;
/*
 * For Ascii offset UTM I check if the file 'local_offset' is in the saveTo directory.
 *      If not, we calculate the current gbs_center and use it directly and write
 *      the value to 'local_offset'.  If 'local_offset' already exists, I simply
 *      read it into gbs_center.  This will cause all tiles to be referenced to
 *      the center defined by 'local_offset'.  One may define 'local_offset' prior
 *      to exporting a Ascii tile and the tile will be adjusted to the local data
 *      provided.        --Dale
 *
 *      Notes:        Currently gbs_radius is unused and will not be correct.
 */
   // Check and possibly set local_offset
   check_local_offset( path, name, utm_center );        // Needed for multiple tiles!
   // Note: local_offset may be changed inside check_local_offset()
   object.set_gbs_center( utm_center );
   object.set_gbs_radius( utm_center.z() );

   object.set_wgs84_nodes( utm_nodes );        // Uses utm_nodes rather than wgs84_nodes!
   object.set_normals( normals );
   object.set_texcoords( texcoords );

   object.set_tris_v( tris_v );
   object.set_tris_tc( tris_tc );
   object.set_tri_materials( tri_materials );

   object.write_ascii( path, name, b );

   return true;
}

// -- This code comes from TerraGear : www.terragear.org

// calculate the global bounding sphere.  Center is the center of the
// tile and zero elevation
double FGSD_TriangleObject::calc_gbs( const point_list &nodes, Point3D &gbs_center ) {
    double dist_squared;
    double radius_squared = 0;
    
    SGBucket b = _tile->getBucket();

    Point3D p( b.get_center_lon() * SGD_DEGREES_TO_RADIANS, 
               b.get_center_lat() * SGD_DEGREES_TO_RADIANS,
               0 );

    gbs_center = sgGeodToCart(p);

    const_point_list_iterator current =  nodes.begin();
    const_point_list_iterator last = nodes.end();
    for ( ; current != last; ++current ) {
        dist_squared = gbs_center.distance3Dsquared(*current);
        if ( dist_squared > radius_squared ) {
            radius_squared = dist_squared;
        }
    }

    return sqrt(radius_squared);
}

bool FGSD_TriangleObject::changeMaterial( const std::string &mat, double lon, double lat )
{
   bool ret = false;
   _lastLocate = _surface.locate( CDTplus::Point( lon, lat ), _lastLocate );

   if ( _lastLocate != CDTplus::Face_handle() ) {
      _lastLocate->material = findMaterial( mat );
      _modified = true;
      for ( int i = 0; i < 3; i++ ) {
         CDTplus::Face_handle n = _lastLocate->neighbor( i );
         int m = _lastLocate->mirror_index( i );
         if ( _lastLocate->material != (size_t)-1 && _lastLocate->material == n->material && _lastLocate->object == n->object ) {
            if ( _surface.is_constrained_edge( _lastLocate->vertex(CDTplus::cw(i)), _lastLocate->vertex(CDTplus::ccw(i)) ) ) {
               _surface.remove_constraint( _lastLocate->vertex(CDTplus::cw(i)),
                                           _lastLocate->vertex(CDTplus::ccw(i)) );
            }
         } else {
            if ( !_surface.is_constrained_edge( _lastLocate->vertex(CDTplus::cw(i)), _lastLocate->vertex(CDTplus::ccw(i)) ) ) {
               _surface.insert_constraint( _lastLocate->vertex(CDTplus::cw(i)),
                                          _lastLocate->vertex(CDTplus::ccw(i)) );
            }
         }
      }
      ret = true;
   }

   return ret;
}

FGSD_TriangleObject::CDTplus::Vertex_handle FGSD_TriangleObject::embedPoint( const Point3D &p )
{
   CDTplus::Vertex_handle v = _surface.insert( CDTplus::Point( p.x(), p.y() ) );
   v->elev = p.z();
   v->interpolate_z = false;
   return v;
}

void FGSD_TriangleObject::embedEdge( CDTplus::Vertex_handle va, CDTplus::Vertex_handle vb )
{
   _surface.insert_constraint( va, vb );
}

void FGSD_TriangleObject::recoverFaceData()
{
   postLoad();
}