public static void Contacts(int particleIndex,
int colliderIndex,
float4 particlePosition,
quaternion particleOrientation,
float4 particleVelocity,
float4 particleRadii,
float deltaTime,
ref NativeArray <BIHNode> bihNodes,
ref NativeArray <Edge> edges,
ref NativeArray <float2> vertices,
EdgeMeshHeader header,
ref BurstAffineTransform colliderToSolver,
ref BurstColliderShape shape,
NativeQueue <BurstContact> .ParallelWriter contacts)
{
float4 colliderSpacePosition = colliderToSolver.InverseTransformPoint(particlePosition);
float4 colliderSpaceVel = colliderToSolver.InverseTransformVector(particleVelocity * deltaTime);
BurstAabb particleBounds = new BurstAabb(colliderSpacePosition,
colliderSpacePosition + colliderSpaceVel,
particleRadii.x / math.cmax(colliderToSolver.scale));
colliderSpacePosition *= colliderToSolver.scale;
BIHTraverse(particleIndex, colliderIndex,
colliderSpacePosition, particleOrientation, colliderSpaceVel, particleRadii, ref particleBounds,
0, ref bihNodes, ref edges, ref vertices, ref header, ref colliderToSolver, ref shape, contacts);
}
public static void GetCellCoordsForBoundsAtLevel(NativeList <int4> coords, BurstAabb bounds, int level, int maxSize = 10)
{
coords.Clear();
float cellSize = CellSizeOfLevel(level);
int3 minCell = GridHash.Quantize(bounds.min.xyz, cellSize);
int3 maxCell = GridHash.Quantize(bounds.max.xyz, cellSize);
maxCell = minCell + math.min(maxCell - minCell, new int3(maxSize));
int3 size = maxCell - minCell + new int3(1);
coords.Capacity = size.x * size.y * size.z;
// TODO: return some sort of iterator trough the cells, not a native array.
for (int x = minCell[0]; x <= maxCell[0]; ++x)
{
for (int y = minCell[1]; y <= maxCell[1]; ++y)
{
for (int z = minCell[2]; z <= maxCell[2]; ++z)
{
coords.Add(new int4(x, y, z, level));
}
}
}
}
// Iterate over all colliders and store those whose cell span has changed.
public void Execute(int i)
{
BurstAabb velocityBounds = bounds[i];
// Expand bounds by rigidbody's linear velocity:
if (shapes[i].rigidbodyIndex >= 0)
{
velocityBounds.Sweep(rigidbodies[shapes[i].rigidbodyIndex].velocity * dt);
}
// Expand bounds by collision material's stick distance:
if (shapes[i].materialIndex >= 0)
{
velocityBounds.Expand(collisionMaterials[shapes[i].materialIndex].stickDistance);
}
float size = velocityBounds.AverageAxisLength();
int level = NativeMultilevelGrid <int> .GridLevelForSize(size);
float cellSize = NativeMultilevelGrid <int> .CellSizeOfLevel(level);
// get new collider bounds cell coordinates:
BurstCellSpan newSpan = new BurstCellSpan(new int4(GridHash.Quantize(velocityBounds.min.xyz, cellSize), level),
new int4(GridHash.Quantize(velocityBounds.max.xyz, cellSize), level));
// if the collider is 2D, project it to the z = 0 cells.
if (shapes[i].is2D != 0)
{
newSpan.min[2] = 0;
newSpan.max[2] = 0;
}
// if the collider is at the tail (removed), we will only remove it from its current cellspan.
// if the new cellspan and the current one are different, we must remove it from its current cellspan and add it to its new one.
if (i >= colliderCount || cellIndices[i] != newSpan)
{
// Add the collider to the list of moving colliders:
movingColliders.Enqueue(new MovingCollider()
{
oldSpan = cellIndices[i],
newSpan = newSpan,
entity = i
});
// Update previous coords:
cellIndices[i] = newSpan;
}
}
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;
}
private void GetCandidatesForBoundsAtLevel(NativeList <int> candidates, BurstAabb cellBounds, int level, bool is2D = false, int maxSize = 10)
{
float cellSize = NativeMultilevelGrid <int> .CellSizeOfLevel(level);
int3 minCell = GridHash.Quantize(cellBounds.min.xyz, cellSize);
int3 maxCell = GridHash.Quantize(cellBounds.max.xyz, cellSize);
maxCell = minCell + math.min(maxCell - minCell, new int3(maxSize));
int3 size = maxCell - minCell + new int3(1);
int cellIndex;
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, level), out 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, level), out cellIndex))
{
var colliderCell = colliderGrid.usedCells[cellIndex];
candidates.AddRange(colliderCell.ContentsPointer, colliderCell.Length);
}
}
}
}
}
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)
{
bounds[i] = new BurstAabb(positions[i] - radii[i].x, positions[i] + radii[i].x);
}
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 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);
}
}
}
}
}
public void Execute(int i)
{
int p = activeParticles[i];
bounds[i] = new BurstAabb(positions[p] - radii[p].x, positions[p] + radii[p].x);
}
public void EncapsulateBounds(BurstAabb bounds)
{
min = math.min(min, bounds.min);
max = math.max(max, bounds.max);
}
