public void Execute(int workItemIndex)
{
int start, end;
batchData.GetConstraintRange(workItemIndex, out start, out end);
for (int i = start; i < end; ++i)
{
int simplexStartA = simplexCounts.GetSimplexStartAndSize(contacts[i].bodyA, out int simplexSizeA);
int simplexStartB = simplexCounts.GetSimplexStartAndSize(contacts[i].bodyB, out int simplexSizeB);
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
BurstConstraintsBatchImpl.ApplyOrientationDelta(particleIndex, constraintParameters.SORFactor, ref orientations, ref orientationDeltas, ref orientationCounts);
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
BurstConstraintsBatchImpl.ApplyOrientationDelta(particleIndex, constraintParameters.SORFactor, ref orientations, ref orientationDeltas, ref orientationCounts);
}
}
}
public void Execute(int i)
{
var contact = contacts[i];
int simplexStart = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize);
// get the material from the first particle in the simplex:
int aMaterialIndex = particleMaterialIndices[simplices[simplexStart]];
bool rollingContacts = aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false;
float4 relativeVelocity = float4.zero;
float4 simplexPrevPosition = float4.zero;
quaternion simplexPrevOrientation = new quaternion(0, 0, 0, 0);
float simplexInvMass = 0;
float4 simplexInvInertia = float4.zero;
float simplexRadius = 0;
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
relativeVelocity += velocities[particleIndex] * contact.pointA[j];
simplexPrevPosition += prevPositions[particleIndex] * contact.pointA[j];
simplexPrevOrientation.value += prevOrientations[particleIndex].value * contact.pointA[j];
simplexInvMass += invMasses[particleIndex] * contact.pointA[j];
simplexInvInertia += invInertiaTensors[particleIndex] * contact.pointA[j];
simplexRadius += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
// if there's a rigidbody present, subtract its velocity from the relative velocity:
int rigidbodyIndex = shapes[contact.bodyB].rigidbodyIndex;
if (rigidbodyIndex >= 0)
{
relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
int bMaterialIndex = shapes[contact.bodyB].materialIndex;
rollingContacts |= bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false;
}
// update contact distance
contact.distance = math.dot(simplexPrevPosition - contact.pointB, contact.normal) - simplexRadius;
// calculate contact point in A's surface:
float4 contactPoint = contact.pointB + contact.normal * contact.distance;
// update contact orthonormal basis:
contact.CalculateBasis(relativeVelocity);
// calculate A's contact mass.
contact.CalculateContactMassesA(simplexInvMass, simplexInvInertia, simplexPrevPosition, simplexPrevOrientation, contactPoint, rollingContacts);
// calculate B's contact mass.
if (rigidbodyIndex >= 0)
{
contact.CalculateContactMassesB(rigidbodies[rigidbodyIndex], inertialFrame.frame);
}
contacts[i] = contact;
}
public void Execute()
{
for (int i = 0; i < contacts.Length; ++i)
{
int simplexStart = simplexCounts.GetSimplexStartAndSize(contacts[i].bodyA, out int simplexSize);
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
BurstConstraintsBatchImpl.ApplyOrientationDelta(particleIndex, constraintParameters.SORFactor, ref orientations, ref orientationDeltas, ref orientationCounts);
}
}
}
public void Execute(int i)
{
int simplexStart = simplexCounts.GetSimplexStartAndSize(i, out int simplexSize);
var bounds = new BurstAabb(float.MaxValue, float.MinValue);
for (int j = 0; j < simplexSize; ++j)
{
int p = simplices[simplexStart + j];
// Find this particle's stick distance:
int m = particleMaterialIndices[p];
float stickDistance = m >= 0 ? collisionMaterials[m].stickDistance : 0;
// Expand simplex bounds, using both the particle's original position and its velocity:
bounds.EncapsulateParticle(positions[p], positions[p] + velocities[p] * continuousCollisionDetection * dt,
math.max(radii[p].x + stickDistance, fluidRadii[p] * 0.5f) + collisionMargin);
}
simplexBounds[i] = bounds;
}
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 simplexPositionA = float4.zero, simplexPositionB = float4.zero;
float simplexRadiusA = 0, simplexRadiusB = 0;
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
simplexPositionA += positions[particleIndex] * contact.pointA[j];
simplexRadiusA += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
simplexPositionB += positions[particleIndex] * contact.pointB[j];
simplexRadiusB += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
float4 posA = simplexPositionA - contact.normal * simplexRadiusA;
float4 posB = simplexPositionB + contact.normal * simplexRadiusB;
// adhesion:
float lambda = contact.SolveAdhesion(posA, posB, material.stickDistance, material.stickiness, substepTime);
// depenetration:
lambda += contact.SolvePenetration(posA, posB, solverParameters.maxDepenetration * substepTime);
// Apply normal impulse to both particles (w/ shock propagation):
if (math.abs(lambda) > BurstMath.epsilon)
{
float shock = solverParameters.shockPropagation * math.dot(contact.normal, math.normalizesafe(gravity));
float4 delta = lambda * contact.normal;
float baryScale = BurstMath.BaryScale(contact.pointA);
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
deltas[particleIndex] += delta * invMasses[particleIndex] * contact.pointA[j] * baryScale * (1 - shock);
counts[particleIndex]++;
}
baryScale = BurstMath.BaryScale(contact.pointB);
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
deltas[particleIndex] -= delta * invMasses[particleIndex] * contact.pointB[j] * baryScale * (1 + shock);
counts[particleIndex]++;
}
}
// Apply position deltas immediately, if using sequential evaluation:
if (constraintParameters.evaluationOrder == Oni.ConstraintParameters.EvaluationOrder.Sequential)
{
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
}
}
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);
float4 simplexVelocityA = float4.zero;
float4 simplexPrevPositionA = float4.zero;
quaternion simplexPrevOrientationA = new quaternion(0, 0, 0, 0);
float simplexRadiusA = 0;
float simplexInvMassA = 0;
float4 simplexInvInertiaA = float4.zero;
float4 simplexVelocityB = float4.zero;
float4 simplexPrevPositionB = float4.zero;
quaternion simplexPrevOrientationB = new quaternion(0, 0, 0, 0);
float simplexRadiusB = 0;
float simplexInvMassB = 0;
float4 simplexInvInertiaB = float4.zero;
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
simplexVelocityA += velocities[particleIndex] * contact.pointA[j];
simplexPrevPositionA += prevPositions[particleIndex] * contact.pointA[j];
simplexPrevOrientationA.value += prevOrientations[particleIndex].value * contact.pointA[j];
simplexInvMassA += invMasses[particleIndex] * contact.pointA[j];
simplexInvInertiaA += invInertiaTensors[particleIndex] * 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];
simplexVelocityB += velocities[particleIndex] * contact.pointB[j];
simplexPrevPositionB += prevPositions[particleIndex] * contact.pointB[j];
simplexPrevOrientationB.value += prevOrientations[particleIndex].value * contact.pointB[j];
simplexInvMassB += invMasses[particleIndex] * contact.pointB[j];
simplexInvInertiaB += invInertiaTensors[particleIndex] * contact.pointB[j];
simplexRadiusB += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointB[j];
}
// update contact distance
float dAB = math.dot(simplexPrevPositionA - simplexPrevPositionB, contact.normal);
contact.distance = dAB - (simplexRadiusA + simplexRadiusB);
// calculate contact points:
float4 contactPointA = simplexPrevPositionB + contact.normal * (contact.distance + simplexRadiusB);
float4 contactPointB = simplexPrevPositionA - contact.normal * (contact.distance + simplexRadiusA);
// update contact basis:
contact.CalculateBasis(simplexVelocityA - simplexVelocityB);
// update contact masses:
int aMaterialIndex = particleMaterialIndices[simplices[simplexStartA]];
int bMaterialIndex = particleMaterialIndices[simplices[simplexStartB]];
bool rollingContacts = (aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false) |
(bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false);
contact.CalculateContactMassesA(simplexInvMassA, simplexInvInertiaA, simplexPrevPositionA, simplexPrevOrientationA, contactPointA, rollingContacts);
contact.CalculateContactMassesB(simplexInvMassB, simplexInvInertiaB, simplexPrevPositionB, simplexPrevOrientationB, contactPointB, rollingContacts);
contacts[i] = contact;
}
}
private void InteractionTest(int A, int B, ref BurstSimplex simplexShape)
{
// skip the pair if their bounds don't intersect:
if (!simplexBounds[A].IntersectsAabb(simplexBounds[B]))
{
return;
}
// get the start index and size of each simplex:
int simplexStartA = simplexCounts.GetSimplexStartAndSize(A, out int simplexSizeA);
int simplexStartB = simplexCounts.GetSimplexStartAndSize(B, out int simplexSizeB);
// immediately reject simplex pairs that share particles:
for (int a = 0; a < simplexSizeA; ++a)
{
for (int b = 0; b < simplexSizeB; ++b)
{
if (simplices[simplexStartA + a] == simplices[simplexStartB + b])
{
return;
}
}
}
// get phases for each simplex:
bool restPositionsEnabled = false;
int groupA = GetSimplexPhase(simplexStartA, simplexSizeA, out Oni.ParticleFlags flagsA, ref restPositionsEnabled);
int groupB = GetSimplexPhase(simplexStartB, simplexSizeB, out Oni.ParticleFlags flagsB, ref restPositionsEnabled);
// if all particles have the same group and none have self-collision, reject the pair.
if (groupA == groupB && (flagsA & flagsB & Oni.ParticleFlags.SelfCollide) == 0)
{
return;
}
// if all simplices are fluid, check their smoothing radii:
if ((flagsA & Oni.ParticleFlags.Fluid) != 0 && (flagsB & Oni.ParticleFlags.Fluid) != 0)
{
int particleA = simplices[simplexStartA];
int particleB = simplices[simplexStartB];
// for fluid we only consider the first particle in each simplex.
float4 predictedPositionA = positions[particleA] + velocities[particleA] * dt;
float4 predictedPositionB = positions[particleB] + velocities[particleB] * dt;
// Calculate particle center distance:
float d2 = math.lengthsq(predictedPositionA - predictedPositionB);
float fluidDistance = math.max(fluidRadii[particleA], fluidRadii[particleB]);
if (d2 <= fluidDistance * fluidDistance)
{
fluidInteractionsQueue.Enqueue(new FluidInteraction {
particleA = particleA, particleB = particleB
});
}
}
else // at least one solid particle is present:
{
// swap simplices so that B is always the one-sided one.
if ((flagsA & Oni.ParticleFlags.OneSided) != 0 && groupA < groupB)
{
ObiUtils.Swap(ref A, ref B);
ObiUtils.Swap(ref simplexStartA, ref simplexStartB);
ObiUtils.Swap(ref simplexSizeA, ref simplexSizeB);
ObiUtils.Swap(ref flagsA, ref flagsB);
ObiUtils.Swap(ref groupA, ref groupB);
}
float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSizeA);
float4 simplexPoint;
simplexShape.simplexStart = simplexStartB;
simplexShape.simplexSize = simplexSizeB;
simplexShape.positions = restPositions;
simplexShape.CacheData();
float simplexRadiusA = 0, simplexRadiusB = 0;
// skip the contact if there's self-intersection at rest:
if (groupA == groupB && restPositionsEnabled)
{
var restPoint = BurstLocalOptimization.Optimize <BurstSimplex>(ref simplexShape, restPositions, radii,
simplices, simplexStartA, simplexSizeA, ref simplexBary, out simplexPoint, 4, 0);
for (int j = 0; j < simplexSizeA; ++j)
{
simplexRadiusA += radii[simplices[simplexStartA + j]].x * simplexBary[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
simplexRadiusB += radii[simplices[simplexStartB + j]].x * restPoint.bary[j];
}
// compare distance along contact normal with radius.
if (math.dot(simplexPoint - restPoint.point, restPoint.normal) < simplexRadiusA + simplexRadiusB)
{
return;
}
}
simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSizeA);
simplexShape.positions = positions;
simplexShape.CacheData();
var surfacePoint = BurstLocalOptimization.Optimize <BurstSimplex>(ref simplexShape, positions, radii,
simplices, simplexStartA, simplexSizeA, ref simplexBary, out simplexPoint, optimizationIterations, optimizationTolerance);
simplexRadiusA = 0; simplexRadiusB = 0;
float4 velocityA = float4.zero, velocityB = float4.zero, normalB = float4.zero;
for (int j = 0; j < simplexSizeA; ++j)
{
int particleIndex = simplices[simplexStartA + j];
simplexRadiusA += radii[particleIndex].x * simplexBary[j];
velocityA += velocities[particleIndex] * simplexBary[j];
}
for (int j = 0; j < simplexSizeB; ++j)
{
int particleIndex = simplices[simplexStartB + j];
simplexRadiusB += radii[particleIndex].x * surfacePoint.bary[j];
velocityB += velocities[particleIndex] * surfacePoint.bary[j];
normalB += normals[particleIndex] * surfacePoint.bary[j];
}
float dAB = math.dot(simplexPoint - surfacePoint.point, surfacePoint.normal);
float vel = math.dot(velocityA - velocityB, surfacePoint.normal);
// check if the projected velocity along the contact normal will get us within collision distance.
if (vel * dt + dAB <= simplexRadiusA + simplexRadiusB + collisionMargin)
{
// adapt collision normal for one-sided simplices:
if ((flagsB & Oni.ParticleFlags.OneSided) != 0 && groupB < groupA)
{
BurstMath.OneSidedNormal(normalB, ref surfacePoint.normal);
}
contactsQueue.Enqueue(new BurstContact()
{
bodyA = A,
bodyB = B,
pointA = simplexBary,
pointB = surfacePoint.bary,
normal = surfacePoint.normal
});
}
}
}
public void Execute(int i)
{
int simplexStart = simplexCounts.GetSimplexStartAndSize(i, out int simplexSize);
BurstAabb simplexBoundsSS = simplexBounds[i];
// get all colliders overlapped by the cell bounds, in all grid levels:
BurstAabb simplexBoundsWS = simplexBoundsSS.Transformed(solverToWorld);
NativeList <int> candidates = new NativeList <int>(Allocator.Temp);
// max size of the particle bounds in cells:
int3 maxSize = new int3(10);
bool is2D = parameters.mode == Oni.SolverParameters.Mode.Mode2D;
for (int l = 0; l < gridLevels.Length; ++l)
{
float cellSize = NativeMultilevelGrid <int> .CellSizeOfLevel(gridLevels[l]);
int3 minCell = GridHash.Quantize(simplexBoundsWS.min.xyz, cellSize);
int3 maxCell = GridHash.Quantize(simplexBoundsWS.max.xyz, cellSize);
maxCell = minCell + math.min(maxCell - minCell, maxSize);
for (int x = minCell[0]; x <= maxCell[0]; ++x)
{
for (int y = minCell[1]; y <= maxCell[1]; ++y)
{
// for 2D mode, project each cell at z == 0 and check them too. This way we ensure 2D colliders
// (which are inserted in cells with z == 0) are accounted for in the broadphase.
if (is2D)
{
if (colliderGrid.TryGetCellIndex(new int4(x, y, 0, gridLevels[l]), out int cellIndex))
{
var colliderCell = colliderGrid.usedCells[cellIndex];
candidates.AddRange(colliderCell.ContentsPointer, colliderCell.Length);
}
}
for (int z = minCell[2]; z <= maxCell[2]; ++z)
{
if (colliderGrid.TryGetCellIndex(new int4(x, y, z, gridLevels[l]), out int cellIndex))
{
var colliderCell = colliderGrid.usedCells[cellIndex];
candidates.AddRange(colliderCell.ContentsPointer, colliderCell.Length);
}
}
}
}
}
if (candidates.Length > 0)
{
// make sure each candidate collider only shows up once in the array:
NativeArray <int> uniqueCandidates = candidates.AsArray();
uniqueCandidates.Sort();
int uniqueCount = uniqueCandidates.Unique();
// iterate over candidate colliders, generating contacts for each one
for (int k = 0; k < uniqueCount; ++k)
{
int c = uniqueCandidates[k];
BurstColliderShape shape = shapes[c];
BurstAabb colliderBoundsWS = bounds[c];
// Expand bounds by rigidbody's linear velocity:
if (shape.rigidbodyIndex >= 0)
{
colliderBoundsWS.Sweep(rigidbodies[shape.rigidbodyIndex].velocity * deltaTime);
}
// Expand bounds by collision material's stick distance:
if (shape.materialIndex >= 0)
{
colliderBoundsWS.Expand(collisionMaterials[shape.materialIndex].stickDistance);
}
// check if any simplex particle and the collider have the same phase:
bool samePhase = false;
for (int j = 0; j < simplexSize; ++j)
{
samePhase |= shape.phase == (phases[simplices[simplexStart + j]] & (int)Oni.ParticleFlags.GroupMask);
}
if (!samePhase && simplexBoundsWS.IntersectsAabb(in colliderBoundsWS, is2D))
{
// generate contacts for the collider:
BurstAffineTransform colliderToSolver = worldToSolver * transforms[c];
GenerateContacts(in shape, in colliderToSolver, c, i, simplexStart, simplexSize, simplexBoundsSS);
}
}
}
}
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 int GetParticleCount(int constraintIndex)
{
simplexCounts.GetSimplexStartAndSize(contacts[constraintIndex].bodyA, out int simplexSizeA);
simplexCounts.GetSimplexStartAndSize(contacts[constraintIndex].bodyB, out int simplexSizeB);
return(simplexSizeA + simplexSizeB);
}
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;
}
}
public void Execute()
{
for (int i = 0; i < contacts.Length; ++i)
{
var contact = contacts[i];
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);
// Get relative velocity at contact point.
// As we do not consider true ellipses for collision detection, particle contact points are never off-axis.
// So particle angular velocity does not contribute to normal impulses, and we can skip it.
float4 simplexPosition = float4.zero;
float4 simplexPrevPosition = float4.zero;
float simplexRadius = 0;
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
simplexPosition += positions[particleIndex] * contact.pointA[j];
simplexPrevPosition += prevPositions[particleIndex] * contact.pointA[j];
simplexRadius += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
}
// project position to the end of the full step:
float4 posA = math.lerp(simplexPrevPosition, simplexPosition, substeps);
posA += -contact.normal * simplexRadius;
float4 posB = contact.pointB;
if (rigidbodyIndex >= 0)
{
posB += BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame) * stepTime;
}
// adhesion:
float lambda = contact.SolveAdhesion(posA, posB, material.stickDistance, material.stickiness, stepTime);
// depenetration:
lambda += contact.SolvePenetration(posA, posB, solverParameters.maxDepenetration * stepTime);
// Apply normal impulse to both simplex and rigidbody:
if (math.abs(lambda) > BurstMath.epsilon)
{
float4 delta = lambda * contact.normal * BurstMath.BaryScale(contact.pointA) / substeps;
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
deltas[particleIndex] += delta * invMasses[particleIndex] * contact.pointA[j];
counts[particleIndex]++;
}
// Apply position deltas immediately, if using sequential evaluation:
if (constraintParameters.evaluationOrder == Oni.ConstraintParameters.EvaluationOrder.Sequential)
{
for (int j = 0; j < simplexSize; ++j)
{
int particleIndex = simplices[simplexStart + j];
BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
}
}
if (rigidbodyIndex >= 0)
{
BurstMath.ApplyImpulse(rigidbodyIndex, -lambda / stepTime * contact.normal, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
}
}
contacts[i] = contact;
}
}