public void Execute(int i) { int k = 0; float maximumMass = 10000; coms[i] = float4.zero; float4x4 Apq = float4x4.zero, Rpq = float4x4.zero; // calculate shape mass, center of mass, and moment matrix: for (int j = 0; j < numIndices[i]; ++j) { k = particleIndices[firstIndex[i] + j]; float mass = maximumMass; if (invMasses[k] > 1.0f / maximumMass) { mass = 1.0f / invMasses[k]; } coms[i] += positions[k] * mass; float4x4 particleR = orientations[k].toMatrix(); float4x4 particleRT = restOrientations[k].toMatrix(); particleR[3][3] = 0; particleRT[3][3] = 0; Rpq += math.mul(particleR, math.mul(math.rcp(invInertiaTensors[k] + new float4(BurstMath.epsilon)).asDiagonal(), math.transpose(particleRT)) ); float4 restPosition = restPositions[k]; restPosition[3] = 0; Apq += mass * BurstMath.multrnsp4(positions[k], restPosition); } if (restComs[i][3] < BurstMath.epsilon) { return; } coms[i] /= restComs[i][3]; // subtract global shape moment: float4 restCom = restComs[i]; restCom[3] = 0; Apq -= restComs[i][3] * BurstMath.multrnsp4(coms[i], restCom); // calculate optimal transform including plastic deformation: float4x4 Apq_def = Rpq + math.mul(Apq, math.transpose(deformation[i])); // extract rotation from transform matrix, using warmstarting and few iterations: constraintOrientations[i] = BurstMath.ExtractRotation(Apq_def, constraintOrientations[i], 2); // finally, obtain rotation matrix: float4x4 R = constraintOrientations[i].toMatrix(); R[3][3] = 0; // calculate particle orientations: if (explicitGroup[i] > 0) { // if the group is explicit, set the orientation for all particles: for (int j = 0; j < numIndices[i]; ++j) { k = particleIndices[firstIndex[i] + j]; orientations[k] = math.mul(constraintOrientations[i], restOrientations[k]); } } else { // set orientation of center particle only: int centerIndex = particleIndices[firstIndex[i]]; orientations[centerIndex] = math.mul(constraintOrientations[i], restOrientations[centerIndex]); } // calculate and accumulate particle goal positions: float4 goal; float4x4 transform = math.mul(R, deformation[i]); for (int j = 0; j < numIndices[i]; ++j) { k = particleIndices[firstIndex[i] + j]; goal = coms[i] + math.mul(transform, restPositions[k] - restComs[i]); deltas[k] += (goal - positions[k]) * shapeMaterialParameters[i * 5]; counts[k]++; } // update plastic deformation: float plastic_yield = shapeMaterialParameters[i * 5 + 1]; float plastic_creep = shapeMaterialParameters[i * 5 + 2]; float plastic_recovery = shapeMaterialParameters[i * 5 + 3]; float max_deform = shapeMaterialParameters[i * 5 + 4]; // if we are allowed to absorb deformation: if (plastic_creep > 0) { R[3][3] = 1; Apq_def[3][3] = 1; // get scale matrix (A = RS so S = Rt * A) and its deviation from the identity matrix: float4x4 deform_matrix = math.mul(math.transpose(R), math.mul(Apq_def, Aqq[i])) - float4x4.identity; // if the amount of deformation exceeds the yield threshold: float norm = deform_matrix.frobeniusNorm(); if (norm > plastic_yield) { // deform the shape permanently: deformation[i] = math.mul(float4x4.identity + plastic_creep * deform_matrix, deformation[i]); // clamp deformation so that it does not exceed a percentage; deform_matrix = deformation[i] - float4x4.identity; norm = deform_matrix.frobeniusNorm(); if (norm > max_deform) { deformation[i] = float4x4.identity + max_deform * deform_matrix / norm; } // if we cannot recover from plastic deformation, recalculate rest shape now: if (plastic_recovery == 0) { RecalculateRestData(i, ref particleIndices, ref firstIndex, ref restComs, ref Aqq, ref deformation, ref numIndices, ref invMasses, ref restPositions, ref restOrientations, ref invInertiaTensors); } } } // if we can recover from plastic deformation, lerp towards non-deformed shape and recalculate rest shape: if (plastic_recovery > 0) { deformation[i] += (float4x4.identity - deformation[i]) * math.min(plastic_recovery * deltaTime, 1.0f); RecalculateRestData(i, ref particleIndices, ref firstIndex, ref restComs, ref Aqq, ref deformation, ref numIndices, ref invMasses, ref restPositions, ref restOrientations, ref invInertiaTensors); } }