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, 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 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); }
private void GenerateContacts(ColliderShape.ShapeType colliderType, int particleIndex, int colliderIndex, float4 particlePosition, quaternion particleOrientation, float4 particleVelocity, float4 particleRadii, BurstAffineTransform colliderToSolver, BurstColliderShape shape, NativeQueue <BurstContact> .ParallelWriter contacts, float dt) { switch (colliderType) { case ColliderShape.ShapeType.Sphere: BurstSphere.Contacts(particleIndex, particlePosition, particleOrientation, particleRadii, colliderIndex, colliderToSolver, shape, contacts); break; case ColliderShape.ShapeType.Box: BurstBox.Contacts(particleIndex, particlePosition, particleOrientation, particleRadii, colliderIndex, colliderToSolver, shape, contacts); break; case ColliderShape.ShapeType.Capsule: BurstCapsule.Contacts(particleIndex, particlePosition, particleOrientation, particleRadii, colliderIndex, colliderToSolver, shape, contacts); break; case ColliderShape.ShapeType.SignedDistanceField: BurstDistanceField.Contacts(particleIndex, colliderIndex, particlePosition, particleOrientation, particleRadii, ref dfNodes, distanceFieldHeaders[shape.dataIndex], colliderToSolver, shape, contacts); break; case ColliderShape.ShapeType.Heightmap: BurstHeightField.Contacts(particleIndex, colliderIndex, particlePosition, particleOrientation, particleRadii, ref heightFieldSamples, heightFieldHeaders[shape.dataIndex], colliderToSolver, shape, contacts); break; case ColliderShape.ShapeType.TriangleMesh: BurstTriangleMesh.Contacts(particleIndex, colliderIndex, particlePosition, particleOrientation, particleVelocity, particleRadii, dt, ref bihNodes, ref triangles, ref vertices, triangleMeshHeaders[shape.dataIndex], ref colliderToSolver, ref shape, contacts); break; case ColliderShape.ShapeType.EdgeMesh: BurstEdgeMesh.Contacts(particleIndex, colliderIndex, particlePosition, particleOrientation, particleVelocity, particleRadii, dt, ref edgeBihNodes, ref edges, ref edgeVertices, edgeMeshHeaders[shape.dataIndex], ref colliderToSolver, ref shape, contacts); break; } }
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 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 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 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); }