public ConstraintDerivative Derivative(PhysicalParticle particle)
{
ConstraintDerivative result = new ConstraintDerivative();
foreach (var pair in pairs)
{
if (pair.Item1 == particle || pair.Item2 == particle)
{
result += constraint.Derivative(particle, pair);
}
}
return(result);
}
private Matrix <float> ComputeJacobian(IPhysicalConstraint constraint, IReadOnlyList <PhysicalParticle> particles)
{
Matrix <float> jacobian = Matrix <float> .Build.Dense(1, GeneralizedCoordinatesPerParticle *particles.Count);
for (int j = 0; j < particles.Count; j++)
{
if (!constraint.AppliesTo(particles[j]))
{
continue;
}
ConstraintDerivative derivative = constraint.Derivative(particles[j]);
int basis = j * GeneralizedCoordinatesPerParticle;
jacobian[0, basis + 0] = derivative.RespectToX;
jacobian[0, basis + 1] = derivative.RespectToY;
jacobian[0, basis + 2] = derivative.RespectToAngle;
}
return(jacobian);
}
private void ComputeJacobian()
{
// Parallel.For(0, Constraints.Count, i =>
for (int i = 0; i < Constraints.Count; i++)
{
var constraint = Constraints[i];
for (int j = 0; j < Particles.Count; j++)
{
if (!constraint.AppliesTo(Particles[j]))
{
continue;
}
ConstraintDerivative derivative = constraint.Derivative(Particles[j]);
int basis = j * GeneralizedCoordinatesPerParticle;
jacobian[i, basis + 0] = derivative.RespectToX;
jacobian[i, basis + 1] = derivative.RespectToY;
jacobian[i, basis + 2] = derivative.RespectToAngle;
}
}
}
