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 Execute(int i)
{
var cell = particleGrid.usedCells[i];
BurstAabb cellBounds = new BurstAabb(float.MaxValue, float.MinValue);
// here we calculate cell bounds that enclose both the predicted position and the original position of all its particles,
// for accurate continuous collision detection.
for (int p = 0; p < cell.Length; ++p)
{
int pIndex = cell[p];
// get collision material stick distance:
float stickDistance = 0;
int materialIndex = particleMaterialIndices[pIndex];
if (materialIndex >= 0)
{
stickDistance = collisionMaterials[materialIndex].stickDistance;
}
cellBounds.EncapsulateParticle(positions[pIndex],
positions[pIndex] + velocities[pIndex] * deltaTime,
radii[pIndex].x + stickDistance);
}
// transform the cell bounds to world space:
cellBounds.Transform(solverToWorld);
// get all colliders overlapped by the cell bounds, in all grid levels:
NativeList <int> candidates = new NativeList <int>(Allocator.Temp);
for (int l = 0; l < gridLevels.Length; ++l)
{
GetCandidatesForBoundsAtLevel(candidates, cellBounds, gridLevels[l], is2D);
}
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 c = 0; c < uniqueCount; ++c)
{
int colliderIndex = uniqueCandidates[c];
BurstColliderShape shape = shapes[colliderIndex];
BurstAabb colliderBounds = bounds[colliderIndex];
BurstAffineTransform colliderToSolver = worldToSolver * transforms[colliderIndex];
// transform collider bounds to solver space:
colliderBounds.Transform(worldToSolver);
// iterate over all particles in the cell:
for (int p = 0; p < cell.Length; ++p)
{
int particleIndex = cell[p];
// skip this pair if particle and collider have the same phase:
if (shape.phase == (phases[particleIndex] & (int)Oni.ParticleFlags.GroupMask))
{
continue;
}
// get collision material stick distance:
float stickDistance = 0;
int materialIndex = particleMaterialIndices[particleIndex];
if (materialIndex >= 0)
{
stickDistance = collisionMaterials[materialIndex].stickDistance;
}
// inflate collider bounds by particle's bounds:
BurstAabb inflatedColliderBounds = colliderBounds;
inflatedColliderBounds.Expand(radii[particleIndex].x * 1.2f + stickDistance);
float4 invDir = math.rcp(velocities[particleIndex] * deltaTime);
// We check particle trajectory ray vs inflated collider aabb
// instead of checking particle vs collider aabbs directly, as this reduces
// the amount of contacts generated for fast moving particles.
if (inflatedColliderBounds.IntersectsRay(positions[particleIndex], invDir, shape.is2D != 0))
{
// generate contacts for the collider:
GenerateContacts(shape.type,
particleIndex, colliderIndex,
positions[particleIndex], orientations[particleIndex], velocities[particleIndex], radii[particleIndex],
colliderToSolver, shape, contactsQueue, deltaTime);
}
}
}
}
}