// http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle
Matrix3r woo::Volumetric::triangleInertia(const Vector3r& v0, const Vector3r& v1, const Vector3r& v2){
Matrix3r V; V<<v0.transpose(),v1.transpose(),v2.transpose(); // rows!
Real a=((v1-v0).cross(v2-v0)).norm(); // twice the triangle area
Matrix3r S; S<<2,1,1, 1,2,1, 1,1,2; S*=(1/24.);
Matrix3r C=a*V.transpose()*S*V;
return Matrix3r::Identity()*C.trace()-C;
};
// grid sampling
Matrix3r woo::Volumetric::tetraInertia_grid(const Vector3r v[4], int div){
AlignedBox3r b; for(int i:{0,1,2,3}) b.extend(v[i]);
std::cerr<<"bbox "<<b.min()<<", "<<b.max()<<std::endl;
Real dd=b.sizes().minCoeff()/div;
Vector3r xyz;
// point inside test: http://steve.hollasch.net/cgindex/geometry/ptintet.html
typedef Eigen::Matrix<Real,4,4> Matrix4r;
Matrix4r M0; M0<<v[0].transpose(),1,v[1].transpose(),1,v[2].transpose(),1,v[3].transpose(),1;
Real D0=M0.determinant();
// Matrix3r I(Matrix3r::Zero());
Matrix3r C(Matrix3r::Zero());
Real dV=pow(dd,3);
// std::ofstream dbg("/tmp/tetra.txt");
for(xyz.x()=b.min().x()+dd/2.; xyz.x()<b.max().x(); xyz.x()+=dd){
for(xyz.y()=b.min().y()+dd/2.; xyz.y()<b.max().y(); xyz.y()+=dd){
for(xyz.z()=b.min().z()+dd/2.; xyz.z()<b.max().z(); xyz.z()+=dd){
bool inside=true;
for(int i:{0,1,2,3}){
Matrix4r D=M0;
D.row(i).head<3>()=xyz;
if(std::signbit(D.determinant())!=std::signbit(D0)){ inside=false; break; }
}
if(inside){
C+=dV*(xyz*xyz.transpose());
// dbg<<xyz[0]<<" "<<xyz[1]<<" "<<xyz[2]<<" "<<dd/2.<<endl;
}
}
}
}
return Matrix3r::Identity()*C.trace()-C;
}
/*! @brief Recalculates inertia tensor of a body after translation away from (default) or towards its centroid.
*
* @param I inertia tensor in the original coordinates; it is assumed to be upper-triangular (elements below the diagonal are ignored).
* @param m mass of the body; if positive, translation is away from the centroid; if negative, towards centroid.
* @param off offset of the new origin from the original origin
* @return inertia tensor in the new coordinate system; the matrix is symmetric.
*/
Matrix3r woo::Volumetric::inertiaTensorTranslate(const Matrix3r& I, const Real m, const Vector3r& off){
// short eigen implementation; check it gives the same result as above
return I+m*(off.dot(off)*Matrix3r::Identity()-off*off.transpose());
}
Matrix3r inertiaTranslate(const Matrix3r& I, const Real m, const Vector3r& off){
return I+m*(off.dot(off)*Matrix3r::Identity()-off*off.transpose());
}
void SphereClumpGeom::recompute(int _div, bool failOk, bool fastOnly){
if((centers.empty() && radii.empty()) || centers.size()!=radii.size()){
if(failOk) { makeInvalid(); return;}
throw std::runtime_error("SphereClumpGeom.recompute: centers and radii must have the same length (len(centers)="+to_string(centers.size())+", len(radii)="+to_string(radii.size())+"), and may not be empty.");
}
div=_div;
// one single sphere: simple
if(centers.size()==1){
pos=centers[0];
ori=Quaternionr::Identity();
volume=(4/3.)*M_PI*pow(radii[0],3);
inertia=Vector3r::Constant((2/5.)*volume*pow(radii[0],2));
equivRad=radii[0];
return;
}
volume=0;
Vector3r Sg=Vector3r::Zero();
Matrix3r Ig=Matrix3r::Zero();
if(_div<=0){
// non-intersecting: Steiner's theorem
for(size_t i=0; i<centers.size(); i++){
const Real& r(radii[i]); const Vector3r& x(centers[i]);
Real v=(4/3.)*M_PI*pow(r,3);
volume+=v;
Sg+=v*x;
Ig+=woo::Volumetric::inertiaTensorTranslate(Vector3r::Constant((2/5.)*v*pow(r,2)).asDiagonal(),v,-1.*x);
}
} else {
// intersecting: grid sampling
Real rMin=Inf; AlignedBox3r aabb;
for(size_t i=0; i<centers.size(); i++){
aabb.extend(centers[i]+Vector3r::Constant(radii[i]));
aabb.extend(centers[i]-Vector3r::Constant(radii[i]));
rMin=min(rMin,radii[i]);
}
if(rMin<=0){
if(failOk){ makeInvalid(); return; }
throw std::runtime_error("SphereClumpGeom.recompute: minimum radius must be positive (not "+to_string(rMin)+")");
}
Real dx=rMin/_div; Real dv=pow(dx,3);
long nCellsApprox=(aabb.sizes()/dx).prod();
// don't compute anything, it would take too long
if(fastOnly && nCellsApprox>1e5){ makeInvalid(); return; }
if(nCellsApprox>1e8) LOG_WARN("SphereClumpGeom: space grid has "<<nCellsApprox<<" cells, computing inertia can take a long time.");
Vector3r x;
for(x.x()=aabb.min().x()+dx/2.; x.x()<aabb.max().x(); x.x()+=dx){
for(x.y()=aabb.min().y()+dx/2.; x.y()<aabb.max().y(); x.y()+=dx){
for(x.z()=aabb.min().z()+dx/2.; x.z()<aabb.max().z(); x.z()+=dx){
for(size_t i=0; i<centers.size(); i++){
if((x-centers[i]).squaredNorm()<pow(radii[i],2)){
volume+=dv;
Sg+=dv*x;
Ig+=dv*(x.dot(x)*Matrix3r::Identity()-x*x.transpose())+/*along princial axes of dv; perhaps negligible?*/Matrix3r(Vector3r::Constant(dv*pow(dx,2)/6.).asDiagonal());
break;
}
}
}
}
}
}
woo::Volumetric::computePrincipalAxes(volume,Sg,Ig,pos,ori,inertia);
equivRad=(inertia.array()/volume).sqrt().mean(); // mean of radii of gyration
}
