public void Execute(int index)
{
int i = activeParticles[index];
// the previous position/orientation is the current position/orientation at the start of the step.
previousPositions[i] = positions[i];
previousOrientations[i] = orientations[i];
if (inverseMasses[i] > 0)
{
float4 effectiveGravity = gravity;
// Adjust gravity for buoyant fluid particles:
if ((phases[i] & (int)Oni.ParticleFlags.Fluid) != 0)
{
effectiveGravity *= -buoyancies[i];
}
// apply external forces and gravity:
float4 vel = velocities[i] + (inverseMasses[i] * externalForces[i] + effectiveGravity) * deltaTime;
// project velocity to 2D plane if needed:
if (is2D)
{
vel[3] = 0;
}
velocities[i] = vel;
}
if (inverseRotationalMasses[i] > 0)
{
// apply external torques (simplification: we don't use full inertia tensor here)
float4 angularVel = angularVelocities[i] + inverseRotationalMasses[i] * externalTorques[i] * deltaTime;
// project angular velocity to 2D plane normal if needed:
if (is2D)
{
angularVel = angularVel.project(new float4(0, 0, 1, 0));
}
angularVelocities[i] = angularVel;
}
// integrate velocities:
positions[i] = BurstIntegration.IntegrateLinear(positions[i], velocities[i], deltaTime);
orientations[i] = BurstIntegration.IntegrateAngular(orientations[i], angularVelocities[i], deltaTime);
}
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 / (deltaTime * deltaTime);
float4 particlePosition = positions[particleIndex];
// 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;
float4 linearRbDelta = float4.zero;
float4 angularRbDelta = float4.zero;
if (rigidbodyIndex >= 0)
{
var rigidbody = rigidbodies[rigidbodyIndex];
linearRbDelta = rigidbodyLinearDeltas[rigidbodyIndex];
angularRbDelta = rigidbodyAngularDeltas[rigidbodyIndex];
// predict world-space position of offset point:
predictedPinOffset = BurstIntegration.IntegrateLinear(predictedPinOffset, rigidbody.GetVelocityAtPoint(worldPinOffset, linearRbDelta, angularRbDelta), deltaTime);
// predict rotation at the end of the step:
predictedRotation = BurstIntegration.IntegrateAngular(predictedRotation, rigidbody.angularVelocity + angularRbDelta, deltaTime);
// 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);
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];
counts[particleIndex]++;
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];
orientationDeltas[particleIndex] = orientDelta;
orientationCounts[particleIndex]++;
if (rigidbodyIndex >= 0)
{
rigidbodies[rigidbodyIndex].ApplyDeltaQuaternion(predictedRotation, math.mul(orientations[particleIndex], dlambdaQ).value * -rigidbodyAngularW, ref angularRbDelta, deltaTime);
}
}
if (rigidbodyIndex >= 0)
{
float4 impulse = correction / deltaTime;
rigidbodies[rigidbodyIndex].ApplyImpulse(-inertialFrame.frame.TransformVector(impulse) * 1, worldPinOffset, ref linearRbDelta, ref angularRbDelta);
rigidbodyLinearDeltas[rigidbodyIndex] = linearRbDelta;
rigidbodyAngularDeltas[rigidbodyIndex] = angularRbDelta;
}
lambdas[i] = lambda;
}
}
