Exemplo n.º 1
0
        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);
        }
Exemplo n.º 2
0
        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;*/
        }
Exemplo n.º 3
0
        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);
                            }
                        }
                    }
                }
            }
        }