public void ApplyDeltaQuaternion(quaternion rotation, quaternion delta, ref float4 angularDelta, float dt)
{
// convert quaternion delta to angular acceleration:
quaternion newRotation = math.normalize(new quaternion(rotation.value + delta.value));
angularDelta += BurstIntegration.DifferentiateAngular(newRotation, rotation, dt);
}
public static float4 GetParticleVelocityAtPoint(float4 position, float4 prevPosition, quaternion orientation, quaternion prevOrientation, float4 point, float dt)
{
// calculate both linear and angular velocities:
float4 linearVelocity = BurstIntegration.DifferentiateLinear(position, prevPosition, dt);
float4 angularVelocity = BurstIntegration.DifferentiateAngular(orientation, prevOrientation, dt);
return(linearVelocity + new float4(math.cross(angularVelocity.xyz, (point - prevPosition).xyz), 0));
}
public static void ApplyDeltaQuaternion(int rigidbodyIndex,
quaternion rotation,
quaternion delta,
NativeArray <float4> angularDeltas,
BurstAffineTransform transform,
float dt)
{
quaternion rotationWS = math.mul(transform.rotation, rotation);
quaternion deltaWS = math.mul(transform.rotation, delta);
// convert quaternion delta to angular acceleration:
quaternion newRotation = math.normalize(new quaternion(rotationWS.value + deltaWS.value));
angularDeltas[rigidbodyIndex] += BurstIntegration.DifferentiateAngular(newRotation, rotationWS, dt);
}
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 Update(float4 position, float4 scale, quaternion rotation, float dt)
{
prevFrame = frame;
float4 prevVelocity = velocity;
float4 prevAngularVelocity = angularVelocity;
frame.translation = position;
frame.rotation = rotation;
frame.scale = scale;
velocity = BurstIntegration.DifferentiateLinear(frame.translation, prevFrame.translation, dt);
angularVelocity = BurstIntegration.DifferentiateAngular(frame.rotation, prevFrame.rotation, dt);
acceleration = BurstIntegration.DifferentiateLinear(velocity, prevVelocity, dt);
angularAcceleration = BurstIntegration.DifferentiateLinear(angularVelocity, prevAngularVelocity, dt);
}
private float4 GetRelativeVelocity(int particleIndexA, int particleIndexB, ref BurstContact contact, ref float4 angularVelocityA, ref float4 angularVelocityB, ref float4 rA, ref float4 rB, bool rollingContacts)
{
// Initialize with particle linear velocity:
float4 velA = (positions[particleIndexA] - prevPositions[particleIndexA]) / dt;
float4 velB = (positions[particleIndexB] - prevPositions[particleIndexB]) / dt;
// Consider angular velocities if rolling contacts are enabled:
if (rollingContacts)
{
angularVelocityA = BurstIntegration.DifferentiateAngular(orientations[particleIndexA], prevOrientations[particleIndexA], dt);
angularVelocityB = BurstIntegration.DifferentiateAngular(orientations[particleIndexB], prevOrientations[particleIndexB], dt);
rA = contact.ContactPointA - prevPositions[particleIndexA];
rB = contact.ContactPointB - prevPositions[particleIndexB];
velA += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
velB += new float4(math.cross(angularVelocityB.xyz, rB.xyz), 0);
}
return(velA - velB);
}
private float4 GetRelativeVelocity(int particleIndex, int rigidbodyIndex, ref BurstContact contact, ref float4 angularVelocityA, ref float4 rA, ref float4 rB, bool rollingContacts)
{
// Initialize with particle linear velocity:
float4 relativeVelocity = (positions[particleIndex] - prevPositions[particleIndex]) / dt;
// Add particle angular velocity if rolling contacts are enabled:
if (rollingContacts)
{
angularVelocityA = BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], dt);
rA = contact.ContactPointA - prevPositions[particleIndex];
relativeVelocity += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
}
// Subtract rigidbody velocity:
if (rigidbodyIndex >= 0)
{
// Note: unlike rA, that is expressed in solver space, rB is expressed in world space.
rB = inertialFrame.frame.TransformPoint(contact.ContactPointB) - rigidbodies[rigidbodyIndex].com;
relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
}
return(relativeVelocity);
}
// The code actually running on the job
public void Execute(int index)
{
int i = activeParticles[index];
// Project particles on the XY plane if we are in 2D mode:
if (is2D)
{
// restrict position to the 2D plane
float4 pos = positions[i];
pos[2] = previousPositions[i][2];
positions[i] = pos;
// restrict rotation to the axis perpendicular to the 2D plane.
quaternion swing, twist;
BurstMath.SwingTwist(orientations[i], new float3(0, 0, 1), out swing, out twist);
orientations[i] = twist;
}
if (inverseMasses[i] > 0)
{
velocities[i] = BurstIntegration.DifferentiateLinear(positions[i], previousPositions[i], deltaTime);
}
else
{
velocities[i] = float4.zero;
}
if (inverseRotationalMasses[i] > 0)
{
angularVelocities[i] = BurstIntegration.DifferentiateAngular(orientations[i], previousOrientations[i], deltaTime);
}
else
{
angularVelocities[i] = float4.zero;
}
}
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;
}
}
public static float4 GetParticleVelocityAtPoint(float4 position, float4 prevPosition, float4 point, float dt)
{
// no angular velocity, so calculate and return linear velocity only:
return(BurstIntegration.DifferentiateLinear(position, prevPosition, dt));
}
public void Execute()
{
for (int i = 0; i < contacts.Length; ++i)
{
var contact = contacts[i];
// Get the indices of the particle and collider involved in this contact:
int indexA = contact.entityA;
int indexB = contact.entityB;
// Skip contacts involving triggers:
if (shapes[indexB].flags > 0)
{
continue;
}
// Get the rigidbody index (might be < 0, in that case there's no rigidbody present)
int rigidbodyIndex = shapes[indexB].rigidbodyIndex;
// Combine collision materials:
BurstCollisionMaterial material = CombineCollisionMaterials(indexA, indexB);
// Calculate relative velocity:
float4 angularVelocityA = float4.zero, rA = float4.zero, rB = float4.zero;
float4 relativeVelocity = GetRelativeVelocity(indexA, rigidbodyIndex, ref contact, ref angularVelocityA, ref rA, ref rB, material.rollingContacts > 0);
// Determine impulse magnitude:
float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, dt);
if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
{
float4 tangentImpulse = impulses.x * contact.tangent;
float4 bitangentImpulse = impulses.y * contact.bitangent;
float4 totalImpulse = tangentImpulse + bitangentImpulse;
deltas[indexA] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt;
counts[indexA]++;
if (rigidbodyIndex >= 0)
{
var rb = rigidbodies[rigidbodyIndex];
float4 worldImpulse = -inertialFrame.frame.TransformVector(totalImpulse);
float4 worldPoint = inertialFrame.frame.TransformPoint(contact.point);
rigidbodyLinearDeltas[rigidbodyIndex] += rb.inverseMass * worldImpulse;
rigidbodyAngularDeltas[rigidbodyIndex] += math.mul(rb.inverseInertiaTensor, new float4(math.cross((worldPoint - rb.com).xyz, worldImpulse.xyz), 0));
}
// Rolling contacts:
if (material.rollingContacts > 0)
{
// Calculate angular velocity deltas due to friction impulse:
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensors[indexA], orientations[indexA]);
float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
float4 angVelDeltaB = float4.zero;
// Final angular velocities, after adding the deltas:
angularVelocityA += angVelDeltaA;
float4 angularVelocityB = float4.zero;
// Calculate weights (inverse masses):
float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
float invMassB = 0;
if (rigidbodyIndex >= 0)
{
angVelDeltaB = math.mul(-rigidbodies[rigidbodyIndex].inverseInertiaTensor, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));
angularVelocityB = rigidbodies[rigidbodyIndex].angularVelocity + angVelDeltaB;
invMassB = math.length(math.mul(rigidbodies[rigidbodyIndex].inverseInertiaTensor, math.normalizesafe(angularVelocityB)));
}
// Calculate rolling axis and angular velocity deltas:
float4 rollAxis = float4.zero;
float rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
angVelDeltaA += rollAxis * rollingImpulse * invMassA;
angVelDeltaB -= rollAxis * rollingImpulse * invMassB;
// Apply orientation delta to particle:
quaternion orientationDelta = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexA], angVelDeltaA);
quaternion qA = orientationDeltas[indexA];
qA.value += orientationDelta.value * dt;
orientationDeltas[indexA] = qA;
orientationCounts[indexA]++;
// Apply angular velocity delta to rigidbody:
if (rigidbodyIndex >= 0)
{
float4 angularDelta = rigidbodyAngularDeltas[rigidbodyIndex];
angularDelta += angVelDeltaB;
rigidbodyAngularDeltas[rigidbodyIndex] = angularDelta;
}
}
}
contacts[i] = contact;
}
}
public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
var contact = contacts[i];
int simplexStartA = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSizeA);
int simplexStartB = simplexCounts.GetSimplexStartAndSize(contact.bodyB, out int simplexSizeB);
// Combine collision materials:
BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStartA], simplices[simplexStartB]);
float4 prevPositionA = float4.zero;
float4 linearVelocityA = float4.zero;
float4 angularVelocityA = float4.zero;
float4 invInertiaTensorA = float4.zero;
quaternion orientationA = new quaternion(0, 0, 0, 0);
float simplexRadiusA = 0;
float4 prevPositionB = float4.zero;
float4 linearVelocityB = float4.zero;
float4 angularVelocityB = float4.zero;
float4 invInertiaTensorB = float4.zero;
quaternion orientationB = new quaternion(0, 0, 0, 0);
float simplexRadiusB = 0;
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
prevPositionA += prevPositions[particleIndex] * contact.pointA[j];
linearVelocityA += BurstIntegration.DifferentiateLinear(positions[particleIndex], prevPositions[particleIndex], substepTime) * contact.pointA[j];
angularVelocityA += BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], substepTime) * contact.pointA[j];
invInertiaTensorA += invInertiaTensors[particleIndex] * contact.pointA[j];
orientationA.value += orientations[particleIndex].value * contact.pointA[j];
simplexRadiusA += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
prevPositionB += prevPositions[particleIndex] * contact.pointB[j];
linearVelocityB += BurstIntegration.DifferentiateLinear(positions[particleIndex], prevPositions[particleIndex], substepTime) * contact.pointB[j];
angularVelocityB += BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], substepTime) * contact.pointB[j];
invInertiaTensorB += invInertiaTensors[particleIndex] * contact.pointB[j];
orientationB.value += orientations[particleIndex].value * contact.pointB[j];
simplexRadiusB += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointB[j];
}
float4 rA = float4.zero, rB = float4.zero;
// Consider angular velocities if rolling contacts are enabled:
if (material.rollingContacts > 0)
{
rA = -contact.normal * simplexRadiusA;
rB = contact.normal * simplexRadiusB;
linearVelocityA += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
linearVelocityB += new float4(math.cross(angularVelocityB.xyz, rB.xyz), 0);
}
// Calculate relative velocity:
float4 relativeVelocity = linearVelocityA - linearVelocityB;
// Calculate friction impulses (in the tangent and bitangent ddirections):
float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, substepTime);
// Apply friction impulses to both particles:
if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
{
float4 tangentImpulse = impulses.x * contact.tangent;
float4 bitangentImpulse = impulses.y * contact.bitangent;
float4 totalImpulse = tangentImpulse + bitangentImpulse;
float baryScale = BurstMath.BaryScale(contact.pointA);
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
deltas[particleIndex] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * substepTime * contact.pointA[j] * baryScale;
counts[particleIndex]++;
}
baryScale = BurstMath.BaryScale(contact.pointB);
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
deltas[particleIndex] -= (tangentImpulse * contact.tangentInvMassB + bitangentImpulse * contact.bitangentInvMassB) * substepTime * contact.pointB[j] * baryScale;
counts[particleIndex]++;
}
// Rolling contacts:
if (material.rollingContacts > 0)
{
// Calculate angular velocity deltas due to friction impulse:
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensorA, orientationA);
float4x4 solverInertiaB = BurstMath.TransformInertiaTensor(invInertiaTensorB, orientationB);
float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
float4 angVelDeltaB = -math.mul(solverInertiaB, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));
// Final angular velocities, after adding the deltas:
angularVelocityA += angVelDeltaA;
angularVelocityB += angVelDeltaB;
// Calculate weights (inverse masses):
float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
float invMassB = math.length(math.mul(solverInertiaB, math.normalizesafe(angularVelocityB)));
// Calculate rolling axis and angular velocity deltas:
float4 rollAxis = float4.zero;
float rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
angVelDeltaA += rollAxis * rollingImpulse * invMassA;
angVelDeltaB -= rollAxis * rollingImpulse * invMassB;
// Apply orientation deltas to particles:
quaternion orientationDeltaA = BurstIntegration.AngularVelocityToSpinQuaternion(orientationA, angVelDeltaA, substepTime);
quaternion orientationDeltaB = BurstIntegration.AngularVelocityToSpinQuaternion(orientationB, angVelDeltaB, substepTime);
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
quaternion qA = orientationDeltas[particleIndex];
qA.value += orientationDeltaA.value;
orientationDeltas[particleIndex] = qA;
orientationCounts[particleIndex]++;
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
quaternion qB = orientationDeltas[particleIndex];
qB.value += orientationDeltaB.value;
orientationDeltas[particleIndex] = qB;
orientationCounts[particleIndex]++;
}
}
}
contacts[i] = contact;
}
}
public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
var contact = contacts[i];
int indexA = contact.entityA;
int indexB = contact.entityB;
// Combine collision materials:
BurstCollisionMaterial material = CombineCollisionMaterials(contact.entityA, contact.entityB);
// Calculate relative velocity:
float4 angularVelocityA = float4.zero, angularVelocityB = float4.zero, rA = float4.zero, rB = float4.zero;
float4 relativeVelocity = GetRelativeVelocity(indexA, indexB, ref contact, ref angularVelocityA, ref angularVelocityB, ref rA, ref rB, material.rollingContacts > 0);
// Calculate friction impulses (in the tangent and bitangent ddirections):
float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, dt);
// Apply friction impulses to both particles:
if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
{
float4 tangentImpulse = impulses.x * contact.tangent;
float4 bitangentImpulse = impulses.y * contact.bitangent;
float4 totalImpulse = tangentImpulse + bitangentImpulse;
deltas[indexA] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt;
deltas[indexB] -= (tangentImpulse * contact.tangentInvMassB + bitangentImpulse * contact.bitangentInvMassB) * dt;
counts[indexA]++;
counts[indexB]++;
// Rolling contacts:
if (material.rollingContacts > 0)
{
// Calculate angular velocity deltas due to friction impulse:
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensors[indexA], orientations[indexA]);
float4x4 solverInertiaB = BurstMath.TransformInertiaTensor(invInertiaTensors[indexB], orientations[indexB]);
float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
float4 angVelDeltaB = -math.mul(solverInertiaB, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));
// Final angular velocities, after adding the deltas:
angularVelocityA += angVelDeltaA;
angularVelocityB += angVelDeltaB;
// Calculate weights (inverse masses):
float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
float invMassB = math.length(math.mul(solverInertiaB, math.normalizesafe(angularVelocityB)));
// Calculate rolling axis and angular velocity deltas:
float4 rollAxis = float4.zero;
float rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
angVelDeltaA += rollAxis * rollingImpulse * invMassA;
angVelDeltaB -= rollAxis * rollingImpulse * invMassB;
// Apply orientation deltas to particles:
quaternion orientationDeltaA = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexA], angVelDeltaA);
quaternion orientationDeltaB = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexB], angVelDeltaB);
quaternion qA = orientationDeltas[indexA];
qA.value += orientationDeltaA.value * dt;
orientationDeltas[indexA] = qA;
orientationCounts[indexA]++;
quaternion qB = orientationDeltas[indexB];
qB.value += orientationDeltaB.value * dt;
orientationDeltas[indexB] = qB;
orientationCounts[indexB]++;
}
}
contacts[i] = contact;
}
}
public void Execute()
{
for (int i = 0; i < contacts.Length; ++i)
{
var contact = contacts[i];
// Get the indices of the particle and collider involved in this contact:
int simplexStart = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize);
int colliderIndex = contact.bodyB;
// Skip contacts involving triggers:
if (shapes[colliderIndex].flags > 0)
{
continue;
}
// Get the rigidbody index (might be < 0, in that case there's no rigidbody present)
int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex;
// Combine collision materials (use material from first particle in simplex)
BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStart], colliderIndex);
// Calculate relative velocity:
float4 rA = float4.zero, rB = float4.zero;
float4 prevPositionA = float4.zero;
float4 linearVelocityA = float4.zero;
float4 angularVelocityA = float4.zero;
float4 invInertiaTensorA = float4.zero;
quaternion orientationA = new quaternion(0, 0, 0, 0);
float simplexRadiusA = 0;
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
prevPositionA += prevPositions[particleIndex] * contact.pointA[j];
linearVelocityA += BurstIntegration.DifferentiateLinear(positions[particleIndex], prevPositions[particleIndex], substepTime) * contact.pointA[j];
angularVelocityA += BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], substepTime) * contact.pointA[j];
invInertiaTensorA += invInertiaTensors[particleIndex] * contact.pointA[j];
orientationA.value += orientations[particleIndex].value * contact.pointA[j];
simplexRadiusA += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
float4 relativeVelocity = linearVelocityA;
// Add particle angular velocity if rolling contacts are enabled:
if (material.rollingContacts > 0)
{
rA = -contact.normal * simplexRadiusA;
relativeVelocity += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
}
// Subtract rigidbody velocity:
if (rigidbodyIndex >= 0)
{
// Note: unlike rA, that is expressed in solver space, rB is expressed in world space.
rB = inertialFrame.frame.TransformPoint(contact.pointB) - rigidbodies[rigidbodyIndex].com;
relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
}
// Determine impulse magnitude:
float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, stepTime);
if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
{
float4 tangentImpulse = impulses.x * contact.tangent;
float4 bitangentImpulse = impulses.y * contact.bitangent;
float4 totalImpulse = tangentImpulse + bitangentImpulse;
float baryScale = BurstMath.BaryScale(contact.pointA);
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
//(tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt;
deltas[particleIndex] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * substepTime * contact.pointA[j] * baryScale;
counts[particleIndex]++;
}
if (rigidbodyIndex >= 0)
{
BurstMath.ApplyImpulse(rigidbodyIndex, -totalImpulse, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
}
// Rolling contacts:
if (material.rollingContacts > 0)
{
// Calculate angular velocity deltas due to friction impulse:
float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensorA, orientationA);
float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
float4 angVelDeltaB = float4.zero;
// Final angular velocities, after adding the deltas:
angularVelocityA += angVelDeltaA;
float4 angularVelocityB = float4.zero;
// Calculate weights (inverse masses):
float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
float invMassB = 0;
if (rigidbodyIndex >= 0)
{
angVelDeltaB = math.mul(-rigidbodies[rigidbodyIndex].inverseInertiaTensor, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));
angularVelocityB = rigidbodies[rigidbodyIndex].angularVelocity + angVelDeltaB;
invMassB = math.length(math.mul(rigidbodies[rigidbodyIndex].inverseInertiaTensor, math.normalizesafe(angularVelocityB)));
}
// Calculate rolling axis and angular velocity deltas:
float4 rollAxis = float4.zero;
float rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
angVelDeltaA += rollAxis * rollingImpulse * invMassA;
angVelDeltaB -= rollAxis * rollingImpulse * invMassB;
// Apply orientation delta to particles:
quaternion orientationDelta = BurstIntegration.AngularVelocityToSpinQuaternion(orientationA, angVelDeltaA, substepTime);
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
quaternion qA = orientationDeltas[particleIndex];
qA.value += orientationDelta.value;
orientationDeltas[particleIndex] = qA;
orientationCounts[particleIndex]++;
}
// Apply angular velocity delta to rigidbody:
if (rigidbodyIndex >= 0)
{
float4 angularDelta = rigidbodyAngularDeltas[rigidbodyIndex];
angularDelta += angVelDeltaB;
rigidbodyAngularDeltas[rigidbodyIndex] = angularDelta;
}
}
}
contacts[i] = contact;
}
}
