public static void Contacts(int particleIndex, float4 position, quaternion orientation, float4 radii, int colliderIndex, BurstAffineTransform transform, BurstColliderShape shape, NativeQueue <BurstContact> .ParallelWriter contacts) { float4 center = shape.center * transform.scale; position = transform.InverseTransformPointUnscaled(position) - center; float radius = shape.size.x * math.cmax(transform.scale.xyz); float distanceToCenter = math.length(position); float4 normal = position / distanceToCenter; BurstContact c = new BurstContact { entityA = particleIndex, entityB = colliderIndex, point = center + normal * radius, normal = normal, }; c.point = transform.TransformPointUnscaled(c.point); c.normal = transform.TransformDirection(c.normal); c.distance = distanceToCenter - radius - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz)); contacts.Enqueue(c); }
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]; // update contact basis: contact.CalculateBasis(velocities[contact.entityA] - velocities[contact.entityB]); // update contact masses: int aMaterialIndex = particleMaterialIndices[contact.entityA]; int bMaterialIndex = particleMaterialIndices[contact.entityB]; bool rollingContacts = (aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false) | (bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false); contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts); contact.CalculateContactMassesB(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts); // update contact distance: float dAB = math.dot(prevPositions[contact.entityA] - prevPositions[contact.entityB], contact.normal); float dA = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityA], radii[contact.entityA].xyz); float dB = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityB], radii[contact.entityB].xyz); contact.distance = dAB - (dA + dB); contacts[i] = contact; } }
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(int i) { var contact = contacts[i]; int aMaterialIndex = particleMaterialIndices[contact.entityA]; bool rollingContacts = aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false; int rigidbodyIndex = shapes[contact.entityB].rigidbodyIndex; if (rigidbodyIndex >= 0) { // update contact basis: float4 relativeVelocity = velocities[contact.entityA] - BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame); contact.CalculateBasis(relativeVelocity); int bMaterialIndex = shapes[contact.entityB].materialIndex; rollingContacts |= bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false; // update contact masses: contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts); contact.CalculateContactMassesB(rigidbodies[rigidbodyIndex], false); } else { // update contact basis: contact.CalculateBasis(velocities[contact.entityA]); // update contact masses: contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts); } // update contact distance float dAB = math.dot(prevPositions[contact.entityA] - contact.point, contact.normal); float dA = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityA], radii[contact.entityA].xyz); float dB = shapes[contact.entityB].contactOffset; contact.distance = dAB - (dA + dB); contacts[i] = contact; }
private static void BIHTraverse(int particleIndex, int colliderIndex, float4 particlePosition, quaternion particleOrientation, float4 particleVelocity, float4 particleRadii, ref BurstAabb particleBounds, int nodeIndex, ref NativeArray <BIHNode> bihNodes, ref NativeArray <Edge> edges, ref NativeArray <float2> vertices, ref EdgeMeshHeader header, ref BurstAffineTransform colliderToSolver, ref BurstColliderShape shape, NativeQueue <BurstContact> .ParallelWriter contacts) { var node = bihNodes[header.firstNode + nodeIndex]; // amount by which we should inflate aabbs: float offset = shape.contactOffset + particleRadii.x; if (node.firstChild >= 0) { // visit min node: if (particleBounds.min[node.axis] - offset <= node.min) { BIHTraverse(particleIndex, colliderIndex, particlePosition, particleOrientation, particleVelocity, particleRadii, ref particleBounds, node.firstChild, ref bihNodes, ref edges, ref vertices, ref header, ref colliderToSolver, ref shape, contacts); } // visit max node: if (particleBounds.max[node.axis] + offset >= node.max) { BIHTraverse(particleIndex, colliderIndex, particlePosition, particleOrientation, particleVelocity, particleRadii, ref particleBounds, node.firstChild + 1, ref bihNodes, ref edges, ref vertices, ref header, ref colliderToSolver, ref shape, contacts); } } else { // precalculate inverse of velocity vector for ray/aabb intersections: float4 invDir = math.rcp(particleVelocity); // contacts against all triangles: for (int i = node.start; i < node.start + node.count; ++i) { Edge t = edges[header.firstEdge + i]; float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0, 0) * colliderToSolver.scale; float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0, 0) * colliderToSolver.scale; BurstAabb aabb = new BurstAabb(v1, v2, 0.01f); aabb.Expand(new float4(offset)); // only generate a contact if the particle trajectory intersects its inflated aabb: if (aabb.IntersectsRay(particlePosition, invDir, true)) { float4 point = BurstMath.NearestPointOnEdge(v1, v2, particlePosition); float4 pointToTri = particlePosition - point; float distance = math.length(pointToTri); if (distance > BurstMath.epsilon) { BurstContact c = new BurstContact() { entityA = particleIndex, entityB = colliderIndex, point = colliderToSolver.TransformPointUnscaled(point), normal = colliderToSolver.TransformDirection(pointToTri / distance), }; c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, particleOrientation, particleRadii.xyz)); contacts.Enqueue(c); } } } } }
public void InteractionTest(int A, int B) { bool samePhase = (phases[A] & (int)Oni.ParticleFlags.GroupMask) == (phases[B] & (int)Oni.ParticleFlags.GroupMask); bool noSelfCollide = (phases[A] & (int)Oni.ParticleFlags.SelfCollide) == 0 || (phases[B] & (int)Oni.ParticleFlags.SelfCollide) == 0; // only particles of a different phase or set to self-collide can interact. if (samePhase && noSelfCollide) { return; } // Predict positions at the end of the whole step: float4 predictedPositionA = positions[A] + velocities[A] * dt; float4 predictedPositionB = positions[B] + velocities[B] * dt; // Calculate particle center distance: float4 dab = predictedPositionA - predictedPositionB; float d2 = math.lengthsq(dab); // if both particles are fluid, check their smoothing radii: if ((phases[A] & (int)Oni.ParticleFlags.Fluid) != 0 && (phases[B] & (int)Oni.ParticleFlags.Fluid) != 0) { float fluidDistance = math.max(fluidRadii[A], fluidRadii[B]); if (d2 <= fluidDistance * fluidDistance) { fluidInteractionsQueue.Enqueue(new FluidInteraction { particleA = A, particleB = B }); } } else // at least one solid particle { float solidDistance = radii[A].x + radii[B].x; // if these particles are self-colliding (have same phase), see if they intersect at rest. if (samePhase && restPositions[A].w > 0.5f && restPositions[B].w > 0.5f) { // if rest positions intersect, return too. float sqr_rest_distance = math.lengthsq(restPositions[A] - restPositions[B]); if (sqr_rest_distance < solidDistance * solidDistance) { return; } } // calculate distance at which particles are able to interact: int matIndexA = particleMaterialIndices[A]; int matIndexB = particleMaterialIndices[B]; float interactionDistance = solidDistance * 1.2f + (matIndexA >= 0 ? collisionMaterials[matIndexA].stickDistance : 0) + (matIndexB >= 0 ? collisionMaterials[matIndexB].stickDistance : 0); // if the distance between their predicted positions is smaller than the interaction distance: if (math.lengthsq(dab) <= interactionDistance * interactionDistance) { // calculate contact normal and distance: float4 normal = positions[A] - positions[B]; float distance = math.length(normal); if (distance > BurstMath.epsilon) { normal /= distance; float rA = BurstMath.EllipsoidRadius(normal, orientations[A], radii[A].xyz); float rB = BurstMath.EllipsoidRadius(normal, orientations[B], radii[B].xyz); // adapt normal for one-sided particles: if ((phases[A] & (int)Oni.ParticleFlags.OneSided) != 0 && (phases[B] & (int)Oni.ParticleFlags.OneSided) != 0) { float3 adjustment = float3.zero; if (rA < rB) { adjustment = math.mul(orientations[A], new float3(0, 0, -1)); } else { adjustment = math.mul(orientations[B], new float3(0, 0, 1)); } float dot = math.dot(normal.xyz, adjustment); if (dot < 0) { normal -= 2 * dot * new float4(adjustment, 0); } } contactsQueue.Enqueue(new BurstContact { entityA = A, entityB = B, point = positions[B] + normal * rB, normal = normal, distance = distance - (rA + rB) }); } } } }
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; } }
public static void Contacts(int particleIndex, int colliderIndex, float4 position, quaternion orientation, float4 radii, ref NativeArray <float> heightMap, HeightFieldHeader header, BurstAffineTransform colliderToSolver, BurstColliderShape shape, NativeQueue <BurstContact> .ParallelWriter contacts) { float4 pos = colliderToSolver.InverseTransformPoint(position); BurstContact c = new BurstContact { entityA = particleIndex, entityB = colliderIndex, }; int resolutionU = (int)shape.center.x; int resolutionV = (int)shape.center.y; // calculate terrain cell size: float cellWidth = shape.size.x / (resolutionU - 1); float cellHeight = shape.size.z / (resolutionV - 1); // calculate particle bounds min/max cells: int2 min = new int2((int)math.floor((pos[0] - radii[0]) / cellWidth), (int)math.floor((pos[2] - radii[0]) / cellHeight)); int2 max = new int2((int)math.floor((pos[0] + radii[0]) / cellWidth), (int)math.floor((pos[2] + radii[0]) / cellHeight)); for (int su = min[0]; su <= max[0]; ++su) { if (su >= 0 && su < resolutionU - 1) { for (int sv = min[1]; sv <= max[1]; ++sv) { if (sv >= 0 && sv < resolutionV - 1) { // calculate neighbor sample indices: int csu1 = math.clamp(su + 1, 0, resolutionU - 1); int csv1 = math.clamp(sv + 1, 0, resolutionV - 1); // sample heights: float h1 = heightMap[header.firstSample + sv * resolutionU + su] * shape.size.y; float h2 = heightMap[header.firstSample + sv * resolutionU + csu1] * shape.size.y; float h3 = heightMap[header.firstSample + csv1 * resolutionU + su] * shape.size.y; float h4 = heightMap[header.firstSample + csv1 * resolutionU + csu1] * shape.size.y; float min_x = su * shape.size.x / (resolutionU - 1); float max_x = csu1 * shape.size.x / (resolutionU - 1); float min_z = sv * shape.size.z / (resolutionV - 1); float max_z = csv1 * shape.size.z / (resolutionV - 1); // contact with the first triangle: float4 pointOnTri = BurstMath.NearestPointOnTri(new float4(min_x, h3, max_z, 0), new float4(max_x, h4, max_z, 0), new float4(min_x, h1, min_z, 0), pos); float4 normal = pos - pointOnTri; float distance = math.length(normal); if (distance > BurstMath.epsilon) { c.normal = normal / distance; c.point = pointOnTri; c.normal = colliderToSolver.TransformDirection(c.normal); c.point = colliderToSolver.TransformPoint(c.point); c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz)); contacts.Enqueue(c); } // contact with the second triangle: pointOnTri = BurstMath.NearestPointOnTri(new float4(min_x, h1, min_z, 0), new float4(max_x, h4, max_z, 0), new float4(max_x, h2, min_z, 0), pos); normal = pos - pointOnTri; distance = math.length(normal); if (distance > BurstMath.epsilon) { c.normal = normal / distance; c.point = pointOnTri; c.normal = colliderToSolver.TransformDirection(c.normal); c.point = colliderToSolver.TransformPoint(c.point); c.distance = distance - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz)); contacts.Enqueue(c); } } } } } }
public static void Contacts(int particleIndex, float4 position, quaternion orientation, float4 radii, int colliderIndex, BurstAffineTransform transform, BurstColliderShape shape, NativeQueue <BurstContact> .ParallelWriter contacts) { BurstContact c = new BurstContact() { entityA = particleIndex, entityB = colliderIndex, }; float4 center = shape.center * transform.scale; position = transform.InverseTransformPointUnscaled(position) - center; int direction = (int)shape.size.z; float radius = shape.size.x * math.max(transform.scale[(direction + 1) % 3], transform.scale[(direction + 2) % 3]); float height = math.max(radius, shape.size.y * 0.5f * transform.scale[direction]); float d = position[direction]; float4 axisProj = float4.zero; float4 cap = float4.zero; axisProj[direction] = d; cap[direction] = height - radius; float4 centerToPoint; float centerToPointNorm; if (d > height - radius) { //one cap centerToPoint = position - cap; centerToPointNorm = math.length(centerToPoint); c.distance = centerToPointNorm - radius; c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL)); c.point = cap + c.normal * radius; } else if (d < -height + radius) { // other cap centerToPoint = position + cap; centerToPointNorm = math.length(centerToPoint); c.distance = centerToPointNorm - radius; c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL)); c.point = -cap + c.normal * radius; } else {//cylinder centerToPoint = position - axisProj; centerToPointNorm = math.length(centerToPoint); c.distance = centerToPointNorm - radius; c.normal = (centerToPoint / (centerToPointNorm + math.FLT_MIN_NORMAL)); c.point = axisProj + c.normal * radius; } c.point += center; c.point = transform.TransformPointUnscaled(c.point); c.normal = transform.TransformDirection(c.normal); c.distance -= shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz); contacts.Enqueue(c); }
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 static void Contacts(int particleIndex, int colliderIndex, float4 position, quaternion orientation, float4 radii, ref NativeArray <BurstDFNode> dfNodes, DistanceFieldHeader header, BurstAffineTransform colliderToSolver, BurstColliderShape shape, NativeQueue <BurstContact> .ParallelWriter contacts) { float4 pos = colliderToSolver.InverseTransformPoint(position); BurstContact c = new BurstContact { entityA = particleIndex, entityB = colliderIndex, }; float4 sample = DFTraverse(pos, 0, ref header, ref dfNodes); c.normal = new float4(math.normalize(sample.xyz), 0); c.point = pos - c.normal * sample[3]; c.normal = colliderToSolver.TransformDirection(c.normal); c.point = colliderToSolver.TransformPoint(c.point); c.distance = sample[3] * math.cmax(colliderToSolver.scale.xyz) - (shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz)); contacts.Enqueue(c); }
public static void Contacts(int particleIndex, float4 position, quaternion orientation, float4 radii, int colliderIndex, BurstAffineTransform transform, BurstColliderShape shape, NativeQueue <BurstContact> .ParallelWriter contacts) { BurstContact c = new BurstContact() { entityA = particleIndex, entityB = colliderIndex, }; float4 center = shape.center * transform.scale; float4 size = shape.size * transform.scale * 0.5f; position = transform.InverseTransformPointUnscaled(position) - center; // Get minimum distance for each axis: float4 distances = size - math.abs(position); // if we are inside the box: if (distances.x >= 0 && distances.y >= 0 && distances.z >= 0) { // find minimum distance in all three axes and the axis index: float min = float.MaxValue; int axis = 0; for (int i = 0; i < 3; ++i) { if (distances[i] < min) { min = distances[i]; axis = i; } } c.normal = float4.zero; c.point = position; c.distance = -distances[axis]; c.normal[axis] = position[axis] > 0 ? 1 : -1; c.point[axis] = size[axis] * c.normal[axis]; } else // we are outside the box: { // clamp point to be inside the box: c.point = math.clamp(position, -size, size); // find distance and direction to clamped point: float4 diff = position - c.point; c.distance = math.length(diff); c.normal = diff / (c.distance + math.FLT_MIN_NORMAL); } c.point += center; c.point = transform.TransformPointUnscaled(c.point); c.normal = transform.TransformDirection(c.normal); c.distance -= shape.contactOffset + BurstMath.EllipsoidRadius(c.normal, orientation, radii.xyz); contacts.Enqueue(c); }
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; } }