Point2 Exact_adaptive_kernel::circumcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3)
{
Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y());
Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y());
Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y());
double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y();
double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y();
double denominator = 0.5/(2.0*signed_area(p1, p2, p3));
BOOST_ASSERT(denominator > 0.0);
double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator;
double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator;
return Point2(p1.x()+dx, p1.y()+dy);
}
Point2 Exact_adaptive_kernel::offcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3, double offconstant)
{
Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y());
Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y());
Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y());
double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y();
double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y();
double p2p3dist = p2p3.x()*p2p3.x() + p2p3.y()*p2p3.y();
double denominator = 0.5/(2.0*Exact_adaptive_kernel::signed_area(p1, p2, p3));
BOOST_ASSERT(denominator > 0.0);
double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator;
double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator;
double dxoff, dyoff;
if ((p2p1dist < p3p1dist) && (p2p1dist < p2p3dist)) {
dxoff = 0.5 * p2p1.x() - offconstant * p2p1.y();
dyoff = 0.5 * p2p1.y() + offconstant * p2p1.x();
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
dx = dxoff;
dy = dyoff;
}
} else if (p3p1dist < p2p3dist) {
dxoff = 0.5 * p3p1.x() + offconstant * p3p1.y();
dyoff = 0.5 * p3p1.y() - offconstant * p3p1.x();
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
dx = dxoff;
dy = dyoff;
}
} else {
dxoff = 0.5 * p2p3.x() - offconstant * p2p3.y();
dyoff = 0.5 * p2p3.y() + offconstant * p2p3.x();
if (dxoff * dxoff + dyoff * dyoff < (dx - p2p1.x()) * (dx - p2p1.x()) + (dy - p2p1.y()) * (dy - p2p1.y())) {
dx = p2p1.x() + dxoff;
dy = p2p1.y() + dyoff;
}
}
return Point2(p1.x()+dx, p1.y()+dy);
}
template<typename PointInT> std::vector<Eigen::Vector2f>
pcl::TextureMapping<PointInT>::mapTexture2Face (
const Eigen::Vector3f &p1,
const Eigen::Vector3f &p2,
const Eigen::Vector3f &p3)
{
std::vector<Eigen::Vector2f> tex_coordinates;
// process for each face
Eigen::Vector3f p1p2 (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]);
Eigen::Vector3f p1p3 (p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]);
Eigen::Vector3f p2p3 (p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]);
// Normalize
p1p2 = p1p2 / std::sqrt (p1p2.dot (p1p2));
p1p3 = p1p3 / std::sqrt (p1p3.dot (p1p3));
p2p3 = p2p3 / std::sqrt (p2p3.dot (p2p3));
// compute vector normal of a face
Eigen::Vector3f f_normal = p1p2.cross (p1p3);
f_normal = f_normal / std::sqrt (f_normal.dot (f_normal));
// project vector field onto the face: vector v1_projected = v1 - Dot(v1, n) * n;
Eigen::Vector3f f_vector_field = vector_field_ - vector_field_.dot (f_normal) * f_normal;
// Normalize
f_vector_field = f_vector_field / std::sqrt (f_vector_field.dot (f_vector_field));
// texture coordinates
Eigen::Vector2f tp1, tp2, tp3;
double alpha = std::acos (f_vector_field.dot (p1p2));
// distance between 3 vertices of triangles
double e1 = (p2 - p3).norm () / f_;
double e2 = (p1 - p3).norm () / f_;
double e3 = (p1 - p2).norm () / f_;
// initialize
tp1[0] = 0.0;
tp1[1] = 0.0;
tp2[0] = static_cast<float> (e3);
tp2[1] = 0.0;
// determine texture coordinate tp3;
double cos_p1 = (e2 * e2 + e3 * e3 - e1 * e1) / (2 * e2 * e3);
double sin_p1 = sqrt (1 - (cos_p1 * cos_p1));
tp3[0] = static_cast<float> (cos_p1 * e2);
tp3[1] = static_cast<float> (sin_p1 * e2);
// rotating by alpha (angle between V and pp1 & pp2)
Eigen::Vector2f r_tp2, r_tp3;
r_tp2[0] = static_cast<float> (tp2[0] * std::cos (alpha) - tp2[1] * std::sin (alpha));
r_tp2[1] = static_cast<float> (tp2[0] * std::sin (alpha) + tp2[1] * std::cos (alpha));
r_tp3[0] = static_cast<float> (tp3[0] * std::cos (alpha) - tp3[1] * std::sin (alpha));
r_tp3[1] = static_cast<float> (tp3[0] * std::sin (alpha) + tp3[1] * std::cos (alpha));
// shifting
tp1[0] = tp1[0];
tp2[0] = r_tp2[0];
tp3[0] = r_tp3[0];
tp1[1] = tp1[1];
tp2[1] = r_tp2[1];
tp3[1] = r_tp3[1];
float min_x = tp1[0];
float min_y = tp1[1];
if (min_x > tp2[0])
min_x = tp2[0];
if (min_x > tp3[0])
min_x = tp3[0];
if (min_y > tp2[1])
min_y = tp2[1];
if (min_y > tp3[1])
min_y = tp3[1];
if (min_x < 0)
{
tp1[0] = tp1[0] - min_x;
tp2[0] = tp2[0] - min_x;
tp3[0] = tp3[0] - min_x;
}
if (min_y < 0)
{
tp1[1] = tp1[1] - min_y;
tp2[1] = tp2[1] - min_y;
tp3[1] = tp3[1] - min_y;
}
tex_coordinates.push_back (tp1);
tex_coordinates.push_back (tp2);
tex_coordinates.push_back (tp3);
return (tex_coordinates);
}