AlignedBox3f Bound(const Mesh& m)
{
float maxflt = std::numeric_limits<float>::max();
Eigen::Vector3f min{ maxflt, maxflt, maxflt };
Eigen::Vector3f max = -min;
for (const auto& tri : m)
{
min = min.cwiseMin(tri.rowwise().minCoeff());
max = max.cwiseMax(tri.rowwise().maxCoeff());
}
return{ min, max };
}
OBB::OBB(Mesh::const_iterator begin, Mesh::const_iterator end)
{
if (begin == end)
{
axes = -ZERO_SIZE * Matrix3f::Identity(); //make it inside out (i guess)
origin = Vector3f::Zero();
return;
}
Vector3f centerOfMass = centroid(begin, end);
Matrix3f inertiaTensor = Matrix3f::Zero();
auto addPt = [&](const Vector3f& pt, float mass)
{
Vector3f lpos = pt - centerOfMass;
inertiaTensor(0, 0) += (lpos.y()*lpos.y() + lpos.z()*lpos.z()) * mass;
inertiaTensor(1, 1) += (lpos.x()*lpos.x() + lpos.z()*lpos.z()) * mass;
inertiaTensor(2, 2) += (lpos.x()*lpos.x() + lpos.y()*lpos.y()) * mass;
inertiaTensor(1, 0) -= lpos.x()*lpos.y() * mass;
inertiaTensor(2, 0) -= lpos.x()*lpos.z() * mass;
inertiaTensor(2, 1) -= lpos.y()*lpos.z() * mass;
};
for (const auto& tri : make_range(begin, end))
{
float area = TriNormal(tri).norm() / 6.f;
addPt(tri.col(0), area);
addPt(tri.col(1), area);
addPt(tri.col(2), area);
}
Eigen::SelfAdjointEigenSolver<Matrix3f> es;
es.computeDirect(inertiaTensor);
axes = es.eigenvectors();
float maxflt = std::numeric_limits<float>::max();
Eigen::Vector3f min{ maxflt, maxflt, maxflt };
Eigen::Vector3f max = -min;
for (const auto& tri : make_range(begin, end))
{
min = min.cwiseMin((axes.transpose() * tri).rowwise().minCoeff());
max = max.cwiseMax((axes.transpose() * tri).rowwise().maxCoeff());
}
extent = (max - min).cwiseMax(ZERO_SIZE) / 2.f;
origin = axes * (min + extent);
}