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;
                }
            }
예제 #2
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;
                }
            }
예제 #3
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;
                }
            }
            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;
                }
            }