|
private Transpose ( |
||
| source | ||
| result | Vector3 | |
| return | void | |
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);
}
