gch_ling
/
kev
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
v4.5
main
develop
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '17e6c0d8ae'
${ noResults }
kev/Drawer/GVision/SurfaceGrid/lib/ASGridInterpolator.cpp

2465 lines
100 KiB
C++
Raw Normal View History Unescape Escape

搬运代码
1 month ago
/*------------------------------------------------------------------------------
* Copyright (c) 2023 by Bai Bing (seread@163.com)
* See COPYING file for copying and redistribution conditions.
*
* Alians IT Studio.
*----------------------------------------------------------------------------*/
#include <algorithm>
#include <array>
#include <cfloat>
#include <chrono>
#include <climits>
#include <cmath>
#include <cstdlib>
#include <execution>
#include <fstream>
#include <functional>
#include <future>
#include <iomanip>
#include <list>
#include <map>
#include <memory>
#include <numeric>
#include <stdexcept>
#include <thread>
#include <utility>
#include <vector>
#include "core/FunCheck.h"
#include "core/Time.h"
#include "utils/Misc.h"
#include "ASTriangle.h"
#include "ASPolygon.h"
#include "ASGridInterpolator.h"
#include "ASGridInterpolatorBlock.h"
#include "interpolator/ASInterpolatorSupport.h"
#include "interpolator/ASInterpolateMethod.h"
#include "linalg/lstsq.h"
#define _LIKELY(x) (x)
namespace ais
{
GridInterpolator::GridInterpolator() : maxIteration_(2000), residual_(0.0001), fillValue_(0), zMean_(std::nan("1")),
dxSize_(0), dySize_(0), alpha_(std::nan("1")), beta_(std::nan("1")), flagSucceed_(false)
{
}
GridInterpolator::~GridInterpolator()
{
}
GridInterpolator::GridInterpolator(const std::vector<ais::PointXYZ> &randomPoints, // random sample points
const std::vector<ais::Point> &areaPoints, // result grid area corners points
std::shared_ptr<ais::FMatrix> targetMatrix, // output result values
const std::vector<ais::BreakLine> &breaklines, // break lines
const std::vector<ais::Fault> &faults // faults
) : randomPoints_(randomPoints), areaPoints_(areaPoints), targetMatrixPtr_(targetMatrix), breaklines_(breaklines), faults_(faults)
{
generate_grid_params(targetMatrixPtr_);
// data statistics included point positions' distance, so it should be called after coordinates_points
if(randomPoints.size() < 1000)
generate_data_statistics();
}
void GridInterpolator::set_area_points(const std::vector<ais::Point> &points)
{
if (points.size() >= 2)
{
areaPoints_.clear();
areaPoints_ = points;
// update internal vars
auto xs = getAxisValues(areaPoints_, Axis3DType::X);
auto ys = getAxisValues(areaPoints_, Axis3DType::Y);
xMinMax_ = getMinMax(xs);
yMinMax_ = getMinMax(ys);
}
}
void GridInterpolator::start(InterpolationMethod method)
{
// before start interpolation, checkout required params
if (targetMatrixPtr_->size() == 0 || areaPoints_.empty())
THROW_INVALID_ARGUMENT("Grid interpolator require completed target matrix and sample points");
// fault may affect the constraint value, do it first
build_fault_impact_matrix(targetMatrixPtr_, pointInFaultMatrixPtr_, faultAffectMatrixPtr_);
// build the constraint matrix
build_constraint_matrix(targetMatrixPtr_, constrainMatrixPtr_);
// update edge nodes by nearest neighbors or fill value
update_edge_node(targetMatrixPtr_);
// check the grid size, if the grid size is too smell to get a smooth result,
// try to estimate a grid with bigger grid size
detect_estimate_factor();
scale_estimate();
// estimate the target matrix for better output
if (estimateMatrixPtr_.get())
estimate_gradients_surface(targetMatrixPtr_);
// update nan nodes with fill value
update_nan_node(targetMatrixPtr_);
// update whole matrix
start_interpolation(targetMatrixPtr_, constrainMatrixPtr_, pointInFaultMatrixPtr_, faultAffectMatrixPtr_, method);
flagEnd_ = true;
}
std::string GridInterpolator::report()
{
std::stringstream ss;
ss << "Iteration: " << iteration_ << " times, interpolations " << (flagSucceed_ ? "Succeed" : "Failed") << std::endl;
if (!faults_.empty())
{
ss << "No data node: " << faultNodesCount_ << std::endl;
}
ss << "Iteration execution time: " << iteration_time() << std::endl;
ss << "Result information: " << std::endl;
ss << "---------------------------------------" << std::endl;
ss << header_info();
ss << std::endl;
return ss.str();
}
std::string GridInterpolator::points_info()
{
std::stringstream ss;
auto pCount = randomPoints_.size();
ss << "Total " << pCount << " simple points : " << std::endl;
ss << "x, y, z" << std::endl;
if (pCount <= 10)
{
for (int i = 0; i < pCount; i++)
{
auto &p = randomPoints_[i];
ss << p.x << ", " << p.y << ", " << p.z << std::endl;
}
}
else
{
for (int i = 0; i <= 5; i++)
{
auto &p = randomPoints_[i];
ss << p.x << ", " << p.y << ", " << p.z << std::endl;
}
ss << "...";
for (auto pi = randomPoints_.cend() - 6; pi != randomPoints_.cend(); pi++)
{
auto &p = *pi;
ss << p.x << ", " << p.y << ", " << p.z << std::endl;
}
}
return ss.str();
}
std::string GridInterpolator::header_info(std::shared_ptr<ais::FMatrix> dataMatrixPtr)
{
if (!dataMatrixPtr.get())
dataMatrixPtr = targetMatrixPtr_;
auto shape = dataMatrixPtr->shape();
std::stringstream ss;
ss << shape.cols << " " << shape.rows << std::endl;
ss << std::fixed << std::setprecision(4);
ss << xMinMax_.first << " " << xMinMax_.second << std::endl;
ss << yMinMax_.first << " " << yMinMax_.second << std::endl;
ss << zMinMax_.first << " " << zMinMax_.second << std::endl;
return ss.str();
}
std::string GridInterpolator::data_info(std::shared_ptr<ais::FMatrix> dataMatrixPtr)
{
if (!dataMatrixPtr.get())
dataMatrixPtr = targetMatrixPtr_;
auto shape = dataMatrixPtr->shape();
std::string out_string;
out_string.reserve(256 * 1024 * 1024);
for (size_t r = 0; r < shape.rows; r++)
{
std::string line;
line.reserve(16 * 1024 * 1024);
for (size_t c = 0; c < shape.cols; c++)
{
line += std::to_string(dataMatrixPtr->at(r, c)) + " ";
}
out_string += line + "\n\n";
}
return out_string;
}
std::string GridInterpolator::iteration_time()
{
std::stringstream ss;
auto durSecs = std::chrono::duration_cast<std::chrono::milliseconds>(endTime_ - startTime_).count() / 1000.0;
auto hours = int(durSecs / 3600);
auto minutes = int((durSecs - hours * 3600) / 60);
auto seconds = durSecs - hours * 3600 - minutes * 60;
ss << hours << ":" << minutes << ":" << std::setprecision(3) << seconds;
return ss.str();
}
std::string GridInterpolator::get_progress_msg()
{
std::stringstream ss;
if (!interpolationMsgs_.empty())
{
std::lock_guard<std::mutex> lk(msgMutex_);
for (auto &s : interpolationMsgs_)
ss << s << std::endl;
// all strings have been transformed to ss, clean internal caches
interpolationMsgs_.clear();
}
return ss.str();
};
void GridInterpolator::dump_result(const char *filename, std::shared_ptr<ais::FMatrix> dataMatrixPtr)
{
if (!dataMatrixPtr.get())
dataMatrixPtr = targetMatrixPtr_;
std::lock_guard<std::mutex> lk(dataMutex_);
std::ofstream outfile;
outfile.open(filename, std::ios::out);
// header
outfile << "DSAA" << std::endl;
outfile << header_info(dataMatrixPtr);
// data
outfile << data_info(dataMatrixPtr);
outfile.close();
}
void GridInterpolator::dump_result_async(const std::string &filename, std::shared_ptr<ais::FMatrix> dataMatrixPtr, int8_t detailLevel)
{
/*
if (detailLevel <= this->detailLevel_ )
auto ret = std::async(std::launch::async,
[this, &filename, &dataMatrixPtr]()
{
dump_result(filename.c_str(), dataMatrixPtr);
return;
});
*/
}
void GridInterpolator::generate_grid_params(std::shared_ptr<ais::FMatrix> targetMatrixPtr)
{
if (targetMatrixPtr->size() != 0)
{
// config default parameters by input samplePoints, areaPoints, and result matrix
auto shape = targetMatrixPtr->shape();
auto totalGridPoints = targetMatrixPtr->size();
// max iteration suggested 1~2 times the number of grid nodes by suffer
maxIteration_ = uint64_t(totalGridPoints * 2);
if (!areaPoints_.empty())
{
auto xs = getAxisValues(areaPoints_, Axis3DType::X);
auto ys = getAxisValues(areaPoints_, Axis3DType::Y);
xMinMax_ = getMinMax(xs);
yMinMax_ = getMinMax(ys);
}
else
{
xMinMax_ = getMinMax(getAxisValues(randomPoints_, Axis3DType::X));
yMinMax_ = getMinMax(getAxisValues(randomPoints_, Axis3DType::Y));
}
auto zs = getAxisValues(randomPoints_, Axis3DType::Z);
zMinMax_ = getMinMax(zs);
zMean_ = std::accumulate(zs.begin(), zs.end(), 0.0) / zs.size();
// residual, default residual_ * (zMax - zMin)
epsMax_ = residual_ * std::abs(zMinMax_.second - zMinMax_.first);
// get spacing of grid matrix
dxSize_ = (xMinMax_.second - xMinMax_.first) / (shape.cols - 1);
dySize_ = (yMinMax_.second - yMinMax_.first) / (shape.rows - 1);
alpha_ = dxSize_ / dySize_;
beta_ = dySize_ / dxSize_;
x_ = generateAxisPoints(xMinMax_.first, xMinMax_.second, shape.cols);
y_ = generateAxisPoints(yMinMax_.first, yMinMax_.second, shape.rows);
// update random point position by current grid params, such as dxSize, dySize
randomPointPositions_ = coordinates_points(randomPoints_);
}
// grid require params incomplete, do nothing.
}
void GridInterpolator::generate_data_statistics()
{
auto shape = targetMatrixPtr_->shape();
// distance for random point matrix
auto dx = xMinMax_.second - xMinMax_.first;
auto dy = yMinMax_.second - yMinMax_.first;
double distanceMax = 0.0, distanceMin = std::sqrt(dx * dx + dy * dy) + 1;
for (auto pia = randomPoints_.begin(); pia != randomPoints_.end() - 1; pia++)
for (auto pib = pia + 1; pib != randomPoints_.end(); pib++)
{
auto distance = pia->distance2D(*pib);
if (distance > distanceMax)
distanceMax = distance;
if (distance < distanceMin)
distanceMin = distance;
}
dataStatistics_["distance_min"] = distanceMin;
dataStatistics_["distance_max"] = distanceMax;
// distance for position matrix
distanceMax = 0.0, distanceMin = std::sqrt(shape.cols * shape.cols + shape.rows * shape.rows) + 1;
for (auto pia = randomPointPositions_.begin(); pia != randomPointPositions_.end() - 1; pia++)
for (auto pib = pia + 1; pib != randomPointPositions_.end(); pib++)
{
auto distance = pia->distance2D(*pib);
if (distance > distanceMax)
distanceMax = distance;
if (distance < distanceMin)
distanceMin = distance;
}
dataStatistics_["position_distance_min"] = distanceMin;
dataStatistics_["position_distance_max"] = distanceMax;
}
std::shared_ptr<ais::Matrix<double>> GridInterpolator::matrix()
{
return targetMatrixPtr_;
}
std::map<std::string, std::any> GridInterpolator::params()
{
auto shape = targetMatrixPtr_->shape();
std::map<std::string, std::any> ret;
ret["x_min"] = xMinMax_.first;
ret["x_max"] = xMinMax_.second;
ret["y_min"] = yMinMax_.first;
ret["y_max"] = yMinMax_.second;
ret["z_min"] = zMinMax_.first;
ret["z_max"] = zMinMax_.second;
ret["x_nodes"] = shape.cols;
ret["y_nodes"] = shape.rows;
ret["x_grid_size"] = dxSize_;
ret["y_grid_size"] = dySize_;
ret["max_iterations"] = maxIteration_;
ret["residual"] = residual_;
ret["eps_max"] = epsMax_;
ret["fill_value"] = fillValue_;
ret["use_multi_threads"] = flagMultiThread_;
ret["detail_level"] = detailLevel_;
return ret;
}
std::string GridInterpolator::params_info()
{
auto shape = targetMatrixPtr_->shape();
std::stringstream ss;
ss << std::fixed << std::setprecision(4);
ss << "Simple points info:" << std::endl;
ss << "---------------------------------------" << std::endl;
ss << "points count: " << randomPoints_.size() << std::endl;
ss << "x_min: " << xMinMax_.first << ", x_max: " << xMinMax_.second << std::endl;
ss << "y_min: " << yMinMax_.first << ", y_max: " << yMinMax_.second << std::endl;
ss << "z_min: " << zMinMax_.first << ", z_max: " << zMinMax_.second << std::endl;
ss << std::endl;
ss << "Target grid info:" << std::endl;
ss << "---------------------------------------" << std::endl;
ss << "Target area:" << std::endl;
for (auto &point : areaPoints_)
{
ss << "\t(" << point.x << ", " << point.y << ")" << std::endl;
}
ss << std::endl;
ss << "x_nodes: " << shape.cols << ", y_nodes: " << shape.rows << std::endl;
ss << "x_grid_size: " << dxSize_ << ", y_grid_size: " << dySize_ << std::endl;
ss << "output detail level: " << int(detailLevel_) << std::endl;
ss << std::endl;
if (!breaklines_.empty())
{
ss << "Breaklines info:" << std::endl;
ss << "---------------------------------------" << std::endl;
for (auto &line : breaklines_)
{
ss << line << std::endl;
}
ss << std::endl;
}
if (!faults_.empty())
{
ss << "Faults info:" << std::endl;
ss << "---------------------------------------" << std::endl;
for (auto &fault : faults_)
{
ss << fault << std::endl;
}
ss << std::endl;
}
ss << "Interpolation info:" << std::endl;
ss << "---------------------------------------" << std::endl;
ss << "max_iteration: " << maxIteration_ << std::endl;
ss << "residual: " << residual_ << std::endl;
ss << "eps_max: " << epsMax_ << std::endl;
ss << "fill_value: " << fillValue_ << std::endl;
ss << "use_multi_threads: " << (flagMultiThread_ ? "True" : "False") << std::endl;
ss << "corner_weight: " << cornerWeight_ << std::endl;
ss << std::endl;
return ss.str();
}
std::string GridInterpolator::dump_data_statistics()
{
// output the data statistics
std::stringstream ss;
ss << "Origin point data statistics:" << std::endl;
ss << "---------------------------------------" << std::endl;
for (auto &kv : dataStatistics_)
ss << kv.first << ": " << kv.second << std::endl;
ss << std::endl;
return ss.str();
}
void GridInterpolator::update_grid_params(const std::map<std::string, std::any> &params)
{
for (auto &kv : params)
{
auto key = kv.first;
auto value = kv.second;
if (key == "max_iteration")
maxIteration_ = std::any_cast<size_t>(value);
else if (key == "residual")
{
residual_ = std::any_cast<double>(value);
epsMax_ = residual_ * (zMinMax_.second - zMinMax_.first);
}
else if (key == "fill_value")
fillValue_ = std::any_cast<double>(value);
else if (key == "use_multi_threads")
flagMultiThread_ = std::any_cast<bool>(value);
else if (key == "fault_edge_level")
faultEdgeLevel_ = std::any_cast<int8_t>(value);
else if (key == "estimate_factor")
estimateFactor_ = std::any_cast<int32_t>(value);
else if (key == "corner_weight")
cornerWeight_ = std::any_cast<int16_t>(value);
else if (key == "detail_level")
detailLevel_ = std::any_cast<int8_t>(value);
}
}
// setup constraints matrix
void GridInterpolator::setup_constraint_point(std::shared_ptr<ais::PMatrix> &cmp, size_t xi, size_t yi, double xGap, double yGap, const ais::PointXYZ &point)
{
auto shape = cmp->shape();
if ((0 <= xi && xi < shape.cols) && (0 <= yi && yi < shape.rows))
{
auto &p = cmp->at(yi, xi);
// test the constraint point in fault or not by its real position
auto cpx = xMinMax_.first + xi * dxSize_;
auto cpy = yMinMax_.first + yi * dySize_;
ais::Point realPosition{cpx, cpy};
bool inFault = false;
for (auto &fault : faults_)
{
// if (realPosition.in_box(fault.box) && (realPosition.in_polylines(fault.points) || realPosition.in_polygon(fault.polygon, 4, dxSize_, dySize_)))
if (realPosition.in_box(fault.box) && (realPosition.in_polylines(fault.points) || realPosition.in_polygon(fault.polygon)))
{
inFault = true;
break;
}
// test line rp--cp cross fault line or not
auto m = fault.points.size();
ais::Point p{point.x, point.y};
for (auto mi = 0; mi < m - 1; mi++)
{
if (line_cross(p, realPosition, fault.points[mi], fault.points[mi + 1]))
{
inFault = true;
break;
}
}
if (inFault)
break;
}
if ((!inFault) && (!p.has_value() || std::abs(xGap * xGap + yGap * yGap) < std::abs(p.x * p.x + p.y * p.y)))
{
// setup constraints point while there is no constraint or current point is more close, and point not in Fault
cmp->put(yi, xi, ais::PointXYZ{xGap, yGap, point.z});
}
}
};
std::vector<PointXYZ> GridInterpolator::generate_constraint_points(const ais::BreakLine &breakline)
{
std::vector<PointXYZ> result;
// build up the breaklines
std::vector<std::pair<ais::PointXYZ, ais::PointXYZ>> lines;
for (auto iter = breakline.points.begin(); iter != breakline.points.end(); iter++)
{
if (iter + 1 != breakline.points.end())
lines.emplace_back(std::make_pair(*iter, *(iter + 1)));
}
for (auto &line : lines)
{
auto pStart = line.first;
auto pEnd = line.second;
// added points at start and end
result.push_back(pStart);
result.push_back(pEnd);
if (pStart.x > pEnd.x)
std::swap(pStart, pEnd);
// added points on line, interpolation point every grid node
double dx = pEnd.x - pStart.x;
double dy = pEnd.y - pStart.y;
double dz = pEnd.z - pStart.z;
if (dx != 0.0 && std::abs(dy / dx) <= 1.0)
{
// use x as interpolation factor
size_t startIndex = (size_t)std::ceil((pStart.x - xMinMax_.first) / dxSize_);
size_t endIndex = (size_t)std::ceil((pEnd.x - xMinMax_.first) / dxSize_);
for (size_t i = startIndex; i < endIndex; i++)
{
auto x = xMinMax_.first + i * dxSize_;
auto y = (x - pStart.x) * dy / dx + pStart.y;
auto z = (x - pStart.x) * dz / dx + pStart.z;
result.push_back({x, y, z});
}
}
else
{
// use y as interpolation factor
size_t startIndex = (size_t)std::ceil((pStart.y - yMinMax_.first) / dySize_);
size_t endIndex = (size_t)std::ceil((pEnd.y - yMinMax_.first) / dySize_);
for (size_t i = startIndex; i < endIndex; i++)
{
auto y = yMinMax_.first + i * dySize_;
auto x = (y - pStart.y) * dx / dy + pStart.x;
auto z = (y - pStart.y) * dz / dy + pStart.z;
result.push_back({x, y, z});
}
}
}
return result;
}
std::vector<PointXYZ> GridInterpolator::get_nearest_samples(size_t x_index, size_t y_index, size_t min_count, size_t max_count)
{
std::vector<PointXYZ> result;
auto centerPoint = PointXYZ(x_index, y_index);
size_t space_count = 0;
while (result.size() < min_count)
{
// build the test area
space_count++;
double volatile xMin = centerPoint.x - space_count;
double volatile xMax = centerPoint.x + space_count;
double volatile yMin = centerPoint.y - space_count;
double volatile yMax = centerPoint.y + space_count;
auto xMaxPosition = (xMinMax_.second - xMinMax_.first) / dxSize_;
auto yMaxPosition = (yMinMax_.second - yMinMax_.first) / dySize_;
xMin = xMin < 0 ? 0 : xMin;
xMax = xMax > xMaxPosition ? xMaxPosition : xMax;
yMin = yMin < 0 ? 0 : yMin;
yMax = yMax > yMaxPosition ? yMaxPosition : yMax;
filter_copy_points({xMin, yMin}, {xMax, yMax}, randomPointPositions_, result);
}
// remove some to far points while the result is more than max_count
if (result.size() > max_count)
{
std::vector<std::pair<double, PointXYZ>> pointDistances;
for (auto iter = result.begin(); iter != result.end(); iter++)
{
// get all distances
auto &p = *iter;
auto dis = centerPoint.distance(p);
pointDistances.push_back(std::make_pair(dis, p));
}
// sort their distances
std::sort(pointDistances.begin(), pointDistances.end(), [](auto &a, auto &b)
{ return a.first < b.first; });
while (pointDistances.size() > max_count)
{
// filter too far points
pointDistances.pop_back();
}
// set result
result.clear();
for (auto &i : pointDistances)
result.push_back(i.second);
}
return result;
}
std::vector<PointXYZ> GridInterpolator::get_range_samples(size_t xIndex, size_t yIndex, double xGridSize, double yGridSize, const std::vector<PointXYZ> &randoms, double radius)
{
auto xMinMaxPosition = getMinMax(getAxisValues(randoms, Axis3DType::X));
auto yMinMaxPosition = getMinMax(getAxisValues(randoms, Axis3DType::Y));
auto dX = xMinMaxPosition.second - xMinMaxPosition.first;
auto dY = yMinMaxPosition.second - yMinMaxPosition.first;
// default is the max value of grid space / 8
double r = radius <= 0.0 ? std::max(dX, dY) / 8 : radius;
std::vector<PointXYZ> result;
PointXYZ center{xMinMaxPosition.first + xIndex * xGridSize,
yMinMaxPosition.first + yIndex * yGridSize};
for (auto &p : randoms)
{
if (center.distance(p) <= r)
result.push_back(p);
}
return result;
}
std::vector<PointXYZ> GridInterpolator::coordinates_points(const std::vector<PointXYZ> &points)
{
std::vector<PointXYZ> result;
for (auto &p : points)
{
auto xPosition = (p.x - xMinMax_.first) / dxSize_;
auto yPosition = (p.y - yMinMax_.first) / dySize_;
result.push_back({xPosition, yPosition, p.z});
}
return result;
}
std::map<size_t, PointXYZ> GridInterpolator::points_hash_map(const std::vector<PointXYZ> &points)
{
std::map<size_t, PointXYZ> result;
for (auto &p : points)
{
result[p.hash2D()] = p;
}
return result;
}
void GridInterpolator::build_constraint_matrix(std::shared_ptr<ais::FMatrix> targetMatrixPtr, std::shared_ptr<ais::PMatrix> &constrainMatrixPtr)
{
std::stringstream msg;
msg << ais::time_str() << " " << __FUNCTION__;
interpolationMsgs_.push_back(msg.str());
if (!targetMatrixPtr)
THROW_INVALID_ARGUMENT("Target matrix not have been initialized");
if (randomPoints_.empty())
THROW_INVALID_ARGUMENT("Random points list is empty");
// build the constraint matrix and filled nan
if (!constrainMatrixPtr.get())
{
constrainMatrixPtr = std::make_shared<ais::PMatrix>(targetMatrixPtr->shape());
constrainMatrixPtr->emptys();
}
auto eps_x = dxSize_ > 1 ? DBL_EPSILON * dxSize_ : DBL_EPSILON;
auto eps_y = dySize_ > 1 ? DBL_EPSILON * dySize_ : DBL_EPSILON;
// fill constraint points by input param
auto fill_constraint_points = [&](const std::vector<PointXYZ> &points)
{
auto size = points.size();
auto threads = size_t(std::thread::hardware_concurrency() * 1.5);
auto count = size_t(size / threads);
std::vector<std::future<uint64_t>> execs;
for (int t = 0; t < threads; t++)
{
// get begin and end param for each thread
auto begin = count * t;
// ensure the last thread fill all rest
auto end = (t == (threads - 1)) ? size : begin + count;
execs.emplace_back(std::async(
std::launch::async, [this, &constrainMatrixPtr, &points, &eps_x, &eps_y](size_t begin, size_t end)
{
for (size_t i = begin; i < end; i++)
{
auto& point = points[i];
// skip any point in the fault
bool inFault = false;
for (auto& fault : faults_)
{
//if (point.in_box(fault.box) && (point.in_polylines(fault.points) || point.in_polygon(fault.polygon, 4, dxSize_, dySize_)))
if (point.in_box(fault.box) && (point.in_polylines(fault.points) || point.in_polygon(fault.polygon)))
{
inFault = true;
break;
}
}
if (inFault)
{
// if point in a fault range, don't set its constraint, because it have been invalid by fault
continue;
}
// get x axis constraints i1--p----i2
auto xk = (point.x - xMinMax_.first) / dxSize_;
size_t i1 = size_t(xk);
size_t i2 = i1 + 1;
auto xGap1 = point.x - (xMinMax_.first + i1 * dxSize_);
auto xGap2 = point.x - (xMinMax_.first + i2 * dxSize_);
if (std::abs(xGap1) < eps_x)
{
// point is on the node, reset i2 & xGap2
i2 = i1;
xGap2 = xGap1;
}
// get y axis constraints j1-----p-j2
auto yk = (point.y - yMinMax_.first) / dySize_;
size_t j1 = size_t(yk);
size_t j2 = j1 + 1;
auto yGap1 = point.y - (yMinMax_.first + j1 * dySize_);
auto yGap2 = point.y - (yMinMax_.first + j2 * dySize_);
if (std::abs(yGap1) < eps_y)
{
// point is on the node, reset j2 & yGap2
j2 = j1;
yGap2 = yGap1;
}
setup_constraint_point(constrainMatrixPtr, i1, j1, xGap1, yGap1, point);
setup_constraint_point(constrainMatrixPtr, i1, j2, xGap1, yGap2, point);
setup_constraint_point(constrainMatrixPtr, i2, j1, xGap2, yGap1, point);
setup_constraint_point(constrainMatrixPtr, i2, j2, xGap2, yGap2, point);
}
return end - begin; },
begin, end));
}
uint64_t filled = 0;
for (auto &e : execs)
{
e.wait();
filled += e.get();
}
if (filled != size)
{
THROW_RUNTIME_ERROR("Setup constraint matrix failed!!");
exit(-1);
}
};
fill_constraint_points(randomPoints_);
// update constraints by breakline
for (auto &bl : breaklines_)
{
auto blConstraintPoints = generate_constraint_points(bl);
fill_constraint_points(blConstraintPoints);
}
// finally, copy the constraint matrix point value to target
std::vector<double> zList;
for (const ais::PointXYZ &p : *constrainMatrixPtr)
{
zList.push_back(p.z);
}
targetMatrixPtr->putMany(0, zList);
}
void GridInterpolator::update_edge_node(std::shared_ptr<ais::FMatrix> &targetMatrixPtr, EdgeNodeValueMethod method)
{
auto tmp = targetMatrixPtr;
// const for shape
auto row = tmp->rows();
auto col = tmp->columns();
auto setEdgeByFillValue = [&](int64_t begin, int64_t end, int64_t step)
{
// pos is the flatten index offset of the point
for (int64_t pos = begin; pos < end; pos += step)
{
auto iter = tmp->begin() + pos;
if (ais::isnan(*iter))
{
tmp->put(pos, this->fillValue_);
// add edge point to constrainMatrixPtr_
// auto [r, c] = targetMatrixPtr_->get_pos(iter);
// constrainMatrixPtr_->put(pos, {double(r), double(c) ,this->fillValue_ });
}
}
};
auto setEdgeByNearestValue = [&](int64_t begin, int64_t end, int64_t step)
{
// pos is the flatten index offset of the point
for (int64_t pos = begin; pos < end; pos += step)
{
auto iter = tmp->begin() + pos;
size_t validCount = 0;
double mean = 0.0;
auto [pos_row, pos_col] = tmp->get_pos(iter);
// loop to setup null points
if (std::isnan(*iter))
{
// set radius to found non-zero value
int32_t radius = 1;
while (validCount == 0)
{
int32_t col_increase = static_cast<int64_t>(pos_col) + static_cast<int64_t>(radius);
int32_t row_increase = static_cast<int64_t>(pos_row) + static_cast<int64_t>(radius);
int32_t cMin = (pos_col - radius) < 0 ? 0 : pos_col - radius;
int32_t cMax = (col_increase) >= col ? (col - 1) : col_increase;
int32_t rMin = (pos_row - radius) < 0 ? 0 : pos_row - radius;
int32_t rMax = (row_increase) >= row ? (row - 1) : row_increase;
for (auto r = rMin; r <= rMax; r++)
{
for (auto c = cMin; c <= cMax; c++)
{
double zValue = *(tmp->begin() + r * col + c);
if (!std::isnan(zValue))
{
mean += zValue;
validCount++;
}
}
}
// zoom out for more neighbors
radius++;
}
mean /= validCount;
tmp->put(pos_row, pos_col, mean);
// add edge point to constrainMatrixPtr_
// auto [r, c] = targetMatrixPtr_->get_pos(iter);
// constrainMatrixPtr_->put(pos, { double(r), double(c) ,this->fillValue_ });
}
}
};
std::function<void(int64_t, int64_t, int64_t)> fun;
if (method == EdgeNodeValueMethod::FILL)
fun = setEdgeByFillValue;
else
fun = setEdgeByNearestValue;
// row-0, bottom border
fun(0, col, 1);
// row-n, top border
fun((row - 1) * col, row * col, 1);
// column-0, left border
fun(0, row * col, col);
// column-n, right border
fun(col - 1, row * col, col);
}
void GridInterpolator::update_nan_node(std::shared_ptr<ais::FMatrix> &targetMatrixPtr)
{
auto tmp = targetMatrixPtr;
size_t count = std::count_if(std::execution::par_unseq, tmp->cbegin(), tmp->cend(), [](double v) {return std::isnan(v); });
if (count > 0 && !std::isnan(fillValue_))
{
// only replace nan when fillValue_ is not nan and count > 0
tmp->replace(std::nan("1"), this->fillValue_);
std::string msg = "Total update nan nodes: " + std::to_string(count);
interpolationMsgs_.push_back(msg);
}
}
std::vector<double> build_xy_data(const Point3D &p)
{
std::vector<double> r{p.x, p.y, 1.0};
return r;
};
bool all_filled(std::shared_ptr<ais::FMatrix> target, size_t begin_index, size_t end_index)
{
const auto function = [](double i) -> bool
{ return !(std::isnan(i) || std::isinf(i)); };
if ((target->cbegin() + begin_index) > target->cend() || (target->cbegin() + end_index) > target->cend())
{
THROW_INVALID_ARGUMENT("Out of target range.");
}
bool ret = std::all_of(std::execution::par_unseq, target->cbegin() + begin_index, target->cend() + end_index, function);
return ret;
}
size_t GridInterpolator::detect_estimate_factor()
{
// After many test, we found when the points count >= 1.5% of total gird node count, the minimum curvature
// will output smooth result after estimated. So the main point for this function is try to found out the
// scale factor for input X/Y grid size, then use this scale factor to increase grid size to get the
// estimate grid, later use the estimate grid and interpolate
// scale * randomPoints.size() / (x grid count * y grid count) > 1.5%
if (estimateFactor_ == 0)
{
auto temp = ((xMinMax_.second - xMinMax_.first) / dxSize_) * ((yMinMax_.second - yMinMax_.first) / dySize_) * .015 / randomPoints_.size();
size_t scale = (size_t)std::ceil(std::sqrt(temp));
if (scale > 1)
estimateFactor_ = scale;
}
return estimateFactor_;
}
void GridInterpolator::scale_estimate()
{
// function start message
std::stringstream msg;
msg << ais::time_str() << " " << __FUNCTION__;
interpolationMsgs_.push_back(msg.str());
// only do estimation while the estimateFactor_ is more than 1
if (estimateFactor_ > 1)
{
std::string m = "Estimate factor: " + std::to_string(estimateFactor_);
interpolationMsgs_.push_back(m);
// 1. build estimate matrix by estimateFactor
// the estimate matrix area is same as target matrix,
// so must reset some class internal vars
dxSize_ *= estimateFactor_;
dySize_ *= estimateFactor_;
size_t xCount = (size_t)std::ceil((xMinMax_.second - xMinMax_.first) / dxSize_) + 1;
size_t yCount = (size_t)std::ceil((yMinMax_.second - yMinMax_.first) / dySize_) + 1;
ais::Shape estimateShape = {yCount, xCount};
estimateMatrixPtr_ = std::make_shared<ais::FMatrix>(estimateShape);
estimateMatrixPtr_->zeros();
generate_grid_params(estimateMatrixPtr_);
// 2. setup estimate matrix faults impact
// matrix which storage the points in fault for estimate
std::shared_ptr<ais::FMatrix> estimatePointInFaultMatrixPtr;
// matrix which storage the fault affect block values for estimate
std::shared_ptr<ais::FMatrix> estimateFaultAffectMatrixPtr;
build_fault_impact_matrix(estimateMatrixPtr_, estimatePointInFaultMatrixPtr, estimateFaultAffectMatrixPtr);
// 3. setup the constrict matrix and initial estimate matrix
std::shared_ptr<ais::PMatrix> estimateConstrainMatrixPtr;
build_constraint_matrix(estimateMatrixPtr_, estimateConstrainMatrixPtr);
// 4. update estimate matrix edge
update_edge_node(estimateMatrixPtr_);
// 5. estimate surface for estimate matrix
estimate_gradients_surface(estimateMatrixPtr_);
// 6. update nan nodes in estimate matrix
update_nan_node(estimateMatrixPtr_);
// 7. go interpolation
start_interpolation(estimateMatrixPtr_, estimateConstrainMatrixPtr, estimatePointInFaultMatrixPtr, estimateFaultAffectMatrixPtr);
//dump_result_async("output_estimated.grd", estimateMatrixPtr_);
// 8. interpolate the estimate to target matrix
interpolate_estimate_2_target();
// release inter vars
estimateConstrainMatrixPtr.reset();
estimatePointInFaultMatrixPtr.reset();
estimateFaultAffectMatrixPtr.reset();
// finally reset minimum curvature params for real interpolation
generate_grid_params(targetMatrixPtr_);
//dump_result_async("estimated2target.grd", targetMatrixPtr_);
}
}
void GridInterpolator::interpolate_estimate_2_target()
{
// function start message
std::stringstream msg;
msg << ais::time_str() << " " << __FUNCTION__;
interpolationMsgs_.push_back(msg.str());
auto tShape = targetMatrixPtr_->shape();
auto eShape = estimateMatrixPtr_->shape();
auto threads = size_t(std::thread::hardware_concurrency() * 1.5);
auto rowInThread = size_t(tShape.rows / threads);
std::vector<std::future<uint64_t>> execs;
for (int t = 0; t < threads; t++)
{
// get begin and end param for each thread
auto begin = rowInThread * t;
// ensure the last thread fill all rest
auto end = (t == (threads - 1)) ? tShape.rows : begin + rowInThread;
// multi-thread execution
execs.emplace_back(std::async(
std::launch::async,
[this, &tShape, &eShape](size_t begin, size_t end)
{
std::atomic<size_t> failedCount = 0;
auto rows = tShape.rows;
auto cols = tShape.cols;
for (int j = begin; j < end; j++)
{
for (int i = 0; i < tShape.cols; i++)
{
auto pos = j * tShape.cols + i;
auto z = targetMatrixPtr_->at(pos);
if (std::isnan(z) || std::abs(z - fillValue_) < 1e-6)
{
// put the interpolate value to target matrix while the value is nan or close to fill value
size_t ei = std::floor(i / estimateFactor_);
size_t ej = std::floor(j / estimateFactor_);
size_t mi = i % estimateFactor_;
size_t mj = j % estimateFactor_;
double zVertical = fillValue_;
double p0z = estimateMatrixPtr_->at(ej, ei);
if (ei < (eShape.cols-1) && ej < (eShape.rows-1))
{
// for all node in good range, use bilinear interpolation
double p1z = estimateMatrixPtr_->at(ej, ei + 1);
double p2z = estimateMatrixPtr_->at(ej + 1, ei + 1);
double p3z = estimateMatrixPtr_->at(ej + 1, ei);
// get z value by bilinear interpolation
double zHorizontal2 = (p2z - p3z) * mi / estimateFactor_ + p3z;
double zHorizontal1 = (p1z - p0z) * mi / estimateFactor_ + p0z;
zVertical = (zHorizontal2 - zHorizontal1) * mj / estimateFactor_ + zHorizontal1;
}
else
{
// for other node(botoom and right edge), just copy p0z point value
zVertical = p0z;
}
targetMatrixPtr_->put(pos, zVertical);
}
}
}
return end - begin;
},
begin, end));
}
uint64_t rowFilled = 0;
for (auto &e : execs)
{
e.wait();
auto filledInThread = e.get();
rowFilled += filledInThread;
// std::string msg = "Interpolate estimate to target row: " + std::to_string(rowFilled);
// interpolationMsgs_.push_back(msg);
}
}
void GridInterpolator::estimate_gradients_surface(std::shared_ptr<ais::FMatrix> &targetMatrixPtr)
{
// function start message
std::stringstream msg;
msg << ais::time_str() << " " << __FUNCTION__;
interpolationMsgs_.push_back(msg.str());
std::atomic<size_t> count = 0;
auto tmp = targetMatrixPtr;
auto shape = targetMatrixPtr->shape();
// before delaunay triangles, added rectangle corner points to cover all rectangle
double rows = shape.rows;
double cols = shape.cols;
// add corner points with its origin value, will reset it later if the value is 0 by the gaussian filter
Point3D p0{0, 0, tmp->at(0, 0)};
Point3D p1{0, rows - 1, tmp->at(rows - 1, 0)};
Point3D p2{cols - 1, rows - 1, tmp->at(rows - 1, cols - 1)};
Point3D p3{cols - 1, 0, tmp->at(0, cols - 1)};
randomPointPositions_.push_back(p0);
randomPointPositions_.push_back(p1);
randomPointPositions_.push_back(p2);
randomPointPositions_.push_back(p3);
std::set<size_t> cornerHashs = {
p0.hash2D(), p1.hash2D(), p2.hash2D(), p3.hash2D()};
// build the delaunay triangles with random point positions, which will use point's grid position to simple calculation
// eg: point(2342.22, 1242.23, 23.3) ==> position(6.1, 42.23, 23.3)
std::vector<Triangle<Point3D>> triangles;
ais::delaunay_triangulation(randomPointPositions_, triangles);
// remove triangle without z value, means the triangle contact to super-triangles
for (auto iter = triangles.begin(); iter != triangles.end();)
{
auto t = (*iter);
auto pz = t.points[0].z * t.points[1].z * t.points[2].z;
if (std::isnan(pz) || std::isinf(pz))
iter = triangles.erase(iter);
else
iter++;
}
// get triangles info, such as triangle points' distance min/max
std::map<size_t, std::set<size_t>> trianglePointLinkMap;
double distanceMax = 0.0, distanceMin = std::numeric_limits<double>::max();
get_triangle_info(triangles, trianglePointLinkMap, distanceMin, distanceMax);
auto updateCornerPointEstimateZ = [this, &trianglePointLinkMap, &rows, &cols, &tmp, &cornerHashs](size_t rowPos, size_t colPos)
{
double ret = std::nan("1");
Point3D point{(double)colPos, (double)rowPos, INFINITY};
auto linkPoints = trianglePointLinkMap[point.hash2D()];
std::vector<double> estimates;
for (auto &rp : randomPointPositions_)
{
if (linkPoints.find(rp.hash2D()) != linkPoints.end() && cornerHashs.find(rp.hash2D()) == cornerHashs.end())
{
// estimate z value by Gaussian with no-corner linked point
double estimateZ = point.gaussian_interpolation_z_value(point.distance2D(rp), rp.z, rows, cols, cornerWeight_);
if (estimateZ > 1e-6)
estimates.push_back(estimateZ);
}
}
if (estimates.size() > 0)
{
ret = std::accumulate(estimates.begin(), estimates.end(), 0.0) / estimates.size();
// update point z for targetMatrixPtr & randomPointPositions_
tmp->put(rowPos, colPos, ret);
for (auto rp = randomPointPositions_.begin(); rp != randomPointPositions_.end(); rp++)
{
if (rp->hash2D() == point.hash2D())
{
rp->z = ret;
break;
}
}
}
return ret;
};
// reset the corner points' z-value to make the rect value more smooth
double updatedZ = updateCornerPointEstimateZ(0, 0);
updatedZ = updateCornerPointEstimateZ(rows - 1, 0);
updatedZ = updateCornerPointEstimateZ(rows - 1, cols - 1);
updatedZ = updateCornerPointEstimateZ(0, cols - 1);
// reset triangles
triangles.clear();
ais::delaunay_triangulation(randomPointPositions_, triangles);
// remove triangle without z value, means the triangle contact to super-triangles
for (auto iter = triangles.begin(); iter != triangles.end();)
{
auto t = (*iter);
auto pz = t.points[0].z * t.points[1].z * t.points[2].z;
if (std::isnan(pz) || std::isinf(pz))
iter = triangles.erase(iter);
else
iter++;
}
// get all coe for all random points
ais::Matrix<double> allXYMatrix(randomPointPositions_.size(), 3);
ais::Matrix<double> allZMatrix(1, randomPointPositions_.size());
for (int pi = 0; pi < randomPointPositions_.size(); pi++)
{
auto &p = randomPointPositions_[pi];
allXYMatrix.putMany(pi, 0, build_xy_data(p));
allZMatrix.put(0, pi, p.z);
}
auto allCoe = ais::linalg::lstsq(allXYMatrix, allZMatrix);
// use multi thread to speed up
auto threads = size_t(std::thread::hardware_concurrency() * 1.5);
// threads = 1;
auto rowInThread = size_t(shape.rows / threads);
std::vector<std::future<uint64_t>> execs;
for (int t = 0; t < threads; t++)
{
// get begin and end param for each thread
auto begin = rowInThread * t;
// ensure the last thread fill all rest
auto end = (t == (threads - 1)) ? shape.rows : begin + rowInThread;
// multi-thread execution
execs.emplace_back(std::async(
std::launch::async, [this, &shape, &tmp, &count, &triangles, &allCoe](size_t begin, size_t end)
{
std::atomic<size_t> failedCount = 0;
auto tsize = triangles.size();
auto rows = shape.rows;
auto cols = shape.cols;
for (size_t nodeJ = begin; nodeJ < end; nodeJ++)
{
for (size_t nodeI = 0; nodeI < shape.cols; nodeI++)
{
auto z = tmp->at(nodeJ, nodeI);
if (ais::isnan(z) || std::abs(z - fillValue_) < 1e-6)
{
std::vector<std::pair<double, double>> interpolations;
// use index as the point position to simple calculation
Point3D nodePoint{ (double)nodeI, (double)nodeJ, INFINITY };
for (int ti = 0 ; ti < tsize; ti++)
{
auto& t = triangles[ti];
if (nodePoint.in_box(t.min_max[0], t.min_max[1]) && nodePoint.in_polygon(t.polygon))
{
auto pa = t.points[0];
auto pb = t.points[1];
auto pc = t.points[2];
auto da = nodePoint.distance(pa);
auto db = nodePoint.distance(pb);
auto dc = nodePoint.distance(pc);
auto za = nodePoint.point_interpolate(pb, pc, pa);
auto zb = nodePoint.point_interpolate(pc, pa, pb);
auto zc = nodePoint.point_interpolate(pa, pb, pc);
interpolations.push_back({ da, za });
interpolations.push_back({ db, zb });
interpolations.push_back({ dc, zc });
break;
}
}
if (interpolations.size() > 0)
{
double avg_inter = interpolations.size() == 0 ? interpolations[0].second : ais::get_mean(interpolations);
if (!std::isinf(avg_inter))
tmp->put(nodeJ, nodeI, avg_inter);
else
tmp->put(nodeJ, nodeI, fillValue_);
count++;
}
else
{
// debug usage
failedCount++;
}
}
}
}
return end - begin; },
begin, end));
}
uint64_t rowFilled = 0;
for (auto &e : execs)
{
e.wait();
auto filledInThread = e.get();
rowFilled += filledInThread;
// std::string msg = "Estimated row: " + std::to_string(rowFilled);
// interpolationMsgs_.push_back(msg);
}
if (count > 0)
{
std::string msg = "Total estimated nodes: " + std::to_string(count);
interpolationMsgs_.push_back(msg);
}
// dump after estimate value
{
// update zMin and zMax
zMinMax_ = getMinMax(*targetMatrixPtr);
// std::lock_guard<std::mutex> lk(msgMutex_);
// std::string msg = "Estimated completed";
// interpolationMsgs_.push_back(msg);
/*
auto ret = std::async(std::launch::async, [&]()
{
std::string filename = "2_Estimated_" + std::to_string(shape.rows) + "x" + std::to_string(shape.cols) + ".grd";
dump_result(filename.c_str());
return; });
*/
}
}
void GridInterpolator::estimate_gradients_surface_2(InitTargetMatrixMethod method)
{
// // if (!estimateTargetMatrix_)
// // return;
//
// // function start message
// std::stringstream msg;
// msg << ais::time_str() << " " << __FUNCTION__;
// interpolationMsgs_.push_back(msg.str());
//
// std::atomic<size_t> count = 0;
// auto tmp = targetMatrixPtr_;
// auto shape = targetMatrixPtr_->shape();
//
// ais::FMatrix updateMatrix(shape);
// updateMatrix.nans();
//
// // storage the constrict values to map
// std::map<size_t, double> constrictValues;
// for (size_t i = 0; i < shape.size(); i++)
// {
// double value = tmp->at(i);
// if (!std::isnan(value) && value != 0.0)
// constrictValues[i] = value;
// }
//
// // build the delaunay triangles with random point positions, which will use point's grid position to simple calculation
// // eg: p(2342.22, 1242.23, 23.3) ==> p(12.2, 42.23, 23.3)
// std::vector<Triangle<Point3D>> triangles;
// ais::delaunay_triangulation(randomPointPositions_, triangles);
//
// // get all coe for all random points
// ais::Matrix<double> allXYMatrix(randomPointPositions_.size(), 3);
// ais::Matrix<double> allZMatrix(1, randomPointPositions_.size());
// for (int pi = 0; pi < randomPointPositions_.size(); pi++)
// {
// auto & p = randomPointPositions_[pi];
// allXYMatrix.putMany(pi, 0, build_xy_data(p));
// allZMatrix.put(0, pi, p.z);
// }
// auto allCoe = ais::linalg::lstsq(allXYMatrix, allZMatrix);
//
// // remove triangle without z value, means the triangle contact to super-triangles
// for (auto iter = triangles.begin(); iter != triangles.end();)
// {
// auto t = (*iter);
// auto pz = t.points[0].z * t.points[1].z * t.points[2].z;
//
// if (std::isnan(pz) || std::isinf(pz))
// iter = triangles.erase(iter);
// else
// iter++;
// }
//
// std::map<size_t, std::set<size_t>> trianglePointLinkMap;
// double distanceMax = 0.0, distanceMin = std::numeric_limits<double>::max();
// get_triangle_info(triangles, trianglePointLinkMap, distanceMin, distanceMax);
//
//
// // use multi thread to speed up
// auto threads = size_t(std::thread::hardware_concurrency() * 1.5);
// #if defined(_DEBUG)
// //threads = 1; // debug usage
// #endif
// auto rowInThread = size_t(shape.rows / threads);
//
// bool allFilled = false;
//
// // check the target matrix have been filled or not
// {
// std::lock_guard<std::mutex> lk(dataMutex_);
// auto zeroCount = std::count(std::execution::par_unseq, tmp->begin(), tmp->end(), 0.0);
// allFilled = all_filled(tmp, 0, shape.size());
// }
//
// while (!allFilled)
// {
// // copy target matrix data to update matrix
// {
// std::lock_guard<std::mutex> lk(dataMutex_);
// updateMatrix.putMany(0, *tmp);
// }
//
// std::vector<std::future<uint64_t>> execs;
// for (int t = 0; t < threads; t++)
// {
// // get begin and end param for each thread
// auto begin = rowInThread * t;
// // ensure the last thread fill all rest
// auto end = (t == (threads - 1)) ? shape.rows : begin + rowInThread;
//
// //============== Solution 5, MA Gaussian filter for all point =========================
// execs.emplace_back(std::async(
// std::launch::async, [this, &shape, &tmp, &count, &constrictValues, &updateMatrix](size_t begin, size_t end)
// {
// for (size_t j = begin; j < end; j++)
// {
// for (size_t i = 0; i < shape.cols; i++)
// {
// // use index as the point position to simple calculation
// Point3D girdPoint{ (double)i, (double)j };
// size_t pos = j * shape.cols + i;
//
// double z = tmp->at(pos);
// if (ais::isnan(z) || constrictValues.find(pos) == constrictValues.end())
// {
// // update all nodes which is not in constrict values
// z = ma_gaussian_filter(i, j, tmp, shape);
// if(!std::isnan(z))
// updateMatrix.put(pos, z);
// }
// else
// {
// // for all constrict values, just copy it
// updateMatrix.put(pos, z);
// }
// }
// }
//
// return end - begin;
// },
// begin, end));
// }
//
// // wait for multi-thread exec
// uint64_t rowFilled = 0;
// for (auto& e : execs)
// {
// e.wait();
// auto filledInThread = e.get();
// rowFilled += filledInThread;
// }
//
// std::string msg = "Completed loop: " + std::to_string(count);
// interpolationMsgs_.push_back(msg);
//
// // copy the update matrix to target
// {
// std::lock_guard<std::mutex> lk(dataMutex_);
// tmp->putMany(0, updateMatrix);
// auto zeroCount = std::count(std::execution::par_unseq, tmp->begin(), tmp->end(), 0.0);
// allFilled = all_filled(tmp, 0, shape.size());
// }
// count++;
// }
//
// if (count > 0)
// {
// std::string msg = "Total estimated loops: " + std::to_string(count);
// interpolationMsgs_.push_back(msg);
// }
}
void GridInterpolator::build_fault_impact_matrix(std::shared_ptr<ais::FMatrix> targetMatrixPtr,
std::shared_ptr<ais::FMatrix> &pointInFaultMatrixPtr, std::shared_ptr<ais::FMatrix> &faultAffectMatrixPtr)
{
// do nothing while the fault is empty
if (faults_.empty())
return;
std::stringstream msg;
msg << ais::time_str() << " " << __FUNCTION__;
interpolationMsgs_.push_back(msg.str());
// pointInFaultMatrix means for all grid points which are in the fault
auto shape = targetMatrixPtr->shape();
if (!pointInFaultMatrixPtr.get())
pointInFaultMatrixPtr = std::make_shared<ais::FMatrix>(shape);
// faultAffectMatrix means for all grid points which iteration block have been affect fault
if (!faultAffectMatrixPtr.get())
{
faultAffectMatrixPtr = std::make_shared<ais::FMatrix>(shape.rows + 4, shape.cols + 4);
faultAffectMatrixPtr->zeros();
}
// build up point-in-fault matrix, used for add fault line on output grid
for (size_t j = 0; j < shape.rows; j++)
{
for (size_t i = 0; i < shape.cols; i++)
{
// build the node point
auto x = xMinMax_.first + i * dxSize_;
auto y = yMinMax_.first + j * dySize_;
auto ui = i + 2;
auto uj = j + 2;
ais::Point p{x, y};
// loop fault to check the node point in fault or not
for (auto &fault : faults_)
{
// use loose test(in_polygon(controlPoints=0) for identification the node point in fault as more as possible
if (p.in_box(fault.box) && (p.in_polylines(fault.points) || p.in_polygon(fault.polygon)))
{
// build a fault point box for fault line constrict
std::vector<ais::Point> faultPointBox = {
{x - 0.5 * dxSize_, y - 0.5 * dySize_},
{x - 0.5 * dxSize_, y + 0.5 * dySize_},
{x + 0.5 * dxSize_, y + 0.5 * dySize_},
{x + 0.5 * dxSize_, y - 0.5 * dySize_}};
int hitCount = 0;
for (auto &fpb : faultPointBox)
{
if (fpb.in_polygon(fault.polygon))
hitCount++;
}
// node point in fault, flag it and break loop for next point
faultAffectMatrixPtr->put(uj, ui, double(1 << hitCount));
// the faults will not overlapped, so don't test anymore when hit one fault, just break loop
break;
}
}
}
}
// block point check position
// iterated points which need be analysis are below
// 0,2
// -1,1 0,1 1,1
// -2,0 -1,0 0,0 1,0 2,0
// -1,-1 0,-1 1,-1
// 0,-2
const std::vector<std::pair<int, int>> insidePoint1 = {{0, 1}, {-1, 0}, {1, 0}, {0, -1}};
const std::vector<std::pair<int, int>> insidePoint2 = {{-1, 1}, {-1, -1}, {1, 1}, {1, -1}};
const std::vector<std::pair<int, int>> insidePoint3 = {
{-2, 2}, {-2, 1}, {-2, 0}, {-2, -1}, {-2, -2}, {-1, 2}, {-1, -2}, {0, 2}, {0, -2}, {1, 2}, {1, -2}, {2, 2}, {2, 1}, {2, 0}, {2, -1}, {2, -2}};
// get block point fault tag value
auto pointFaultTagValue = [](const std::shared_ptr<ais::FMatrix> pifMatrixPtr, size_t i, size_t j, const std::pair<int, int> &p, bool force = false)
{
long ret = 0;
auto shape = pifMatrixPtr->shape();
auto pos = (j + 2 + p.second) * shape.cols + i + 2 + p.first;
auto pInFault = pifMatrixPtr->at(pos);
if (pInFault > 0.0 || force)
{
ret = 1 << ((p.second + 2) * 5 + (p.first + 2));
}
return ret;
};
// get point fault tag for faults
auto pointFaultAffectValue = [&insidePoint1, &insidePoint2, &insidePoint3, this, &pointFaultTagValue](const std::shared_ptr<ais::FMatrix> pifMatrixPtr, size_t i, size_t j, const ais::Fault &fault)
{
long ret = 0;
auto &fps = fault.points;
size_t m = fault.points.size();
auto x = xMinMax_.first + i * dxSize_;
auto y = yMinMax_.first + j * dySize_;
ais::Point p1{x, y};
auto faultBoxMin = fault.box[0];
auto faultBoxMax = fault.box[2];
if (x < (faultBoxMin.x - 2 * dxSize_) || x > (faultBoxMax.x + 2 * dxSize_) ||
y < (faultBoxMin.y - 2 * dySize_) || y > (faultBoxMax.y + 2 * dySize_))
{
// any point out of fault box will skip it.
return ret;
}
// check insidePoint1
for (auto &p : insidePoint1)
{
// loop inside nodes to build the line p(0,0) --- p(inside point)
auto x2 = xMinMax_.first + (i + p.first) * dxSize_;
auto y2 = yMinMax_.first + (j + p.second) * dySize_;
ais::Point p2{x2, y2};
for (auto mi = 0; mi < m - 1; mi++)
// loop fault line points to build another line
if (line_cross(p1, p2, fps[mi], fps[((mi + 1) % m)]))
{
// the fault node line cross fault line
ret |= pointFaultTagValue(pifMatrixPtr, i, j, p, true);
// for expend point
if (p.first != 0)
{
// for x axis
auto newX = p.first > 0 ? 2 : -2;
auto pExtPos = (p.second + 2) * 5 + (newX + 2);
ret |= 1 << pExtPos;
}
else
{
// for y axis
auto newY = p.second > 0 ? 2 : -2;
auto pExtPos = (newY + 2) * 5 + (p.first + 2);
ret |= 1 << pExtPos;
}
// bingo, the line p1--p2 cross fault, break loop check next
break;
}
}
// check insidePoint2
for (auto &p : insidePoint2)
{
// loop inside nodes to build the line p(0,0) --- p(inside point)
auto x2 = xMinMax_.first + (i + p.first) * dxSize_;
auto y2 = yMinMax_.first + (j + p.second) * dySize_;
ais::Point p2{x2, y2};
for (auto mi = 0; mi < m - 1; mi++)
// loop fault line points to build another line
if (line_cross(p1, p2, fps[mi], fps[((mi + 1) % m)]))
{
// the fault box node line cross fault line
ret |= pointFaultTagValue(pifMatrixPtr, i, j, p, true);
// for expend point
auto newX = p.first > 0 ? 2 : -2;
auto newY = p.second > 0 ? 2 : -2;
auto pExtPos = (newY + 2) * 5 + (newX + 2);
ret |= 1 << pExtPos;
// bingo, the line p1--p2 cross fault, break loop check next
break;
}
}
// check insidePoint3
for (auto &p : insidePoint3)
{
if (ret | pointFaultTagValue(pifMatrixPtr, i, j, p, true))
{
// already have been setup, skip and continue
continue;
}
auto x2 = xMinMax_.first + (i + p.first) * dxSize_;
auto y2 = yMinMax_.first + (j + p.second) * dySize_;
ais::Point p2{x2, y2};
for (auto mi = 0; mi < m - 1; mi++)
if (line_cross(p1, p2, fps[mi], fps[((mi + 1) % m)]))
{
ret |= pointFaultTagValue(pifMatrixPtr, i, j, p, true);
}
}
return ret;
};
// build up pointInFaultMatrixPtr_
for (size_t j = 0; j < shape.rows; j++)
{
for (size_t i = 0; i < shape.cols; i++)
{
// 0 means the point not effected by fault
long tempZ = 0;
// check center point first
tempZ |= pointFaultTagValue(faultAffectMatrixPtr, i, j, {0, 0});
if (tempZ > 0)
{
// central point in the fault, break fault loop to flag it and check next point
pointInFaultMatrixPtr->put(j, i, double(tempZ));
continue;
}
// 1. for node points in fault polygon
for (auto &p : insidePoint1)
{
// check inner point, if in polygon, flag it and it's expanded point
auto pValue = pointFaultTagValue(faultAffectMatrixPtr, i, j, p);
if (pValue > 0)
{
tempZ |= pValue;
if (p.first != 0)
{
auto newX = p.first > 0 ? 2 : -2;
auto pExtPos = (p.second + 2) * 5 + (newX + 2);
tempZ |= 1 << pExtPos;
}
else
{
auto newY = p.second > 0 ? 2 : -2;
auto pExtPos = (newY + 2) * 5 + (p.first + 2);
tempZ |= 1 << pExtPos;
}
}
}
// if (j == 470 && i == 495)
//{
// std::cerr << "After pass point1 Point(470, 495): " << tempZ << std::endl;
// }
for (auto &p : insidePoint2)
{
// check inner point, if in polygon, flag it and it's expanded point
auto pValue = pointFaultTagValue(faultAffectMatrixPtr, i, j, p);
if (pValue > 0)
{
tempZ |= pValue;
auto newX = p.first > 0 ? 2 : -2;
auto newY = p.second > 0 ? 2 : -2;
auto pExtPos = (newY + 2) * 5 + (newX + 2);
tempZ |= 1 << pExtPos;
}
}
if (j == 470 && i == 495)
{
std::cerr << "After pass point2 Point(470, 495): " << tempZ << std::endl;
}
for (auto &p : insidePoint3)
{
tempZ |= pointFaultTagValue(faultAffectMatrixPtr, i, j, p);
}
if (j == 470 && i == 495)
{
std::cerr << "After pass point3 Point(470, 495): " << tempZ << std::endl;
}
// 2. for fault line cross block but not on the point
for (auto &fault : faults_)
{
tempZ |= pointFaultAffectValue(faultAffectMatrixPtr, i, j, fault);
}
if (j == 470 && i == 495)
{
std::cerr << "After pass line cross check Point(470, 495): " << tempZ << std::endl;
}
// finally put the point fault effect to the matrix
pointInFaultMatrixPtr->put(j, i, double(tempZ));
}
}
}
void GridInterpolator::start_interpolation(std::shared_ptr<ais::FMatrix> &targetMatrixPtr, std::shared_ptr<ais::PMatrix> &constrainMatrixPtr,
std::shared_ptr<ais::FMatrix> &pointInFaultMatrixPtr, std::shared_ptr<ais::FMatrix> &faultAffectMatrixPtr,
InterpolationMethod method)
{
if (method != InterpolationMethod::MINIMUM_CURVATURE)
THROW_INVALID_ARGUMENT("Only support minimum curvature interpolations method right now.");
startTime_ = std::chrono::system_clock::now();
// setup running flag
flagRunning_ = true;
// selected interpolation method to start working thread
update_min_curv_interpolation(targetMatrixPtr, constrainMatrixPtr, pointInFaultMatrixPtr, faultAffectMatrixPtr);
}
bool GridInterpolator::update_min_curv_interpolation(std::shared_ptr<ais::FMatrix> &targetMatrixPtr, std::shared_ptr<ais::PMatrix> &constrainMatrixPtr,
std::shared_ptr<ais::FMatrix> &pointInFaultMatrixPtr, std::shared_ptr<ais::FMatrix> &faultAffectMatrixPtr,
const std::vector<ais::Point> &targetArea)
{
{
std::lock_guard<std::mutex> lk(msgMutex_);
interpolationMsgs_.push_back(ais::time_str() + " Start interpolations:");
interpolationMsgs_.push_back("=======================================");
}
// build a expansion matrix for minimum curvature interpolation
// due to the principle of minimum curvature interpolation,
// we need to expand 2 additional points on each edge of the target matrix
// PS: all matrix object in this function should alloc in heap, due the matrix is very big
auto shape = targetMatrixPtr->shape();
auto updateMatrixPtr = std::make_shared<ais::FMatrix>(shape.rows + 4, shape.cols + 4);
updateMatrixPtr->emptys();
// build a interpolation params matrix
auto interpolationParamsPtr = std::make_shared<ais::Matrix<ais::MCI_APoints>>(shape);
interpolationParamsPtr->emptys();
auto sPtr = std::make_shared<ais::FMatrix>(shape);
auto tPtr = std::make_shared<ais::FMatrix>(shape);
sPtr->zeros();
tPtr->zeros();
// origin matrix reference for share_ptr<ais::Matrix>
auto &umo = *updateMatrixPtr;
auto &cmp = *constrainMatrixPtr;
// calc interpolation alpha
double alpha0 = -1.0 / (6 + 8 * alpha_ * alpha_ + 6 * (std::pow(alpha_, 4)));
double alpha1 = std::pow(alpha_, 4);
double alpha2 = 2 * (alpha_ * alpha_);
double alpha3 = -4 * (1 + alpha_ * alpha_);
double alpha4 = -4 * (1 + alpha_ * alpha_) * alpha_ * alpha_;
double alpha5 = -2 * (1 + alpha_ * alpha_);
double alpha6 = -2 * (1 + alpha_ * alpha_) * alpha_ * alpha_;
double beta1 = 2 * std::pow((1 + alpha_ * alpha_), 2);
double beta2 = (1 + std::pow(alpha_, 4));
// loop constraint matrix to get the interpolated params
for (int j = 0; j < shape.rows; j++)
{
for (int i = 0; i < shape.cols; i++)
{
auto cp = cmp(j, i);
if (!ais::isnan(cp))
{
sPtr->at(j, i) = cp.x > 0 ? 1 : -1;
tPtr->at(j, i) = cp.y > 0 ? 1 : -1;
ais::PointXYZ up{std::abs(cp.x / dxSize_), std::abs(cp.y / dySize_), cp.z};
constrainMatrixPtr->put(j, i, up);
double denominator = beta2 * (up.x + up.y) * (1 + up.x + up.y) +
beta1 * (up.x * up.y + up.x + up.y + 1);
denominator *= 2.0;
double a0 = -(up.x + up.y) * (1 + up.x + up.y) / denominator;
double a1 = beta1 * up.x * (1 + up.x + 2 * up.y) / denominator;
double a2 = -2 * beta1 * up.x * (up.x + up.y) / denominator;
double a3 = -2 * beta1 * up.y * (up.x + up.y) / denominator;
double a4 = -beta1 * up.x * (1 + up.x) / denominator;
double a5 = 2 * beta1 / denominator;
interpolationParamsPtr->put(j, i, {a0, a1, a2, a3, a4, a5});
}
}
}
// copy original data to expected matrix for interpolation
for (int j = 0; j < shape.rows; j++)
{
for (int i = 0; i < shape.cols; i++)
{
auto zValue = targetMatrixPtr->at(j, i);
umo(j + 2, i + 2) = zValue;
}
}
// setup the starting values for interpolation
double epsError = std::abs(epsMax_) * 2;
iteration_ = 0;
// setup the thread start row index vector
std::list<size_t> threadRowIndex;
auto threadRowStep = 6;
for (size_t tri = 0; tri < shape.rows; tri += threadRowStep)
{
threadRowIndex.push_back(tri);
}
auto copy_to_target_matrix = [&](const ais::FMatrix &source)
{
std::lock_guard<std::mutex> lk(dataMutex_);
// copy interpolation value to target matrix
for (int j = 0; j < shape.rows; j++)
{
for (int i = 0; i < shape.cols; i++)
{
targetMatrixPtr->at(j, i) = source(j + 2, i + 2);
}
}
};
// thread function
std::function<double(size_t, size_t, const ais::Shape &)> iterationWorker = [&umo, &cmp, &interpolationParamsPtr,
&sPtr, &tPtr, this,
&alpha0, &alpha1, &alpha2, &alpha3,
&alpha4, &alpha5, &alpha6](size_t rowStart, size_t step, const ais::Shape &shape)
{
size_t rowEnd = rowStart + step > shape.rows ? shape.rows : rowStart + step;
double epsErrorInThread = 0.0;
for (size_t j = rowStart; j < rowEnd; j++)
{
for (size_t i = 0; i < shape.cols; i++)
{
auto ui = i + 2;
auto uj = j + 2;
auto cp = cmp(j, i);
double temp = 0;
if (_LIKELY(std::isnan(cp.z)))
{
// WITHOUT constrain point
temp = (umo(uj, ui + 2) + umo(uj, ui - 2)) +
alpha1 * (umo(uj + 2, ui) + umo(uj - 2, ui)) +
alpha2 * (umo(uj + 1, ui + 1) + umo(uj + 1, ui - 1) + umo(uj - 1, ui + 1) + umo(uj - 1, ui - 1)) +
alpha3 * (umo(uj, ui + 1) + umo(uj, ui - 1)) +
alpha4 * (umo(uj + 1, ui) + umo(uj - 1, ui));
temp *= alpha0;
}
else
{
// with constrain point
auto ias = interpolationParamsPtr->at(j, i);
auto s = sPtr->at(j, i);
auto t = tPtr->at(j, i);
temp = (umo(uj, ui + 2) + umo(uj, ui - 2)) +
alpha1 * (umo(uj + 2, ui) + umo(uj - 2, ui)) +
alpha2 * (umo(uj + 1, ui + 1) + umo(uj + 1, ui - 1) + umo(uj - 1, ui + 1) + umo(uj - 1, ui - 1)) +
alpha5 * (umo(uj, ui + 1) + umo(uj, ui - 1)) + alpha6 * (umo(uj + 1, ui) + umo(uj - 1, ui));
temp *= ias[0];
temp += ias[0] * alpha5 * (-umo(uj + t, ui - s) + 2 * umo(uj, ui - s) + alpha2 * umo(uj - t, ui) + umo(uj - t, ui + s)) +
ias[1] * umo(uj + t, ui - s) + ias[2] * umo(uj, ui - s) +
ias[3] * umo(uj - t, ui) + ias[4] * umo(uj - t, ui + s) +
ias[5] * cp.z;
}
// over relaxation adjustments
temp = temp * 1.4 - 0.4 * umo(uj, ui);
epsErrorInThread = std::max(std::abs(temp - umo(uj, ui)), epsErrorInThread);
{
umo(uj, ui) = temp;
}
}
}
return epsErrorInThread;
};
// thread function with fault
std::function<double(size_t, size_t, const ais::Shape &)> iterationFaultWorker = [&umo, &cmp, &interpolationParamsPtr,
&sPtr, &tPtr, this,
&alpha0, &alpha1, &alpha2, &alpha3,
&alpha4, &alpha5, &alpha6](size_t rowStart, size_t step, const ais::Shape &shape)
{
size_t rowEnd = rowStart + step > shape.rows ? shape.rows : rowStart + step;
double epsErrorInThread = 0.0;
for (size_t j = rowStart; j < rowEnd; j++)
{
for (size_t i = 0; i < shape.cols; i++)
{
auto ui = i + 2;
auto uj = j + 2;
auto cp = cmp(j, i);
// check fault condition
long fpz = long(pointInFaultMatrixPtr_->at(j, i));
double temp = 0.0;
if (fpz > 0)
{
// fault inspected node, build the block data for iteration
// copy umo data to a block
ais::GridInterpolatorBlock blockData(umo, uj, ui);
auto pos = umo.index(uj, ui);
// use pre-iter block if existed
ais::GridInterpolatorBlock<> blockIter(fillValue_);
ais::GridInterpolatorBlock<> &block = blockIter;
if (faultEdgePointExpandBoxes_.find(pos) != faultEdgePointExpandBoxes_.cend())
{
std::lock_guard<std::mutex> lk(mapMutex_);
block = faultEdgePointExpandBoxes_[pos];
}
else
{
// make init values for block
for (int index = 0; index < 25; index++)
{
if (fpz & (1 << index))
{
// build init value for block nodes
block[index] = blockData.mock_value9(fpz, index, alpha_);
}
}
// if(j == 470 && i == 495)
//{
// std::cerr << "-----------------------------------------" << std::endl;
// std::cerr << "Point: " << pos << " Mask: " << fpz << std::endl;
// block.print();
// std::cerr << "-----------------------------------------" << std::endl;
// }
}
// Due to the fault mock value based on the pre-data value, so split this operations into 2 loop, don't merge them
// 1. copy pre-data to block
for (int index = 0; index < 25; index++)
{
if ((fpz & (1 << index)) == 0)
{
// copy non-fault value from origin data
block[index] = blockData[index];
}
}
if (j == 470 && i == 495 && iteration_ < 100)
{
std::cerr << "--------------Before Mock-------------" << std::endl;
std::cerr << "Point: " << pos << " Mask: " << fpz << " Iteration: " << iteration_ << std::endl;
block.print();
}
// 2. mock fault inspect node in block
if (faultEdgePointExpandBoxes_.find(pos) != faultEdgePointExpandBoxes_.cend())
{
for (int index = 0; index < 25; index++)
{
if (fpz & (1 << index))
{
// block[index] = block.mock_value(fpz);
block[index] = block.mock_value4(fpz, index);
// block[index] = block.mock_value10(fpz, index, alpha_);
}
}
if (j == 470 && i == 495 && iteration_ < 100)
{
std::cerr << "--------------After Mock-------------" << std::endl;
std::cerr << "Point: " << pos << " Mask: " << fpz << " Iteration: " << iteration_ << std::endl;
block.print();
}
}
// storage the block to map, for next iteration
{
std::lock_guard<std::mutex> lk(mapMutex_);
faultEdgePointExpandBoxes_[pos] = block;
}
// use data block to calculate node value
if (std::isnan(cp.z))
{
// WITHOUT constrain point
temp = (block(0, 2) + block(0, -2)) +
alpha1 * (block(2, 0) + block(-2, 0)) +
alpha2 * (block(1, 1) + block(1, -1) + block(-1, 1) + block(-1, -1)) +
alpha3 * (block(0, 1) + block(0, -1)) +
alpha4 * (block(1, 0) + block(-1, 0));
temp *= alpha0;
}
else
{
// with constrain point
auto ias = interpolationParamsPtr->at(j, i);
auto s = sPtr->at(j, i);
auto t = tPtr->at(j, i);
temp = (block(0, 2) + block(0, -2));
temp += alpha1 * (block(2, 0) + block(-2, 0));
temp += alpha2 * (block(1, 1) + block(1, -1) + block(-1, 1) + block(-1, -1));
temp += alpha5 * (block(0, 1) + block(0, -1));
temp += alpha6 * (block(1, 0) + block(-1, 0));
temp *= ias[0];
temp += ias[0] * alpha5 * (-block(t, -s) + 2 * block(0, -s) + alpha2 * block(-t, 0) + block(-t, s));
temp += ias[1] * block(t, -s) + ias[2] * block(0, -s) + ias[3] * block(-t, 0) + ias[4] * block(-t, s) + ias[5] * cp.z;
}
// flash in-fault (0,0) point in map for next iteration
if (fpz & (1 << 12))
{
std::lock_guard<std::mutex> lk(mapMutex_);
faultEdgePointExpandBoxes_[pos][12] = temp * 1.4 - 0.4 * umo(uj, ui);
}
}
else
{
// normal node
if (std::isnan(cp.z))
{
// WITHOUT constrain point
temp = (umo(uj, ui + 2) + umo(uj, ui - 2)) +
alpha1 * (umo(uj + 2, ui) + umo(uj - 2, ui)) +
alpha2 * (umo(uj + 1, ui + 1) + umo(uj + 1, ui - 1) + umo(uj - 1, ui + 1) + umo(uj - 1, ui - 1)) +
alpha3 * (umo(uj, ui + 1) + umo(uj, ui - 1)) +
alpha4 * (umo(uj + 1, ui) + umo(uj - 1, ui));
temp *= alpha0;
}
else
{
// with constrain point
auto ias = interpolationParamsPtr->at(j, i);
auto s = sPtr->at(j, i);
auto t = tPtr->at(j, i);
temp = (umo(uj, ui + 2) + umo(uj, ui - 2)) +
alpha1 * (umo(uj + 2, ui) + umo(uj - 2, ui)) +
alpha2 * (umo(uj + 1, ui + 1) + umo(uj + 1, ui - 1) + umo(uj - 1, ui + 1) + umo(uj - 1, ui - 1)) +
alpha5 * (umo(uj, ui + 1) + umo(uj, ui - 1)) + alpha6 * (umo(uj + 1, ui) + umo(uj - 1, ui));
temp *= ias[0];
temp += ias[0] * alpha5 * (-umo(uj + t, ui - s) + 2 * umo(uj, ui - s) + alpha2 * umo(uj - t, ui) + umo(uj - t, ui + s)) +
ias[1] * umo(uj + t, ui - s) + ias[2] * umo(uj, ui - s) +
ias[3] * umo(uj - t, ui) + ias[4] * umo(uj - t, ui + s) +
ias[5] * cp.z;
}
}
// over relaxation adjustments
temp = temp * 1.4 - 0.4 * umo(uj, ui);
epsErrorInThread = std::max(std::abs(temp - umo(uj, ui)), epsErrorInThread);
{
umo(uj, ui) = temp;
}
}
}
return epsErrorInThread;
};
// dump init value
{
// copy middle iteration result to target
copy_to_target_matrix(umo);
// update zMin and zMax
zMinMax_ = getMinMax(*targetMatrixPtr);
std::lock_guard<std::mutex> lk(msgMutex_);
std::string msg = "Iterating: " + std::to_string(iteration_);
interpolationMsgs_.push_back(msg);
//auto ret = std::async(std::launch::async, [&]()
// { std::string filename = "init_0.grd";
//dump_result(filename.c_str());
//return; });
}
// main interpolation loop
while (epsError > epsMax_ && iteration_ < maxIteration_ && flagRunning_)
{
epsError = 0.0;
update_min_curv_boundary(updateMatrixPtr, shape.cols, shape.rows, alpha_);
// dump updated boundary value
if (iteration_ == 0)
{
// copy middle iteration result to target
copy_to_target_matrix(umo);
// update zMin and zMax
zMinMax_ = getMinMax(*targetMatrixPtr);
std::lock_guard<std::mutex> lk(msgMutex_);
std::string msg = "Iterating: " + std::to_string(iteration_);
interpolationMsgs_.push_back(msg);
//auto ret = std::async(std::launch::async, [&]()
// { std::string filename = "updated_edge_0.grd";
//dump_result(filename.c_str());
//return; });
}
if (flagMultiThread_)
{
// ********* multi-thread *********
std::list<size_t> rowIndex = threadRowIndex;
std::vector<double> threadEpsErrors;
std::vector<std::future<double>> epsFutures;
// start threads
while (!rowIndex.empty())
{
size_t rowStart = rowIndex.front();
rowIndex.pop_front();
if (faults_.empty())
{
epsFutures.emplace_back(std::async(std::launch::async, iterationWorker, rowStart, threadRowStep, shape));
}
else
{
epsFutures.emplace_back(std::async(std::launch::async, iterationFaultWorker, rowStart, threadRowStep, shape));
}
}
// waiting for results
for (auto &ef : epsFutures)
{
ef.wait();
threadEpsErrors.push_back(ef.get());
}
epsError = *std::max_element(std::execution::par_unseq, threadEpsErrors.begin(), threadEpsErrors.end());
}
else
{
// ********* 1 thread async iterative *********
// auto worker = std::async(std::launch::async, iterationWorker, 0, shape.rows, shape);
// worker.wait();
// epsError = worker.get();
// ********* main thread sync iterative *********
if (faults_.empty())
{
epsError = iterationWorker(0, shape.rows, shape);
}
else
{
epsError = iterationFaultWorker(0, shape.rows, shape);
}
}
// dump progress while every 100 iteration
if (iteration_ % 100 == 0)
{
// copy middle iteration result to target
copy_to_target_matrix(umo);
// update zMin and zMax
zMinMax_ = getMinMax(*targetMatrixPtr);
std::lock_guard<std::mutex> lk(msgMutex_);
std::string msg = "Iterating: " + std::to_string(iteration_);
interpolationMsgs_.push_back(msg);
//auto ret = std::async(std::launch::async, [&]()
std::string filename = "progress_" + std::to_string(iteration_) + ".grd";
int8_t level = iteration_ % 1000 == 0 ? 1 : 2;
//dump_result_async(filename, targetMatrixPtr_, level);
}
iteration_++;
}
{
std::lock_guard<std::mutex> lk(msgMutex_);
interpolationMsgs_.push_back("=======================================");
interpolationMsgs_.push_back(ais::time_str() + " Completed interpolations:\n");
}
// copy to target and update zMin and zMax
copy_to_target_matrix(umo);
zMinMax_ = getMinMax(*targetMatrixPtr);
// setup fault point, due to the surfer will not set points to infinity, not sure this behavior make reasonable
if (!faults_.empty() && faultEdgeLevel_ >= 0)
{
int faultEdgeLimitation = 1 << faultEdgeLevel_;
for (size_t j = 0; j < shape.rows; j++)
{
for (size_t i = 0; i < shape.cols; i++)
{
auto pointInFault = faultAffectMatrixPtr_->at(j + 2, i + 2);
if (pointInFault >= faultEdgeLimitation)
{
targetMatrixPtr->put(j, i, INFINITY);
faultNodesCount_++;
}
}
}
}
endTime_ = std::chrono::system_clock::now();
// std::cout << "------------------End of min curv interpolation-----------------" << std::endl;
// std::cout << data_info();
// release unused memory
updateMatrixPtr.reset();
interpolationParamsPtr.reset();
pointInFaultMatrixPtr_.reset();
faultAffectMatrixPtr_.reset();
sPtr.reset();
tPtr.reset();
flagSucceed_ = (iteration_ < maxIteration_);
return flagSucceed_;
}
void GridInterpolator::update_min_curv_boundary(std::shared_ptr<ais::FMatrix> &updateMatrixPtr, size_t column, size_t row, double alpha)
{
// updateMatrixPtr are extened matrix, [0:x+3, 0:y+3],
// extened 2 point at each side of the matrix
//
// e, e, e, e, ~, e, e, e, e | y+3
// e, i, i, i, ~ ,i ,i, i, e | y+2
// e ,i ,0 ,0, ~, 0 ,0, i, e | y+1
// e ,i ,0 ,0, ~, 0 ,0, i, e | y
// ... ... |
// e, i, 0, 0, ~, 0, 0, i, e | 2
// e, i, i, i, ~, i, i, i, e | 1
// e, e, e, e, ~, e, e, e, e | 0
// ----------------------------+
// 0 1 2 3 ~ x x+1 x+2 x+3
// std::cout << "--------------------Update min curv boundary--------------------" << std::endl;
// updateMatrixPtr->print();
// std::cout << "----------------------------------------------------------------" << std::endl;
auto &ump = *updateMatrixPtr;
auto up_shape = ump.shape();
if (up_shape.rows != (row + 4) || up_shape.cols != (column + 4))
{
THROW_INVALID_ARGUMENT("Extended matrix values not matching");
}
auto ej = row + 2; // extened matrix j for y axis
auto ei = column + 2; // extened matrix i for x axis
// points at left&right side of extended boundary
for (size_t j = 0; j < row; j++)
{
auto uj = j + 2;
auto ui = ei;
ump(uj, 1) = 2 * ump(uj, 2) - ump(uj, 3); // left
ump(uj, ui) = 2 * ump(uj, ui - 1) - ump(uj, ui - 2); // right
}
// points at down&top side of extended boundary
for (size_t i = 0; i <= column; i++)
{
auto uj = ej;
auto ui = i + 2;
ump(1, ui) = 2 * ump(2, ui) - ump(3, ui); // bottom
ump(uj, ui) = 2 * ump(uj - 1, ui) - ump(uj - 2, ui); // top
}
// points at corners
ump(1, 1) = ump(1, 3) + ump(3, 1) - ump(3, 3); // left-down
ump(1, ei) = ump(3, ei) + ump(1, ei - 2) - ump(3, ei - 2); // right-down
ump(ej, 1) = ump(ej, 3) + ump(ej - 2, 1) - ump(ej - 2, 3); // left-top
ump(ej, ei) = ump(ej - 2, ei) + ump(ej, ei - 2) - ump(ej - 2, ei - 2); // right-top
// exterior extension points
double alpha1 = alpha * alpha;
double alpha2 = -2 * (1 + alpha1);
double beta = 1 / alpha;
double beta1 = beta * beta;
double beta2 = -2 * (1 + beta1);
for (size_t j = 0; j < row; j++)
{
auto uj = j + 2;
auto p_ij = [&](size_t i)
{
auto a1 = alpha1 * (ump(uj + 1, i + 1) - ump(uj + 1, i - 1) + ump(uj - 1, i + 1) - ump(uj - 1, i - 1));
auto a2 = alpha2 * (ump(uj, i + 1) - ump(uj, i - 1));
return a1 + a2;
};
// left
ump(uj, 0) = ump(uj, 4) + p_ij(2);
// right
ump(uj, column + 3) = ump(uj, column - 1) - p_ij(column + 1);
}
for (size_t i = 0; i < column; i++)
{
auto ui = i + 2;
auto q_ij = [&](size_t j)
{
auto b1 = beta1 * (ump(j + 1, ui + 1) - ump(j - 1, ui + 1) + ump(j + 1, ui - 1) - ump(j - 1, ui - 1));
auto b2 = beta2 * (ump(j + 1, ui) - ump(j - 1, ui));
return b1 + b2;
};
// bottom
ump(0, ui) = ump(4, ui) + q_ij(2);
// top
ump(row + 3, ui) = ump(row - 1, ui) - q_ij(row + 1);
}
// std::cout << "--------------------End of min curv boundary--------------------" << std::endl;
// updateMatrixPtr->print();
// std::cout << "----------------------------------------------------------------" << std::endl;
// std::cout << ump(79, 34) << std::endl;
// ais::GridInterpolatorBlock block(ump, 79, 34);
// block.print();
}
} // namespace ais