public void SolveIteration() { Vector3Width4 linearVelocityA = new Vector3Width4(ref VelocitiesA0.LinearVelocity, ref VelocitiesA1.LinearVelocity, ref VelocitiesA2.LinearVelocity, ref VelocitiesA3.LinearVelocity); Vector3Width4 angularVelocityA = new Vector3Width4(ref VelocitiesA0.AngularVelocity, ref VelocitiesA1.AngularVelocity, ref VelocitiesA2.AngularVelocity, ref VelocitiesA3.AngularVelocity); Vector3Width4 linearVelocityB = new Vector3Width4(ref VelocitiesB0.LinearVelocity, ref VelocitiesB1.LinearVelocity, ref VelocitiesB2.LinearVelocity, ref VelocitiesB3.LinearVelocity); Vector3Width4 angularVelocityB = new Vector3Width4(ref VelocitiesB0.AngularVelocity, ref VelocitiesB1.AngularVelocity, ref VelocitiesB2.AngularVelocity, ref VelocitiesB3.AngularVelocity); for (int i = 0; i < ConstraintCount; ++i) { Constraints[i].SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); } //for (int i = 0; i < 4; ++i) //{ // a.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); //} //a.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); //b.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); //c.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); //d.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); Vector3Width4.Transpose(ref linearVelocityA, out VelocitiesA0.LinearVelocity, out VelocitiesA1.LinearVelocity, out VelocitiesA2.LinearVelocity, out VelocitiesA3.LinearVelocity); Vector3Width4.Transpose(ref linearVelocityB, out VelocitiesB0.LinearVelocity, out VelocitiesB1.LinearVelocity, out VelocitiesB2.LinearVelocity, out VelocitiesB3.LinearVelocity); Vector3Width4.Transpose(ref angularVelocityA, out VelocitiesA0.AngularVelocity, out VelocitiesA1.AngularVelocity, out VelocitiesA2.AngularVelocity, out VelocitiesA3.AngularVelocity); Vector3Width4.Transpose(ref angularVelocityB, out VelocitiesB0.AngularVelocity, out VelocitiesB1.AngularVelocity, out VelocitiesB2.AngularVelocity, out VelocitiesB3.AngularVelocity); }
public void SolveIteration() { Vector3Width4 linearVelocityA = new Vector3Width4(ref VelocitiesA0.LinearVelocity, ref VelocitiesA1.LinearVelocity, ref VelocitiesA2.LinearVelocity, ref VelocitiesA3.LinearVelocity); Vector3Width4 angularVelocityA = new Vector3Width4(ref VelocitiesA0.AngularVelocity, ref VelocitiesA1.AngularVelocity, ref VelocitiesA2.AngularVelocity, ref VelocitiesA3.AngularVelocity); Vector3Width4 linearVelocityB = new Vector3Width4(ref VelocitiesB0.LinearVelocity, ref VelocitiesB1.LinearVelocity, ref VelocitiesB2.LinearVelocity, ref VelocitiesB3.LinearVelocity); Vector3Width4 angularVelocityB = new Vector3Width4(ref VelocitiesB0.AngularVelocity, ref VelocitiesB1.AngularVelocity, ref VelocitiesB2.AngularVelocity, ref VelocitiesB3.AngularVelocity); Vector4 linearA, angularA, linearB, angularB; Vector3Width4.Dot(ref LinearJacobianA, ref linearVelocityA, out linearA); Vector3Width4.Dot(ref AngularJacobianA, ref angularVelocityA, out angularA); Vector3Width4.Dot(ref LinearJacobianB, ref linearVelocityB, out linearB); Vector3Width4.Dot(ref AngularJacobianB, ref angularVelocityB, out angularB); var lambda = EffectiveMass * (linearA + angularA + linearB + angularB + PenetrationBias - AccumulatedImpulse * Softness); var previous = AccumulatedImpulse; AccumulatedImpulse = Vector4.Max(Vector4.Zero, AccumulatedImpulse + lambda); lambda = AccumulatedImpulse - previous; ApplyImpulse(ref lambda, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB); Vector3Width4.Transpose(ref linearVelocityA, out VelocitiesA0.LinearVelocity, out VelocitiesA1.LinearVelocity, out VelocitiesA2.LinearVelocity, out VelocitiesA3.LinearVelocity); Vector3Width4.Transpose(ref linearVelocityB, out VelocitiesB0.LinearVelocity, out VelocitiesB1.LinearVelocity, out VelocitiesB2.LinearVelocity, out VelocitiesB3.LinearVelocity); Vector3Width4.Transpose(ref angularVelocityA, out VelocitiesA0.AngularVelocity, out VelocitiesA1.AngularVelocity, out VelocitiesA2.AngularVelocity, out VelocitiesA3.AngularVelocity); Vector3Width4.Transpose(ref angularVelocityB, out VelocitiesB0.AngularVelocity, out VelocitiesB1.AngularVelocity, out VelocitiesB2.AngularVelocity, out VelocitiesB3.AngularVelocity); }
void ApplyImpulse(ref Vector4 lambda) { Vector3Width4 linearChangeA, linearChangeB; Vector3Width4.Multiply(ref LinearJacobianITA, ref lambda, out linearChangeA); Vector3Width4.Multiply(ref LinearJacobianITB, ref lambda, out linearChangeB); Vector3 linearChangeA0, linearChangeA1, linearChangeA2, linearChangeA3; Vector3 linearChangeB0, linearChangeB1, linearChangeB2, linearChangeB3; Vector3Width4.Transpose(ref linearChangeA, out linearChangeA0, out linearChangeA1, out linearChangeA2, out linearChangeA3); Vector3Width4.Transpose(ref linearChangeB, out linearChangeB0, out linearChangeB1, out linearChangeB2, out linearChangeB3); //World inertia available, so no need for extra transforms. Vector3Width4 angularChangeA, angularChangeB; Vector3Width4.Multiply(ref AngularJacobianITA, ref lambda, out angularChangeA); Vector3Width4.Multiply(ref AngularJacobianITB, ref lambda, out angularChangeB); Vector3 angularChangeA0, angularChangeA1, angularChangeA2, angularChangeA3; Vector3 angularChangeB0, angularChangeB1, angularChangeB2, angularChangeB3; Vector3Width4.Transpose(ref angularChangeA, out angularChangeA0, out angularChangeA1, out angularChangeA2, out angularChangeA3); Vector3Width4.Transpose(ref angularChangeB, out angularChangeB0, out angularChangeB1, out angularChangeB2, out angularChangeB3); VelocitiesA0.LinearVelocity -= linearChangeA0; VelocitiesA0.AngularVelocity -= angularChangeA0; VelocitiesA1.LinearVelocity -= linearChangeA1; VelocitiesA1.AngularVelocity -= angularChangeA1; VelocitiesA2.LinearVelocity -= linearChangeA2; VelocitiesA2.AngularVelocity -= angularChangeA2; VelocitiesA3.LinearVelocity -= linearChangeA3; VelocitiesA3.AngularVelocity -= angularChangeA3; VelocitiesB0.LinearVelocity -= linearChangeB0; VelocitiesB0.AngularVelocity -= angularChangeB0; VelocitiesB1.LinearVelocity -= linearChangeB1; VelocitiesB1.AngularVelocity -= angularChangeB1; VelocitiesB2.LinearVelocity -= linearChangeB2; VelocitiesB2.AngularVelocity -= angularChangeB2; VelocitiesB3.LinearVelocity -= linearChangeB3; VelocitiesB3.AngularVelocity -= angularChangeB3; }
public void SolveIteration() { Vector3Width4 velocities = new Vector3Width4(ref ConnectionA.LinearVelocity, ref ConnectionA.AngularVelocity, ref ConnectionB.LinearVelocity, ref ConnectionB.AngularVelocity); Vector4 velocityContributions; Vector3Width4.Dot(ref velocities, ref Jacobians, out velocityContributions); var lambda = EffectiveMass * ( velocityContributions.X + velocityContributions.Y + velocityContributions.Z + velocityContributions.W + PenetrationBias - AccumulatedImpulse * Softness); var previous = AccumulatedImpulse; AccumulatedImpulse = Math.Max(0, AccumulatedImpulse + lambda); lambda = AccumulatedImpulse - previous; ApplyImpulse(lambda, ref velocities); Vector3Width4.Transpose(ref velocities, out ConnectionA.LinearVelocity, out ConnectionA.AngularVelocity, out ConnectionB.LinearVelocity, out ConnectionB.AngularVelocity); }