public void Execute() { for (int i = 0; i < activeConstraintCount; ++i) { int particleIndex = particleIndices[i]; int colliderIndex = colliderIndices[i]; // no collider to pin to, so ignore the constraint. if (colliderIndex < 0) { continue; } int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex; // calculate time adjusted compliances float2 compliances = stiffnesses[i].xy / (substepTime * substepTime); // project particle position to the end of the full step: float4 particlePosition = math.lerp(prevPositions[particleIndex], positions[particleIndex], substeps); // express pin offset in world space: float4 worldPinOffset = transforms[colliderIndex].TransformPoint(offsets[i]); float4 predictedPinOffset = worldPinOffset; quaternion predictedRotation = transforms[colliderIndex].rotation; float rigidbodyLinearW = 0; float rigidbodyAngularW = 0; if (rigidbodyIndex >= 0) { var rigidbody = rigidbodies[rigidbodyIndex]; // predict offset point position: float4 velocityAtPoint = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, inertialFrame.frame.InverseTransformPoint(worldPinOffset), rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame); predictedPinOffset = BurstIntegration.IntegrateLinear(predictedPinOffset, inertialFrame.frame.TransformVector(velocityAtPoint), stepTime); // predict rotation at the end of the step: predictedRotation = BurstIntegration.IntegrateAngular(predictedRotation, rigidbody.angularVelocity + rigidbodyAngularDeltas[rigidbodyIndex], stepTime); // calculate linear and angular rigidbody weights: rigidbodyLinearW = rigidbody.inverseMass; rigidbodyAngularW = BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor, worldPinOffset - rigidbody.com, math.normalizesafe(inertialFrame.frame.TransformPoint(particlePosition) - predictedPinOffset)); } // Transform pin position to solver space for constraint solving: predictedPinOffset = inertialFrame.frame.InverseTransformPoint(predictedPinOffset); predictedRotation = math.mul(math.conjugate(inertialFrame.frame.rotation), predictedRotation); float4 gradient = particlePosition - predictedPinOffset; float constraint = math.length(gradient); float4 gradientDir = gradient / (constraint + BurstMath.epsilon); float4 lambda = lambdas[i]; float linearDLambda = (-constraint - compliances.x * lambda.w) / (invMasses[particleIndex] + rigidbodyLinearW + rigidbodyAngularW + compliances.x + BurstMath.epsilon); lambda.w += linearDLambda; float4 correction = linearDLambda * gradientDir; deltas[particleIndex] += correction * invMasses[particleIndex] / substeps; counts[particleIndex]++; if (rigidbodyIndex >= 0) { BurstMath.ApplyImpulse(rigidbodyIndex, -correction / stepTime * 1, inertialFrame.frame.InverseTransformPoint(worldPinOffset), rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame); } if (rigidbodyAngularW > 0 || invRotationalMasses[particleIndex] > 0) { // bend/twist constraint: quaternion omega = math.mul(math.conjugate(orientations[particleIndex]), predictedRotation); //darboux vector quaternion omega_plus; omega_plus.value = omega.value + restDarboux[i].value; //delta Omega with - omega_0 omega.value -= restDarboux[i].value; //delta Omega with + omega_0 if (math.lengthsq(omega.value) > math.lengthsq(omega_plus.value)) { omega = omega_plus; } float3 dlambda = (omega.value.xyz - compliances.y * lambda.xyz) / new float3(compliances.y + invRotationalMasses[particleIndex] + rigidbodyAngularW + BurstMath.epsilon); lambda.xyz += dlambda; //discrete Darboux vector does not have vanishing scalar part quaternion dlambdaQ = new quaternion(dlambda[0], dlambda[1], dlambda[2], 0); quaternion orientDelta = orientationDeltas[particleIndex]; orientDelta.value += math.mul(predictedRotation, dlambdaQ).value *invRotationalMasses[particleIndex] / substeps; orientationDeltas[particleIndex] = orientDelta; orientationCounts[particleIndex]++; if (rigidbodyIndex >= 0) { BurstMath.ApplyDeltaQuaternion(rigidbodyIndex, predictedRotation, -math.mul(orientations[particleIndex], dlambdaQ).value *rigidbodyAngularW, rigidbodyAngularDeltas, inertialFrame.frame, stepTime); } } lambdas[i] = lambda; } }