void Sonar2DUtils::DouglasPeucker(const Sonar2DUtils::tBottomLine& bottomLineFt, float epsilon, Sonar2DUtils::tBottomLine& returnResults)
{
const unsigned int minPoints = 3;
if(bottomLineFt.size() < minPoints)
{
returnResults = bottomLineFt;
}
else
{
Sonar2DUtils::tBottomLine lineSegment;
lineSegment.push_back( *bottomLineFt.begin() );
lineSegment.push_back( bottomLineFt.back() );
float dMax = 0.0f;
Sonar2DUtils::tBottomLine::const_iterator iterMax;
Sonar2DUtils::tBottomLine::const_iterator iterEnd = bottomLineFt.begin() + (bottomLineFt.size() - 2);
// Find the point with maximum distance, excluding the first and last point of the segment.
for(Sonar2DUtils::tBottomLine::const_iterator iter=bottomLineFt.begin()+1; iter!=iterEnd; ++iter )
{
float d = ShortestDistanceToSegment( *iter, lineSegment );
if( d > dMax )
{
dMax = d;
iterMax = iter;
}
}
// If max distance is greater than epsilon, recursively simplify
if( dMax > epsilon)
{
// Split up new lines
Sonar2DUtils::tBottomLine line1( bottomLineFt.begin(), (iterMax+1) );
Sonar2DUtils::tBottomLine line2( iterMax, bottomLineFt.end() );
Sonar2DUtils::tBottomLine results1;
DouglasPeucker(line1, epsilon, results1 );
Sonar2DUtils::tBottomLine results2;
DouglasPeucker(line2, epsilon, results2 );
if(results1.size() < minPoints)
{
returnResults.push_back( *results1.begin() );
}
else
{
returnResults = Sonar2DUtils::tBottomLine( results1.begin(), (results1.begin() + results1.size() - 2) );
}
returnResults.insert( returnResults.end(), results2.begin(), results2.end() );
}
else
{
returnResults.push_back( *bottomLineFt.begin() );
returnResults.push_back( bottomLineFt.back() );
}
}
}
// Recursive function used by cpPolylineSimplifyCurves().
static cpPolyline *
DouglasPeucker(
cpVect *verts, cpPolyline *reduced,
int length, int start, int end,
cpFloat min, cpFloat tol
){
// Early exit if the points are adjacent
if((end - start + length)%length < 2) return reduced;
cpVect a = verts[start];
cpVect b = verts[end];
// Check if the length is below the threshold
if(cpvnear(a, b, min) && cpPolylineIsShort(verts, length, start, end, min)) return reduced;
// Find the maximal vertex to split and recurse on
cpFloat max = 0.0;
int maxi = start;
cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
cpFloat d = cpvdot(n, a);
for(int i=Next(start, length); i!=end; i=Next(i, length)){
cpFloat dist = fabs(cpvdot(n, verts[i]) - d);
if(dist > max){
max = dist;
maxi = i;
}
}
if(max > tol){
reduced = DouglasPeucker(verts, reduced, length, start, maxi, min, tol);
reduced = cpPolylinePush(reduced, verts[maxi]);
reduced = DouglasPeucker(verts, reduced, length, maxi, end, min, tol);
}
return reduced;
}
// Recursively reduce the vertex count on a polyline. Works best for smooth shapes.
// 'tol' is the maximum error for the reduction.
// The reduced polyline will never be farther than this distance from the original polyline.
cpPolyline *
cpPolylineSimplifyCurves(cpPolyline *line, cpFloat tol)
{
cpPolyline *reduced = cpPolylineMake(line->count);
cpFloat min = tol/2.0f;
if(cpPolylineIsClosed(line)){
int start, end;
cpLoopIndexes(line->verts, line->count - 1, &start, &end);
reduced = cpPolylinePush(reduced, line->verts[start]);
reduced = DouglasPeucker(line->verts, reduced, line->count - 1, start, end, min, tol);
reduced = cpPolylinePush(reduced, line->verts[end]);
reduced = DouglasPeucker(line->verts, reduced, line->count - 1, end, start, min, tol);
reduced = cpPolylinePush(reduced, line->verts[start]);
} else {
reduced = cpPolylinePush(reduced, line->verts[0]);
reduced = DouglasPeucker(line->verts, reduced, line->count, 0, line->count - 1, min, tol);
reduced = cpPolylinePush(reduced, line->verts[line->count - 1]);
}
return cpPolylineShrink(reduced);
}
// Build PolyTessGeo Object from OGR Polygon
// Using OpenGL/GLU tesselator
int PolyTessGeo::PolyTessGeoGL(OGRPolygon *poly, bool bSENC_SM, double ref_lat, double ref_lon)
{
#ifdef ocpnUSE_GL
int iir, ip;
int *cntr;
GLdouble *geoPt;
wxString sout;
wxString sout1;
wxString stemp;
// Make a quick sanity check of the polygon coherence
bool b_ok = true;
OGRLineString *tls = poly->getExteriorRing();
if(!tls) {
b_ok = false;
}
else {
int tnpta = poly->getExteriorRing()->getNumPoints();
if(tnpta < 3 )
b_ok = false;
}
for( iir=0 ; iir < poly->getNumInteriorRings() ; iir++)
{
int tnptr = poly->getInteriorRing(iir)->getNumPoints();
if( tnptr < 3 )
b_ok = false;
}
if( !b_ok )
return ERROR_BAD_OGRPOLY;
#ifdef __WXMSW__
// If using the OpenGL dlls provided with Windows,
// load the dll and establish addresses of the entry points needed
#ifdef USE_GLU_DLL
if(!s_glu_dll_ready)
{
s_hGLU_DLL = LoadLibrary("glu32.dll");
if (s_hGLU_DLL != NULL)
{
s_lpfnTessProperty = (LPFNDLLTESSPROPERTY)GetProcAddress(s_hGLU_DLL,"gluTessProperty");
s_lpfnNewTess = (LPFNDLLNEWTESS)GetProcAddress(s_hGLU_DLL, "gluNewTess");
s_lpfnTessBeginContour = (LPFNDLLTESSBEGINCONTOUR)GetProcAddress(s_hGLU_DLL, "gluTessBeginContour");
s_lpfnTessEndContour = (LPFNDLLTESSENDCONTOUR)GetProcAddress(s_hGLU_DLL, "gluTessEndContour");
s_lpfnTessBeginPolygon = (LPFNDLLTESSBEGINPOLYGON)GetProcAddress(s_hGLU_DLL, "gluTessBeginPolygon");
s_lpfnTessEndPolygon = (LPFNDLLTESSENDPOLYGON)GetProcAddress(s_hGLU_DLL, "gluTessEndPolygon");
s_lpfnDeleteTess = (LPFNDLLDELETETESS)GetProcAddress(s_hGLU_DLL, "gluDeleteTess");
s_lpfnTessVertex = (LPFNDLLTESSVERTEX)GetProcAddress(s_hGLU_DLL, "gluTessVertex");
s_lpfnTessCallback = (LPFNDLLTESSCALLBACK)GetProcAddress(s_hGLU_DLL, "gluTessCallback");
s_glu_dll_ready = true;
}
else
{
return ERROR_NO_DLL;
}
}
#endif
#endif
// Allocate a work buffer, which will be grown as needed
#define NINIT_BUFFER_LEN 10000
s_pwork_buf = (GLdouble *)malloc(NINIT_BUFFER_LEN * 2 * sizeof(GLdouble));
s_buf_len = NINIT_BUFFER_LEN * 2;
s_buf_idx = 0;
// Create an array to hold pointers to allocated vertices created by "combine" callback,
// so that they may be deleted after tesselation.
s_pCombineVertexArray = new wxArrayPtrVoid;
// Create tesselator
GLUtessobj = gluNewTess();
// Register the callbacks
gluTessCallback(GLUtessobj, GLU_TESS_BEGIN, (GLvoid (__CALL_CONVENTION *) ())&beginCallback);
gluTessCallback(GLUtessobj, GLU_TESS_BEGIN, (GLvoid (__CALL_CONVENTION *) ())&beginCallback);
gluTessCallback(GLUtessobj, GLU_TESS_VERTEX, (GLvoid (__CALL_CONVENTION *) ())&vertexCallback);
gluTessCallback(GLUtessobj, GLU_TESS_END, (GLvoid (__CALL_CONVENTION *) ())&endCallback);
gluTessCallback(GLUtessobj, GLU_TESS_COMBINE, (GLvoid (__CALL_CONVENTION *) ())&combineCallback);
// gluTessCallback(GLUtessobj, GLU_TESS_ERROR, (GLvoid (__CALL_CONVENTION *) ())&errorCallback);
// glShadeModel(GL_SMOOTH);
gluTessProperty(GLUtessobj, GLU_TESS_WINDING_RULE,
GLU_TESS_WINDING_POSITIVE );
// gluTess algorithm internally selects vertically oriented triangle strips and fans.
// This orientation is not optimal for conventional memory-mapped raster display shape filling.
// We can "fool" the algorithm by interchanging the x and y vertex values passed to gluTessVertex
// and then reverting x and y on the resulting vertexCallbacks.
// In this implementation, we will explicitely set the preferred orientation.
//Set the preferred orientation
tess_orient = TESS_HORZ; // prefer horizontal tristrips
// PolyGeo BBox as lat/lon
OGREnvelope Envelope;
poly->getEnvelope(&Envelope);
xmin = Envelope.MinX;
ymin = Envelope.MinY;
xmax = Envelope.MaxX;
ymax = Envelope.MaxY;
// Get total number of contours
m_ncnt = 1; // always exterior ring
int nint = poly->getNumInteriorRings(); // interior rings
m_ncnt += nint;
// Allocate cntr array
cntr = (int *)malloc(m_ncnt * sizeof(int));
// Get total number of points(vertices)
int npta = poly->getExteriorRing()->getNumPoints();
cntr[0] = npta;
npta += 2; // fluff
for( iir=0 ; iir < nint ; iir++)
{
int nptr = poly->getInteriorRing(iir)->getNumPoints();
cntr[iir+1] = nptr;
npta += nptr + 2;
}
// printf("pPoly npta: %d\n", npta);
geoPt = (GLdouble *)malloc((npta) * 3 * sizeof(GLdouble)); // vertex array
// Grow the work buffer if necessary
if((npta * 4) > s_buf_len)
{
s_pwork_buf = (GLdouble *)realloc(s_pwork_buf, npta * 4 * sizeof(GLdouble));
s_buf_len = npta * 4;
}
// Define the polygon
gluTessBeginPolygon(GLUtessobj, NULL);
// Create input structures
// Exterior Ring
int npte = poly->getExteriorRing()->getNumPoints();
cntr[0] = npte;
GLdouble *ppt = geoPt;
// Check and account for winding direction of ring
bool cw = !(poly->getExteriorRing()->isClockwise() == 0);
double x0, y0, x, y;
OGRPoint p;
if(cw)
{
poly->getExteriorRing()->getPoint(0, &p);
x0 = p.getX();
y0 = p.getY();
}
else
{
poly->getExteriorRing()->getPoint(npte-1, &p);
x0 = p.getX();
y0 = p.getY();
}
// Transcribe contour to an array of doubles, with duplicates eliminated
double *DPbuffer = (double *)malloc(npte * 2 * sizeof(double));
double *DPrun = DPbuffer;
int nPoints = npte;
for(ip = 0 ; ip < npte ; ip++)
{
int pidx;
if(cw)
pidx = npte - ip - 1;
else
pidx = ip;
poly->getExteriorRing()->getPoint(pidx, &p);
x = p.getX();
y = p.getY();
if( ((fabs(x-x0) > EQUAL_EPS) || (fabs(y-y0) > EQUAL_EPS)))
{
GLdouble *ppt_temp = ppt;
if(tess_orient == TESS_VERT)
{
*DPrun++ = x;
*DPrun++ = y;
}
else
{
*DPrun++ = y;
*DPrun++ = x;
}
x0 = x;
y0 = y;
}
else
nPoints--;
}
if(nPoints > 5 && (m_LOD_meters > .01)){
index_keep.Clear();
index_keep.Add(0);
index_keep.Add(nPoints-1);
index_keep.Add(1);
index_keep.Add(nPoints-2);
DouglasPeucker(DPbuffer, 1, nPoints-2, m_LOD_meters/(1852 * 60), &index_keep);
// printf("DP Reduction: %d/%d\n", index_keep.GetCount(), nPoints);
g_keep += index_keep.GetCount();
g_orig += nPoints;
// printf("...................Running: %g\n", (double)g_keep/g_orig);
}
else {
index_keep.Clear();
for(int i = 0 ; i < nPoints ; i++)
index_keep.Add(i);
}
cntr[0] = index_keep.GetCount();
// Mark the keepers by adding a simple constant to X
for(unsigned int i=0 ; i < index_keep.GetCount() ; i++){
int k = index_keep.Item(i);
DPbuffer[2*k] += 2000.;
}
// Declare the gluContour and copy the points
gluTessBeginContour(GLUtessobj);
DPrun = DPbuffer;
for(ip = 0 ; ip < nPoints ; ip++)
{
x = *DPrun++;
y = *DPrun++;
if(x > 1000.){
GLdouble *ppt_top = ppt;
*ppt++ = x-2000;
*ppt++ = y;
*ppt++ = 0;
gluTessVertex( GLUtessobj, ppt_top, ppt_top ) ;
}
}
gluTessEndContour(GLUtessobj);
free(DPbuffer);
// Now the interior contours
for(iir=0 ; iir < nint ; iir++)
{
gluTessBeginContour(GLUtessobj);
int npti = poly->getInteriorRing(iir)->getNumPoints();
// Check and account for winding direction of ring
bool cw = !(poly->getInteriorRing(iir)->isClockwise() == 0);
if(!cw)
{
poly->getInteriorRing(iir)->getPoint(0, &p);
x0 = p.getX();
y0 = p.getY();
}
else
{
poly->getInteriorRing(iir)->getPoint(npti-1, &p);
x0 = p.getX();
y0 = p.getY();
}
// Transcribe points to vertex array, in proper order with no duplicates
// also, accounting for tess_orient
for(int ip = 0 ; ip < npti ; ip++)
{
OGRPoint p;
int pidx;
if(!cw) // interior contours must be cw
pidx = npti - ip - 1;
else
pidx = ip;
poly->getInteriorRing(iir)->getPoint(pidx, &p);
x = p.getX();
y = p.getY();
if((fabs(x-x0) > EQUAL_EPS) || (fabs(y-y0) > EQUAL_EPS))
{
GLdouble *ppt_temp = ppt;
if(tess_orient == TESS_VERT)
{
*ppt++ = x;
*ppt++ = y;
}
else
{
*ppt++ = y;
*ppt++ = x;
}
*ppt++ = 0.0;
gluTessVertex( GLUtessobj, ppt_temp, ppt_temp ) ;
// printf("tess from Poly, internal vertex %d %g %g\n", ip, x, y);
}
else
cntr[iir+1]--;
x0 = x;
y0 = y;
}
gluTessEndContour(GLUtessobj);
}
// Store some SM conversion data in static store,
// for callback access
s_ref_lat = ref_lat;
s_ref_lon = ref_lon;
s_bSENC_SM = bSENC_SM;
s_bmerc_transform = false;
// Ready to kick off the tesselator
s_pTPG_Last = NULL;
s_pTPG_Head = NULL;
s_nvmax = 0;
gluTessEndPolygon(GLUtessobj); // here it goes
m_nvertex_max = s_nvmax; // record largest vertex count, updates in callback
// Tesselation all done, so...
// Create the data structures
m_ppg_head = new PolyTriGroup;
m_ppg_head->m_bSMSENC = s_bSENC_SM;
m_ppg_head->nContours = m_ncnt;
m_ppg_head->pn_vertex = cntr; // pointer to array of poly vertex counts
m_ppg_head->data_type = DATA_TYPE_DOUBLE;
// Transcribe the raw geometry buffer
// Converting to float as we go, and
// allowing for tess_orient
// Also, convert to SM if requested
// Recalculate the size of the geometry buffer
int nptfinal = cntr[0] + 2;
for(int i=0 ; i < nint ; i++)
nptfinal += cntr[i+1] + 2;
// No longer need the full geometry in the SENC,
nptfinal = 1;
m_nwkb = (nptfinal + 1) * 2 * sizeof(float);
m_ppg_head->pgroup_geom = (float *)calloc(sizeof(float), (nptfinal + 1) * 2);
float *vro = m_ppg_head->pgroup_geom;
ppt = geoPt;
float tx,ty;
for(ip = 0 ; ip < nptfinal ; ip++)
{
if(TESS_HORZ == tess_orient)
{
ty = *ppt++;
tx = *ppt++;
}
else
{
tx = *ppt++;
ty = *ppt++;
}
if(bSENC_SM)
{
// Calculate SM from chart common reference point
double easting, northing;
toSM(ty, tx, ref_lat, ref_lon, &easting, &northing);
*vro++ = easting; // x
*vro++ = northing; // y
}
else
{
*vro++ = tx; // lon
*vro++ = ty; // lat
}
ppt++; // skip z
}
m_ppg_head->tri_prim_head = s_pTPG_Head; // head of linked list of TriPrims
// Convert the Triangle vertex arrays into a single memory allocation of floats
// to reduce SENC size and enable efficient access later
// First calculate the total byte size
int total_byte_size = 2 * sizeof(float);
TriPrim *p_tp = m_ppg_head->tri_prim_head;
while( p_tp ) {
total_byte_size += p_tp->nVert * 2 * sizeof(float);
p_tp = p_tp->p_next; // pick up the next in chain
}
float *vbuf = (float *)malloc(total_byte_size);
p_tp = m_ppg_head->tri_prim_head;
float *p_run = vbuf;
while( p_tp ) {
float *pfbuf = p_run;
GLdouble *pdouble_buf = (GLdouble *)p_tp->p_vertex;
for( int i=0 ; i < p_tp->nVert * 2 ; ++i){
float x = (float)( *((GLdouble *)pdouble_buf) );
pdouble_buf++;
*p_run++ = x;
}
free(p_tp->p_vertex);
p_tp->p_vertex = (double *)pfbuf;
p_tp = p_tp->p_next; // pick up the next in chain
}
m_ppg_head->bsingle_alloc = true;
m_ppg_head->single_buffer = (unsigned char *)vbuf;
m_ppg_head->single_buffer_size = total_byte_size;
m_ppg_head->data_type = DATA_TYPE_FLOAT;
gluDeleteTess(GLUtessobj);
free( s_pwork_buf );
s_pwork_buf = NULL;
free (geoPt);
// Free up any "Combine" vertices created
for(unsigned int i = 0; i < s_pCombineVertexArray->GetCount() ; i++)
free (s_pCombineVertexArray->Item(i));
delete s_pCombineVertexArray;
m_bOK = true;
#endif // #ifdef ocpnUSE_GL
return 0;
}
