public static void ProjectFluidConstraints(Vector4[] particles, float[] densities, float[] lambdas, int particlesCount, int[] particlesNeighbours, int[] particlesNeighboursCount, int maxNeighboursPerParticle, float restDensity, PBFKernels kernels, Vector4[] deltaPositions)
{
for (int idA = 0; idA < particlesCount; idA++)
{
Vector3 predPosA = particles[idA];
float lambda = lambdas[idA];
Vector3 deltaP = new Vector3();
for (int nId = 0; nId < particlesNeighboursCount[idA]; nId++)
{
int idB = particlesNeighbours[idA * maxNeighboursPerParticle + nId];
Vector3 predPosB = particles[idB];
float lambdaSum = lambda + lambdas[idB];
float sCorr = kernels.sCorrCalc(predPosA, predPosB);
deltaP += kernels.WSpiky(predPosA, predPosB) * (lambdaSum + sCorr);
}
deltaPositions[idA] = deltaP / restDensity;
}
}
public static void CalculateLambdas(Vector4[] positions, float[] densities, float[] lambdas, int particlesCount, int[] particlesNeighbours, int[] particlesNeighboursCount, int maxNeighboursPerParticle, float restDensity, float epsilonLambda, PBFKernels kernels)
{
for (int idA = 0; idA < particlesCount; idA++)
{
Vector3 predPosA = positions[idA];
float density = 0.0f;
Vector3 gradientI = new Vector3();
float sumGradients = 0.0f;
for (int nId = 0; nId < particlesNeighboursCount[idA]; nId++)
{
int idB = particlesNeighbours[idA * maxNeighboursPerParticle + nId];
Vector3 predPosB = positions[idB];
//Calculate the lambda value for pressure correction
density += kernels.WPoly6(predPosA, predPosB);
//Calculate gradient with respect to j
Vector3 gradientJ = kernels.WSpiky(predPosA, predPosB) / restDensity;
//float3 gradientJ = gradWPoly6(predPosA, predPosB) / REST_DENSITY;
//Add magnitude squared to sum
//sumGradients += glm::length2(gradientJ);
sumGradients += Vector3.Dot(gradientJ, gradientJ);
gradientI += gradientJ;
}
//Add the particle i gradient magnitude squared to sum
sumGradients += Vector3.Dot(gradientI, gradientI);
float densityConstraint = (density / restDensity) - 1.0f;
densities[idA] = density;
lambdas[idA] = (-1.0f) * densityConstraint / (sumGradients + epsilonLambda);
}
}