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