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;
}
}