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