Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F81328751
interpolator.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Thu, Sep 5, 09:20
Size
121 KB
Mime Type
text/x-c
Expires
Sat, Sep 7, 09:20 (1 d, 23 h)
Engine
blob
Format
Raw Data
Handle
20480101
Attached To
rTAMAAS tamaas
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__
Event Timeline
Log In to Comment