// ---------------------------------------------------------------------------------------------- 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(); }