Пример #1
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 simplexStart  = simplexCounts.GetSimplexStartAndSize(contact.bodyA, out int simplexSize);
                    int colliderIndex = contact.bodyB;

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

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

                    // Combine collision materials (use material from first particle in simplex)
                    BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStart], colliderIndex);

                    // Calculate relative velocity:
                    float4 rA = float4.zero, rB = float4.zero;

                    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;

                    for (int j = 0; j < simplexSize; ++j)
                    {
                        int particleIndex = simplices[simplexStart + 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];
                    }

                    float4 relativeVelocity      = linearVelocityA;

                    // Add particle angular velocity if rolling contacts are enabled:
                    if (material.rollingContacts > 0)
                    {
                        rA = -contact.normal * simplexRadiusA;
                        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.pointB) - rigidbodies[rigidbodyIndex].com;
                        relativeVelocity -= BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
                    }

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

                    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 < simplexSize; ++j)
                        {
                            int particleIndex = simplices[simplexStart + j];
                            //(tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * dt;
                            deltas[particleIndex] += (tangentImpulse * contact.tangentInvMassA + bitangentImpulse * contact.bitangentInvMassA) * substepTime * contact.pointA[j] * baryScale;
                            counts[particleIndex]++;
                        }

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

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

                            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 particles:
                            quaternion orientationDelta = BurstIntegration.AngularVelocityToSpinQuaternion(orientationA, angVelDeltaA, substepTime);

                            for (int j = 0; j < simplexSize; ++j)
                            {
                                int        particleIndex = simplices[simplexStart + j];
                                quaternion qA            = orientationDeltas[particleIndex];
                                qA.value += orientationDelta.value;
                                orientationDeltas[particleIndex] = qA;
                                orientationCounts[particleIndex]++;
                            }

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

                    contacts[i] = contact;
                }
            }
            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 / (substepTime * substepTime);

                    // project particle position to the end of the full step:
                    float4 particlePosition = math.lerp(prevPositions[particleIndex], positions[particleIndex], substeps);

                    // 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;

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

                        // predict offset point position:
                        float4 velocityAtPoint = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, inertialFrame.frame.InverseTransformPoint(worldPinOffset), rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
                        predictedPinOffset = BurstIntegration.IntegrateLinear(predictedPinOffset, inertialFrame.frame.TransformVector(velocityAtPoint), stepTime);

                        // predict rotation at the end of the step:
                        predictedRotation = BurstIntegration.IntegrateAngular(predictedRotation, rigidbody.angularVelocity + rigidbodyAngularDeltas[rigidbodyIndex], stepTime);

                        // 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);
                    predictedRotation  = math.mul(math.conjugate(inertialFrame.frame.rotation), predictedRotation);

                    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] / substeps;
                    counts[particleIndex]++;

                    if (rigidbodyIndex >= 0)
                    {
                        BurstMath.ApplyImpulse(rigidbodyIndex,
                                               -correction / stepTime * 1,
                                               inertialFrame.frame.InverseTransformPoint(worldPinOffset),
                                               rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
                    }

                    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] / substeps;
                        orientationDeltas[particleIndex] = orientDelta;
                        orientationCounts[particleIndex]++;

                        if (rigidbodyIndex >= 0)
                        {
                            BurstMath.ApplyDeltaQuaternion(rigidbodyIndex,
                                                           predictedRotation,
                                                           -math.mul(orientations[particleIndex], dlambdaQ).value *rigidbodyAngularW,
                                                           rigidbodyAngularDeltas, inertialFrame.frame, stepTime);
                        }
                    }

                    lambdas[i] = lambda;
                }
            }
            public void Execute()
            {
                for (int i = 0; i < contacts.Length; ++i)
                {
                    var contact = contacts[i];

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

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

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

                    // Combine collision materials (use material from first particle in simplex)
                    BurstCollisionMaterial material = CombineCollisionMaterials(simplices[simplexStart], colliderIndex);

                    // Get relative velocity at contact point.
                    // 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.
                    float4 simplexPosition     = float4.zero;
                    float4 simplexPrevPosition = float4.zero;
                    float  simplexRadius       = 0;

                    for (int j = 0; j < simplexSize; ++j)
                    {
                        int particleIndex = simplices[simplexStart + j];
                        simplexPosition     += positions[particleIndex] * contact.pointA[j];
                        simplexPrevPosition += prevPositions[particleIndex] * contact.pointA[j];
                        simplexRadius       += BurstMath.EllipsoidRadius(contact.normal, orientations[particleIndex], radii[particleIndex].xyz) * contact.pointA[j];
                    }

                    // project position to the end of the full step:
                    float4 posA = math.lerp(simplexPrevPosition, simplexPosition, substeps);
                    posA += -contact.normal * simplexRadius;

                    float4 posB = contact.pointB;

                    if (rigidbodyIndex >= 0)
                    {
                        posB += BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame) * stepTime;
                    }

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

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

                    // Apply normal impulse to both simplex and rigidbody:
                    if (math.abs(lambda) > BurstMath.epsilon)
                    {
                        float4 delta = lambda * contact.normal * BurstMath.BaryScale(contact.pointA) / substeps;
                        for (int j = 0; j < simplexSize; ++j)
                        {
                            int particleIndex = simplices[simplexStart + j];
                            deltas[particleIndex] += delta * invMasses[particleIndex] * contact.pointA[j];
                            counts[particleIndex]++;
                        }

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

                        if (rigidbodyIndex >= 0)
                        {
                            BurstMath.ApplyImpulse(rigidbodyIndex, -lambda / stepTime * contact.normal, contact.pointB, rigidbodies, rigidbodyLinearDeltas, rigidbodyAngularDeltas, inertialFrame.frame);
                        }
                    }

                    contacts[i] = contact;
                }
            }