std::vector<Vector3D> path2root(kdres *node){
std::vector<Vector3D> result;
Vector3D x = *((node_data*)kd_res_item_data(node))->x;
result.push_back(x);
kdres *parent = ((node_data*)kd_res_item_data(node))->parent;
while( parent != 0 ){
x = *((node_data*)kd_res_item_data(parent))->x;
result.push_back(x);
parent = ((node_data*)kd_res_item_data(parent))->parent;
}
return result;
}
/**
* @function getNearestNeighbor
*/
int JG_RRT::getNearestNeighbor( const Eigen::VectorXd &qsample )
{
struct kdres* result = kd_nearest( kdTree, qsample.data() );
uintptr_t nearest = (uintptr_t) kd_res_item_data( result );
activeNode = nearest;
return nearest;
}
// Takes a kdres set and returns a list of nodes
GSList *opttree_kdtree_to_gslist (state_t * state, kdres_t *kdres) {
GSList *node_list = NULL;
kd_res_rewind (kdres);
while (!kd_res_end(kdres)) {
node_t * node_curr = kd_res_item_data (kdres);
node_list = g_slist_prepend (node_list, node_curr);
kd_res_next (kdres);
}
return node_list;
}
// Find the nearest neighbor in the tree
node_t* opttree_find_nearest_neighbor (opttree_t *self, state_t *state_from) {
node_t *min_node = NULL;
kdres_t *kdres = kd_nearest (self->kdtree, optsystem_get_state_key (self->optsys, state_from));
if (kd_res_end (kdres)) {
printf ("ERROR: No nearest neighbors\n");
exit(1);
}
min_node = kd_res_item_data (kdres);
kd_res_free (kdres);
return min_node;
}
void Discontinuity::lookTopo ( Mesh &msh, double &mshCol, double &mshLon,
double &mshRad, int &mshInd )
{
Constants con;
if ( mshRad > ( con.R_EARTH - 100 ) )
{
kdres *set = kd_nearest3 ( elvTree, mshCol, mshLon, con.R_EARTH );
void *ind = kd_res_item_data ( set );
int point = * ( int * ) ind;
msh.elv[mshInd] = elv[point];
}
}
PetscReal ExodusModel::getMaterialParameterAtPoint(const std::vector<double> point,
const std::string parameter_name) {
// Ensure dimensions are consistent.
assert(point.size() == mNumberDimension);
// Get spatial index.
kdres *set = kd_nearest(mKdTree, point.data());
auto spatial_index = *(int *) kd_res_item_data(set);
kd_res_free(set);
// Get parameter index.
int i = 0;
int parameter_index;
for (auto &name: mNodalVariableNames) { if (name == parameter_name) parameter_index = i; i++; }
return mNodalVariables[parameter_index * mNumberNodalVariables + spatial_index];
}
void Discontinuity::lookCrust ( Mesh &msh, double &mshCol, double &mshLon,
double &mshRad, int &mshInd, bool &checkCrust,
bool &smoothCrust, double &upTap,
double &downTap, Model_file &mod )
{
Constants con;
double rho, vpv;
if ( mshRad > (con.R_EARTH - 100) )
{
kdres *set = kd_nearest3 ( crustTree, mshCol, mshLon, con.R_EARTH );
void *ind = kd_res_item_data ( set );
int point = * ( int * ) ind;
kd_res_free (set);
/* The moho is defined in a weird way ( depth from sea level if in the
ocean, and depth from elevation if in the crust). First, convert crust
elevation to km, and then decided whether we're taking the sea level or
or crustial surface as reference */
double ref;
if ( msh.elv[mshInd] <= 0. )
{
ref = con.R_EARTH;
}
else
{
ref = con.R_EARTH + msh.elv[mshInd] / 1000.;
}
// Do a bilinear interpolation on the Depth.
double interpDep;
double interpVs;
getCrustDepth ( mshCol, mshLon, point, interpDep, "dep" );
getCrustDepth ( mshCol, mshLon, point, interpVs, "vel" );
/* Here get crustal thickness for tapering purposes (differs if we're in)
oceanic of continental crust. This is the thickness of the crust, minus an
ocean layer. */
double crustThick = 0.;
if ( msh.elv[mshInd] <= 0. )
{
crustThick = interpDep + ( msh.elv[mshInd] );
}
else
{
crustThick = interpDep;
}
/* Figure out whether we need to smooth the crust */
smoothCrust = false;
smoothCosine ( crustThick, interpDep, mshRad, downTap, upTap, smoothCrust );
/* If we're in the crust */
// This used to say if mshRad >= (ref - interpDep) as well.
if ( mod.intentions == "CRUST" )
{
double crust_vsv = interpVs - con.aniCorrection;
double crust_vsh = interpVs;
// Scaling from isotropic vs to rho (Fichtner, multiscale)
rho = 0.2277 * crust_vsh + 2.016;
// Scaling from vs to vp
vpv = 1.5399 * crust_vsh + 0.840;
double N = rho * crust_vsh * crust_vsh;
double L = rho * crust_vsv * crust_vsv;
double A = rho * vpv * vpv;
double S = A - 2 * N;
double F = A - 2 * L;
checkCrust = true;
msh.c11[mshInd] = upTap * A + downTap * msh.c11[mshInd];
msh.c22[mshInd] = upTap * A + downTap * msh.c22[mshInd];
msh.c33[mshInd] = upTap * A + downTap * msh.c33[mshInd];
msh.c12[mshInd] = upTap * F + downTap * msh.c12[mshInd];
msh.c13[mshInd] = upTap * F + downTap * msh.c13[mshInd];
msh.c23[mshInd] = upTap * S + downTap * msh.c23[mshInd];
msh.c44[mshInd] = upTap * N + downTap * msh.c44[mshInd];
msh.c55[mshInd] = upTap * L + downTap * msh.c55[mshInd];
msh.c66[mshInd] = upTap * L + downTap * msh.c66[mshInd];
msh.rho[mshInd] = upTap * rho + downTap * msh.rho[mshInd];
}
if ( mshRad >= (ref - interpDep) )
{
checkCrust = true;
}
}
if ( mshRad <= con.R_EARTH - 100 )
{
upTap = 0.;
downTap = 1.;
}
}
void Discontinuity::getCrustDepth ( double &mshCol, double &mshLon, int &point,
double &par, std::string mode )
{
Constants con;
// Stride of one degree in crust07.
int degStri=2;
// Initializations.
int lonDir = 0;
int colDir = 0;
// Create vector to hold 4 nodes of smoothing square.
std::vector <int> nodes;
nodes.reserve (4);
// Determine on which side of the requested col. the closest point is.
if ( mshCol > crust_col_deg_unpack[point] )
colDir = 1;
if ( mshCol <= crust_col_deg_unpack[point] )
colDir = -1;
// Determine on which side of the requested lon. the closest point is.
if ( mshLon > crust_lon_deg_unpack[point] )
lonDir = 1;
if ( mshLon <= crust_lon_deg_unpack[point] )
lonDir = -1;
// Create the KDTree requests for four points of the interpolating square.
double col1 = crust_col_deg_unpack[point];
double col2 = crust_col_deg_unpack[point] + colDir * degStri;
double lon1 = crust_lon_deg_unpack[point];
double lon2 = crust_lon_deg_unpack[point] + lonDir * degStri;
// Fix col. wrapping.
if ( col2 > 180 )
col2 = 180 - ( col1 - 180 );
if ( col2 < 0 )
col2 = 0;
// Fix lon. wrapping.
if ( lon2 > 180 )
lon2 = lon2 - 360.;
if ( lon2 < -180 )
lon2 = lon2 + 360.;
// Find the four points of the rectangle in spherical space.
kdres *set1 = kd_nearest3 ( crustTree, col1, lon1, con.R_EARTH );
kdres *set2 = kd_nearest3 ( crustTree, col1, lon2, con.R_EARTH );
kdres *set3 = kd_nearest3 ( crustTree, col2, lon1, con.R_EARTH );
kdres *set4 = kd_nearest3 ( crustTree, col2, lon2, con.R_EARTH );
// Extract the indices from the result structures.
void *ind1 = kd_res_item_data ( set1 );
void *ind2 = kd_res_item_data ( set2 );
void *ind3 = kd_res_item_data ( set3 );
void *ind4 = kd_res_item_data ( set4 );
// Build the square based on the four possible quadrants.
if ( colDir == 1 && lonDir == (-1) )
{
nodes[0] = * ( int * ) ind2;
nodes[1] = * ( int * ) ind1;
nodes[2] = * ( int * ) ind3;
nodes[3] = * ( int * ) ind4;
}
else if ( colDir == 1 && lonDir == 1 )
{
nodes[0] = * ( int * ) ind1;
nodes[1] = * ( int * ) ind2;
nodes[2] = * ( int * ) ind4;
nodes[3] = * ( int * ) ind3;
}
else if ( colDir == (-1) && lonDir == 1 )
{
nodes[0] = * ( int * ) ind3;
nodes[1] = * ( int * ) ind4;
nodes[2] = * ( int * ) ind2;
nodes[3] = * ( int * ) ind1;
}
else if ( colDir == (-1) && lonDir == (-1) )
{
nodes[0] = * ( int * ) ind4;
nodes[1] = * ( int * ) ind3;
nodes[2] = * ( int * ) ind1;
nodes[3] = * ( int * ) ind2;
}
// Deallocate memory for results.
kd_res_free ( set1 );
kd_res_free ( set2 );
kd_res_free ( set3 );
kd_res_free ( set4 );
// Conditionally build the weights depending on the quandrants selected above.
double t = 0;
double u = 0;
if ( colDir == 1 )
t = ( mshCol - col1 ) / ( col2 - col1 );
if ( colDir == (-1) )
t = ( mshCol - col2 ) / ( col1 - col2 );
if ( lonDir == 1 )
u = ( mshLon - lon1 ) / ( lon2 - lon1 );
if ( lonDir == (-1) )
u = ( mshLon - lon2 ) / ( lon1 - lon2 );
/* Get the bilinearly interpolated value. Inspired by Numerical recipes,
but with a swwitched axis. */
if ( mode == "dep" )
{
par = (1 - t) * (1 - u) * crust_dp[0][nodes[0]] +
t * (1 - u) * crust_dp[0][nodes[3]] +
t * u * crust_dp[0][nodes[2]] +
(1 - t) * u * crust_dp[0][nodes[1]];
}
if ( mode == "vel" )
{
par = (1 - t) * (1 - u) * crust_vs[0][nodes[0]] +
t * (1 - u) * crust_vs[0][nodes[3]] +
t * u * crust_vs[0][nodes[2]] +
(1 - t) * u * crust_vs[0][nodes[1]];
}
#ifdef VISUAL_DEBUG
std::ofstream myfile;
myfile.open ( "surfInterp.txt", std::ios::out );
myfile << crust_col_deg_unpack[nodes[0]] << " "
<< crust_lon_deg_unpack[nodes[0]] << " " << crust_dp[0][nodes[0]]
<< std::endl;
myfile << crust_col_deg_unpack[nodes[1]] << " "
<< crust_lon_deg_unpack[nodes[1]] << " " << crust_dp[0][nodes[1]]
<< std::endl;
myfile << crust_col_deg_unpack[nodes[2]] << " "
<< crust_lon_deg_unpack[nodes[2]] << " " << crust_dp[0][nodes[2]]
<< std::endl;
myfile << crust_col_deg_unpack[nodes[3]] << " "
<< crust_lon_deg_unpack[nodes[3]] << " " << crust_dp[0][nodes[3]]
<< std::endl;
myfile << mshCol << " " << mshLon << " " << dep
<< std::endl;
std::cout << colDir << " " << lonDir << std::endl;
std::cout << mshCol << " " << col1 << " " << col2 << " " << std::endl;
std::cout << mshLon << " " << lon1 << " " << lon2 << " " << std::endl;
std::cout << t << " " << u << std::endl;
std::cout << crust_dp[0][nodes[0]] << " " << crust_dp[0][nodes[1]] << " "
<< crust_dp[0][nodes[2]] << " " << crust_dp[0][nodes[3]] << std::endl;
std::cout << dep << std::endl;
myfile.close();
std::cin.get();
std::cout << colDir << " " << lonDir << std::endl;
std::cout << mshCol << " " << col1 << " " << col2 << " " << std::endl;
std::cout << mshLon << " " << lon1 << " " << lon2 << " " << std::endl;
std::cout << t << " " << u << std::endl;
std::cout << crust_dp[0][nodes[0]] << " " << crust_dp[0][nodes[1]] << " "
<< crust_dp[0][nodes[2]] << " " << crust_dp[0][nodes[3]] << std::endl;
std::cout << dep << std::endl;
#endif
}
