public void Execute(int workItemIndex)
            {
                int start, end;

                batchData.GetConstraintRange(workItemIndex, out start, out end);

                for (int i = start; i < end; ++i)
                {
                    var contact = contacts[i];

                    // update contact basis:
                    contact.CalculateBasis(velocities[contact.entityA] - velocities[contact.entityB]);

                    // update contact masses:
                    int  aMaterialIndex  = particleMaterialIndices[contact.entityA];
                    int  bMaterialIndex  = particleMaterialIndices[contact.entityB];
                    bool rollingContacts = (aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false) |
                                           (bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false);

                    contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts);
                    contact.CalculateContactMassesB(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts);

                    // update contact distance:
                    float dAB = math.dot(prevPositions[contact.entityA] - prevPositions[contact.entityB], contact.normal);
                    float dA  = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityA], radii[contact.entityA].xyz);
                    float dB  = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityB], radii[contact.entityB].xyz);
                    contact.distance = dAB - (dA + dB);

                    contacts[i] = contact;
                }
            }
Exemplo n.º 2
0
            public void Execute(int p)
            {
                int i = fluidParticles[p];

                if (smoothPositions[i].w > 0)
                {
                    float3   singularValues;
                    float3x3 u;
                    BurstMath.EigenSolve(anisotropies[i] / smoothPositions[i].w, out singularValues, out u); //TODO: smoothPositions.w is always 1? we divided it all by w in AverageSmoothPositionsJob...

                    float  max = singularValues[0];
                    float3 s   = math.max(singularValues, new float3(max / maxAnisotropy)) / max * principalRadii[i].x;

                    principalAxes[i * 3]     = new float4(u.c0, s.x);
                    principalAxes[i * 3 + 1] = new float4(u.c1, s.y);
                    principalAxes[i * 3 + 2] = new float4(u.c2, s.z);
                }
                else
                {
                    float radius = principalRadii[i].x / maxAnisotropy;
                    principalAxes[i * 3]     = new float4(1, 0, 0, radius);
                    principalAxes[i * 3 + 1] = new float4(0, 1, 0, radius);
                    principalAxes[i * 3 + 2] = new float4(0, 0, 1, radius);
                }

                renderablePositions[i] = smoothPositions[i];
            }
Exemplo n.º 3
0
        public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
        {
            float4 center = shape.center * transform.scale;

            point = transform.InverseTransformPointUnscaled(point) - center;

            if (shape.is2D != 0)
            {
                point[2] = 0;
            }

            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]);
            float4 halfVector = float4.zero;

            halfVector[direction] = height - radius;

            float4 centerLine       = BurstMath.NearestPointOnEdge(-halfVector, halfVector, point, out float mu);
            float4 centerToPoint    = point - centerLine;
            float  distanceToCenter = math.length(centerToPoint);

            float4 normal = centerToPoint / (distanceToCenter + BurstMath.epsilon);

            projectedPoint.point  = transform.TransformPointUnscaled(center + centerLine + normal * (radius + shape.contactOffset));
            projectedPoint.normal = transform.TransformDirection(normal);
        }
Exemplo n.º 4
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);
        }
            public void Execute(int i)
            {
                var contact = contacts[i];

                int simplexStart = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize);

                // get the material from the first particle in the simplex:
                int  aMaterialIndex  = particleMaterialIndices[simplices[simplexStart]];
                bool rollingContacts = aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false;

                float4     relativeVelocity       = float4.zero;
                float4     simplexPrevPosition    = float4.zero;
                quaternion simplexPrevOrientation = new quaternion(0, 0, 0, 0);
                float      simplexInvMass         = 0;
                float4     simplexInvInertia      = float4.zero;
                float      simplexRadius          = 0;

                for (int j = 0; j < simplexSize; ++j)
                {
                    int particleIndex = simplices[simplexStart + j];
                    relativeVelocity             += velocities[particleIndex] * contact.pointA[j];
                    simplexPrevPosition          += prevPositions[particleIndex] * contact.pointA[j];
                    simplexPrevOrientation.value += prevOrientations[particleIndex].value * contact.pointA[j];
                    simplexInvMass    += invMasses[particleIndex] * contact.pointA[j];
                    simplexInvInertia += invInertiaTensors[particleIndex] * contact.pointA[j];
                    simplexRadius     += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
                }

                // if there's a rigidbody present, subtract its velocity from the relative velocity:
                int rigidbodyIndex = shapes[contact.bodyB].rigidbodyIndex;

                if (rigidbodyIndex >= 0)
                {
                    relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);

                    int bMaterialIndex = shapes[contact.bodyB].materialIndex;
                    rollingContacts |= bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false;
                }

                // update contact distance
                contact.distance = math.dot(simplexPrevPosition - contact.pointB, contact.normal) - simplexRadius;

                // calculate contact point in A's surface:
                float4 contactPoint = contact.pointB + contact.normal * contact.distance;

                // update contact orthonormal basis:
                contact.CalculateBasis(relativeVelocity);

                // calculate A's contact mass.
                contact.CalculateContactMassesA(simplexInvMass, simplexInvInertia, simplexPrevPosition, simplexPrevOrientation, contactPoint, rollingContacts);

                // calculate B's contact mass.
                if (rigidbodyIndex >= 0)
                {
                    contact.CalculateContactMassesB(rigidbodies[rigidbodyIndex], inertialFrame.frame);
                }

                contacts[i] = contact;
            }
Exemplo n.º 6
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);
        }
        public void CalculateContactMassesB(BurstRigidbody rigidbody, bool rollingContacts)
        {
            // initialize inverse linear masses:
            normalInvMassB = tangentInvMassB = bitangentInvMassB = rigidbody.inverseMass;

            if (rollingContacts)
            {
                float4 rB = ContactPointB - rigidbody.com;

                normalInvMassB    += BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor, rB, normal);
                tangentInvMassB   += BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor, rB, tangent);
                bitangentInvMassB += BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor, rB, bitangent);
            }
        }
Exemplo n.º 8
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.º 9
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);
        }
Exemplo n.º 10
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);
            }
Exemplo n.º 11
0
            public void Execute(int workItemIndex)
            {
                int start, end;

                batchData.GetConstraintRange(workItemIndex, out start, out end);

                for (int i = start; i < end; ++i)
                {
                    var pair = pairs[i];

                    float4 distanceA = renderablePositions[pair.particleB] - smoothPositions[pair.particleA];
                    float4 distanceB = renderablePositions[pair.particleA] - smoothPositions[pair.particleB];

                    anisotropies[pair.particleA] += BurstMath.multrnsp(distanceA, distanceA) * pair.avgKernel;
                    anisotropies[pair.particleB] += BurstMath.multrnsp(distanceB, distanceB) * pair.avgKernel;
                }
            }
        public void CalculateContactMassesA(ref NativeArray <float> invMasses,
                                            ref NativeArray <float4> prevPositions,
                                            ref NativeArray <quaternion> orientations,
                                            ref NativeArray <float4> inverseInertiaTensors, bool rollingContacts)
        {
            // initialize inverse linear masses:
            normalInvMassA = tangentInvMassA = bitangentInvMassA = invMasses[entityA];

            if (rollingContacts)
            {
                float4   rA             = ContactPointA - prevPositions[entityA];
                float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(inverseInertiaTensors[entityA], orientations[entityA]);

                normalInvMassA    += BurstMath.RotationalInvMass(solverInertiaA, rA, normal);
                tangentInvMassA   += BurstMath.RotationalInvMass(solverInertiaA, rA, tangent);
                bitangentInvMassA += BurstMath.RotationalInvMass(solverInertiaA, rA, bitangent);
            }
        }
Exemplo n.º 13
0
        public void Evaluate(float4 point, ref BurstLocalOptimization.SurfacePoint projectedPoint)
        {
            point = transform.InverseTransformPointUnscaled(point);

            if (shape.is2D != 0)
            {
                point[2] = 0;
            }

            Edge   t  = edges[header.firstEdge + dataOffset];
            float4 v1 = (new float4(vertices[header.firstVertex + t.i1], 0) + shape.center) * transform.scale;
            float4 v2 = (new float4(vertices[header.firstVertex + t.i2], 0) + shape.center) * transform.scale;

            float4 nearestPoint = BurstMath.NearestPointOnEdge(v1, v2, point, out float mu);
            float4 normal       = math.normalizesafe(point - nearestPoint);

            projectedPoint.normal = transform.TransformDirection(normal);
            projectedPoint.point  = transform.TransformPointUnscaled(nearestPoint + normal * shape.contactOffset);
        }
Exemplo n.º 14
0
        public void CalculateContactMassesA(float invMass,
                                            float4 inverseInertiaTensor,
                                            float4 position,
                                            quaternion orientation,
                                            float4 contactPoint,
                                            bool rollingContacts)
        {
            // initialize inverse linear masses:
            normalInvMassA = tangentInvMassA = bitangentInvMassA = invMass;

            if (rollingContacts)
            {
                float4   rA             = contactPoint - position;
                float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(inverseInertiaTensor, orientation);

                normalInvMassA    += BurstMath.RotationalInvMass(solverInertiaA, rA, normal);
                tangentInvMassA   += BurstMath.RotationalInvMass(solverInertiaA, rA, tangent);
                bitangentInvMassA += BurstMath.RotationalInvMass(solverInertiaA, rA, bitangent);
            }
        }
Exemplo n.º 15
0
            public void Execute(int i)
            {
                var contact = contacts[i];

                int  aMaterialIndex  = particleMaterialIndices[contact.entityA];
                bool rollingContacts = aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false;

                int rigidbodyIndex = shapes[contact.entityB].rigidbodyIndex;

                if (rigidbodyIndex >= 0)
                {
                    // update contact basis:
                    float4 relativeVelocity = velocities[contact.entityA] - BurstMath.GetRigidbodyVelocityAtPoint(rigidbodies[rigidbodyIndex], contact.ContactPointB, rigidbodyLinearDeltas[rigidbodyIndex], rigidbodyAngularDeltas[rigidbodyIndex], inertialFrame.frame);
                    contact.CalculateBasis(relativeVelocity);

                    int bMaterialIndex = shapes[contact.entityB].materialIndex;
                    rollingContacts |= bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false;

                    // update contact masses:
                    contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts);
                    contact.CalculateContactMassesB(rigidbodies[rigidbodyIndex], false);
                }
                else
                {
                    // update contact basis:
                    contact.CalculateBasis(velocities[contact.entityA]);

                    // update contact masses:
                    contact.CalculateContactMassesA(ref invMasses, ref prevPositions, ref prevOrientations, ref invInertiaTensors, rollingContacts);
                }

                // update contact distance
                float dAB = math.dot(prevPositions[contact.entityA] - contact.point, contact.normal);
                float dA  = BurstMath.EllipsoidRadius(contact.normal, prevOrientations[contact.entityA], radii[contact.entityA].xyz);
                float dB  = shapes[contact.entityB].contactOffset;

                contact.distance = dAB - (dA + dB);

                contacts[i] = contact;
            }
Exemplo n.º 16
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);
            }
        // The code actually running on the job
        public void Execute(int index)
        {
            int i = activeParticles[index];

            // Project particles on the XY plane if we are in 2D mode:
            if (is2D)
            {
                // restrict position to the 2D plane
                float4 pos = positions[i];
                pos[2]       = previousPositions[i][2];
                positions[i] = pos;

                // restrict rotation to the axis perpendicular to the 2D plane.
                quaternion swing, twist;
                BurstMath.SwingTwist(orientations[i], new float3(0, 0, 1), out swing, out twist);
                orientations[i] = twist;
            }

            if (inverseMasses[i] > 0)
            {
                velocities[i] = BurstIntegration.DifferentiateLinear(positions[i], previousPositions[i], deltaTime);
            }
            else
            {
                velocities[i] = float4.zero;
            }

            if (inverseRotationalMasses[i] > 0)
            {
                angularVelocities[i] = BurstIntegration.DifferentiateAngular(orientations[i], previousOrientations[i], deltaTime);
            }
            else
            {
                angularVelocities[i] = float4.zero;
            }
        }
            public void Execute(int i)
            {
                int   k           = 0;
                float maximumMass = 10000;

                coms[i] = float4.zero;
                float4x4 Apq = float4x4.zero, Rpq = float4x4.zero;

                // calculate shape mass, center of mass, and moment matrix:
                for (int j = 0; j < numIndices[i]; ++j)
                {
                    k = particleIndices[firstIndex[i] + j];

                    float mass = maximumMass;
                    if (invMasses[k] > 1.0f / maximumMass)
                    {
                        mass = 1.0f / invMasses[k];
                    }

                    coms[i] += positions[k] * mass;

                    float4x4 particleR  = orientations[k].toMatrix();
                    float4x4 particleRT = restOrientations[k].toMatrix();
                    particleR[3][3]  = 0;
                    particleRT[3][3] = 0;

                    Rpq += math.mul(particleR,
                                    math.mul(math.rcp(invInertiaTensors[k] + new float4(BurstMath.epsilon)).asDiagonal(),
                                             math.transpose(particleRT))
                                    );

                    float4 restPosition = restPositions[k];
                    restPosition[3] = 0;

                    Apq += mass * BurstMath.multrnsp4(positions[k], restPosition);
                }

                if (restComs[i][3] < BurstMath.epsilon)
                {
                    return;
                }

                coms[i] /= restComs[i][3];

                // subtract global shape moment:
                float4 restCom = restComs[i];

                restCom[3] = 0;

                Apq -= restComs[i][3] * BurstMath.multrnsp4(coms[i], restCom);

                // calculate optimal transform including plastic deformation:
                float4x4 Apq_def = Rpq + math.mul(Apq, math.transpose(deformation[i]));

                // extract rotation from transform matrix, using warmstarting and few iterations:
                constraintOrientations[i] = BurstMath.ExtractRotation(Apq_def, constraintOrientations[i], 2);

                // finally, obtain rotation matrix:
                float4x4 R = constraintOrientations[i].toMatrix();

                R[3][3] = 0;

                // calculate particle orientations:
                if (explicitGroup[i] > 0)
                {
                    // if the group is explicit, set the orientation for all particles:
                    for (int j = 0; j < numIndices[i]; ++j)
                    {
                        k = particleIndices[firstIndex[i] + j];
                        orientations[k] = math.mul(constraintOrientations[i], restOrientations[k]);
                    }
                }
                else
                {
                    // set orientation of center particle only:
                    int centerIndex = particleIndices[firstIndex[i]];
                    orientations[centerIndex] = math.mul(constraintOrientations[i], restOrientations[centerIndex]);
                }

                // calculate and accumulate particle goal positions:
                float4   goal;
                float4x4 transform = math.mul(R, deformation[i]);

                for (int j = 0; j < numIndices[i]; ++j)
                {
                    k          = particleIndices[firstIndex[i] + j];
                    goal       = coms[i] + math.mul(transform, restPositions[k] - restComs[i]);
                    deltas[k] += (goal - positions[k]) * shapeMaterialParameters[i * 5];
                    counts[k]++;
                }

                // update plastic deformation:
                float plastic_yield    = shapeMaterialParameters[i * 5 + 1];
                float plastic_creep    = shapeMaterialParameters[i * 5 + 2];
                float plastic_recovery = shapeMaterialParameters[i * 5 + 3];
                float max_deform       = shapeMaterialParameters[i * 5 + 4];

                // if we are allowed to absorb deformation:
                if (plastic_creep > 0)
                {
                    R[3][3]       = 1;
                    Apq_def[3][3] = 1;

                    // get scale matrix (A = RS so S = Rt * A) and its deviation from the identity matrix:
                    float4x4 deform_matrix = math.mul(math.transpose(R), math.mul(Apq_def, Aqq[i])) - float4x4.identity;

                    // if the amount of deformation exceeds the yield threshold:
                    float norm = deform_matrix.frobeniusNorm();
                    if (norm > plastic_yield)
                    {
                        // deform the shape permanently:
                        deformation[i] = math.mul(float4x4.identity + plastic_creep * deform_matrix, deformation[i]);

                        // clamp deformation so that it does not exceed a percentage;
                        deform_matrix = deformation[i] - float4x4.identity;
                        norm          = deform_matrix.frobeniusNorm();
                        if (norm > max_deform)
                        {
                            deformation[i] = float4x4.identity + max_deform * deform_matrix / norm;
                        }

                        // if we cannot recover from plastic deformation, recalculate rest shape now:
                        if (plastic_recovery == 0)
                        {
                            RecalculateRestData(i,
                                                ref particleIndices,
                                                ref firstIndex,
                                                ref restComs,
                                                ref Aqq,
                                                ref deformation,
                                                ref numIndices,
                                                ref invMasses,
                                                ref restPositions,
                                                ref restOrientations,
                                                ref invInertiaTensors);
                        }
                    }
                }

                // if we can recover from plastic deformation, lerp towards non-deformed shape and recalculate rest shape:
                if (plastic_recovery > 0)
                {
                    deformation[i] += (float4x4.identity - deformation[i]) * math.min(plastic_recovery * deltaTime, 1.0f);
                    RecalculateRestData(i,
                                        ref particleIndices,
                                        ref firstIndex,
                                        ref restComs,
                                        ref Aqq,
                                        ref deformation,
                                        ref numIndices,
                                        ref invMasses,
                                        ref restPositions,
                                        ref restOrientations,
                                        ref invInertiaTensors);
                }
            }
        protected static void RecalculateRestData(int i,
                                                  ref NativeArray <int> particleIndices,
                                                  ref NativeArray <int> firstIndex,
                                                  ref NativeArray <float4> restComs,
                                                  ref NativeArray <float4x4> Aqq,
                                                  ref NativeArray <float4x4> deformation,
                                                  ref NativeArray <int> numIndices,
                                                  ref NativeArray <float> invMasses,
                                                  ref NativeArray <float4> restPositions,
                                                  ref NativeArray <quaternion> restOrientations,
                                                  ref NativeArray <float4> invInertiaTensors)
        {
            int   k           = 0;
            float maximumMass = 10000;

            // initialize rest center of mass and shape matrix:
            restComs[i] = float4.zero;
            Aqq[i]      = float4x4.zero;

            float4   restCom = float4.zero;
            float4x4 _Aqq = float4x4.zero, _Rqq = float4x4.zero;

            // calculate rest center of mass, shape mass and Aqq matrix.
            for (int j = 0; j < numIndices[i]; ++j)
            {
                k = particleIndices[firstIndex[i] + j];

                float mass = maximumMass;
                if (invMasses[k] > 1.0f / maximumMass)
                {
                    mass = 1.0f / invMasses[k];
                }

                restCom += restPositions[k] * mass;

                float4x4 particleR = restOrientations[k].toMatrix();
                particleR[3][3] = 0;

                _Rqq += math.mul(particleR,
                                 math.mul(math.rcp(invInertiaTensors[k] + new float4(BurstMath.epsilon)).asDiagonal(),
                                          math.transpose(particleR))
                                 );

                float4 restPosition = restPositions[k];
                restPosition[3] = 0;

                _Aqq += mass * BurstMath.multrnsp4(restPosition, restPosition);
            }


            if (restCom[3] < BurstMath.epsilon)
            {
                return;
            }

            restCom.xyz /= restCom[3];
            restComs[i]  = restCom;

            restCom[3] = 0;
            _Aqq      -= restComs[i][3] * BurstMath.multrnsp4(restCom, restCom);
            _Aqq[3][3] = 1; // so that the determinant is never 0 due to all-zeros row/column.

            Aqq[i] = math.inverse(_Rqq + math.mul(deformation[i], math.mul(_Aqq, math.transpose(deformation[i]))));
        }
Exemplo n.º 20
0
            private void InteractionTest(int A, int B, ref BurstSimplex simplexShape)
            {
                // skip the pair if their bounds don't intersect:
                if (!simplexBounds[A].IntersectsAabb(simplexBounds[B]))
                {
                    return;
                }

                // get the start index and size of each simplex:
                int simplexStartA = simplexCounts.GetSimplexStartAndSize(A, out int simplexSizeA);
                int simplexStartB = simplexCounts.GetSimplexStartAndSize(B, out int simplexSizeB);

                // immediately reject simplex pairs that share particles:
                for (int a = 0; a < simplexSizeA; ++a)
                {
                    for (int b = 0; b < simplexSizeB; ++b)
                    {
                        if (simplices[simplexStartA + a] == simplices[simplexStartB + b])
                        {
                            return;
                        }
                    }
                }

                // get phases for each simplex:
                bool restPositionsEnabled = false;
                int  groupA = GetSimplexPhase(simplexStartA, simplexSizeA, out Oni.ParticleFlags flagsA, ref restPositionsEnabled);
                int  groupB = GetSimplexPhase(simplexStartB, simplexSizeB, out Oni.ParticleFlags flagsB, ref restPositionsEnabled);

                // if all particles have the same group and none have self-collision, reject the pair.
                if (groupA == groupB && (flagsA & flagsB & Oni.ParticleFlags.SelfCollide) == 0)
                {
                    return;
                }

                // if all simplices are fluid, check their smoothing radii:
                if ((flagsA & Oni.ParticleFlags.Fluid) != 0 && (flagsB & Oni.ParticleFlags.Fluid) != 0)
                {
                    int particleA = simplices[simplexStartA];
                    int particleB = simplices[simplexStartB];

                    // for fluid we only consider the first particle in each simplex.
                    float4 predictedPositionA = positions[particleA] + velocities[particleA] * dt;
                    float4 predictedPositionB = positions[particleB] + velocities[particleB] * dt;

                    // Calculate particle center distance:
                    float d2 = math.lengthsq(predictedPositionA - predictedPositionB);

                    float fluidDistance = math.max(fluidRadii[particleA], fluidRadii[particleB]);
                    if (d2 <= fluidDistance * fluidDistance)
                    {
                        fluidInteractionsQueue.Enqueue(new FluidInteraction {
                            particleA = particleA, particleB = particleB
                        });
                    }
                }
                else // at least one solid particle is present:
                {
                    // swap simplices so that B is always the one-sided one.
                    if ((flagsA & Oni.ParticleFlags.OneSided) != 0 && groupA < groupB)
                    {
                        ObiUtils.Swap(ref A, ref B);
                        ObiUtils.Swap(ref simplexStartA, ref simplexStartB);
                        ObiUtils.Swap(ref simplexSizeA, ref simplexSizeB);
                        ObiUtils.Swap(ref flagsA, ref flagsB);
                        ObiUtils.Swap(ref groupA, ref groupB);
                    }

                    float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSizeA);
                    float4 simplexPoint;

                    simplexShape.simplexStart = simplexStartB;
                    simplexShape.simplexSize  = simplexSizeB;
                    simplexShape.positions    = restPositions;
                    simplexShape.CacheData();

                    float simplexRadiusA = 0, simplexRadiusB = 0;

                    // skip the contact if there's self-intersection at rest:
                    if (groupA == groupB && restPositionsEnabled)
                    {
                        var restPoint = BurstLocalOptimization.Optimize <BurstSimplex>(ref simplexShape, restPositions, radii,
                                                                                       simplices, simplexStartA, simplexSizeA, ref simplexBary, out simplexPoint, 4, 0);

                        for (int j = 0; j < simplexSizeA; ++j)
                        {
                            simplexRadiusA += radii[simplices[simplexStartA + j]].x * simplexBary[j];
                        }

                        for (int j = 0; j < simplexSizeB; ++j)
                        {
                            simplexRadiusB += radii[simplices[simplexStartB + j]].x * restPoint.bary[j];
                        }

                        // compare distance along contact normal with radius.
                        if (math.dot(simplexPoint - restPoint.point, restPoint.normal) < simplexRadiusA + simplexRadiusB)
                        {
                            return;
                        }
                    }

                    simplexBary            = BurstMath.BarycenterForSimplexOfSize(simplexSizeA);
                    simplexShape.positions = positions;
                    simplexShape.CacheData();

                    var surfacePoint = BurstLocalOptimization.Optimize <BurstSimplex>(ref simplexShape, positions, radii,
                                                                                      simplices, simplexStartA, simplexSizeA, ref simplexBary, out simplexPoint, optimizationIterations, optimizationTolerance);

                    simplexRadiusA = 0; simplexRadiusB = 0;
                    float4 velocityA = float4.zero, velocityB = float4.zero, normalB = float4.zero;

                    for (int j = 0; j < simplexSizeA; ++j)
                    {
                        int particleIndex = simplices[simplexStartA + j];
                        simplexRadiusA += radii[particleIndex].x * simplexBary[j];
                        velocityA      += velocities[particleIndex] * simplexBary[j];
                    }

                    for (int j = 0; j < simplexSizeB; ++j)
                    {
                        int particleIndex = simplices[simplexStartB + j];
                        simplexRadiusB += radii[particleIndex].x * surfacePoint.bary[j];
                        velocityB      += velocities[particleIndex] * surfacePoint.bary[j];
                        normalB        += normals[particleIndex] * surfacePoint.bary[j];
                    }

                    float dAB = math.dot(simplexPoint - surfacePoint.point, surfacePoint.normal);
                    float vel = math.dot(velocityA - velocityB, surfacePoint.normal);

                    // check if the projected velocity along the contact normal will get us within collision distance.
                    if (vel * dt + dAB <= simplexRadiusA + simplexRadiusB + collisionMargin)
                    {
                        // adapt collision normal for one-sided simplices:
                        if ((flagsB & Oni.ParticleFlags.OneSided) != 0 && groupB < groupA)
                        {
                            BurstMath.OneSidedNormal(normalB, ref surfacePoint.normal);
                        }

                        contactsQueue.Enqueue(new BurstContact()
                        {
                            bodyA  = A,
                            bodyB  = B,
                            pointA = simplexBary,
                            pointB = surfacePoint.bary,
                            normal = surfacePoint.normal
                        });
                    }
                }
            }
        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);
                            }
                        }
                    }
                }
            }
        }
Exemplo n.º 22
0
            public void Execute()
            {
                for (int i = 0; i < activeConstraintCount; ++i)
                {
                    int particleIndex = particleIndices[i];
                    int colliderIndex = colliderIndices[i];

                    // no collider to pin to, so ignore the constraint.
                    if (colliderIndex < 0)
                    {
                        continue;
                    }

                    int rigidbodyIndex = shapes[colliderIndex].rigidbodyIndex;

                    // calculate time adjusted compliances
                    float2 compliances = stiffnesses[i].xy / (deltaTime * deltaTime);

                    float4 particlePosition = positions[particleIndex];

                    // express pin offset in world space:
                    float4     worldPinOffset     = transforms[colliderIndex].TransformPoint(offsets[i]);
                    float4     predictedPinOffset = worldPinOffset;
                    quaternion predictedRotation  = transforms[colliderIndex].rotation;

                    float rigidbodyLinearW  = 0;
                    float rigidbodyAngularW = 0;

                    float4 linearRbDelta  = float4.zero;
                    float4 angularRbDelta = float4.zero;

                    if (rigidbodyIndex >= 0)
                    {
                        var rigidbody = rigidbodies[rigidbodyIndex];
                        linearRbDelta  = rigidbodyLinearDeltas[rigidbodyIndex];
                        angularRbDelta = rigidbodyAngularDeltas[rigidbodyIndex];

                        // predict world-space position of offset point:
                        predictedPinOffset = BurstIntegration.IntegrateLinear(predictedPinOffset, rigidbody.GetVelocityAtPoint(worldPinOffset, linearRbDelta, angularRbDelta), deltaTime);

                        // predict rotation at the end of the step:
                        predictedRotation = BurstIntegration.IntegrateAngular(predictedRotation, rigidbody.angularVelocity + angularRbDelta, deltaTime);

                        // calculate linear and angular rigidbody weights:
                        rigidbodyLinearW  = rigidbody.inverseMass;
                        rigidbodyAngularW = BurstMath.RotationalInvMass(rigidbody.inverseInertiaTensor,
                                                                        worldPinOffset - rigidbody.com,
                                                                        math.normalizesafe(inertialFrame.frame.TransformPoint(particlePosition) - predictedPinOffset));
                    }

                    // Transform pin position to solver space for constraint solving:
                    predictedPinOffset = inertialFrame.frame.InverseTransformPoint(predictedPinOffset);

                    float4 gradient    = particlePosition - predictedPinOffset;
                    float  constraint  = math.length(gradient);
                    float4 gradientDir = gradient / (constraint + BurstMath.epsilon);

                    float4 lambda        = lambdas[i];
                    float  linearDLambda = (-constraint - compliances.x * lambda.w) / (invMasses[particleIndex] + rigidbodyLinearW + rigidbodyAngularW + compliances.x + BurstMath.epsilon);
                    lambda.w += linearDLambda;
                    float4 correction = linearDLambda * gradientDir;

                    deltas[particleIndex] += correction * invMasses[particleIndex];
                    counts[particleIndex]++;

                    if (rigidbodyAngularW > 0 || invRotationalMasses[particleIndex] > 0)
                    {
                        // bend/twist constraint:
                        quaternion omega = math.mul(math.conjugate(orientations[particleIndex]), predictedRotation);   //darboux vector

                        quaternion omega_plus;
                        omega_plus.value = omega.value + restDarboux[i].value;  //delta Omega with - omega_0
                        omega.value     -= restDarboux[i].value;                //delta Omega with + omega_0
                        if (math.lengthsq(omega.value) > math.lengthsq(omega_plus.value))
                        {
                            omega = omega_plus;
                        }

                        float3 dlambda = (omega.value.xyz - compliances.y * lambda.xyz) / new float3(compliances.y + invRotationalMasses[particleIndex] + rigidbodyAngularW + BurstMath.epsilon);
                        lambda.xyz += dlambda;

                        //discrete Darboux vector does not have vanishing scalar part
                        quaternion dlambdaQ = new quaternion(dlambda[0], dlambda[1], dlambda[2], 0);

                        quaternion orientDelta = orientationDeltas[particleIndex];
                        orientDelta.value += math.mul(predictedRotation, dlambdaQ).value *invRotationalMasses[particleIndex];
                        orientationDeltas[particleIndex] = orientDelta;
                        orientationCounts[particleIndex]++;

                        if (rigidbodyIndex >= 0)
                        {
                            rigidbodies[rigidbodyIndex].ApplyDeltaQuaternion(predictedRotation, math.mul(orientations[particleIndex], dlambdaQ).value * -rigidbodyAngularW, ref angularRbDelta, deltaTime);
                        }
                    }

                    if (rigidbodyIndex >= 0)
                    {
                        float4 impulse = correction / deltaTime;

                        rigidbodies[rigidbodyIndex].ApplyImpulse(-inertialFrame.frame.TransformVector(impulse) * 1, worldPinOffset, ref linearRbDelta, ref angularRbDelta);
                        rigidbodyLinearDeltas[rigidbodyIndex]  = linearRbDelta;
                        rigidbodyAngularDeltas[rigidbodyIndex] = angularRbDelta;
                    }

                    lambdas[i] = lambda;
                }
            }
Exemplo n.º 23
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;

            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 InteractionTest(int A, int B)
            {
                bool samePhase     = (phases[A] & (int)Oni.ParticleFlags.GroupMask) == (phases[B] & (int)Oni.ParticleFlags.GroupMask);
                bool noSelfCollide = (phases[A] & (int)Oni.ParticleFlags.SelfCollide) == 0 || (phases[B] & (int)Oni.ParticleFlags.SelfCollide) == 0;

                // only particles of a different phase or set to self-collide can interact.
                if (samePhase && noSelfCollide)
                {
                    return;
                }

                // Predict positions at the end of the whole step:
                float4 predictedPositionA = positions[A] + velocities[A] * dt;
                float4 predictedPositionB = positions[B] + velocities[B] * dt;

                // Calculate particle center distance:
                float4 dab = predictedPositionA - predictedPositionB;
                float  d2  = math.lengthsq(dab);

                // if both particles are fluid, check their smoothing radii:
                if ((phases[A] & (int)Oni.ParticleFlags.Fluid) != 0 &&
                    (phases[B] & (int)Oni.ParticleFlags.Fluid) != 0)
                {
                    float fluidDistance = math.max(fluidRadii[A], fluidRadii[B]);
                    if (d2 <= fluidDistance * fluidDistance)
                    {
                        fluidInteractionsQueue.Enqueue(new FluidInteraction {
                            particleA = A, particleB = B
                        });
                    }
                }
                else // at least one solid particle
                {
                    float solidDistance = radii[A].x + radii[B].x;

                    // if these particles are self-colliding (have same phase), see if they intersect at rest.
                    if (samePhase &&
                        restPositions[A].w > 0.5f &&
                        restPositions[B].w > 0.5f)
                    {
                        // if rest positions intersect, return too.
                        float sqr_rest_distance = math.lengthsq(restPositions[A] - restPositions[B]);
                        if (sqr_rest_distance < solidDistance * solidDistance)
                        {
                            return;
                        }
                    }

                    // calculate distance at which particles are able to interact:
                    int   matIndexA           = particleMaterialIndices[A];
                    int   matIndexB           = particleMaterialIndices[B];
                    float interactionDistance = solidDistance * 1.2f + (matIndexA >= 0 ? collisionMaterials[matIndexA].stickDistance : 0) +
                                                (matIndexB >= 0 ? collisionMaterials[matIndexB].stickDistance : 0);

                    // if the distance between their predicted positions is smaller than the interaction distance:
                    if (math.lengthsq(dab) <= interactionDistance * interactionDistance)
                    {
                        // calculate contact normal and distance:
                        float4 normal   = positions[A] - positions[B];
                        float  distance = math.length(normal);

                        if (distance > BurstMath.epsilon)
                        {
                            normal /= distance;

                            float rA = BurstMath.EllipsoidRadius(normal, orientations[A], radii[A].xyz);
                            float rB = BurstMath.EllipsoidRadius(normal, orientations[B], radii[B].xyz);

                            // adapt normal for one-sided particles:
                            if ((phases[A] & (int)Oni.ParticleFlags.OneSided) != 0 &&
                                (phases[B] & (int)Oni.ParticleFlags.OneSided) != 0)
                            {
                                float3 adjustment = float3.zero;
                                if (rA < rB)
                                {
                                    adjustment = math.mul(orientations[A], new float3(0, 0, -1));
                                }
                                else
                                {
                                    adjustment = math.mul(orientations[B], new float3(0, 0, 1));
                                }

                                float dot = math.dot(normal.xyz, adjustment);
                                if (dot < 0)
                                {
                                    normal -= 2 * dot * new float4(adjustment, 0);
                                }
                            }

                            contactsQueue.Enqueue(new BurstContact
                            {
                                entityA  = A,
                                entityB  = B,
                                point    = positions[B] + normal * rB,
                                normal   = normal,
                                distance = distance - (rA + rB)
                            });
                        }
                    }
                }
            }
            public void Execute(int workItemIndex)
            {
                int start, end;

                batchData.GetConstraintRange(workItemIndex, out start, out end);

                for (int i = start; i < end; ++i)
                {
                    var contact = contacts[i];

                    int simplexStartA = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSizeA);
                    int simplexStartB = simplexCounts.GetSimplexStartAndSize(contact.bodyB, out int simplexSizeB);

                    float4     simplexVelocityA        = float4.zero;
                    float4     simplexPrevPositionA    = float4.zero;
                    quaternion simplexPrevOrientationA = new quaternion(0, 0, 0, 0);
                    float      simplexRadiusA          = 0;
                    float      simplexInvMassA         = 0;
                    float4     simplexInvInertiaA      = float4.zero;

                    float4     simplexVelocityB        = float4.zero;
                    float4     simplexPrevPositionB    = float4.zero;
                    quaternion simplexPrevOrientationB = new quaternion(0, 0, 0, 0);
                    float      simplexRadiusB          = 0;
                    float      simplexInvMassB         = 0;
                    float4     simplexInvInertiaB      = float4.zero;

                    for (int j = 0; j < simplexSizeA; ++j)
                    {
                        int particleIndex = simplices[simplexStartA + j];
                        simplexVelocityA              += velocities[particleIndex] * contact.pointA[j];
                        simplexPrevPositionA          += prevPositions[particleIndex] * contact.pointA[j];
                        simplexPrevOrientationA.value += prevOrientations[particleIndex].value * contact.pointA[j];
                        simplexInvMassA    += invMasses[particleIndex] * contact.pointA[j];
                        simplexInvInertiaA += invInertiaTensors[particleIndex] * contact.pointA[j];
                        simplexRadiusA     += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
                    }

                    for (int j = 0; j < simplexSizeB; ++j)
                    {
                        int particleIndex = simplices[simplexStartB + j];
                        simplexVelocityB              += velocities[particleIndex] * contact.pointB[j];
                        simplexPrevPositionB          += prevPositions[particleIndex] * contact.pointB[j];
                        simplexPrevOrientationB.value += prevOrientations[particleIndex].value * contact.pointB[j];
                        simplexInvMassB    += invMasses[particleIndex] * contact.pointB[j];
                        simplexInvInertiaB += invInertiaTensors[particleIndex] * contact.pointB[j];
                        simplexRadiusB     += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointB[j];
                    }

                    // update contact distance
                    float dAB = math.dot(simplexPrevPositionA - simplexPrevPositionB, contact.normal);
                    contact.distance = dAB - (simplexRadiusA + simplexRadiusB);

                    // calculate contact points:
                    float4 contactPointA = simplexPrevPositionB + contact.normal * (contact.distance + simplexRadiusB);
                    float4 contactPointB = simplexPrevPositionA - contact.normal * (contact.distance + simplexRadiusA);

                    // update contact basis:
                    contact.CalculateBasis(simplexVelocityA - simplexVelocityB);

                    // update contact masses:
                    int  aMaterialIndex  = particleMaterialIndices[simplices[simplexStartA]];
                    int  bMaterialIndex  = particleMaterialIndices[simplices[simplexStartB]];
                    bool rollingContacts = (aMaterialIndex >= 0 ? collisionMaterials[aMaterialIndex].rollingContacts > 0 : false) |
                                           (bMaterialIndex >= 0 ? collisionMaterials[bMaterialIndex].rollingContacts > 0 : false);

                    contact.CalculateContactMassesA(simplexInvMassA, simplexInvInertiaA, simplexPrevPositionA, simplexPrevOrientationA, contactPointA, rollingContacts);
                    contact.CalculateContactMassesB(simplexInvMassB, simplexInvInertiaB, simplexPrevPositionB, simplexPrevOrientationB, contactPointB, rollingContacts);

                    contacts[i] = contact;
                }
            }
Exemplo n.º 26
0
            public void Execute(int workItemIndex)
            {
                int start, end;

                batchData.GetConstraintRange(workItemIndex, out start, out end);

                for (int i = start; i < end; ++i)
                {
                    var contact = contacts[i];

                    int indexA = contact.entityA;
                    int indexB = contact.entityB;

                    // Combine collision materials:
                    BurstCollisionMaterial material = CombineCollisionMaterials(contact.entityA, contact.entityB);

                    // Calculate relative velocity:
                    float4 angularVelocityA = float4.zero, angularVelocityB = float4.zero, rA = float4.zero, rB = float4.zero;
                    float4 relativeVelocity = GetRelativeVelocity(indexA, indexB, ref contact, ref angularVelocityA, ref angularVelocityB, ref rA, ref rB, material.rollingContacts > 0);

                    // Calculate friction impulses (in the tangent and bitangent ddirections):
                    float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, dt);

                    // Apply friction impulses to both particles:
                    if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
                    {
                        float4 tangentImpulse   = impulses.x * contact.tangent;
                        float4 bitangentImpulse = impulses.y * contact.bitangent;
                        float4 totalImpulse     = tangentImpulse + bitangentImpulse;

                        deltas[indexA] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt;
                        deltas[indexB] -= (tangentImpulse * contact.tangentInvMassB + bitangentImpulse * contact.bitangentInvMassB) * dt;
                        counts[indexA]++;
                        counts[indexB]++;

                        // Rolling contacts:
                        if (material.rollingContacts > 0)
                        {
                            // Calculate angular velocity deltas due to friction impulse:
                            float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensors[indexA], orientations[indexA]);
                            float4x4 solverInertiaB = BurstMath.TransformInertiaTensor(invInertiaTensors[indexB], orientations[indexB]);

                            float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
                            float4 angVelDeltaB = -math.mul(solverInertiaB, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));

                            // Final angular velocities, after adding the deltas:
                            angularVelocityA += angVelDeltaA;
                            angularVelocityB += angVelDeltaB;

                            // Calculate weights (inverse masses):
                            float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
                            float invMassB = math.length(math.mul(solverInertiaB, math.normalizesafe(angularVelocityB)));

                            // Calculate rolling axis and angular velocity deltas:
                            float4 rollAxis       = float4.zero;
                            float  rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
                            angVelDeltaA += rollAxis * rollingImpulse * invMassA;
                            angVelDeltaB -= rollAxis * rollingImpulse * invMassB;

                            // Apply orientation deltas to particles:
                            quaternion orientationDeltaA = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexA], angVelDeltaA);
                            quaternion orientationDeltaB = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexB], angVelDeltaB);

                            quaternion qA = orientationDeltas[indexA];
                            qA.value += orientationDeltaA.value * dt;
                            orientationDeltas[indexA] = qA;
                            orientationCounts[indexA]++;

                            quaternion qB = orientationDeltas[indexB];
                            qB.value += orientationDeltaB.value * dt;
                            orientationDeltas[indexB] = qB;
                            orientationCounts[indexB]++;
                        }
                    }

                    contacts[i] = contact;
                }
            }
            public void Execute(int workItemIndex)
            {
                int start, end;

                batchData.GetConstraintRange(workItemIndex, out start, out end);

                for (int i = start; i < end; ++i)
                {
                    var contact = contacts[i];

                    int simplexStartA = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSizeA);
                    int simplexStartB = simplexCounts.GetSimplexStartAndSize(contact.bodyB, out int simplexSizeB);


                    // Combine collision materials:
                    BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStartA], simplices[simplexStartB]);

                    float4 simplexPositionA = float4.zero, simplexPositionB = float4.zero;
                    float  simplexRadiusA = 0, simplexRadiusB = 0;

                    for (int j = 0; j < simplexSizeA; ++j)
                    {
                        int particleIndex = simplices[simplexStartA + j];
                        simplexPositionA += positions[particleIndex] * contact.pointA[j];
                        simplexRadiusA   += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
                    }
                    for (int j = 0; j < simplexSizeB; ++j)
                    {
                        int particleIndex = simplices[simplexStartB + j];
                        simplexPositionB += positions[particleIndex] * contact.pointB[j];
                        simplexRadiusB   += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
                    }

                    float4 posA = simplexPositionA - contact.normal * simplexRadiusA;
                    float4 posB = simplexPositionB + contact.normal * simplexRadiusB;

                    // adhesion:
                    float lambda = contact.SolveAdhesion(posA, posB, material.stickDistance, material.stickiness, substepTime);

                    // depenetration:
                    lambda += contact.SolvePenetration(posA, posB, solverParameters.maxDepenetration * substepTime);

                    // Apply normal impulse to both particles (w/ shock propagation):
                    if (math.abs(lambda) > BurstMath.epsilon)
                    {
                        float  shock = solverParameters.shockPropagation * math.dot(contact.normal, math.normalizesafe(gravity));
                        float4 delta = lambda * contact.normal;

                        float baryScale = BurstMath.BaryScale(contact.pointA);
                        for (int j = 0; j < simplexSizeA; ++j)
                        {
                            int particleIndex = simplices[simplexStartA + j];
                            deltas[particleIndex] += delta * invMasses[particleIndex] * contact.pointA[j] * baryScale * (1 - shock);
                            counts[particleIndex]++;
                        }

                        baryScale = BurstMath.BaryScale(contact.pointB);
                        for (int j = 0; j < simplexSizeB; ++j)
                        {
                            int particleIndex = simplices[simplexStartB + j];
                            deltas[particleIndex] -= delta * invMasses[particleIndex] * contact.pointB[j] * baryScale * (1 + shock);
                            counts[particleIndex]++;
                        }
                    }

                    // Apply position deltas immediately, if using sequential evaluation:
                    if (constraintParameters.evaluationOrder == Oni.ConstraintParameters.EvaluationOrder.Sequential)
                    {
                        for (int j = 0; j < simplexSizeA; ++j)
                        {
                            int particleIndex = simplices[simplexStartA + j];
                            BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
                        }

                        for (int j = 0; j < simplexSizeB; ++j)
                        {
                            int particleIndex = simplices[simplexStartB + j];
                            BurstConstraintsBatchImpl.ApplyPositionDelta(particleIndex, constraintParameters.SORFactor, ref positions, ref deltas, ref counts);
                        }
                    }

                    contacts[i] = contact;
                }
            }
Exemplo n.º 28
0
            public void Execute(int workItemIndex)
            {
                int start, end;

                batchData.GetConstraintRange(workItemIndex, out start, out end);

                for (int i = start; i < end; ++i)
                {
                    var contact = contacts[i];

                    int simplexStartA = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSizeA);
                    int simplexStartB = simplexCounts.GetSimplexStartAndSize(contact.bodyB, out int simplexSizeB);

                    // Combine collision materials:
                    BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStartA], simplices[simplexStartB]);

                    float4     prevPositionA     = float4.zero;
                    float4     linearVelocityA   = float4.zero;
                    float4     angularVelocityA  = float4.zero;
                    float4     invInertiaTensorA = float4.zero;
                    quaternion orientationA      = new quaternion(0, 0, 0, 0);
                    float      simplexRadiusA    = 0;

                    float4     prevPositionB     = float4.zero;
                    float4     linearVelocityB   = float4.zero;
                    float4     angularVelocityB  = float4.zero;
                    float4     invInertiaTensorB = float4.zero;
                    quaternion orientationB      = new quaternion(0, 0, 0, 0);
                    float      simplexRadiusB    = 0;

                    for (int j = 0; j < simplexSizeA; ++j)
                    {
                        int particleIndex = simplices[simplexStartA + j];
                        prevPositionA      += prevPositions[particleIndex] * contact.pointA[j];
                        linearVelocityA    += BurstIntegration.DifferentiateLinear(positions[particleIndex], prevPositions[particleIndex], substepTime) * contact.pointA[j];
                        angularVelocityA   += BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], substepTime) * contact.pointA[j];
                        invInertiaTensorA  += invInertiaTensors[particleIndex] * contact.pointA[j];
                        orientationA.value += orientations[particleIndex].value * contact.pointA[j];
                        simplexRadiusA     += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
                    }
                    for (int j = 0; j < simplexSizeB; ++j)
                    {
                        int particleIndex = simplices[simplexStartB + j];
                        prevPositionB      += prevPositions[particleIndex] * contact.pointB[j];
                        linearVelocityB    += BurstIntegration.DifferentiateLinear(positions[particleIndex], prevPositions[particleIndex], substepTime) * contact.pointB[j];
                        angularVelocityB   += BurstIntegration.DifferentiateAngular(orientations[particleIndex], prevOrientations[particleIndex], substepTime) * contact.pointB[j];
                        invInertiaTensorB  += invInertiaTensors[particleIndex] * contact.pointB[j];
                        orientationB.value += orientations[particleIndex].value * contact.pointB[j];
                        simplexRadiusB     += BurstMath.EllipsoidRadius(contact.normal, prevOrientations[particleIndex], radii[particleIndex].xyz) * contact.pointB[j];
                    }

                    float4 rA = float4.zero, rB = float4.zero;

                    // Consider angular velocities if rolling contacts are enabled:
                    if (material.rollingContacts > 0)
                    {
                        rA = -contact.normal * simplexRadiusA;
                        rB = contact.normal * simplexRadiusB;

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

                    // Calculate relative velocity:
                    float4 relativeVelocity = linearVelocityA - linearVelocityB;

                    // Calculate friction impulses (in the tangent and bitangent ddirections):
                    float2 impulses         = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, substepTime);

                    // Apply friction impulses to both particles:
                    if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
                    {
                        float4 tangentImpulse   = impulses.x * contact.tangent;
                        float4 bitangentImpulse = impulses.y * contact.bitangent;
                        float4 totalImpulse     = tangentImpulse + bitangentImpulse;

                        float baryScale = BurstMath.BaryScale(contact.pointA);
                        for (int j = 0; j < simplexSizeA; ++j)
                        {
                            int particleIndex = simplices[simplexStartA + j];
                            deltas[particleIndex] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * substepTime * contact.pointA[j] * baryScale;
                            counts[particleIndex]++;
                        }

                        baryScale = BurstMath.BaryScale(contact.pointB);
                        for (int j = 0; j < simplexSizeB; ++j)
                        {
                            int particleIndex = simplices[simplexStartB + j];
                            deltas[particleIndex] -= (tangentImpulse * contact.tangentInvMassB + bitangentImpulse * contact.bitangentInvMassB) * substepTime * contact.pointB[j] * baryScale;
                            counts[particleIndex]++;
                        }

                        // Rolling contacts:
                        if (material.rollingContacts > 0)
                        {
                            // Calculate angular velocity deltas due to friction impulse:
                            float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensorA, orientationA);
                            float4x4 solverInertiaB = BurstMath.TransformInertiaTensor(invInertiaTensorB, orientationB);

                            float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
                            float4 angVelDeltaB = -math.mul(solverInertiaB, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));

                            // Final angular velocities, after adding the deltas:
                            angularVelocityA += angVelDeltaA;
                            angularVelocityB += angVelDeltaB;

                            // Calculate weights (inverse masses):
                            float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
                            float invMassB = math.length(math.mul(solverInertiaB, math.normalizesafe(angularVelocityB)));

                            // Calculate rolling axis and angular velocity deltas:
                            float4 rollAxis       = float4.zero;
                            float  rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
                            angVelDeltaA += rollAxis * rollingImpulse * invMassA;
                            angVelDeltaB -= rollAxis * rollingImpulse * invMassB;

                            // Apply orientation deltas to particles:
                            quaternion orientationDeltaA = BurstIntegration.AngularVelocityToSpinQuaternion(orientationA, angVelDeltaA, substepTime);
                            quaternion orientationDeltaB = BurstIntegration.AngularVelocityToSpinQuaternion(orientationB, angVelDeltaB, substepTime);

                            for (int j = 0; j < simplexSizeA; ++j)
                            {
                                int        particleIndex = simplices[simplexStartA + j];
                                quaternion qA            = orientationDeltas[particleIndex];
                                qA.value += orientationDeltaA.value;
                                orientationDeltas[particleIndex] = qA;
                                orientationCounts[particleIndex]++;
                            }

                            for (int j = 0; j < simplexSizeB; ++j)
                            {
                                int        particleIndex = simplices[simplexStartB + j];
                                quaternion qB            = orientationDeltas[particleIndex];
                                qB.value += orientationDeltaB.value;
                                orientationDeltas[particleIndex] = qB;
                                orientationCounts[particleIndex]++;
                            }
                        }
                    }

                    contacts[i] = contact;
                }
            }
Exemplo n.º 29
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);
                        }
                    }
                }
            }
        }
Exemplo n.º 30
0
            public void Execute()
            {
                for (int i = 0; i < contacts.Length; ++i)
                {
                    var contact = contacts[i];

                    // Get the indices of the particle and collider involved in this contact:
                    int indexA = contact.entityA;
                    int indexB = contact.entityB;

                    // Skip contacts involving triggers:
                    if (shapes[indexB].flags > 0)
                    {
                        continue;
                    }

                    // Get the rigidbody index (might be < 0, in that case there's no rigidbody present)
                    int rigidbodyIndex = shapes[indexB].rigidbodyIndex;

                    // Combine collision materials:
                    BurstCollisionMaterial material = CombineCollisionMaterials(indexA, indexB);

                    // Calculate relative velocity:
                    float4 angularVelocityA = float4.zero, rA = float4.zero, rB = float4.zero;
                    float4 relativeVelocity = GetRelativeVelocity(indexA, rigidbodyIndex, ref contact, ref angularVelocityA, ref rA, ref rB, material.rollingContacts > 0);

                    // Determine impulse magnitude:
                    float2 impulses = contact.SolveFriction(relativeVelocity, material.staticFriction, material.dynamicFriction, dt);

                    if (math.abs(impulses.x) > BurstMath.epsilon || math.abs(impulses.y) > BurstMath.epsilon)
                    {
                        float4 tangentImpulse   = impulses.x * contact.tangent;
                        float4 bitangentImpulse = impulses.y * contact.bitangent;
                        float4 totalImpulse     = tangentImpulse + bitangentImpulse;

                        deltas[indexA] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt;
                        counts[indexA]++;

                        if (rigidbodyIndex >= 0)
                        {
                            var rb = rigidbodies[rigidbodyIndex];

                            float4 worldImpulse = -inertialFrame.frame.TransformVector(totalImpulse);
                            float4 worldPoint   = inertialFrame.frame.TransformPoint(contact.point);

                            rigidbodyLinearDeltas[rigidbodyIndex]  += rb.inverseMass * worldImpulse;
                            rigidbodyAngularDeltas[rigidbodyIndex] += math.mul(rb.inverseInertiaTensor, new float4(math.cross((worldPoint - rb.com).xyz, worldImpulse.xyz), 0));
                        }

                        // Rolling contacts:
                        if (material.rollingContacts > 0)
                        {
                            // Calculate angular velocity deltas due to friction impulse:
                            float4x4 solverInertiaA = BurstMath.TransformInertiaTensor(invInertiaTensors[indexA], orientations[indexA]);

                            float4 angVelDeltaA = math.mul(solverInertiaA, new float4(math.cross(rA.xyz, totalImpulse.xyz), 0));
                            float4 angVelDeltaB = float4.zero;

                            // Final angular velocities, after adding the deltas:
                            angularVelocityA += angVelDeltaA;
                            float4 angularVelocityB = float4.zero;

                            // Calculate weights (inverse masses):
                            float invMassA = math.length(math.mul(solverInertiaA, math.normalizesafe(angularVelocityA)));
                            float invMassB = 0;

                            if (rigidbodyIndex >= 0)
                            {
                                angVelDeltaB     = math.mul(-rigidbodies[rigidbodyIndex].inverseInertiaTensor, new float4(math.cross(rB.xyz, totalImpulse.xyz), 0));
                                angularVelocityB = rigidbodies[rigidbodyIndex].angularVelocity + angVelDeltaB;
                                invMassB         = math.length(math.mul(rigidbodies[rigidbodyIndex].inverseInertiaTensor, math.normalizesafe(angularVelocityB)));
                            }

                            // Calculate rolling axis and angular velocity deltas:
                            float4 rollAxis       = float4.zero;
                            float  rollingImpulse = contact.SolveRollingFriction(angularVelocityA, angularVelocityB, material.rollingFriction, invMassA, invMassB, ref rollAxis);
                            angVelDeltaA += rollAxis * rollingImpulse * invMassA;
                            angVelDeltaB -= rollAxis * rollingImpulse * invMassB;

                            // Apply orientation delta to particle:
                            quaternion orientationDelta = BurstIntegration.AngularVelocityToSpinQuaternion(orientations[indexA], angVelDeltaA);

                            quaternion qA = orientationDeltas[indexA];
                            qA.value += orientationDelta.value * dt;
                            orientationDeltas[indexA] = qA;
                            orientationCounts[indexA]++;

                            // Apply angular velocity delta to rigidbody:
                            if (rigidbodyIndex >= 0)
                            {
                                float4 angularDelta = rigidbodyAngularDeltas[rigidbodyIndex];
                                angularDelta += angVelDeltaB;
                                rigidbodyAngularDeltas[rigidbodyIndex] = angularDelta;
                            }
                        }
                    }

                    contacts[i] = contact;
                }
            }