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