// ----------------------------------------------------------------------------------------------
bool PositionBasedFluids::computePBFLagrangeMultiplier(
const unsigned int particleIndex,
const unsigned int numberOfParticles,
const Vector3r x[],
const Real mass[],
const Vector3r boundaryX[],
const Real boundaryPsi[],
const Real density,
const unsigned int numNeighbors,
const unsigned int neighbors[],
const Real density0,
const bool boundaryHandling,
Real &lambda)
{
const Real eps = 1.0e-6;
// Evaluate constraint function
const Real C = std::max(density / density0 - 1.0, 0.0); // clamp to prevent particle clumping at surface
if (C != 0.0)
{
// Compute gradients dC/dx_j
Real sum_grad_C2 = 0.0;
Vector3r gradC_i(0.0, 0.0, 0.0);
for (unsigned int j = 0; j < numNeighbors; j++)
{
const unsigned int neighborIndex = neighbors[j];
if (neighborIndex < numberOfParticles) // Test if fluid particle
{
const Vector3r gradC_j = -mass[neighborIndex] / density0 * CubicKernel::gradW(x[particleIndex] - x[neighborIndex]);
sum_grad_C2 += gradC_j.squaredNorm();
gradC_i -= gradC_j;
}
else if (boundaryHandling)
{
// Boundary: Akinci2012
const Vector3r gradC_j = -boundaryPsi[neighborIndex - numberOfParticles] / density0 * CubicKernel::gradW(x[particleIndex] - boundaryX[neighborIndex - numberOfParticles]);
sum_grad_C2 += gradC_j.squaredNorm();
gradC_i -= gradC_j;
}
}
sum_grad_C2 += gradC_i.squaredNorm();
// Compute lambda
lambda = -C / (sum_grad_C2 + eps);
}
else
lambda = 0.0;
return true;
}
void NewtonIntegrator::updateEnergy(const shared_ptr<Body>& b, const State* state, const Vector3r& fluctVel, const Vector3r& f, const Vector3r& m){
assert(b->isStandalone() || b->isClump());
// always positive dissipation, by-component: |F_i|*|v_i|*damping*dt (|T_i|*|ω_i|*damping*dt for rotations)
if(damping!=0. && state->isDamped){
scene->energy->add(fluctVel.cwise().abs().dot(f.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,/*non-incremental*/false);
// when the aspherical integrator is used, torque is damped instead of ang acceleration; this code is only approximate
scene->energy->add(state->angVel.cwise().abs().dot(m.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,false);
}
// kinetic energy
Real Etrans=.5*state->mass*fluctVel.squaredNorm();
Real Erot;
// rotational terms
if(b->isAspherical()){
Matrix3r mI; mI<<state->inertia[0],0,0, 0,state->inertia[1],0, 0,0,state->inertia[2];
Matrix3r T(state->ori);
Erot=.5*b->state->angVel.transpose().dot((T.transpose()*mI*T)*b->state->angVel);
} else { Erot=0.5*state->angVel.dot(state->inertia.cwise()*state->angVel); }
if(!kinSplit) scene->energy->add(Etrans+Erot,"kinetic",kinEnergyIx,/*non-incremental*/true);
else{ scene->energy->add(Etrans,"kinTrans",kinEnergyTransIx,true); scene->energy->add(Erot,"kinRot",kinEnergyRotIx,true); }
}
Real periPtDistSq(const Vector3r& p1, const Vector3r& p2){
Vector3r dr;
for(int ax=0; ax<3; ax++) dr[ax]=min(cellWrapRel(p1[ax],p2[ax],p2[ax]+cellSize[ax]),cellWrapRel(p2[ax],p1[ax],p1[ax]+cellSize[ax]));
return dr.squaredNorm();
}
