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);
            }
        }
        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);
            }
        }
Пример #3
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);
            }
        }
Пример #4
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;
                }
            }