コード例 #1
0
ファイル: BurstDistanceField.cs プロジェクト: duncgibbs/Naxos
        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);
        }
コード例 #2
0
        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);
        }
コード例 #3
0
        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);
                        }
                    }
                }
            }
        }
コード例 #4
0
        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);
                            }
                        }
                    }
                }
            }
        }
コード例 #5
0
ファイル: BurstCapsule.cs プロジェクト: duncgibbs/Naxos
        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);
        }
コード例 #6
0
            private float4 GetRelativeVelocity(int particleIndex, int rigidbodyIndex, ref BurstContact contact, ref float4 angularVelocityA, ref float4 rA, ref float4 rB, bool rollingContacts)
            {
                // Initialize with particle linear velocity:
                float4 relativeVelocity = (positions[particleIndex] - prevPositions[particleIndex]) / dt;

                // Add particle angular velocity if rolling contacts are enabled:
                if (rollingContacts)
                {
                    angularVelocityA = BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], dt);
                    rA = contact.ContactPointA - prevPositions[particleIndex];
                    relativeVelocity += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
                }

                // Subtract rigidbody velocity:
                if (rigidbodyIndex >= 0)
                {
                    // Note: unlike rA, that is expressed in solver space, rB is expressed in world space.
                    rB = inertialFrame.frame.TransformPoint(contact.ContactPointB) - rigidbodies[rigidbodyIndex].com;
                    relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
                }

                return(relativeVelocity);
            }
コード例 #7
0
            private float4 GetRelativeVelocity(int particleIndexA, int particleIndexB, ref BurstContact contact, ref float4 angularVelocityA, ref float4 angularVelocityB, ref float4 rA, ref float4 rB, bool rollingContacts)
            {
                // Initialize with particle linear velocity:
                float4 velA = (positions[particleIndexA] - prevPositions[particleIndexA]) / dt;
                float4 velB = (positions[particleIndexB] - prevPositions[particleIndexB]) / dt;

                // Consider angular velocities if rolling contacts are enabled:
                if (rollingContacts)
                {
                    angularVelocityA = BurstIntegration.DifferentiateAngular(orientations[particleIndexA], prevOrientations[particleIndexA], dt);
                    angularVelocityB = BurstIntegration.DifferentiateAngular(orientations[particleIndexB], prevOrientations[particleIndexB], dt);

                    rA = contact.ContactPointA - prevPositions[particleIndexA];
                    rB = contact.ContactPointB - prevPositions[particleIndexB];

                    velA += new float4(math.cross(angularVelocityA.xyz, rA.xyz), 0);
                    velB += new float4(math.cross(angularVelocityB.xyz, rB.xyz), 0);
                }

                return(velA - velB);
            }
コード例 #8
0
            private float4 GetRelativeVelocity(int particleIndex, int rigidbodyIndex, ref BurstContact contact)
            {
                // Initialize with particle linear velocity:
                float4 relativeVelocity = (positions[particleIndex] - prevPositions[particleIndex]) / dt;

                // As we do not consider true ellipses for collision detection, particle contact points are never off-axis.
                // So particle angular velocity does not contribute to normal impulses, and we can skip it.

                // Subtract rigidbody velocity:
                if (rigidbodyIndex >= 0)
                {
                    relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
                }

                return(relativeVelocity);
            }
コード例 #9
0
        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);
        }