Exemple #1
0
            public void Execute()
            {
                for (int iteration = 0; iteration < IterationsCount; iteration++)
                {
                    // For each constraint....
                    for (int i = 0; i < ConstraintsArray.Length; i++)
                    {
                        Velocity        velA     = VelocityFromEntity[ConstraintsArray[i].RigidbodyA];
                        Velocity        velB     = VelocityFromEntity[ConstraintsArray[i].RigidbodyB];
                        AngularVelocity angVelA  = AngularVelocityFromEntity[ConstraintsArray[i].RigidbodyA];
                        AngularVelocity angVelB  = AngularVelocityFromEntity[ConstraintsArray[i].RigidbodyB];
                        float           invMassA = RigidBodyFromEntity[ConstraintsArray[i].RigidbodyA].InverseMass;
                        float           invMassB = RigidBodyFromEntity[ConstraintsArray[i].RigidbodyB].InverseMass;
                        //float3x3 invMomentA = math.inverse(RigidBodyFromEntity[ConstraintsArray[i].RigidbodyA].MomentOfInertia);
                        //float3x3 invMomentB = math.inverse(RigidBodyFromEntity[ConstraintsArray[i].RigidbodyB].MomentOfInertia);
                        float3x3 invMomentA = new float3x3();
                        float3x3 invMomentB = new float3x3();

                        // Build matrices for this constraint
                        float12 constraintJacobianMatrix = ConstraintsArray[i].ConstraintJacobianMatrix;

                        float12 velocityMatrix = new float12
                        {
                            aa = velA.Value,
                            ab = angVelA.Value,
                            ba = velB.Value,
                            bb = angVelB.Value,
                        };

                        float12x12Mass massInverseMatrix = new float12x12Mass
                        {
                            massA          = new float3(invMassA, invMassA, invMassA),
                            inertiaTensorA = invMomentA,
                            massB          = new float3(invMassB, invMassB, invMassB),
                            inertiaTensorB = invMomentB,
                        };

                        // Calculate lambda
                        float lambda      = 0f;
                        float baumgarte   = (BaumgarteBias / DeltaTime) * ConstraintsArray[i].BaumgarteDepth;
                        float numerator   = (-(constraintJacobianMatrix * velocityMatrix) + baumgarte);
                        float demunerator = constraintJacobianMatrix * massInverseMatrix * constraintJacobianMatrix;
                        lambda = numerator / demunerator;

                        // Calculate and apply resulting forces
                        float12 constraintImpulses        = constraintJacobianMatrix * lambda;
                        float12 constraintVelocityChanges = massInverseMatrix * constraintImpulses;

                        velA.Value    += constraintVelocityChanges.aa;
                        angVelA.Value += constraintVelocityChanges.ab;
                        velB.Value    += constraintVelocityChanges.ba;
                        angVelB.Value += constraintVelocityChanges.bb;

                        VelocityFromEntity[ConstraintsArray[i].RigidbodyA] = velA;
                        //AngularVelocityFromEntity[ConstraintsArray[i].RigidbodyA] = angVelA;
                        VelocityFromEntity[ConstraintsArray[i].RigidbodyB] = velB;
                        //AngularVelocityFromEntity[ConstraintsArray[i].RigidbodyB] = angVelB;
                    }
                }
            }
 public void Execute(int index)
 {
     if (AngularDrag[index].Value > 0f)
     {
         AngularVelocity a = AngularVelocity[index];
         a.Value = a.Value * math.clamp(1f - AngularDrag[index].Value, 0f, 1f);
         AngularVelocity[index] = a;
     }
 }