shadows, textures, resource managers... shaders...
[laserbrain_demo] / src / machine.cc
1 #include <stdlib.h>
2 #include <string.h>
3 #include <math.h>
4 #include <float.h>
5 #include <assert.h>
6 #include "opengl.h"
7 #include "machine.h"
8
9 static float delta_angle(float a, float b);
10
11 Machine::Machine()
12 {
13         meshing = 0;
14         meshing_valid = false;
15         visited = 0;
16 }
17
18 Machine::~Machine()
19 {
20         int ngears = (int)gears.size();
21         for(int i=0; i<ngears; i++) {
22                 delete gears[i];
23         }
24
25         if(meshing) {
26                 delete [] meshing[0];
27                 delete [] meshing;
28         }
29         delete [] visited;
30 }
31
32 void Machine::add_gear(Gear *g)
33 {
34         if(gearidx.find(g) != gearidx.end()) {
35                 return; // already have this gear
36         }
37         gearidx[g] = gears.size();
38         gears.push_back(g);
39         meshing_valid = false;
40 }
41
42 void Machine::add_motor(int gearidx, float speed_hz)
43 {
44         Motor m;
45         m.drive = gearidx;
46         m.speed = speed_hz;
47         motors.push_back(m);
48 }
49
50 void Machine::invalidate_meshing()
51 {
52         meshing_valid = false;
53 }
54
55 void Machine::calc_meshing()
56 {
57         int ngears = (int)gears.size();
58
59         if(!meshing) {
60                 meshing = new bool*[ngears];
61                 meshing[0] = new bool[ngears * ngears];
62
63                 for(int i=1; i<ngears; i++) {
64                         meshing[i] = meshing[i - 1] + ngears;
65                 }
66         }
67
68         if(!visited) {
69                 visited = new bool[ngears];
70         }
71
72         // we're going to need the planar position of each gear on its plane, so let's cache it
73         Vec3 *ppos = (Vec3*)alloca(ngears * sizeof *ppos);
74         for(int i=0; i<ngears; i++) {
75                 ppos[i] = gears[i]->get_position();
76         }
77
78         for(int i=0; i<ngears; i++) {
79                 for(int j=i; j<ngears; j++) {
80                         meshing[i][j] = meshing[j][i] = false;
81
82                         if(i == j || gears[i]->get_super() == gears[j] || gears[j]->get_super() == gears[i]) {
83                                 // don't attempt meshing if it's the same gear, or they are attached to each other
84                                 continue;
85                         }
86
87                         if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
88                                 // co-planar, just check Z range after inverse-transforming to the XY plane
89                                 if(fabs(ppos[i].z - ppos[j].z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
90                                         continue;
91                                 }
92                                 // Z interval match, check distance
93                                 float dsq = length_sq(ppos[i].xy() - ppos[j].xy());
94
95                                 float outer_rad_sum = gears[i]->radius + gears[j]->radius;
96                                 float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
97
98                                 if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
99                                         //printf("connecting co-planar gears %d - %d\n", i, j);
100                                         meshing[i][j] = meshing[j][i] = true;
101                                 }
102
103                         } else {
104                                 /* TODO: not co-planar
105                                  * - calc line of intersection between the two planes
106                                  * - find distance of each gear to that line
107                                  * - profit...
108                                  */
109                         }
110                 }
111         }
112
113         // fix the initial angles so that teeth mesh as best as possible
114         // should work in one pass as long as the gear train is not impossible
115         for(int i=0; i<ngears; i++) {
116                 /*float rnd = gears[i]->angle + gears[i]->get_angular_pitch() / 2.0;
117                 float snap = rnd - fmod(rnd, gears[i]->get_angular_pitch());
118                 gears[i]->set_angle(snap);*/
119                 gears[i]->set_angular_offset(0);
120         }
121
122         for(int i=0; i<ngears; i++) {
123                 for(int j=i; j<ngears; j++) {
124                         if(meshing[i][j]) {
125                                 assert(i != j);
126
127                                 Vec2 dir = normalize(ppos[j].xy() - ppos[i].xy());
128                                 float rel_angle = atan2(dir.y, dir.x);
129
130                                 float frac_i = fmod((gears[i]->init_angle + rel_angle) / gears[i]->get_angular_pitch() + 100.0, 1.0);
131                                 float frac_j = fmod((gears[j]->init_angle - rel_angle) / gears[j]->get_angular_pitch() + 100.0, 1.0);
132                                 assert(frac_i >= 0.0 && frac_j >= 0.0);
133                                 float delta = frac_j - frac_i;
134
135                                 float correction = 0.5 - delta;
136                                 float prev_offs = gears[j]->get_angular_offset();
137                                 gears[j]->set_angular_offset(prev_offs + correction * gears[j]->get_angular_pitch());
138                         }
139                 }
140         }
141
142         /*
143         printf("meshing graph\n");
144         for(int i=0; i<ngears; i++) {
145                 putchar(' ');
146                 for(int j=0; j<ngears; j++) {
147                         printf("| %d ", meshing[i][j] ? 1 : 0);
148                 }
149                 printf("|\n");
150         }
151         */
152 }
153
154 void Machine::update_gear(int idx, float angle)
155 {
156         Gear *gear = gears[idx];
157
158         if(visited[idx]) {
159                 if(delta_angle(angle, gear->angle) > 0.25 / gear->nteeth) {
160                         fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
161                                         gear->name.c_str(), idx);
162                         gear->angle = 0;
163                 }
164                 return;
165         }
166
167         gear->set_angle(angle);
168         visited[idx] = true;
169
170         // propagate to meshing gears (depth-first)
171         int ngears = (int)gears.size();
172         for(int i=0; i<ngears; i++) {
173                 if(!meshing[idx][i]) continue;
174                 assert(idx != i);
175
176                 float ratio = -(float)gear->nteeth / (float)gears[i]->nteeth;
177                 update_gear(i, angle * ratio);
178         }
179
180         // propagate to rigidly attached gears
181         if(gear->supergear) {
182                 int supidx = gearidx[gear->supergear];
183                 update_gear(supidx, angle);
184         }
185
186         int nsub = (int)gear->subgears.size();
187         for(int i=0; i<nsub; i++) {
188                 int subidx = gearidx[gear->subgears[i]];
189                 update_gear(subidx, angle);
190         }
191 }
192
193 void Machine::update(float dt)
194 {
195         int ngears = (int)gears.size();
196
197         if(!meshing_valid) {
198                 calc_meshing();
199                 meshing_valid = true;
200         }
201
202         memset(visited, 0, ngears * sizeof *visited);
203         for(size_t i=0; i<motors.size(); i++) {
204                 int gidx = motors[i].drive;
205                 if(gidx < 0) continue;
206
207                 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
208         }
209 }
210
211 void Machine::draw() const
212 {
213         for(size_t i=0; i<gears.size(); i++) {
214                 gears[i]->draw();
215         }
216
217         float dcol[] = {0.4, 0.4, 0.4, 1.0};
218         float scol[] = {0, 0, 0, 0};
219         glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, dcol);
220         glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, scol);
221
222         glBegin(GL_QUADS);
223         glNormal3f(0, 1, 0);
224         glVertex3f(-300, -100, 300);
225         glVertex3f(300, -100, 300);
226         glVertex3f(300, -100, -300);
227         glVertex3f(-300, -100, -300);
228         glEnd();
229 }
230
231 Gear *Machine::intersect_gear(const Ray &ray, HitPoint *hitp) const
232 {
233         Gear *res = 0;
234         HitPoint nearest;
235         nearest.dist = FLT_MAX;
236
237         for(size_t i=0; i<gears.size(); i++) {
238                 Vec3 pos = gears[i]->get_global_position();
239                 float rad = gears[i]->radius;
240
241                 Plane plane = Plane(pos, gears[i]->axis);
242
243                 HitPoint hit;
244                 if(plane.intersect(ray, &hit) && hit.dist < nearest.dist &&
245                                 length_sq(hit.pos - pos) <= rad * rad) {
246                         nearest = hit;
247                         res = gears[i];
248                 }
249         }
250
251         if(hitp) *hitp = nearest;
252         return res;
253 }
254
255 static float delta_angle(float a, float b)
256 {
257         float api = fmod(a + M_PI, 2.0 * M_PI);
258         float bpi = fmod(b + M_PI, 2.0 * M_PI);
259         return std::min(fabs(a - b), fabs(api - bpi));
260 }