MoI

#pragma once


//NOTE:  ptl::array_ref isn't included here, but this is easy to replace with whatever you use(std::vector should work fine)
// Vec3sp is a standard vec3 of 32 bit float

namespace phys {

//sampling based approximations of volume & movement of inertia
struct MomentAndVolume {
Vec3sp MomementVolume; //This does not include density yet. You must multiply by the density for the correct Moment
Vec3sp CenterOfMass;
float Volume; //scale by density to get mass
};

//Get Moment, volume, and center of mass.
//Result does not include density, you must apply that to the result yourself
//center should be the center of the field, generally [0,0,0]
//The points should be sampled at a regular interval in x/y/z,  half_step is half of this interval
inline MomentAndVolume CalcMomentAndVolume(ptl::array_ref<Vec3sp> points,const float*         distance, Vec3sp center, float half_step){

//The moment of inertia has the volume baked into here. You can compare it against listed values by dividing by the total volume at the end
Vec3sp moment(0.f);
float volume = 0.f;

size_t Count = points.size();
float cellDiam = half_step*2.f;
float cellVol = cellDiam*cellDiam*cellDiam;
float filled = 1.f/ cellDiam;
float filledCelRad = filled*half_step;
float negfilled = -filled;
Vec3sp PosWeightedByVolume(0.f);

for (size_t i = 0; i < Count; ++i) {
Vec3sp point = points[i];
float d = distance[i];

Vec3sp p = point - center;
//determine % of the cell is filled-- this is only approximate..
float filledRatio = math::saturate(d*negfilled + filledCelRad); //math::clamp(-(d - cellRad) *filled, 0.f, 1.f);
float vol = cellVol*filledRatio;
volume += vol;

PosWeightedByVolume += point*vol;

//squared distance to each axis
float dx = p.y()*p.y() + p.z()*p.z();
float dy = p.x()*p.x() + p.z()*p.z();
float dz = p.x()*p.x() + p.y()*p.y();

moment[0] += dx*vol;
moment[1] += dy*vol;
moment[2] += dz*vol;
}
Vec3sp CenterOfMass = PosWeightedByVolume * (1.f / volume);
float centerOfMassError = maxElem(absPerElem(CenterOfMass- center));

float maxCenterOfMassError = (half_step*0.2f);

//if our center was not the center of mass, we need to recalculate the moment of inertia using the correct value
if (centerOfMassError > maxCenterOfMassError) {
moment = Vec3sp(0.f);

for (size_t i = 0; i < Count; ++i) {
Vec3sp point = points[i];
float d = distance[i];

Vec3sp p = point - CenterOfMass;

float dx = p.y()*p.y() + p.z()*p.z();
float dy = p.x()*p.x() + p.z()*p.z();
float dz = p.x()*p.x() + p.y()*p.y();

float filledRatio = math::saturate(d*negfilled + filledCelRad);
float vol = cellVol*filledRatio;

moment[0] += dx*vol;
moment[1] += dy*vol;
moment[2] += dz*vol;
}
}
else {
CenterOfMass = Vec3sp(0.f);//set to zero so we can skip dealing with this minor center of mass offsets
}

return{
moment,
CenterOfMass,
volume
};
}
}