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;
}
}