void Surf::ApplyES( vec3d uw, double t )
{
double grm1 = m_GridDensityPtr->m_GrowRatio() - 1.0;
int nmapu = m_SrcMap.size();
int nmapw = m_SrcMap[0].size();
int ibase, jbase;
double u = uw.x();
double w = uw.y();
UWtoTargetMapij( u, w, ibase, jbase );
vec3d p = m_SurfCore.CompPnt( u, w );
int iadd[] = { 0, 1, 0, 1 };
int jadd[] = { 0, 0, 1, 1 };
for( int i = 0; i < 4; i++ )
{
int itarget = ibase + iadd[i];
int jtarget = jbase + jadd[i];
if( itarget < nmapu && itarget >= 0 && jtarget < nmapw && jtarget >= 0 )
{
vec3d p2 = m_SrcMap[ itarget ][ jtarget ].m_pt;
double r = ( p2 - p ).mag();
double targetstr = t + r * grm1;
if( m_SrcMap[ itarget ][ jtarget ].m_str > targetstr )
{
m_SrcMap[ itarget ][ jtarget ].m_str = targetstr;
WalkMap( itarget, jtarget, itarget, jtarget );
}
}
}
}
void Surf::WalkMap( int istart, int jstart, int icurrent, int jcurrent )
{
static const int iadd[] = { -1, 1, 0, 0 };
static const int jadd[] = { 0, 0, -1, 1 };
for( int i = 0; i < 4; i++ )
{
static int itarget;
itarget = icurrent + iadd[i];
static int jtarget;
jtarget = jcurrent + jadd[i];
if( itarget < m_SrcMap.size() && itarget >= 0 && jtarget < m_SrcMap[0].size() && jtarget >= 0 )
{
static double targetstr;
targetstr = m_SrcMap[istart][jstart].m_str +
( m_SrcMap[ itarget ][ jtarget ].m_pt - m_SrcMap[istart][jstart].m_pt ).mag() *
(m_GridDensityPtr->m_GrowRatio() - 1.0);
if( m_SrcMap[ itarget ][ jtarget ].m_str > targetstr )
{
m_SrcMap[ itarget ][ jtarget ].m_str = targetstr;
WalkMap( istart, jstart, itarget, jtarget );
}
}
}
}
void Surf::WalkMap( int istart, int jstart, int kstart, int icurrent, int jcurrent )
{
static int iadd[] = { -1, 1, 0, 0 };
static int jadd[] = { 0, 0, -1, 1 };
for( int i = 0; i < 4; i++ )
{
static int itarget;
itarget = icurrent + iadd[i];
static int jtarget;
jtarget = jcurrent + jadd[i];
if( itarget < m_SrcMap.size() && itarget >= 0 && jtarget < m_SrcMap[0].size() && jtarget >= 0 )
{
if( m_SrcMap[ itarget ][ jtarget ].m_maxvisited < kstart )
{
m_SrcMap[ itarget ][ jtarget ].m_maxvisited = kstart;
static double targetstr;
targetstr = m_SrcMap[ istart ][ jstart ].m_str +
( m_SrcMap[ itarget ][ jtarget ].m_pt - m_SrcMap[ istart ][ jstart ].m_pt ).mag() *
( m_GridDensityPtr->m_GrowRatio() - 1.0 );
if( m_SrcMap[ itarget ][ jtarget ].m_str > targetstr )
{
// Mark dominated as progress is made
m_SrcMap[ itarget ][ jtarget ].m_dominated = true;
m_SrcMap[ itarget ][ jtarget ].m_str = targetstr;
WalkMap( istart, jstart, kstart, itarget, jtarget );
}
}
}
}
}
void Surf::LimitTargetMap( MSCloud &es_cloud, MSTree &es_tree, double minmap )
{
double grm1 = m_GridDensityPtr->GetGrowRatio() - 1.0;
double tmin = min( minmap, es_cloud.sources[0]->m_str );
SearchParams params;
params.sorted = false;
int nmapu = m_SrcMap.size();
int nmapw = m_SrcMap[0].size();
// Loop over surface evaluating source strength and curvature
for( int i = 0; i < nmapu ; i++ )
{
// double u = ( 1.0 * i ) / ( m_NumMap - 1 );
for( int j = 0; j < nmapw ; j++ )
{
// double w = ( 1.0 * j ) / ( m_NumMap - 1 );
double *query_pt = m_SrcMap[i][j].m_pt.v;
double t = m_SrcMap[i][j].m_str;
double torig = t;
double rmax = ( t - tmin ) / grm1;
if( rmax > 0.0 )
{
double r2max = rmax * rmax;
MSTreeResults es_matches;
int nMatches = es_tree.radiusSearch( query_pt, r2max, es_matches, params );
for (int k = 0; k < nMatches; k++ )
{
int imatch = es_matches[k].first;
double r = sqrt( es_matches[k].second );
double str = es_cloud.sources[imatch]->m_str;
double ts = str + grm1 * r;
t = min( t, ts );
}
if( t < torig )
{
m_SrcMap[i][j].m_str = t;
pair< int, int > ijstart( i, j );
WalkMap( ijstart, ijstart );
}
}
}
}
}
void Surf::WalkMap( pair< int, int > ijstart, int kstart, pair< int, int > ijcurrent )
{
int iadd[] = { -1, 1, 0, 0 };
int jadd[] = { 0, 0, -1, 1 };
double grm1 = m_GridDensityPtr->GetGrowRatio() - 1.0;
int nmapu = m_SrcMap.size();
int nmapw = m_SrcMap[0].size();
int istart = ijstart.first;
int jstart = ijstart.second;
MapSource srcstart = m_SrcMap[istart][jstart];
vec3d p = srcstart.m_pt;
double str = srcstart.m_str;
int icur = ijcurrent.first;
int jcur = ijcurrent.second;
for(int i = 0; i < 4; i++ )
{
int itarget = icur + iadd[i];
int jtarget = jcur + jadd[i];
if( itarget < nmapu && itarget >= 0 && jtarget < nmapw && jtarget >= 0 )
{
if( m_SrcMap[ itarget ][ jtarget ].m_maxvisited < kstart )
{
m_SrcMap[ itarget ][ jtarget ].m_maxvisited = kstart;
vec3d p2 = m_SrcMap[ itarget ][ jtarget ].m_pt;
double r = ( p2 - p ).mag();
double targetstr = str + r * grm1;
if( m_SrcMap[ itarget ][ jtarget ].m_str > targetstr )
{
// Mark dominated as progress is made
m_SrcMap[ itarget ][ jtarget ].m_dominated = true;
m_SrcMap[ itarget ][ jtarget ].m_str = targetstr;
pair< int, int > ijtarget( itarget, jtarget );
WalkMap( ijstart, kstart, ijtarget );
}
}
}
}
}
void Surf::LimitTargetMap()
{
int nmapu = m_SrcMap.size();
int nmapw = m_SrcMap[0].size();
int nmap = nmapu * nmapw;
// Create size sortable index of array i,j coordinates
vector< pair < double, pair < int, int > > > index;
index.resize( nmap );
int k = 0;
for( int i = 0; i < nmapu ; i++ )
{
for( int j = 0; j < nmapw ; j++ )
{
pair< int, int > ij( i, j );
pair < double, pair < int, int > > id( m_SrcMap[i][j].m_str, ij );
index[k] = id;
k++;
m_SrcMap[i][j].m_maxvisited = -1; // Reset traversal limiter.
}
}
// Sort index
std::sort( index.begin(), index.end(), indxcompare );
// Start from smallest
for( int k = 0; k < nmap; k++ )
{
pair< int, int > ij = index[k].second;
int i = ij.first;
int j = ij.second;
MapSource src = m_SrcMap[i][j];
// Recursively limit from small to large (skip if dominated)
if( !src.m_dominated )
{
WalkMap( i, j, k, i, j );
}
}
}