Exemple #1
0
        public void ApplyDeltaQuaternion(quaternion rotation, quaternion delta, ref float4 angularDelta, float dt)
        {
            // convert quaternion delta to angular acceleration:
            quaternion newRotation = math.normalize(new quaternion(rotation.value + delta.value));

            angularDelta += BurstIntegration.DifferentiateAngular(newRotation, rotation, dt);
        }
Exemple #2
0
        public static float4 GetParticleVelocityAtPoint(float4 position, float4 prevPosition, quaternion orientation, quaternion prevOrientation, float4 point, float dt)
        {
            // calculate both linear and angular velocities:
            float4 linearVelocity  = BurstIntegration.DifferentiateLinear(position, prevPosition, dt);
            float4 angularVelocity = BurstIntegration.DifferentiateAngular(orientation, prevOrientation, dt);

            return(linearVelocity + new float4(math.cross(angularVelocity.xyz, (point - prevPosition).xyz), 0));
        }
Exemple #3
0
        public static void ApplyDeltaQuaternion(int rigidbodyIndex,
                                                quaternion rotation,
                                                quaternion delta,
                                                NativeArray <float4> angularDeltas,
                                                BurstAffineTransform transform,
                                                float dt)
        {
            quaternion rotationWS = math.mul(transform.rotation, rotation);
            quaternion deltaWS    = math.mul(transform.rotation, delta);

            // convert quaternion delta to angular acceleration:
            quaternion newRotation = math.normalize(new quaternion(rotationWS.value + deltaWS.value));

            angularDeltas[rigidbodyIndex] += BurstIntegration.DifferentiateAngular(newRotation, rotationWS, dt);
        }
        public void Execute(int index)
        {
            int i = activeParticles[index];

            // the previous position/orientation is the current position/orientation at the start of the step.
            previousPositions[i]    = positions[i];
            previousOrientations[i] = orientations[i];

            if (inverseMasses[i] > 0)
            {
                float4 effectiveGravity = gravity;

                // Adjust gravity for buoyant fluid particles:
                if ((phases[i] & (int)Oni.ParticleFlags.Fluid) != 0)
                {
                    effectiveGravity *= -buoyancies[i];
                }

                // apply external forces and gravity:
                float4 vel = velocities[i] + (inverseMasses[i] * externalForces[i] + effectiveGravity) * deltaTime;

                // project velocity to 2D plane if needed:
                if (is2D)
                {
                    vel[3] = 0;
                }

                velocities[i] = vel;
            }

            if (inverseRotationalMasses[i] > 0)
            {
                // apply external torques (simplification: we don't use full inertia tensor here)
                float4 angularVel = angularVelocities[i] + inverseRotationalMasses[i] * externalTorques[i] * deltaTime;

                // project angular velocity to 2D plane normal if needed:
                if (is2D)
                {
                    angularVel = angularVel.project(new float4(0, 0, 1, 0));
                }

                angularVelocities[i] = angularVel;
            }

            // integrate velocities:
            positions[i]    = BurstIntegration.IntegrateLinear(positions[i], velocities[i], deltaTime);
            orientations[i] = BurstIntegration.IntegrateAngular(orientations[i], angularVelocities[i], deltaTime);
        }
        public void Update(float4 position, float4 scale, quaternion rotation, float dt)
        {
            prevFrame = frame;
            float4 prevVelocity        = velocity;
            float4 prevAngularVelocity = angularVelocity;

            frame.translation = position;
            frame.rotation    = rotation;
            frame.scale       = scale;

            velocity        = BurstIntegration.DifferentiateLinear(frame.translation, prevFrame.translation, dt);
            angularVelocity = BurstIntegration.DifferentiateAngular(frame.rotation, prevFrame.rotation, dt);

            acceleration        = BurstIntegration.DifferentiateLinear(velocity, prevVelocity, dt);
            angularAcceleration = BurstIntegration.DifferentiateLinear(angularVelocity, prevAngularVelocity, dt);
        }
Exemple #6
0
            private float4 GetRelativeVelocity(int particleIndexA, int particleIndexB, ref BurstContact contact, ref float4 angularVelocityA, ref float4 angularVelocityB, ref float4 rA, ref float4 rB, bool rollingContacts)
            {
                // Initialize with particle linear velocity:
                float4 velA = (positions[particleIndexA] - prevPositions[particleIndexA]) / dt;
                float4 velB = (positions[particleIndexB] - prevPositions[particleIndexB]) / dt;

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

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

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

                return(velA - velB);
            }
Exemple #7
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;
            }
        }
Exemple #9
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;
                }
            }
Exemple #10
0
 public static float4 GetParticleVelocityAtPoint(float4 position, float4 prevPosition, float4 point, float dt)
 {
     // no angular velocity, so calculate and return linear velocity only:
     return(BurstIntegration.DifferentiateLinear(position, prevPosition, dt));
 }
Exemple #11
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;
                }
            }
Exemple #12
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;
                }
            }
Exemple #13
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;
                }
            }
Exemple #14
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;
                }
            }