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 ForceSimulation(params IForce[] forces)
        {
            ForceCalculator = new ForceCalculator(forces);

            Data          = np.empty(new Shape(0, ForceCalculator.TotalParams));
            VelocitySlice = ForceCalculator.VelocitySlice;
            PositionSlice = ForceCalculator.PositionSlice;
        }
        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);
        }
 public ConstraintsDecorator(ForceCalculator forceCalculator, IList <IConstraint> constraints)
 {
     ForceCalculator = forceCalculator;
     Constraints     = constraints;
 }