public NDArray CalculateAcceleration(NDArray data)
{
if (Constraints.Count == 0)
{
return(ForceCalculator.CalculateAcceleration(data));
}
var flattenedVelocities = data[Slice.All, ForceCalculator.VelocitySlice].flatten().reshape(-1, 1);
var masses = data[Slice.All, ForceCalculator.MassSlice];
var repeated = np.repeat(masses, 2);
var massDiag = diag(repeated);
var constraintGradient = np.zeros(Constraints.Count, 2 * data.shape[0]);
var h = np.zeros(Constraints.Count, 1);
for (var i = 0; i < Constraints.Count; i++)
{
constraintGradient[i] = Constraints[i].gradient(data[Slice.All, ForceCalculator.PositionSlice]);
h[i] = np.dot(
np.dot(transpose(flattenedVelocities),
Constraints[i].hessian(data[Slice.All, ForceCalculator.PositionSlice])), flattenedVelocities);
}
var forces = masses * ForceCalculator.CalculateAcceleration(data);
var a = massDiag.hstack(-transpose(constraintGradient))
.vstack(constraintGradient.hstack(np.zeros(Constraints.Count, Constraints.Count)));
var b = forces.flatten().reshape(-1, 1).vstack(-h);
var solved = np.dot(inv(a), b);
return(solved[new Slice(0, data.shape[0] * 2)].reshape(-1, 2));
}
public virtual void Tick(double time)
{
// Use runge kutta later
if (Data.shape[0] <= 0)
{
return;
}
var fParamZeros = np.zeros_like(Data[Slice.All, new Slice(4)]);
var k1 = time * Data[Slice.All, VelocitySlice].hstack(ForceCalculator.CalculateAcceleration(Data));
var k1d2 = Data + (k1 / 2).hstack(fParamZeros);
var k2 = time * k1d2[Slice.All, VelocitySlice].hstack(ForceCalculator.CalculateAcceleration(k1d2));
var k2d2 = Data + (k2 / 2).hstack(fParamZeros);
var k3 = time * k2d2[Slice.All, VelocitySlice].hstack(ForceCalculator.CalculateAcceleration(k2d2));
var k3d4 = Data + k3.hstack(fParamZeros);
var k4 = time * k3d4[Slice.All, VelocitySlice].hstack(ForceCalculator.CalculateAcceleration(k3d4));
Data += 1.0 / 6.0 * (k1 + 2 * k2 + 2 * k3 + k4).hstack(fParamZeros);
}