inochi2d.phys.system source code

1 /*
2     Copyright © 2022, Inochi2D Project
3     Distributed under the 2-Clause BSD License, see LICENSE file.
4 
5     Author Asahi Lina
6 */
7 module inochi2d.phys.system;
8 import inochi2d;
9 import std.math : isFinite;
10 
11 abstract class PhysicsSystem {
12 private:
13     ulong[float*] variableMap;
14     float*[] refs;
15     float[] derivative;
16 
17     float t;
18 
19 protected:
20     /**
21         Add a float variable to the simulation
22     */
23     ulong addVariable(float* var) {
24         ulong index = refs.length;
25 
26         variableMap[var] = index;
27         refs ~= var;
28 
29         return index;
30     }
31 
32     /**
33         Add a vec2 variable to the simulation
34     */
35     ulong addVariable(vec2* var) {
36         ulong index = addVariable(&(var.vector[0]));
37         addVariable(&(var.vector[1]));
38         return index;
39     }
40 
41     /**
42         Set the derivative of a variable (solver input) by index
43     */
44     void setD(ulong index, float value) {
45         derivative[index] = value;
46     }
47 
48     /**
49         Set the derivative of a float variable (solver input)
50     */
51     void setD(ref float var, float value) {
52         ulong index = variableMap[&var];
53         setD(variableMap[&var], value);
54     }
55 
56     /**
57         Set the derivative of a vec2 variable (solver input)
58     */
59     void setD(ref vec2 var, vec2 value) {
60         setD(var.vector[0], value.x);
61         setD(var.vector[1], value.y);
62     }
63 
64     float[] getState() {
65         float[] vals;
66 
67         foreach(idx, ptr; refs) {
68             vals ~= *ptr;
69         }
70 
71         return vals;
72     }
73 
74     void setState(float[] vals) {
75         foreach(idx, ptr; refs) {
76             *ptr = vals[idx];
77         }
78     }
79 
80     /**
81         Evaluate the simulation at a given time
82     */
83     abstract void eval(float t);
84 
85 public:
86     /**
87         Run a simulation tick (Runge-Kutta method)
88     */
89     void tick(float h) {
90         float[] cur = getState();
91         float[] tmp;
92         tmp.length = cur.length;
93         derivative.length = cur.length;
94         foreach(i; 0..derivative.length)
95             derivative[i] = 0;
96 
97         eval(t);
98         float[] k1 = derivative.dup;
99 
100         foreach(i; 0..cur.length)
101             *refs[i] = cur[i] + h * k1[i] / 2f;
102         eval(t + h / 2f);
103         float[] k2 = derivative.dup;
104 
105         foreach(i; 0..cur.length)
106             *refs[i] = cur[i] + h * k2[i] / 2f;
107         eval(t + h / 2f);
108         float[] k3 = derivative.dup;
109 
110         foreach(i; 0..cur.length)
111             *refs[i] = cur[i] + h * k3[i];
112         eval(t + h);
113         float[] k4 = derivative.dup;
114 
115         foreach(i; 0..cur.length) {
116             *refs[i] = cur[i] + h * (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) / 6f;
117             if (!isFinite(*refs[i])) {
118                 // Simulation failed, revert
119                 foreach(j; 0..cur.length)
120                     *refs[j] = cur[j];
121                 break;
122             }
123         }
124 
125         t += h;
126     }
127 
128 public:
129 
130     /**
131         Updates the anchor for the physics system
132     */
133     abstract void updateAnchor();
134 
135     /**
136         Draw debug
137     */
138     abstract void drawDebug(mat4 trans = mat4.identity);
139 }
140