1 | using System;
|
---|
2 |
|
---|
3 | namespace Netron.Diagramming.Core.Layout.Force {
|
---|
4 |
|
---|
5 | ///<summary>
|
---|
6 | /// Updates velocity and position data using the 4th-Order Runge-Kutta method.
|
---|
7 | /// It is slower but more accurate than other techniques such as Euler's Method.
|
---|
8 | /// The technique requires re-evaluating forces 4 times for a given timestep.
|
---|
9 | /// </summary>
|
---|
10 | public class RungeKuttaIntegrator : IIntegrator {
|
---|
11 |
|
---|
12 |
|
---|
13 | /// <summary>
|
---|
14 | /// Integrates the specified simulation.
|
---|
15 | /// </summary>
|
---|
16 | /// <param name="sim">The sim.</param>
|
---|
17 | /// <param name="timestep">The timestep.</param>
|
---|
18 | public void Integrate(ForceSimulator sim, long timestep) {
|
---|
19 | float speedLimit = sim.SpeedLimit;
|
---|
20 | float vx, vy, v, coeff;
|
---|
21 | float[,] k, l;
|
---|
22 |
|
---|
23 | foreach (ForceItem item in sim.Items) {
|
---|
24 | coeff = timestep / item.Mass;
|
---|
25 | k = item.RungeKuttaTemp1;
|
---|
26 | l = item.RungeKuttaTemp2;
|
---|
27 | item.PreviousLocation[0] = item.Location[0];
|
---|
28 | item.PreviousLocation[1] = item.Location[1];
|
---|
29 | k[0, 0] = timestep * item.Velocity[0];
|
---|
30 | k[0, 1] = timestep * item.Velocity[1];
|
---|
31 | l[0, 0] = coeff * item.Force[0];
|
---|
32 | l[0, 1] = coeff * item.Force[1];
|
---|
33 |
|
---|
34 | // Set the position to the new predicted position
|
---|
35 | item.Location[0] += 0.5f * k[0, 0];
|
---|
36 | item.Location[1] += 0.5f * k[0, 1];
|
---|
37 | }
|
---|
38 |
|
---|
39 | // recalculate forces
|
---|
40 | sim.Accumulate();
|
---|
41 |
|
---|
42 | foreach (ForceItem item in sim.Items) {
|
---|
43 | coeff = timestep / item.Mass;
|
---|
44 | k = item.RungeKuttaTemp1;
|
---|
45 | l = item.RungeKuttaTemp2;
|
---|
46 | vx = item.Velocity[0] + .5f * l[0, 0];
|
---|
47 | vy = item.Velocity[1] + .5f * l[0, 1];
|
---|
48 | v = (float)Math.Sqrt(vx * vx + vy * vy);
|
---|
49 | if (v > speedLimit) {
|
---|
50 | vx = speedLimit * vx / v;
|
---|
51 | vy = speedLimit * vy / v;
|
---|
52 | }
|
---|
53 | k[1, 0] = timestep * vx;
|
---|
54 | k[1, 1] = timestep * vy;
|
---|
55 | l[1, 0] = coeff * item.Force[0];
|
---|
56 | l[1, 1] = coeff * item.Force[1];
|
---|
57 |
|
---|
58 | // Set the position to the new predicted position
|
---|
59 | item.Location[0] = item.PreviousLocation[0] + 0.5f * k[1, 0];
|
---|
60 | item.Location[1] = item.PreviousLocation[1] + 0.5f * k[1, 1];
|
---|
61 | }
|
---|
62 |
|
---|
63 | // recalculate forces
|
---|
64 | sim.Accumulate();
|
---|
65 |
|
---|
66 | foreach (ForceItem item in sim.Items) {
|
---|
67 | coeff = timestep / item.Mass;
|
---|
68 | k = item.RungeKuttaTemp1;
|
---|
69 | l = item.RungeKuttaTemp2;
|
---|
70 | vx = item.Velocity[0] + .5f * l[1, 0];
|
---|
71 | vy = item.Velocity[1] + .5f * l[1, 1];
|
---|
72 | v = (float)Math.Sqrt(vx * vx + vy * vy);
|
---|
73 | if (v > speedLimit) {
|
---|
74 | vx = speedLimit * vx / v;
|
---|
75 | vy = speedLimit * vy / v;
|
---|
76 | }
|
---|
77 | k[2, 0] = timestep * vx;
|
---|
78 | k[2, 1] = timestep * vy;
|
---|
79 | l[2, 0] = coeff * item.Force[0];
|
---|
80 | l[2, 1] = coeff * item.Force[1];
|
---|
81 |
|
---|
82 | // Set the position to the new predicted position
|
---|
83 | item.Location[0] = item.PreviousLocation[0] + 0.5f * k[2, 0];
|
---|
84 | item.Location[1] = item.PreviousLocation[1] + 0.5f * k[2, 1];
|
---|
85 | }
|
---|
86 |
|
---|
87 | // recalculate forces
|
---|
88 | sim.Accumulate();
|
---|
89 |
|
---|
90 | foreach (ForceItem item in sim.Items) {
|
---|
91 | coeff = timestep / item.Mass;
|
---|
92 | k = item.RungeKuttaTemp1;
|
---|
93 | l = item.RungeKuttaTemp2;
|
---|
94 | float[] p = item.PreviousLocation;
|
---|
95 | vx = item.Velocity[0] + l[2, 0];
|
---|
96 | vy = item.Velocity[1] + l[2, 1];
|
---|
97 | v = (float)Math.Sqrt(vx * vx + vy * vy);
|
---|
98 | if (v > speedLimit) {
|
---|
99 | vx = speedLimit * vx / v;
|
---|
100 | vy = speedLimit * vy / v;
|
---|
101 | }
|
---|
102 | k[3, 0] = timestep * vx;
|
---|
103 | k[3, 1] = timestep * vy;
|
---|
104 | l[3, 0] = coeff * item.Force[0];
|
---|
105 | l[3, 1] = coeff * item.Force[1];
|
---|
106 | item.Location[0] = p[0] + (k[0, 0] + k[3, 0]) / 6.0f + (k[1, 0] + k[2, 0]) / 3.0f;
|
---|
107 | item.Location[1] = p[1] + (k[0, 1] + k[3, 1]) / 6.0f + (k[1, 1] + k[2, 1]) / 3.0f;
|
---|
108 |
|
---|
109 | vx = (l[0, 0] + l[3, 0]) / 6.0f + (l[1, 0] + l[2, 0]) / 3.0f;
|
---|
110 | vy = (l[0, 1] + l[3, 1]) / 6.0f + (l[1, 1] + l[2, 1]) / 3.0f;
|
---|
111 | v = (float)Math.Sqrt(vx * vx + vy * vy);
|
---|
112 | if (v > speedLimit) {
|
---|
113 | vx = speedLimit * vx / v;
|
---|
114 | vy = speedLimit * vy / v;
|
---|
115 | }
|
---|
116 | item.Velocity[0] += vx;
|
---|
117 | item.Velocity[1] += vy;
|
---|
118 | }
|
---|
119 | }
|
---|
120 |
|
---|
121 | }
|
---|
122 | }
|
---|