public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint) { point = transform.InverseTransformPoint(point); float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary); float4 normal = math.normalizesafe(point - nearestPoint); // flip the contact normal if it points below ground: BurstMath.OneSidedNormal(triNormal, ref normal); projectedPoint.point = transform.TransformPoint(nearestPoint + normal * shape.contactOffset); projectedPoint.normal = transform.TransformDirection(normal); }
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint) { switch (simplexSize) { case 1: { float4 p1 = positions[simplices[simplexStart]]; projectedPoint.bary = new float4(1, 0, 0, 0); projectedPoint.point = p1; } break; case 2: { float4 p1 = positions[simplices[simplexStart]]; float4 p2 = positions[simplices[simplexStart + 1]]; BurstMath.NearestPointOnEdge(p1, p2, point, out float mu); projectedPoint.bary = new float4(1 - mu, mu, 0, 0); projectedPoint.point = p1 * projectedPoint.bary[0] + p2 * projectedPoint.bary[1]; } break; case 3: projectedPoint.point = BurstMath.NearestPointOnTri(tri, point, out projectedPoint.bary); break; } projectedPoint.normal = math.normalizesafe(point - projectedPoint.point); /*float radius1 = radii[simplices[simplexStart]].x; * float radius2 = radii[simplices[simplexStart+1]].x; * * float invLen2 = 1.0f / math.lengthsq(p1 - p2); * float dl = (radius1 - radius2) * invLen2; * float sl = math.sqrt(1.0f / invLen2 - math.pow(radius1 - radius2, 2)) * math.sqrt(invLen2); * float adj_radii1 = radius1 * sl; * float adj_radii2 = radius2 * sl; * * float trange1 = radius1 * dl; * float trange2 = 1 + radius2 * dl; * * float adj_t = (mu - trange1) / (trange2 - trange1); * float radius = adj_radii1 + adj_t * (adj_radii2 - adj_radii1); * * float4 centerToPoint = point - centerLine; * float4 normal = centerToPoint / (math.length(centerToPoint) + BurstMath.epsilon); * * projectedPoint.point = centerLine + normal * radius; * projectedPoint.normal = normal;*/ }
public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint) { point = transform.InverseTransformPointUnscaled(point); if (shape.is2D != 0) { point[2] = 0; } float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary); float4 normal = math.normalizesafe(point - nearestPoint); projectedPoint.point = transform.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset); projectedPoint.normal = transform.TransformDirection(normal); }
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 <Triangle> triangles, ref NativeArray <float3> vertices, ref TriangleMeshHeader 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 triangles, 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 triangles, 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) { Triangle t = triangles[header.firstTriangle + i]; float4 v1 = new float4(vertices[header.firstVertex + t.i1], 0) * colliderToSolver.scale; float4 v2 = new float4(vertices[header.firstVertex + t.i2], 0) * colliderToSolver.scale; float4 v3 = new float4(vertices[header.firstVertex + t.i3], 0) * colliderToSolver.scale; BurstAabb aabb = new BurstAabb(v1, v2, v3, 0.01f); aabb.Expand(new float4(offset)); // only generate a contact if the particle trajectory intersects its inflated aabb: if (aabb.IntersectsRay(particlePosition, invDir)) { float4 point = BurstMath.NearestPointOnTri(v1, v2, v3, 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); } } } } } }