MatrixX3f Surface::compute_normals(const MatrixX3f& rr, const MatrixX3i& tris)
{
printf("\tcomputing normals\n");
// first, compute triangle normals
MatrixX3f r1(tris.rows(),3); MatrixX3f r2(tris.rows(),3); MatrixX3f r3(tris.rows(),3);
for(qint32 i = 0; i < tris.rows(); ++i)
{
r1.row(i) = rr.row(tris(i, 0));
r2.row(i) = rr.row(tris(i, 1));
r3.row(i) = rr.row(tris(i, 2));
}
MatrixX3f x = r2 - r1;
MatrixX3f y = r3 - r1;
MatrixX3f tri_nn(x.rows(),y.cols());
tri_nn.col(0) = x.col(1).cwiseProduct(y.col(2)) - x.col(2).cwiseProduct(y.col(1));
tri_nn.col(1) = x.col(2).cwiseProduct(y.col(0)) - x.col(0).cwiseProduct(y.col(2));
tri_nn.col(2) = x.col(0).cwiseProduct(y.col(1)) - x.col(1).cwiseProduct(y.col(0));
// Triangle normals and areas
MatrixX3f tmp = tri_nn.cwiseProduct(tri_nn);
VectorXf normSize = tmp.rowwise().sum();
normSize = normSize.cwiseSqrt();
for(qint32 i = 0; i < normSize.size(); ++i)
if(normSize(i) != 0)
tri_nn.row(i) /= normSize(i);
MatrixX3f nn = MatrixX3f::Zero(rr.rows(), 3);
for(qint32 p = 0; p < tris.rows(); ++p)
{
Vector3i verts = tris.row(p);
for(qint32 j = 0; j < verts.size(); ++j)
nn.row(verts(j)) = tri_nn.row(p);
}
tmp = nn.cwiseProduct(nn);
normSize = tmp.rowwise().sum();
normSize = normSize.cwiseSqrt();
for(qint32 i = 0; i < normSize.size(); ++i)
if(normSize(i) != 0)
nn.row(i) /= normSize(i);
return nn;
}
Sphere Sphere::fit_sphere(const MatrixX3f& points)
{
const VectorXf& x = points.col(0);
const VectorXf& y = points.col(1);
const VectorXf& z = points.col(2);
VectorXf point_means = points.colwise().mean();
VectorXf x_mean_free = x.array() - point_means(0);
VectorXf y_mean_free = y.array() - point_means(1);
VectorXf z_mean_free = z.array() - point_means(2);
Matrix3f A;
A << (x.cwiseProduct(x_mean_free)).mean(), 2*(x.cwiseProduct(y_mean_free)).mean(), 2*(x.cwiseProduct(z_mean_free)).mean(),
0, (y.cwiseProduct(y_mean_free)).mean(), 2*(y.cwiseProduct(z_mean_free)).mean(),
0, 0, (z.cwiseProduct(z_mean_free)).mean();
Matrix3f A_T = A.transpose();
A += A_T;
Vector3f b;
VectorXf sq_sum = x.array().pow(2)+y.array().pow(2)+z.array().pow(2);
b << (sq_sum.cwiseProduct(x_mean_free)).mean(),
(sq_sum.cwiseProduct(y_mean_free)).mean(),
(sq_sum.cwiseProduct(z_mean_free)).mean();
Vector3f center = A.ldlt().solve(b);
MatrixX3f tmp(points.rows(),3);
tmp.col(0) = x.array() - center(0);
tmp.col(1) = y.array() - center(1);
tmp.col(2) = z.array() - center(2);
float r = sqrt(tmp.array().pow(2).rowwise().sum().mean());
return Sphere(center, r);
}
