Homec4science
Diffusion tamaas (ci-fix-static-checks) Browse

tamaas/src/interpolator.cpp
0e9a9f1f8405ci-fix-static-checks

Last Change
Raw File

interpolator.cpp
View Options

/**
*
* @author Guillaume Anciaux <guillaume.anciaux@epfl.ch>
*
* @section LICENSE
*
* Copyright (©) 2016 EPFL (Ecole Polytechnique Fédérale de
* Lausanne) Laboratory (LSMS - Laboratoire de Simulation en Mécanique des
* Solides)
*
* Tamaas is free software: you can redistribute it and/or modify it under the
* terms of the GNU Lesser General Public License as published by the Free
* Software Foundation, either version 3 of the License, or (at your option) any
* later version.
*
* Tamaas is distributed in the hope that it will be useful, but WITHOUT ANY
* WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
* A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
* details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Tamaas. If not, see <http://www.gnu.org/licenses/>.
*
*/
/* -------------------------------------------------------------------------- */
#include "interpolator.hh"
__BEGIN_TAMAAS__
using namespace::std;
const Real DEF_SLOPE = 1.23456789e10;
const int shift = -1;
Real Interpolator::getRelativeElement(const vector<Real>& line, int act_pos, int shift)
{
int length = line.size();
int new_pos = act_pos + shift;
if (new_pos >= 0 && new_pos < length)
return line[new_pos];
else if (new_pos < 0)
return line[length + new_pos];
else
return line[new_pos - length];
}
/* ---------------------------------------------------------------------- */
Real Interpolator::getRelativeElement(Surface& surface, int act_pos_x, int act_pos_y, int shift_x, int shift_y)
{
int size = surface.n;
int new_pos_x = act_pos_x + shift_x,
new_pos_y = act_pos_y + shift_y;
if (new_pos_x >= 0 && new_pos_x < size)
{
if (new_pos_y >= 0 && new_pos_y < size)
return surface(new_pos_x,new_pos_y).re;
else if (new_pos_y < 0)
return surface(new_pos_x,size+new_pos_y).re;
else
return surface(new_pos_x,new_pos_y-size).re;
}
else if (new_pos_x < 0)
{
if (new_pos_y >= 0 && new_pos_y < size)
return surface(size+new_pos_x,new_pos_y).re;
else if (new_pos_y < 0)
return surface(size+new_pos_x,size+new_pos_y).re;
else
return surface(size+new_pos_x,new_pos_y-size).re;
}
else
{
if (new_pos_y >= 0 && new_pos_y < size)
return surface(-size+new_pos_x,new_pos_y).re;
else if (new_pos_y < 0)
return surface(-size+new_pos_x,size+new_pos_y).re;
else
return surface(-size+new_pos_x,new_pos_y-size).re;
}
}
/* ---------------------------------------------------------------- */
int Interpolator::interpolate(bool forward, bool horizontal, vector<vector<Real> >& interpolated_surface, int ix, int iy,
vector<Real>& original_points, INTER_FUNCTION_TYPE type,
vector<int>& deja_interpolated, vector<Real>& slope, Real radius, Real val, int steps)
{
if (type == ZERO)
{
for (int i = 0; i < num_inter_points; i++)
assign_value(interpolated_surface, ix + (1-horizontal)*i, iy + horizontal*i, 0.);
assign_value(deja_interpolated, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points - 1, 1);
assign_value(slope, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points, 0.);
return 1;
}
else if (type == SQ_ROOT)
{
INTER_FUNC func(SQ_ROOT, original_points, radius);
if (forward) // :: from noncontact to contact
{
for (int i = 0; i < 2*num_inter_points; i++)
assign_value(interpolated_surface, ix + (1-horizontal)*i, iy + horizontal*i, func.get_value( i/(2.*num_inter_points) ));
assign_value(deja_interpolated, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points - 1, 1);
assign_value(deja_interpolated, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points, 1);
assign_value(deja_interpolated, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points + 1, 1);
assign_value(slope, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points + 2, 0.5*func.get_derivative(1.));
return 2;
}
else // backward :: from contact to noncontact
{
for (int i = 0; i < 2*num_inter_points; i++)
assign_value(interpolated_surface, ix - (1-horizontal)*i, iy - horizontal*i, func.get_value( i/(2.*num_inter_points) ));
assign_value(deja_interpolated, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points - 1, 1);
assign_value(deja_interpolated, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points - 2, 1);
assign_value(slope, (1-horizontal)*ix/num_inter_points + horizontal*iy/num_inter_points - 2, -0.5*func.get_derivative(1.) );
return 1;
}
}
else if (type == QUADRATIC_BEZIER) // See "http://en.wikipedia.org/wiki/B%C3%A9zier_curve"
{
Real t,
p0 = original_points[1],
p2 = original_points[2],
m0 = ( original_points[1] - original_points[0] ),
m1 = ( original_points[3] - original_points[2] ),
p1 = 0.5 * ( (p0 + m0*0.5) + (p2 - m1*0.5) );
// Interpolation
for (int i = 0; i < num_inter_points; i++)
{
t = i/Real(num_inter_points);
assign_value(interpolated_surface, ix + (1-horizontal)*i, iy + horizontal*i, pow(1-t,2.)*p0 + 2*(1-t)*t*p1 + t*t*p2);
}
return 1;
}
else if (type == CUBIC_BEZIER) // See "http://en.wikipedia.org/wiki/B%C3%A9zier_curve"
{
Real t, alpha = 0.3,
p0 = original_points[1],
p3 = original_points[2],
m0 = 0.5 * ( original_points[2] - original_points[0] ),
m1 = 0.5 * ( original_points[3] - original_points[1] ),
p1 = p0 + m0 * alpha,
p2 = p3 - m1 * alpha;
// Interpolation
for (int i = 0; i < num_inter_points; i++)
{
t = i/Real(num_inter_points);
assign_value(interpolated_surface, ix + (1-horizontal)*i, iy + horizontal*i, pow(1-t,3)*p0 + 3*pow(1-t,2)*t*p1 + 3*(1-t)*t*t*p2 + t*t*t*p3);
}
return 1;
}
else if (type == CUBIC_HERMITE) // See "http://en.wikipedia.org/wiki/Cubic_interpolation", Cardinal Spline, alpha in [0;1]
{
// int ci = horizontal*ix/num_inter_points + (1-horizontal)*iy/num_inter_points;
// if ( !deja_interpolated[ci] )
// {
Real t, alpha = 0.,
p0 = original_points[1],
p1 = original_points[2],
m0 = (1-alpha) * 0.5 * ( original_points[2] - original_points[0] ),
m1 = (1-alpha) * 0.5 * ( original_points[3] - original_points[1] );
// Left slope
/*
if (fabs(slope[ci] - DEF_SLOPE) < 0.1)
m0 = 0.5 * ( original_points[3] - original_points[1] );
else
m0 = slope[ci];
// Right slope
int ci_1;
if (ci + 1 < int(slope.size()))
ci_1 = ci + 1;
else
ci_1 = 0;
if (fabs(slope[ci_1] - DEF_SLOPE) < 0.1)
m1 = 0.5 * ( original_points[2] - original_points[0] );
else
m1 = slope[ci_1];
*/
// Interpolation
for (int i = 0; i < num_inter_points; i++)
{
t = i/Real(num_inter_points);
Real t3 = t*t*t, t2 = t*t;
assign_value(interpolated_surface, ix + (1-horizontal)*i, iy + horizontal*i, (2*t3 - 3*t2 + 1)*p0 + (t3 - 2*t2 + t)*m0 + (-2*t3 + 3*t2)*p1 + (t3 - t2)*m1);
}
return 1;
}
else if (type == LINEAR)
{
INTER_FUNC func(LINEAR, original_points);
for (int i = 0; i < num_inter_points; i++)
assign_value(interpolated_surface, ix + (1-horizontal)*i, iy + horizontal*i, func.get_value( i/Real(num_inter_points) ) );
return 1;
}
else if (type == PARABOLA)
{
cout << endl << "Error: unknown interpolation type." << endl;
exit(1);
}
else if (type == TEST_INTER)
{
if (val == 0.)
val = 3.;
if (forward)
{
for (int i = 0; i < steps*num_inter_points; i++)
assign_value(interpolated_surface, ix + (1-horizontal)*i, iy + horizontal*i, val);
return steps;
}
else
{
for (int i = 0; i < steps*num_inter_points; i++)
assign_value(interpolated_surface, ix - (1-horizontal)*i, iy - horizontal*i, val);
return steps;
}
}
else
{
cout << endl << "Error: unknown interpolation type." << endl;
exit(1);
}
}
/* ---------------------------------------------------------------- */
void Interpolator::assign_value(vector<Real>& vec, int i, Real value)
{
if ( i >= 0 && i < int(vec.size()) )
vec[i] = value;
}
void Interpolator::assign_value(vector<int>& vec, int i, Real value)
{
if ( i >= 0 && i < int(vec.size()) )
vec[i] = value;
}
void Interpolator::assign_value(vector<vector<Real> >& surf, int i, int j, Real value)
{
if ( i >= 0 && i < int(surf.size()) && j >= 0 && j < int(surf[i].size()) )
surf[i][j] = value;
}
/* ---------------------------------------------------------------- */
void Interpolator::make_by_patch2(Surface& original, int ix, int iy, int patch_size, int margin, bool parabola, Real max_value, vector<vector<Real> >& pdfs,
Real& area, Real& perimeter, Real& pressure, vector<vector<Real> >& interpolated_surface, string fname)
{
//o test
bool to_be_interpolated = false;
for (int iline = 0; iline < patch_size; iline++)
for (int jline = 0; jline < patch_size; jline++)
if (getRelativeElement(original,ix,iy,iline,jline) > 1e-14)
{
to_be_interpolated = true;
break;
}
if (!to_be_interpolated)
return;
//x test
int size = patch_size+2*margin;
bool concave_1,
concave_2;
vector<Real> data_points(4),
interp,
slope(size, DEF_SLOPE),
original_line(patch_size+2*margin, 0.);
vector<int> deja_interpolated(size, 0);
/////////////////////////////////////////////////////////////////////////////////
// INTERPOLATION ALONG PROFILES
/////////////////////////////////////////////////////////////////////////////////
for (int iline = 0; iline < patch_size+margin*2; iline++)
{
for (unsigned int i = 0; i < deja_interpolated.size(); i++)
deja_interpolated[i] = 0;
for (unsigned int i = 0; i < slope.size(); i++)
slope[i] = DEF_SLOPE;
for (unsigned int i = 0; i < interpolated_surface[iline].size(); i++)
assign_value(interpolated_surface, iline*num_inter_points, i, 0.);
for (int i = 0; i < patch_size+margin*2; i++)
original_line[i] = getRelativeElement(original, ix, iy, iline-margin, i-margin);
int i = 1;
do
{
if (original_line[i-1] == 0 && original_line[i] == 0)
{
i+=interpolate(true, true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, ZERO, deja_interpolated, slope);
continue;
}
else if (original_line[i-1] == 0)
{
for (int k = 0; k < 4; k++)
data_points[k] = getRelativeElement(original_line,i,k);
interp.resize(2);
interp[0] = data_points[0];
interp[1] = data_points[1];
Real radius = find_dist_to_max(true, original_line, i);
i += interpolate(true, true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
continue;
}
else if (original_line[i] == 0)
{
for (int k = 1; k < 3; k++)
data_points[k-1] = getRelativeElement(original_line,i,-k);
interp.resize(2);
interp[0] = data_points[0];
interp[1] = data_points[1];
Real radius = find_dist_to_max(false, original_line, i);
i += interpolate(false, true, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
continue;
}
else
{
/*
interp.resize(2);
for (int k = 0; k < 2; k++)
interp[k] = getRelativeElement(original_line, i-1, k);
i += interpolate(true, true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, LINEAR, deja_interpolated, slope);
*/
for (int k = 0; k < 4; k++)
data_points[k] = getRelativeElement(original_line, i-1, k-1);
i += interpolate(true, true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, data_points, QUADRATIC_BEZIER, deja_interpolated, slope);
//i += interpolate(true, true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, data_points, CUBIC_BEZIER, deja_interpolated, slope);
//i += interpolate(true, true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, data_points, CUBIC_HERMITE, deja_interpolated, slope);
continue;
}
}
while (i < size);
///////////////////////////////////////////////////////////////////////////////
// CUBIC INTERPOLATION
//////////////////////////////////////////////////////////////////////////////
/*
i = 1;
do
{
interp.resize(4);
interp[0] = getRelativeElement(original_line,i-1,-1);
interp[1] = getRelativeElement(original_line,i-1,0);
interp[2] = getRelativeElement(original_line,i-1,1);
interp[3] = getRelativeElement(original_line,i-1,2);
if (interp[1] > 1e-14 && interp[2] > 1e-14)
i += interpolate(false, true, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, CUBIC_HERMITE, deja_interpolated, slope);
else
i++;
}
while (i < size);
*/
// Plot files
if (fname != "" && iline >= margin && iline < patch_size+margin)
{
std::ofstream file;
std::stringstream max_value_str;
max_value_str << int(100*max_value);
file.open((fname+"_profiles_"+max_value_str.str()).c_str(),ios_base::app);
file.precision(15);
for (int j = num_inter_points*margin; j < num_inter_points*(size-margin); j++)
// if (interpolated_surface[iline*num_inter_points][j] > 1e-14)
file << (ix+iline)*num_inter_points << " " << iy*num_inter_points+j << " "
<< interpolated_surface[iline*num_inter_points][j] << " " << deja_interpolated[int(j/num_inter_points)] << " " << slope[int(j/num_inter_points)] << endl;
file.close();
}
}
///////////////////////////////////////////////////////////////////////////////////////
// ORTHOGONAL INTERPOLATION
/////////////////////////////////////////////////////////////////////////////////
for (int iline = 0; iline < (patch_size+margin*2)*num_inter_points; iline++)
{
for (unsigned int i = 0; i < slope.size(); i++)
{
slope[i] = DEF_SLOPE;
deja_interpolated[i] = 0;
}
for (int jline = 0; jline < patch_size+margin*2; jline++)
original_line[jline] = interpolated_surface[jline*num_inter_points][iline];
for (int jline = 0; jline < (patch_size+margin*2)*num_inter_points; jline++)
if (jline % num_inter_points != 0)
assign_value(interpolated_surface,jline,iline,0.);
int i = 1;
do
{
if (original_line[i-1] == 0 && original_line[i] == 0)
{
i+=interpolate(true, false, interpolated_surface, (i-1)*num_inter_points, iline, interp, ZERO, deja_interpolated, slope);
continue;
}
else if (original_line[i-1] == 0)
{
for (int k = 0; k < 4; k++)
data_points[k] = getRelativeElement(original_line,i,k);
interp.resize(2);
interp[0] = data_points[0];
interp[1] = data_points[1];
Real radius = find_dist_to_max(true, original_line, i);
i += interpolate(true, false, interpolated_surface, (i-1)*num_inter_points, iline, interp, SQ_ROOT, deja_interpolated, slope, radius);
continue;
}
else if (original_line[i] == 0)
{
for (int k = 1; k < 3; k++)
data_points[k-1] = getRelativeElement(original_line,i,-k);
interp.resize(2);
interp[0] = data_points[0];
interp[1] = data_points[1];
Real radius = find_dist_to_max(false, original_line, i);
i += interpolate(false, false, interpolated_surface, i*num_inter_points, iline, interp, SQ_ROOT, deja_interpolated, slope, radius);
continue;
}
else
{
/*
interp.resize(2);
for (int k = 0; k < 2; k++)
interp[k] = getRelativeElement(original_line, i-1, -k);
i += interpolate(true, false, interpolated_surface, (i-1)*num_inter_points, iline, interp, LINEAR, deja_interpolated, slope);
*/
for (int k = 0; k < 4; k++)
data_points[k] = getRelativeElement(original_line, i-1, k-1);
i += interpolate(true, false, interpolated_surface, (i-1)*num_inter_points, iline, data_points, QUADRATIC_BEZIER, deja_interpolated, slope);
//i += interpolate(true, false, interpolated_surface, (i-1)*num_inter_points, iline, data_points, CUBIC_BEZIER, deja_interpolated, slope);
//i += interpolate(true, false, interpolated_surface, (i-1)*num_inter_points, iline, data_points, CUBIC_HERMITE, deja_interpolated, slope);
continue;
/*
for (int k = 0; k < 2; k++)
data_points[k] = getRelativeElement(original_line,i-1,k);
interp.resize(2);
interp[0] = data_points[0];
interp[1] = data_points[1];
i += interpolate(true, false, interpolated_surface, (i-1)*num_inter_points, iline, interp, LINEAR, deja_interpolated, slope);
continue;
*/
}
}
while (i < size);
}
// Compute statistics
Real value;
for (unsigned int i = num_inter_points*margin; i < interpolated_surface.size()-num_inter_points*margin; i++)
{
for (unsigned int j = num_inter_points*margin; j < interpolated_surface[i].size()-num_inter_points*margin; j++)
{
value = interpolated_surface[i][j];
pressure+=value;
if (value > 1e-14)
area+=1.;
for (unsigned int k = 0; k < pdfs.size() && value > 1e-14; k++)
{
int num = int((1-(max_value - value)/max_value)*pdfs[k].size());
if (num >= int(pdfs[k].size()))
num = pdfs[k].size()-1;
pdfs[k][num] += 1.;
}
}
}
// Plot files
if (fname != "")
{
std::ofstream file;
std::stringstream max_value_str;
max_value_str << int(100*max_value);
// interpolated surface
file.open((fname+"_"+max_value_str.str()).c_str(),ios_base::app);
file.precision(15);
for (unsigned int i = num_inter_points*margin; i < interpolated_surface.size()-num_inter_points*margin; i++)
for (unsigned int j = num_inter_points*margin; j < interpolated_surface[i].size()-num_inter_points*margin; j++)
file << (ix-margin)*num_inter_points+i << " " << (iy-margin)*num_inter_points+j << " " << interpolated_surface[i][j] << " " << endl;
file.close();
}
/*
/////////////////////////////////////////////////////////////////////////////////
// GO ALONG THE LINE :: original_line
/////////////////////////////////////////////////////////////////////////////////
int i = 1;
do
{
//////////////////////////////////////////////
// non-contact 0------0-----?
// [i-1] [i] [i+1]
//////////////////////////////////////////////
if (original_line[i-1] == 0 && original_line[i] == 0)
{
//i+=interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, ZERO, deja_interpolated, slope);
i+=interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope,0,0);
continue;
}
//////////////////////////////////////////////
// contact 1------1------?
// [i-1] [i] [i+1]
//////////////////////////////////////////////
else
{
/////////////////////////////////////////////////////////////////////////////
// From non-contact to contact 0-----1-----?
// [i-1] [i] [i+1]
/////////////////////////////////////////////////////////////////////////////
if (original_line[i-1] == 0)
{
for (int k = 0; k < 4; k++)
data_points[k] = getRelativeElement(original_line,i,k);
concave_1 = if_concave(original_line,i);
concave_2 = if_concave(original_line,i+1);
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Only few points are nonzero 0-----1-----0 OR 0-----1-----1------0
// [i-1] [i] [i+1] [i-1] [i] [i+1] [i+2]
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if (data_points[1]*data_points[2] < 1e-14)
{
////////////////////////////////////////////////////////////////////////////////////////////
// Singular point in contact 0-----1-----0
// [i-1] [i] [i+1]
////////////////////////////////////////////////////////////////////////////////////////////
if (data_points[1] < 1e-14)
{
Real a = num_inter_points/2.,
p0 = data_points[0]/a,
x, x2, a2 = a*a;
for (int j = 0; j <= 2*num_inter_points; j++)
{
x = j-num_inter_points;
x2 = x*x;
if (a2 > x2)
assign_value(interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points+j, p0*sqrt(a2 - x2));
else
assign_value(interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points+j, 0.);
}
assign_value(deja_interpolated, i-1, 1);
assign_value(deja_interpolated, i, 1);
i+=2;
continue;
}
////////////////////////////////////////////////////////////////////////////////////////////
// Only two points are in contact 0-----1-----1------0
// [i-1] [i] [i+1] [i+2]
////////////////////////////////////////////////////////////////////////////////////////////
else
{
interp.resize(2);
interp[0] = data_points[0];
interp[1] = data_points[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
deja_interpolated[i-1] = 1;
for (int k = 0; k < 3; k++)
{
for (int t = 0; t < num_inter_points; t++)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+k)*num_inter_points+t, func.get_value( (k*num_inter_points+t)/(3.*num_inter_points) ) );
assign_value(deja_interpolated, i-1+k, 1);
}
i+=3;
continue;
}
}
/////////////////////////////////////////////////////////////////////////////////////////
// Sufficient number of points for interpolation 0------1------1------1------?
// [i-1] [i] [i+1] [i+2] ...
/////////////////////////////////////////////////////////////////////////////////////////
else
{
if (data_points[2] < data_points[1] && data_points[1] > data_points[0])
{
interp.resize(2);
interp[0] = data_points[0];
interp[1] = data_points[1];
//i += interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, 0.5);
i += interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, 0.5, 2);
continue;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = data_points[1];
interp[1] = data_points[2];
Real radius = find_dist_to_max(true, original_line, i);
INTER_FUNC func(SQ_ROOT, interp, radius);
if ( !(func.get_value(0.) > 0 || interp[1] < interp[0]) )
{
//i += interpolate(true, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
i += interpolate(true, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, radius, 3);
continue;
}
else
{
interp[0] = data_points[0];
interp[1] = data_points[1];
//i += interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
i += interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, radius, 4);
continue;
}
}
else
{
if (concave_1 || !parabola)
{
interp.resize(2);
interp[0] = data_points[1];
interp[1] = data_points[2];
Real radius = find_dist_to_max(true, original_line, i);
INTER_FUNC func(SQ_ROOT, interp, radius);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
//i += interpolate(true, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
i += interpolate(true, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, radius, 5);
continue;
}
else
{
interp[0] = data_points[0];
interp[1] = data_points[1];
//i += interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
i += interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, radius, 6);
continue;
}
}
else
{
interp.resize(3);
interp[0] = data_points[0];
interp[1] = data_points[1];
interp[2] = data_points[2];
INTER_FUNC func(PARABOLA,interp);
//i += interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, PARABOLA, deja_interpolated, slope);
i += interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, 7);
}
}
}
}
/////////////////////////////////////////////////////////////////////////////
// From contact to non-contact 1-----0
// [i-1] [i]
/////////////////////////////////////////////////////////////////////////////
else if (original_line[i] == 0)
{
if (data_points[2] < data_points[1] && data_points[1] > data_points[0])
{
interp.resize(2);
interp[0] = data_points[0];
interp[1] = data_points[1];
//i += interpolate(false, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, 0.5);
i += interpolate(false, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, 0.5);
continue;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = data_points[1];
interp[1] = data_points[2];
Real radius = find_dist_to_max(true, original_line, i);
cout << "* * * * * * 1, p[0] = " << interp[0] << "; p[1] = " << interp[1] << "; a = " << radius << endl;
INTER_FUNC func(SQ_ROOT, interp, radius);
if ( !(func.get_value(0.) > 0 || interp[1] < interp[0]) )
{
//i += interpolate(false, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
i += interpolate(false, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, radius);
continue;
}
else
{
interp[0] = data_points[0];
interp[1] = data_points[1];
//i += interpolate(false, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
i += interpolate(false, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, radius);
continue;
}
}
else
{
if (concave_1 || !parabola)
{
interp.resize(2);
interp[0] = data_points[1];
interp[1] = data_points[2];
Real radius = find_dist_to_max(true, original_line, i);
cout << "* * * * * * 2" << endl;
INTER_FUNC func(SQ_ROOT, interp, radius);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
//i += interpolate(false, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
i += interpolate(false, interpolated_surface, iline*num_inter_points, i*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, radius);
continue;
}
else
{
interp[0] = data_points[0];
interp[1] = data_points[1];
//i += interpolate(false, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, SQ_ROOT, deja_interpolated, slope, radius);
i += interpolate(false, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope, radius);
continue;
}
}
else
{
interp.resize(3);
interp[0] = data_points[0];
interp[1] = data_points[1];
interp[2] = data_points[2];
cout << "* * * * * * 3" << endl;
INTER_FUNC func(PARABOLA,interp);
//i += interpolate(false, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, PARABOLA, deja_interpolated, slope);
i += interpolate(false, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope);
}
}
}
else // Cubic
{
i+=interpolate(true, interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, TEST_INTER, deja_interpolated, slope,0,5.);
continue;
}
}
}
while (i < size);
*/
}
/* ---------------------------------------------------------------- */
void Interpolator::make_by_patch(Surface& original, int ix, int iy, int patch_size, int margin, bool parabola, Real max_value, vector<vector<Real> >& pdfs,
Real& area, Real& perimeter, Real& pressure, vector<vector<Real> >& interpolated_surface, string fname)
{
// test
bool to_be_interpolated = false;
for (int iline = 0; iline < patch_size; iline++)
for (int jline = 0; jline < patch_size; jline++)
if (getRelativeElement(original,ix,iy,iline,jline) != 0)
{
to_be_interpolated = true;
break;
}
if (!to_be_interpolated)
return;
//oe test
int size = (patch_size+2*margin);
Real value;
vector<Real> convexity1(3), convexity(3), interp;
vector<int> deja_interpolated(size,0);
vector<Real> slope(size,DEF_SLOPE);
vector<Real> original_line(patch_size+2*margin);
vector<vector<Real> > deja_interpolated_m;
deja_interpolated_m.resize(0);
/////////////////////////////////////////////////////////////////////////////////
// INTERPOLATION ALONG PROFILES
/////////////////////////////////////////////////////////////////////////////////
for (int iline = 0; iline < patch_size+margin*2; iline++)
{
for (unsigned int i = 0; i < deja_interpolated.size(); i++)
deja_interpolated[i] = 0;
for (unsigned int i = 0; i < interpolated_surface[iline].size(); i++)
assign_value(interpolated_surface,iline*num_inter_points,i, 0.); // 0
for (int jline = 0; jline < patch_size+margin*2; jline++)
original_line[jline] = getRelativeElement(original,ix,iy,iline-margin,jline-margin);
int i = 1;
do
{
// non-contact
if (original_line[i-1] == 0 && original_line[i] == 0)
// i+=interpolate(interpolated_surface, iline*num_inter_points, (i-1)*num_inter_points, interp, ZERO, deja_interpolated, slope);
{
for (int j = 0; j < num_inter_points; j++)
assign_value(interpolated_surface,iline*num_inter_points,(i-1)*num_inter_points+j, 0.);
deja_interpolated[i-1] = 1;
slope[i] = 0.;
i++;
}
// contact
else
{
// ================================================================================================
// ================================================================================================
// From non-contact to contact
// ================================================================================================
// ================================================================================================
if (original_line[i-1] == 0)
{
convexity1[0] = getRelativeElement(original_line,i,0);
convexity1[1] = getRelativeElement(original_line,i,1);
convexity1[2] = getRelativeElement(original_line,i,2);
convexity[0] = getRelativeElement(original_line,i,1);
convexity[1] = getRelativeElement(original_line,i,2);
convexity[2] = getRelativeElement(original_line,i,3);
bool concave_1 = if_concave(convexity1),
concave_2 = if_concave(convexity);
// Only few points are nonzero
if (convexity1[0]*convexity1[1]*convexity1[2]*convexity[2] == 0.)
{
if (convexity1[1] == 0.)
{
Real a = num_inter_points/2.,
p0 = convexity1[0]/a;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
Real x = (i-1+j)*num_inter_points+k;
if (a*a > (x-i*num_inter_points)*(x-i*num_inter_points))
{
value = p0*sqrt(a*a - (x-i*num_inter_points)*(x-i*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, value);
}
else
{
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, 0.);
}
}
if (i-1+j<size)
deja_interpolated[i-1+j] = 1;
}
i+=2;
continue;
}
else if (convexity1[1] != 0. && (convexity[1] == 0. || convexity[2] == 0.) )
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int k = 0; k < num_inter_points; k++)
assign_value(interpolated_surface,iline*num_inter_points,(i-1)*num_inter_points+k, 0.);
deja_interpolated[i-1] = 1;
if (convexity[1] == 0.)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points)); // STRANGE THAT IT'S NOT 3 in the denominator
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, value);
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+3 < size)
slope[i+3] = func.get_derivative(1.)/3.;
i+=3;
}
else if (convexity[2] == 0.)
{
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points)); // SHOULD BE 4, NO?
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, value);
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+3 < size)
slope[i+3] = func.get_derivative(1.)/4.;
i+=3;
}
}
}
// Sufficient number of points is nonzero
else
{
if (convexity1[2] < convexity1[1] && convexity1[1] > convexity1[0])
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, value);
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+1<size)
slope[i+1] = func.get_derivative(1.)/2.;
i+=1;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
assign_value(interpolated_surface,iline*num_inter_points,(i-1)*num_inter_points+k, 0.);
deja_interpolated[i-1] = 1;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j+1 < size)
assign_value(interpolated_surface,iline*num_inter_points,(i+j)*num_inter_points+k, value);
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+1 < size)
slope[i+1] = func.get_derivative(1.)/2.;
i+=2;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, value);
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+1 < size)
slope[i+1] = func.get_derivative(1.)/2.;
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i+2 < size)
assign_value(interpolated_surface,iline*num_inter_points,(i+1)*num_inter_points+k, func.get_value(k/(1.*num_inter_points)));
if (i+1 < size)
deja_interpolated[i+1] = 1;
if (i+2 < size)
slope[i+2] = func.get_derivative(1.);
i+=2;
}
}
else
{
if (concave_1 || !parabola) //func.get_value(-0.5) > 0.)
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
assign_value(interpolated_surface,iline*num_inter_points,(i-1)*num_inter_points+k, 0.);
deja_interpolated[i-1] = 1;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j+1 < size)
assign_value(interpolated_surface,iline*num_inter_points,(i+j)*num_inter_points+k, value);
}
if (i+j<size)
deja_interpolated[i+j] = 1;
}
if (i+3 < size)
slope[i+3] = func.get_derivative(1.)/2.;
i+=2;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, value);
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+2 < size)
slope[i+2] = func.get_derivative(1.)/2.;
i+=2;
}
}
else
{
interp.resize(3);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
interp[2] = convexity1[2];
INTER_FUNC func(PARABOLA,interp);
Real x_edge = -0.5;
bool start = false, stop = false;
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value(-0.5+(j*num_inter_points+k)/(2.*num_inter_points));
if (value > 0. && func.get_derivative(-0.5+(j*num_inter_points+k)/(2.*num_inter_points)) > 0.)
{
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, value);
if (!start && value > tolerance)
{
stop=true;
break;
}
start=true;
}
else
{
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, 0.);
x_edge = -0.5+(j*num_inter_points+k)/(2.*num_inter_points);
}
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
if (stop)
break;
}
if (!stop)
{
if (i+3<size)
slope[i+3] = func.get_derivative(1.)/2.;
i+=3; // IGO
}
else // Use square root interpolation
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,iline*num_inter_points,(i-1+j)*num_inter_points+k, value);
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+1<size)
slope[i+1] = func.get_derivative(1.)/2.;
i+=2;
}
}
}
}
}
// ================================================================================================
// ================================================================================================
// From contact to non-contact
// ================================================================================================
// ================================================================================================
else if (original_line[i] == 0.)
{
convexity1[0] = getRelativeElement(original_line,i-1,0);
convexity1[1] = getRelativeElement(original_line,i-1,-1);
convexity1[2] = getRelativeElement(original_line,i-1,-2);
convexity[0] = getRelativeElement(original_line,i-1,-1);
convexity[1] = getRelativeElement(original_line,i-1,-2);
convexity[2] = getRelativeElement(original_line,i-1,-3);
bool concave_1 = if_concave(convexity1),
concave_2 = if_concave(convexity);
if (convexity1[0]*convexity1[1]*convexity1[2]*convexity[2] == 0.)
{
if (convexity1[1] == 0.)
{
Real a = num_inter_points/2.,
p0 = convexity1[0]/a;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
Real x = (i-1+j)*num_inter_points+k;
if (a*a > (x-i*num_inter_points)*(x-i*num_inter_points))
{
value = p0*sqrt(a*a - (x-i*num_inter_points)*(x-i*num_inter_points));
if (i-j-2 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-1-j)*num_inter_points-k, value);
}
else
{
if (i-j-2 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-1-j)*num_inter_points-k, 0.);
}
}
if (i-j-2+shift>=0 && i-j-2+shift<size)
deja_interpolated[i-j-2+shift] = 1;
}
i++;
continue;
}
}
else
{
if (convexity1[2] < convexity1[1] && convexity1[1] > convexity1[0])
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-j)*num_inter_points-k, value);
}
if (i-j-1+shift>=0 && i-j-1+shift<size)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-2>=0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line, i, -k) > getRelativeElement(original_line, i, -k-1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
if (i-1 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i)*num_inter_points-k, 0.);
if (i-1+shift < size)
deja_interpolated[i-1+shift] = 1;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-2 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-1-j)*num_inter_points-k, value);
}
if (i-j-2+shift>=0 && i-j-2+shift<size)
deja_interpolated[i-j-2+shift] = 1;
}
if (i-2>=0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-j)*num_inter_points-k, value);
}
if (i-j-1+shift>=0 && i-j-1+shift<size)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-2>=0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
}
else
{
if (concave_1 || !parabola) //func.get_value(-0.5) > 0.)
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line, i, -k) > getRelativeElement(original_line, i, -k-1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
if (i >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i+1)*num_inter_points-k, 0.);
if (i+shift < size)
deja_interpolated[i+shift] = 1;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-2 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-1-j)*num_inter_points-k, value);
}
if (i-j-1+shift>=0 && i-j-1+shift<size)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-2>=0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-j)*num_inter_points-k, value);
}
if (i-j-1+shift>=0 && i-j-1+shift<size)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-2>=0)
slope[i-1] = -func.get_derivative(1.)/2.;
i++;
}
}
else
{
interp.resize(3);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
interp[2] = convexity1[2];
INTER_FUNC func(PARABOLA,interp);
Real x_edge = -0.5;
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value(-0.5+(j*num_inter_points+k)/(2.*num_inter_points));
if (value > 0.)
{
if (i-j-1 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-j)*num_inter_points-k, value);
}
else
{
if (i-j-1 >= 0)
assign_value(interpolated_surface,iline*num_inter_points,(i-j)*num_inter_points-k, 0.);
x_edge = -0.5+(j*num_inter_points+k)/(2.*num_inter_points);
}
}
if (i-j-1+shift>=0)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-3>=0)
slope[i-3] = -func.get_derivative(1.)/2.;
}
}
}
}
else
{
interp.resize(2);
interp[0] = original_line[i-1];
interp[1] = original_line[i];
INTER_FUNC f(LINEAR,interp);
if (i-2+shift < size && i-2+shift >=0)
deja_interpolated[i-2+shift] = 0;
for (int k = 0; k < num_inter_points; k++)
assign_value(interpolated_surface,iline*num_inter_points,(i-1)*num_inter_points+k, f.get_value(k/Real(num_inter_points)));
}
i++;
// ================================================================================================
// ================================================================================================
// ================================================================================================
// ================================================================================================
}
}
while (i < size);
// Cubic interpolation, see "http://en.wikipedia.org/wiki/Cubic_interpolation", Catmull–Rom spline
for (int ip = 1; ip < int(original_line.size()); ip++)
{
if (!deja_interpolated[ip-1])
{
Real m0, m1, t,
p0 = original_line[ip],
p1 = getRelativeElement(original_line,ip,1);
if (p0 > 1e-14 || p1 > 1e-14)
{
// Left slope
if (fabs(slope[ip] - DEF_SLOPE) < 0.1)
m0 = 0.5 * ( getRelativeElement(original_line,ip,1) - getRelativeElement(original_line,ip,-1) );
else
m0 = slope[ip];
// Right slope
int ip_1;
if (ip + 1 < int(original_line.size()))
ip_1 = ip + 1;
else
ip_1 = 0;
if (fabs(slope[ip_1] - DEF_SLOPE) < 0.1)
m1 = 0.5 * ( getRelativeElement(original_line,ip,2) - original_line[ip] );
else
m1 = slope[ip_1];
// Interpolation
for (int j = 0; j < num_inter_points; j++)
{
t = j/Real(num_inter_points);
Real t3 = t*t*t, t2 = t*t;
assign_value(interpolated_surface,iline*num_inter_points,ip*num_inter_points+j, (2*t3 - 3*t2 + 1)*p0 + (t3 - 2*t2 + t)*m0 + (-2*t3 + 3*t2)*p1 + (t3 - t2)*m1);
}
}
else
{
for (int j = 0; j < num_inter_points; j++)
assign_value(interpolated_surface,iline*num_inter_points,ip*num_inter_points+j, 0.);
}
}
}
// Plot files
if (fname != "")
{
std::ofstream file;
std::stringstream max_value_str;
max_value_str << int(100*max_value);
// interpolated surface
file.open((fname+"_profiles_"+max_value_str.str()).c_str(),ios_base::app);
file.precision(15);
// for (int ii = margin; ii < size-margin; ii++)
// file << ix+iline << " " << iy+ii << " 0. " << deja_interpolated[ii] << endl;
for (int j = num_inter_points*margin; j < num_inter_points*(size-margin); j++)
file << (ix+iline)*num_inter_points << " " << iy*num_inter_points+j << " "
<< interpolated_surface[iline*num_inter_points][j] << " " << deja_interpolated[int(j/num_inter_points)] << endl;
file.close();
}
}
/////////////////////////////////////////////////////////////////////////////////
// END OF :: INTERPOLATION ALONG PROFILES
/////////////////////////////////////////////////////////////////////////////////
// Plot files
// if (fname != "")
// {
// std::ofstream file;
// std::stringstream max_value_str;
// max_value_str << int(100*max_value);
// // interpolated surface
// file.open((fname+"_profiles_"+max_value_str.str()).c_str(),ios_base::app);
// file.precision(15);
// for (unsigned int i = margin; i < (interpolated_surface.size()/num_inter_points)-margin; i++)
// for (unsigned int j = num_inter_points*margin; j < interpolated_surface[i].size()-num_inter_points*margin; j++)
// file << ix*num_inter_points+(i-margin)*num_inter_points << " " << iy*num_inter_points+j-margin*num_inter_points << " "
// << interpolated_surface[i*num_inter_points][j] << " " << deja_interpolated_m[i][j/num_inter_points] << endl;
// file.close();
// }
/////////////////////////////////////////////////////////////////////////////////
// ORTHOGONAL INTERPOLATION
/////////////////////////////////////////////////////////////////////////////////
for (int iline = 0; iline < (patch_size+margin*2)*num_inter_points; iline++)
{
for (unsigned int i = 0; i < slope.size(); i++)
{
slope[i] = DEF_SLOPE;
deja_interpolated[i] = 0;
}
for (int jline = 0; jline < patch_size+margin*2; jline++)
original_line[jline] = interpolated_surface[jline*num_inter_points][iline];
for (int jline = 0; jline < (patch_size+margin*2)*num_inter_points; jline++)
if (jline % num_inter_points != 0)
assign_value(interpolated_surface,jline,iline,0.);
int i = 1;
do
{
// non-contact
if (original_line[i-1] < 1e-14 && original_line[i] < 1e-14)
{
for (int j = 0; j < num_inter_points; j++)
assign_value(interpolated_surface,(i-1)*num_inter_points+j,iline,0.);
deja_interpolated[i-1] = 1;
slope[i] = 0.;
i++;
}
// contact
else
{
// ================================================================================================
// ================================================================================================
// From non-contact to contact
// ================================================================================================
// ================================================================================================
if (original_line[i-1] == 0)
{
convexity1[0] = getRelativeElement(original_line,i,0);
convexity1[1] = getRelativeElement(original_line,i,1);
convexity1[2] = getRelativeElement(original_line,i,2);
convexity[0] = getRelativeElement(original_line,i,1);
convexity[1] = getRelativeElement(original_line,i,2);
convexity[2] = getRelativeElement(original_line,i,3);
bool concave_1 = if_concave(convexity1),
concave_2 = if_concave(convexity);
// Only few points are nonzero
if (convexity1[0]*convexity1[1]*convexity1[2]*convexity[2] == 0.)
{
if (convexity1[1] == 0.)
{
Real a = num_inter_points/2.,
p0 = convexity1[0]/a;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
Real x = (i-1+j)*num_inter_points+k;
if (a*a > (x-i*num_inter_points)*(x-i*num_inter_points))
{
value = p0*sqrt(a*a - (x-i*num_inter_points)*(x-i*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline,value);
}
else
{
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline,0.);
}
}
if (i+j-1 < size)
deja_interpolated[i-1+j] = 1;
}
i+=2;
continue;
}
else if (convexity1[1] != 0. && (convexity[1] == 0. || convexity[2] == 0.) )
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int k = 0; k < num_inter_points; k++)
assign_value(interpolated_surface,(i-1)*num_inter_points+k,iline,0.);
if (convexity[1] == 0.)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points)); // STRANGE THAT IT'S NOT 3 in the denominator
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline,value);
}
if (i+j-1 < size)
deja_interpolated[i-1+j] = 1;
}
if (i+3<size)
slope[i+3] = func.get_derivative(1.)/3.; // !!
i+=3;
}
else if (convexity[2] == 0.)
{
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points)); // SHOULD BE 4, NO?
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline,value);
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+3<size)
slope[i+3] = func.get_derivative(1.)/4.; // !!
i+=3;
}
}
}
// Sufficient number of points is nonzero
else
{
if (convexity1[2] < convexity1[1] && convexity1[1] > convexity1[0])
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline, value); //1
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+1<size)
slope[i+1] = func.get_derivative(1.)/2.; // !!
i+=1;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
assign_value(interpolated_surface,(i-1)*num_inter_points+k,iline,0.);
deja_interpolated[i-1] = 1;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j+1 < size)
assign_value(interpolated_surface,(i+j)*num_inter_points+k,iline, value); //2
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+1<size)
slope[i+1] = func.get_derivative(1.)/2.; // !!
i+=2;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size && i+j-1>= 0)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline, value); //3 //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
}
if (i+j-1 < size && i+j-1 >= 0)
deja_interpolated[i+j-1] = 1;
}
if (i+1<size)
slope[i+1] = func.get_derivative(1.)/2.; // !!
/*
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i+2 < size)
assign_value(interpolated_surface,(i+1)*num_inter_points+k,iline,func.get_value(k/(1.*num_inter_points)));
if (i+1<size)
deja_interpolated[i+1] = 1;
if (i+2<size)
slope[i+2] = func.get_derivative(1.); // !!
*/
i+=2;
}
}
else
{
if (concave_1 || !parabola) //func.get_value(-0.5) > 0.)
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
assign_value(interpolated_surface,(i-1)*num_inter_points+k,iline,0.);
deja_interpolated[i-1] = 1;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j+1 < size)
assign_value(interpolated_surface,(i+j)*num_inter_points+k,iline, value); //4
}
if (i+j < size)
deja_interpolated[i+j] = 1;
}
if (i+3 < size)
slope[i+3] = func.get_derivative(1.)/2.; // !!
i+=2;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline, value); //5
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+2 < size)
slope[i+2] = func.get_derivative(1.)/2.; // !!
i+=2;
}
}
else
{
interp.resize(3);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
interp[2] = convexity1[2];
INTER_FUNC func(PARABOLA,interp);
Real x_edge = -0.5;
bool start = false, stop = false;
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value(-0.5+(j*num_inter_points+k)/(2.*num_inter_points));
if (value > 0. && func.get_derivative(-0.5+(j*num_inter_points+k)/(2.*num_inter_points)) > 0.)
{
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline,value);
if (!start && value > tolerance)
{
stop=true;
break;
}
start=true;
}
else
{
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline,0.);
x_edge = -0.5+(j*num_inter_points+k)/(2.*num_inter_points);
}
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
if (stop)
break;
}
if (!stop)
{
if (i+3 < size)
slope[i+3] = func.get_derivative(1.)/2.;
i+=3; // IGO
}
else // Use square root interpolation
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
assign_value(interpolated_surface,(i-1+j)*num_inter_points+k,iline, value); //6
}
if (i-1+j < size)
deja_interpolated[i-1+j] = 1;
}
if (i+1 < size)
slope[i+1] = func.get_derivative(1.)/2.;
i+=2;
}
}
}
}
}
// ================================================================================================
// ================================================================================================
// From contact to non-contact
// ================================================================================================
// ================================================================================================
else if (original_line[i] == 0.)
{
convexity1[0] = getRelativeElement(original_line,i-1,0);
convexity1[1] = getRelativeElement(original_line,i-1,-1);
convexity1[2] = getRelativeElement(original_line,i-1,-2);
convexity[0] = getRelativeElement(original_line,i-1,-1);
convexity[1] = getRelativeElement(original_line,i-1,-2);
convexity[2] = getRelativeElement(original_line,i-1,-3);
bool concave_1 = if_concave(convexity1),
concave_2 = if_concave(convexity);
if (convexity1[0]*convexity1[1]*convexity1[2]*convexity[2] == 0.)
{
if (convexity1[1] == 0.)
{
Real a = num_inter_points/2.,
p0 = convexity1[0]/a;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
Real x = (i-1+j)*num_inter_points+k;
if (a*a > (x-i*num_inter_points)*(x-i*num_inter_points))
{
value = p0*sqrt(a*a - (x-i*num_inter_points)*(x-i*num_inter_points));
if (i-j-2 >= 0)
assign_value(interpolated_surface,(i-1-j)*num_inter_points-k,iline,value);
}
else
{
if (i-j-2 >= 0)
assign_value(interpolated_surface,(i-1-j)*num_inter_points-k,iline,0.);
}
}
if (i-j-2+shift >= 0)
deja_interpolated[i-j-2+shift] = 1;
}
i++;
continue;
}
}
else
{
if (convexity1[2] < convexity1[1] && convexity1[1] > convexity1[0])
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
assign_value(interpolated_surface,(i-j)*num_inter_points-k,iline, value); //-1
}
if (i-j-1+shift >= 0)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-2>=0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line, i, -k) > getRelativeElement(original_line, i, -k-1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
if (i-1 >= 0)
assign_value(interpolated_surface,(i)*num_inter_points-k,iline,0.);
if (i-1+shift < size)
deja_interpolated[i-1+shift] = 1;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-2 >= 0)
assign_value(interpolated_surface,(i-1-j)*num_inter_points-k,iline, value); //-2
}
if (i-j-2+shift>=0)
deja_interpolated[i-j-2+shift] = 1;
}
if (i-2>=0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
assign_value(interpolated_surface,(i-j)*num_inter_points-k,iline, value); //-3
}
if (i-j-1+shift>=0)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-2>=0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
}
else
{
if (concave_1 || !parabola) //func.get_value(-0.5) > 0.)
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line, i, -k) > getRelativeElement(original_line, i, -k-1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
if (i >= 0)
assign_value(interpolated_surface,(i+1)*num_inter_points-k,iline,0.);
if (i+shift < size)
deja_interpolated[i+shift] = 1;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-2 >= 0)
assign_value(interpolated_surface,(i-1-j)*num_inter_points-k,iline, value); //-4
}
if (i-j-1+shift >= 0)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-2 >= 0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
assign_value(interpolated_surface,(i-j)*num_inter_points-k,iline, value); //-5
}
if (i-j-1+shift >= 0)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-2 >= 0)
slope[i-2] = -func.get_derivative(1.)/2.;
i++;
}
}
else
{
interp.resize(3);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
interp[2] = convexity1[2];
INTER_FUNC func(PARABOLA,interp);
Real x_edge = -0.5;
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value(-0.5+(j*num_inter_points+k)/(2.*num_inter_points));
if (value > 0.)
{
if (i-j-1 >= 0)
assign_value(interpolated_surface,(i-j)*num_inter_points-k,iline,value);
}
else
{
if (i-j-1 >= 0)
assign_value(interpolated_surface,(i-j)*num_inter_points-k,iline,0.);
x_edge = -0.5+(j*num_inter_points+k)/(2.*num_inter_points); //IGO
}
}
if (i-j-1+shift >= 0)
deja_interpolated[i-j-1+shift] = 1;
}
if (i-3 >= 0)
slope[i-3] = -func.get_derivative(1.)/2.;
}
}
}
}
else
{
if (i-2+shift >=0)
deja_interpolated[i-2+shift] = 0;
/*
interp.resize(2);
interp[0] = original_line[i-1];
interp[1] = original_line[i];
INTER_FUNC f(LINEAR,interp);
if (i-2+shift >=0)
deja_interpolated[i-2+shift] = 0;
for (int k = 0; k < num_inter_points; k++)
assign_value(interpolated_surface,(i-1)*num_inter_points+k,iline,f.get_value(k/Real(num_inter_points)));
*/
}
i++;
// ================================================================================================
// ================================================================================================
// ================================================================================================
// ================================================================================================
}
}
while (i < size);
// Cubic interpolation, see "http://en.wikipedia.org/wiki/Cubic_interpolation", Catmull–Rom spline
for (unsigned int ip = 1; ip < original_line.size(); ip++)
{
if (!deja_interpolated[ip-1])
{
Real m0, m1, t,
p0 = original_line[ip],
p1 = getRelativeElement(original_line,ip,1);
if (p0 > 1e-14 && p1 > 1e-14)
{
// Left slope
if (fabs(slope[ip] - DEF_SLOPE) < 0.1)
m0 = 0.5 * ( getRelativeElement(original_line,ip,1) - getRelativeElement(original_line,ip,-1) );
else
m0 = slope[ip];
// Right slope
int ip_1;
if (ip + 1 < original_line.size())
ip_1 = ip + 1;
else
ip_1 = 0;
if (fabs(slope[ip_1] - DEF_SLOPE) < 0.1)
m1 = 0.5 * ( getRelativeElement(original_line,ip,2) - original_line[ip] );
else
m1 = slope[ip_1];
// Interpolation
for (int j = 0; j < num_inter_points; j++)
{
t = j/Real(num_inter_points);
Real t3 = t*t*t, t2 = t*t;
assign_value(interpolated_surface,ip*num_inter_points+j,iline,(2*t3 - 3*t2 + 1)*p0 + (t3 - 2*t2 + t)*m0 + (-2*t3 + 3*t2)*p1 + (t3 - t2)*m1);
}
}
/*
else
{
for (int j = 0; j < num_inter_points; j++)
assign_value(interpolated_surface,ip*num_inter_points+j,iline,0);
}
*/
}
else
{
// for (int j = 0; j < num_inter_points; j++)
// assign_value(interpolated_surface,ip*num_inter_points+j,iline,5);
}
}
}
/////////////////////////////////////////////////////////////////////////////////
// END OF :: ORTHOGONAL INTERPOLATION
/////////////////////////////////////////////////////////////////////////////////
// Compute statistics
for (unsigned int i = num_inter_points*margin; i < interpolated_surface.size()-num_inter_points*margin; i++)
{
for (unsigned int j = num_inter_points*margin; j < interpolated_surface[i].size()-num_inter_points*margin; j++)
{
value = interpolated_surface[i][j];
pressure+=value;
if (value > 1e-14)
area+=1.;
for (unsigned int k = 0; k < pdfs.size() && value > 1e-14; k++)
{
int num = int((1-(max_value - value)/max_value)*pdfs[k].size());
if (num >= int(pdfs[k].size()))
num = pdfs[k].size()-1;
pdfs[k][num] += 1.;
}
}
}
// Plot files
if (fname != "")
{
std::ofstream file;
std::stringstream max_value_str;
max_value_str << int(100*max_value);
// interpolated surface
file.open((fname+"_"+max_value_str.str()).c_str(),ios_base::app);
file.precision(15);
for (unsigned int i = num_inter_points*margin; i < interpolated_surface.size()-num_inter_points*margin; i++)
for (unsigned int j = num_inter_points*margin; j < interpolated_surface[i].size()-num_inter_points*margin; j++)
file << ix*num_inter_points+i-num_inter_points*margin << " " << iy*num_inter_points+j-num_inter_points*margin << " " << interpolated_surface[i][j] << " " << endl;
file.close();
}
}
/* ---------------------------------------------------------------- */
void Interpolator::make_by_profile(Surface original, int iline, bool parabola, bool vertical, Real max_value, vector<vector<Real> >& pdfs,
Real& area, Real& perimeter, Real& pressure, string fname)
{
Real value;
int size = original.n,g=0,g_=0;
vector<Real> original_line(size), interpolated_line(num_inter_points*size,0.);
vector<Real> convexity1(3), convexity(3), interp;
vector<bool> deja_interpolated(size);
vector<Real> slope(size,DEF_SLOPE);
if (!vertical) // Interpolate in rows
{
for (int j = 0; j < size; j++)
original_line[j] = original(iline,j).re;
}
else // Interpolate in columns
{
for (int j = 0; j < size; j++)
original_line[j] = original(j,iline).re;
}
int i = 1;
do
{
// non-contact
if (original_line[i-1] == 0 && original_line[i] == 0)
{
for (int j = 0; j < num_inter_points; j++)
interpolated_line[(i-1)*num_inter_points+j] = 0.;
deja_interpolated[i-1] = 1;
slope[i] = 0.;
i++;
}
// contact
else
{
// ================================================================================================
// ================================================================================================
// From non-contact to contact
// ================================================================================================
// ================================================================================================
if (original_line[i-1] == 0)
{
convexity1[0] = getRelativeElement(original_line,i,0);
convexity1[1] = getRelativeElement(original_line,i,1);
convexity1[2] = getRelativeElement(original_line,i,2);
convexity[0] = getRelativeElement(original_line,i,1);
convexity[1] = getRelativeElement(original_line,i,2);
convexity[2] = getRelativeElement(original_line,i,3);
bool concave_1 = if_concave(convexity1),
concave_2 = if_concave(convexity);
if (convexity1[0]*convexity1[1]*convexity1[2]*convexity[2] == 0.)
{
if (convexity1[1] == 0.)
{
Real a = num_inter_points/2.,
p0 = convexity1[0]/a;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
Real x = (i-1+j)*num_inter_points+k;
if (a*a > (x-i*num_inter_points)*(x-i*num_inter_points))
{
value = p0*sqrt(a*a - (x-i*num_inter_points)*(x-i*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
else
{
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = 0.;
}
}
deja_interpolated[i-1+j] = 1;
}
i+=2;
continue;
}
else if (convexity1[1] != 0. && (convexity[1] == 0. || convexity[2] == 0.) )
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int k = 0; k < num_inter_points; k++)
interpolated_line[(i-1)*num_inter_points+k] = 0.;
if (convexity[1] == 0.)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points)); // STRANGE THAT IT'S NOT 3 in the denominator
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
deja_interpolated[i-1+j] = 1;
}
slope[i+3] = func.get_derivative(1.)/3.; // !!
i+=3;
}
else if (convexity[2] == 0.)
{
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points)); // SHOULD BE 4, NO?
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
deja_interpolated[i-1+j] = 1;
}
slope[i+3] = func.get_derivative(1.)/4.; // !!
i+=3;
}
}
}
else
{
if (convexity1[2] < convexity1[1] && convexity1[1] > convexity1[0])
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
deja_interpolated[i-1+j] = 1;
}
slope[i+1] = func.get_derivative(1.)/2.; // !!
i+=1;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
interpolated_line[(i-1)*num_inter_points+k] = 0.;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j+1 < size)
interpolated_line[(i+j)*num_inter_points+k] = value;
}
deja_interpolated[i-1+j] = 1;
}
slope[i+1] = func.get_derivative(1.)/2.; // !!
i+=2;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
deja_interpolated[i-1+j] = 1;
}
slope[i+1] = func.get_derivative(1.)/2.; // !!
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i+2 < size)
interpolated_line[(i+1)*num_inter_points+k] = func.get_value(k/(1.*num_inter_points));
deja_interpolated[i+1] = 1;
slope[i+2] = func.get_derivative(1.); // !!
i+=2;
}
}
else
{
if (concave_1 || !parabola) //func.get_value(-0.5) > 0.)
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
interpolated_line[(i-1)*num_inter_points+k] = 0.;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j+1 < size)
interpolated_line[(i+j)*num_inter_points+k] = value;
}
deja_interpolated[i+j] = 1;
}
slope[i+3] = func.get_derivative(1.)/2.; // !!
i+=2;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
deja_interpolated[i-1+j] = 1;
}
slope[i+2] = func.get_derivative(1.)/2.; // !!
/*
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i+2 < size)
interpolated_line[(i+1)*num_inter_points+k] = func.get_value(k/(1.*num_inter_points));
deja_interpolated[i+1] = 1;
slope[i+2] = func.get_derivative(1.)/2.; // !!
*/
i+=2;
}
}
else
{
interp.resize(3);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
interp[2] = convexity1[2];
INTER_FUNC func(PARABOLA,interp);
Real x_edge = -0.5;
bool start = false, stop = false;
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value(-0.5+(j*num_inter_points+k)/(2.*num_inter_points));
if (value > 0. && func.get_derivative(-0.5+(j*num_inter_points+k)/(2.*num_inter_points)) > 0.)
{
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
if (!start && value > tolerance)
{
stop=true;
break;
}
start=true;
}
else
{
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = 0.;
x_edge = -0.5+(j*num_inter_points+k)/(2.*num_inter_points);
}
}
deja_interpolated[i-1+j] = 1;
if (stop)
break;
}
if (!stop)
{
slope[i+3] = func.get_derivative(1.)/2.;
i+=3; // IGO
}
else // Use square root interpolation
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
}
/* !!!!!!!!
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i+2 < size)
interpolated_line[(i+1)*num_inter_points+k] = func.get_value(k/(1.*num_inter_points));
*/
slope[i+1] = func.get_derivative(1.)/2.;
i+=2;
}
}
}
}
}
// ================================================================================================
// ================================================================================================
// From contact to non-contact
// ================================================================================================
// ================================================================================================
else if (original_line[i] == 0.)
{
convexity1[0] = getRelativeElement(original_line,i-1,0);
convexity1[1] = getRelativeElement(original_line,i-1,-1);
convexity1[2] = getRelativeElement(original_line,i-1,-2);
convexity[0] = getRelativeElement(original_line,i-1,-1);
convexity[1] = getRelativeElement(original_line,i-1,-2);
convexity[2] = getRelativeElement(original_line,i-1,-3);
bool concave_1 = if_concave(convexity1),
concave_2 = if_concave(convexity);
if (convexity1[0]*convexity1[1]*convexity1[2]*convexity[2] == 0.)
{
if (convexity1[1] == 0.)
{
Real a = num_inter_points/2.,
p0 = convexity1[0]/a;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
Real x = (i-1+j)*num_inter_points+k;
if (a*a > (x-i*num_inter_points)*(x-i*num_inter_points))
{
value = p0*sqrt(a*a - (x-i*num_inter_points)*(x-i*num_inter_points));
if (i-j-2 >= 0)
interpolated_line[(i-1-j)*num_inter_points-k] = value;
}
else
{
if (i-j-2 >= 0)
interpolated_line[(i-1-j)*num_inter_points-k] = 0.;
}
}
deja_interpolated[i-j-2] = 1;
}
i++;
continue;
}
}
else
{
if (convexity1[2] < convexity1[1] && convexity1[1] > convexity1[0])
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = value;
}
deja_interpolated[i-j-2] = 1;
}
slope[i-2] = -func.get_derivative(1.)/2.;
//i+=2;
i++;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line, i, -k) > getRelativeElement(original_line, i, -k-1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
if (i-1 >= 0)
interpolated_line[(i)*num_inter_points-k] = 0.;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-2 >= 0)
interpolated_line[(i-1-j)*num_inter_points-k] = value;
}
deja_interpolated[i-j-2] = 1;
}
slope[i-2] = -func.get_derivative(1.)/2.;
// i+=2;
i++;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = value;
}
deja_interpolated[i-j-2] = 1;
}
slope[i-2] = -func.get_derivative(1.)/2.;
/*
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i-3 >= 0)
interpolated_line[(i-2)*num_inter_points-k] = func.get_value(k/(1.*num_inter_points));
*/
// i+=2;
i++;
}
}
else
{
if (concave_1 || !parabola) //func.get_value(-0.5) > 0.)
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line, i, -k) > getRelativeElement(original_line, i, -k-1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
if (i >= 0)
interpolated_line[(i+1)*num_inter_points-k] = 0.;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-2 >= 0)
interpolated_line[(i-1-j)*num_inter_points-k] = value;
}
deja_interpolated[i-j-2] = 1;
}
slope[i-2] = -func.get_derivative(1.)/2.;
// i+=2;
i++;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = value;
}
deja_interpolated[i-j-2] = 1;
}
slope[i-2] = -func.get_derivative(1.)/2.;
/*
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i-3 >= 0)
interpolated_line[(i-2)*num_inter_points-k] = func.get_value(k/(1.*num_inter_points));
*/
// i+=2;
i++;
}
}
else
{
interp.resize(3);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
interp[2] = convexity1[2];
INTER_FUNC func(PARABOLA,interp);
Real x_edge = -0.5;
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value(-0.5+(j*num_inter_points+k)/(2.*num_inter_points));
if (value > 0.)
{
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = value;
}
else
{
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = 0.;
x_edge = -0.5+(j*num_inter_points+k)/(2.*num_inter_points); //IGO
}
}
deja_interpolated[i-j-2] = 1;
}
slope[i-3] = -func.get_derivative(1.)/2.;
}
}
}
}
else
{
interp.resize(2);
interp[0] = original_line[i-1];
interp[1] = original_line[i];
INTER_FUNC f(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
{
interpolated_line[(i-1)*num_inter_points+k] = f.get_value(k/Real(num_inter_points));
}
}
i++;
// ================================================================================================
// ================================================================================================
// ================================================================================================
// ================================================================================================
}
if (g > i)
exit(1);
g_=g;
g=i;
}
while (i < size);
// Cubic interpolation, see "http://en.wikipedia.org/wiki/Cubic_interpolation", Catmull–Rom spline
for (unsigned int ip = 1; ip < original_line.size(); ip++)
{
if (!deja_interpolated[ip-1])
{
Real m0, m1, t,
p0 = original_line[ip],
p1 = getRelativeElement(original_line,ip,1);
// Left slope
if (fabs(slope[ip] - DEF_SLOPE) < 0.1)
m0 = 0.5 * ( getRelativeElement(original_line,ip,1) - getRelativeElement(original_line,ip,-1) );
else
m0 = slope[ip];
// Right slope
int ip_1;
if (ip + 1 < original_line.size())
ip_1 = ip + 1;
else
ip_1 = 0;
if (fabs(slope[ip_1] - DEF_SLOPE) < 0.1)
m1 = 0.5 * ( getRelativeElement(original_line,ip,2) - original_line[ip] );
else
m1 = slope[ip_1];
// Interpolation
for (int j = 0; j < num_inter_points; j++)
{
t = j/Real(num_inter_points);
Real t3 = t*t*t, t2 = t*t;
// cout << "p0 = " << p0 << "; p1 = " << p1 << "; m0 = " << m0 << "; m1 = " << m1 << "; --> t = " << t << "; y(t) = " << (2*t3 - 3*t2 + 1)*p0 + (t3 - 2*t2 + t)*m0 + (-2*t3 + 3*t2)*p1 + (t3 - t2)*m1 << endl;
interpolated_line[ip*num_inter_points+j] = (2*t3 - 3*t2 + 1)*p0 + (t3 - 2*t2 + t)*m0 + (-2*t3 + 3*t2)*p1 + (t3 - t2)*m1;
}
}
}
// Compute statistics
for (unsigned int i = 0; i < interpolated_line.size(); i++)
{
value = interpolated_line[i];
pressure+=value;
if (value > 1e-14)
area+=1.;
if ( (getRelativeElement(interpolated_line, i, 1) < 1e-14 && value > 1e-14) ||
(getRelativeElement(interpolated_line, i, 1) > 1e-14 && value < 1e-14) )
perimeter+=1.;
for (unsigned int j = 0; j < pdfs.size() && value > 1e-14; j++)
{
int num = int((1-(max_value - value)/max_value)*pdfs[j].size());
if (num >= int(pdfs[j].size()))
num = pdfs[j].size()-1;
pdfs[j][num] += 1.;
}
}
// Plot files
if (fname != "")
{
std::ofstream file;
std::stringstream max_value_str;
max_value_str << max_value;
// original line
file.open((fname+"_"+max_value_str.str()).c_str(),ios_base::app);
file.precision(15);
for (unsigned int j = 0; j < original_line.size(); j++)
file << iline << " " << j << " " << original_line[j] << " " << deja_interpolated[j] << " " << slope[j] << endl;
file << std::endl;
if (iline == original.n-1)
file << std::endl;
file.close();
// interpolated line
file.open((fname+"_inter_"+max_value_str.str()).c_str(),ios_base::app);
file.precision(15);
for (unsigned int j = 0; j < original_line.size(); j++)
{
if (fabs(slope[j]) < 1e3)
for (int k = 0; k < num_inter_points; k++)
file << iline << " " << j*num_inter_points+k << " " << interpolated_line[j*num_inter_points+k] << " " << original_line[j]+k*slope[j]/Real(num_inter_points) << endl;
else
for (int k = 0; k < num_inter_points; k++)
file << iline << " " << j*num_inter_points+k << " " << interpolated_line[j*num_inter_points+k] << " " << original_line[j] << endl;
}
file << std::endl;
if (iline == original.n-1)
file << std::endl;
file.close();
}
}
/* ---------------------------------------------------------------- */
vector< vector<Real> > Interpolator::make_by_profile(Surface original, bool parabola, bool vertical)
{
Real value;
int size = original.n;
vector< vector<Real> > result(size);
vector<Real> original_line(size), interpolated_line(num_inter_points*size,0.);
vector<Real> convexity1(3), convexity(3), interp;
for (int iline = 0; iline < size; iline++)
{
cout << "\xd Interpolator :: " << int((iline+1)*100./size) << "\% completed";
if (!vertical) // Interpolate in rows
{
for (int j = 0; j < size; j++)
original_line[j] = original(iline,j).re;
}
else // Interpolate in columns
{
for (int j = 0; j < size; j++)
original_line[j] = original(j,iline).re;
}
int i = 1;
do
{
// non-contact
if (original_line[i-1] == 0 && original_line[i] == 0)
{
for (int j = 0; j < num_inter_points; j++)
interpolated_line[(i-1)*num_inter_points+j] = 0.;
i++;
}
// contact
else
{
// ================================================================================================
// ================================================================================================
// From non-contact to contact
// ================================================================================================
// ================================================================================================
if (original_line[i-1] == 0)
{
convexity1[0] = getRelativeElement(original_line,i,0);
convexity1[1] = getRelativeElement(original_line,i,1);
convexity1[2] = getRelativeElement(original_line,i,2);
convexity[0] = getRelativeElement(original_line,i,1);
convexity[1] = getRelativeElement(original_line,i,2);
convexity[2] = getRelativeElement(original_line,i,3);
bool concave_1 = if_concave(convexity1),
concave_2 = if_concave(convexity);
if (convexity1[0]*convexity1[1]*convexity1[2]*convexity[2] == 0.)
{
if (convexity1[1] == 0.)
{
Real a = num_inter_points/2.,
p0 = convexity1[0]/a;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
Real x = (i-1+j)*num_inter_points+k;
if (a*a > (x-i*num_inter_points)*(x-i*num_inter_points))
{
value = p0*sqrt(a*a - (x-i*num_inter_points)*(x-i*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
else
{
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = 0.;
}
}
}
i+=2;
continue;
}
else if (convexity1[1] != 0. && (convexity[1] == 0. || convexity[2] == 0.) )
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int k = 0; k < num_inter_points; k++)
interpolated_line[(i-1)*num_inter_points+k] = 0.;
if (convexity[1] == 0.)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
}
i+=3;
}
else if (convexity[2] == 0.)
{
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
}
i+=3;
}
}
}
else
{
if (convexity1[2] < convexity1[1] && convexity1[1] > convexity1[0])
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
}
i+=1;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
interpolated_line[(i-1)*num_inter_points+k] = 0.;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j+1 < size)
interpolated_line[(i+j)*num_inter_points+k] = value;
}
}
i+=2;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
}
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i+2 < size)
interpolated_line[(i+1)*num_inter_points+k] = func.get_value(k/(1.*num_inter_points));
i+=2;
}
}
else
{
if (concave_1 || !parabola) //func.get_value(-0.5) > 0.)
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
interpolated_line[(i-1)*num_inter_points+k] = 0.;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j+1 < size)
interpolated_line[(i+j)*num_inter_points+k] = value;
}
}
i+=2;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
}
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i+2 < size)
interpolated_line[(i+1)*num_inter_points+k] = func.get_value(k/(1.*num_inter_points));
i+=2;
}
}
else
{
interp.resize(3);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
interp[2] = convexity1[2];
INTER_FUNC func(PARABOLA,interp);
Real x_edge = -0.5;
bool start = false, stop = false;
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value(-0.5+(j*num_inter_points+k)/(2.*num_inter_points));
if (value > 0. && func.get_derivative(-0.5+(j*num_inter_points+k)/(2.*num_inter_points)) > 0.)
{
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
if (!start && value > tolerance)
{
stop=true;
break;
}
start=true;
}
else
{
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = 0.;
x_edge = -0.5+(j*num_inter_points+k)/(2.*num_inter_points);
}
}
if (stop)
break;
}
if (!stop)
{
i+=3; // IGO
}
else // Use square root interpolation
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line,i,k) > getRelativeElement(original_line,i,k+1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i+j < size)
interpolated_line[(i-1+j)*num_inter_points+k] = value;
}
}
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i+2 < size)
interpolated_line[(i+1)*num_inter_points+k] = func.get_value(k/(1.*num_inter_points));
i+=2;
}
}
}
}
}
// ================================================================================================
// ================================================================================================
// From contact to non-contact
// ================================================================================================
// ================================================================================================
else if (original_line[i] == 0.)
{
convexity1[0] = getRelativeElement(original_line,i-1,0);
convexity1[1] = getRelativeElement(original_line,i-1,-1);
convexity1[2] = getRelativeElement(original_line,i-1,-2);
convexity[0] = getRelativeElement(original_line,i-1,-1);
convexity[1] = getRelativeElement(original_line,i-1,-2);
convexity[2] = getRelativeElement(original_line,i-1,-3);
bool concave_1 = if_concave(convexity1),
concave_2 = if_concave(convexity);
if (convexity1[0]*convexity1[1]*convexity1[2]*convexity[2] == 0.)
{
if (convexity1[1] == 0.)
{
Real a = num_inter_points/2.,
p0 = convexity1[0]/a;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
Real x = (i-1+j)*num_inter_points+k;
if (a*a > (x-i*num_inter_points)*(x-i*num_inter_points))
{
value = p0*sqrt(a*a - (x-i*num_inter_points)*(x-i*num_inter_points));
if (i-j-2 >= 0)
interpolated_line[(i-1-j)*num_inter_points-k] = value;
}
else
{
if (i-j-2 >= 0)
interpolated_line[(i-1-j)*num_inter_points-k] = 0.;
}
}
}
// i+=2;
i++;
continue;
}
}
else
{
if (convexity1[2] < convexity1[1] && convexity1[1] > convexity1[0])
{
interp.resize(2);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,0.5);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = value;
}
}
//i+=2;
i++;
}
else if (concave_2) // It's concave for sure, so we use square root interpolation.
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line, i, -k) > getRelativeElement(original_line, i, -k-1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
if (i-1 >= 0)
interpolated_line[(i)*num_inter_points-k] = 0.;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-2 >= 0)
interpolated_line[(i-1-j)*num_inter_points-k] = value;
}
}
// i+=2;
i++;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = value;
}
}
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i-3 >= 0)
interpolated_line[(i-2)*num_inter_points-k] = func.get_value(k/(1.*num_inter_points));
// i+=2;
i++;
}
}
else
{
if (concave_1 || !parabola) //func.get_value(-0.5) > 0.)
{
interp.resize(2);
interp[0] = convexity[0];
interp[1] = convexity[1];
Real a = 0; // contact radius
for (int k = 0; k < size; k++)
if ( getRelativeElement(original_line, i, -k) > getRelativeElement(original_line, i, -k-1) )
{
a = Real(k+1);
break;
}
INTER_FUNC func(SQ_ROOT,interp,a);
if (!(func.get_value(0.) > 0 || interp[1] < interp[0]))
{
for (int k = 0; k < num_inter_points; k++)
if (i >= 0)
interpolated_line[(i+1)*num_inter_points-k] = 0.;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-2 >= 0)
interpolated_line[(i-1-j)*num_inter_points-k] = value;
}
}
// i+=2;
i++;
}
else
{
interp[0] = convexity1[0];
interp[1] = convexity1[1];
INTER_FUNC func(SQ_ROOT,interp,a);
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value((j*num_inter_points+k)/(2.*num_inter_points));
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = value;
}
}
interp[0] = convexity[0];
interp[1] = convexity[1];
func = INTER_FUNC(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
if (i-3 >= 0)
interpolated_line[(i-2)*num_inter_points-k] = func.get_value(k/(1.*num_inter_points));
// i+=2;
i++;
}
}
else
{
interp.resize(3);
interp[0] = convexity1[0];
interp[1] = convexity1[1];
interp[2] = convexity1[2];
INTER_FUNC func(PARABOLA,interp);
Real x_edge = -0.5;
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < num_inter_points; k++)
{
value = func.get_value(-0.5+(j*num_inter_points+k)/(2.*num_inter_points));
if (value > 0.)
{
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = value;
}
else
{
if (i-j-1 >= 0)
interpolated_line[(i-j)*num_inter_points-k] = 0.;
x_edge = -0.5+(j*num_inter_points+k)/(2.*num_inter_points); //IGO
}
}
}
}
}
}
}
else
{
interp.resize(2);
interp[0] = original_line[i-1];
interp[1] = original_line[i];
INTER_FUNC f(LINEAR,interp);
for (int k = 0; k < num_inter_points; k++)
{
interpolated_line[(i-1)*num_inter_points+k] = f.get_value(k/Real(num_inter_points));
}
}
i++;
// ================================================================================================
// ================================================================================================
// ================================================================================================
// ================================================================================================
}
}
while (i < size);
result[iline] = interpolated_line;
}
cout << endl;
return result;
}
/* -------------------------------------------------------------------------- */
bool Interpolator::if_concave(vector<Real>& points)
{
if (points.size() != 3)
{
std::cout << std::endl << "ERROR : needs exactly 3 points to function <if_concave>" << std::endl;
exit(1);
}
Real y0 = points[0], y1 = points[1], y2 = points[2];
if ((y2-y1) < (y1-y0))
return true;
else
return false;
}
/* -------------------------------------------------------------------------- */
bool Interpolator::if_concave(vector<Real>& points, int start)
{
vector<Real> p(3);
for (int i = 0; i < 3; i++)
p[i] = getRelativeElement(points,start,i);
if ( (p[2]-p[1]) < (p[1]-p[0]) )
return true;
else
return false;
}
/* -------------------------------------------------------------------------- */
Real Interpolator::find_dist_to_max(bool forward, vector<Real>& points, int start)
{
if (forward)
{
for (unsigned int i = 0; i < points.size(); i++)
if ( getRelativeElement(points, start, i) > getRelativeElement(points, start, i+1) )
return Real(i+1);
}
else
{
for (unsigned int i = 0; i < points.size(); i++)
if ( getRelativeElement(points, start, -i) > getRelativeElement(points, start, -i-1) )
return Real(i+1);
}
return 0;
}
__END_TAMAAS__
c4science · Help