gears meshing, machines of gears, machines of MADNESS

[antikythera] / src / machine.cc

diff --git a/src/machine.cc b/src/machine.cc
new file mode 100644 (file)
index 0000000..716fc89
--- /dev/null
+++ b/src/machine.cc
@@ -0,0 +1,151 @@
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <assert.h>
+#include "machine.h"
+
+static float delta_angle(float a, float b);
+static float signed_delta_angle(float a, float b);
+
+Machine::Machine()
+{
+       meshing = 0;
+       visited = 0;
+}
+
+Machine::~Machine()
+{
+       int ngears = (int)gears.size();
+       for(int i=0; i<ngears; i++) {
+               delete gears[i];
+       }
+
+       if(meshing) {
+               for(int i=0; i<ngears; i++) {
+                       delete [] meshing[i];
+               }
+               delete [] meshing;
+       }
+       delete [] visited;
+}
+
+void Machine::add_gear(Gear *g)
+{
+       gears.push_back(g);
+}
+
+void Machine::add_motor(int gearidx, float speed_hz)
+{
+       Motor m;
+       m.drive = gearidx;
+       m.speed = speed_hz;
+       motors.push_back(m);
+}
+
+void Machine::calc_meshing()
+{
+       int ngears = (int)gears.size();
+
+       if(!meshing) {
+               meshing = new bool*[ngears];
+               for(int i=0; i<ngears; i++) {
+                       meshing[i] = new bool[ngears];
+               }
+       }
+
+       if(!visited) {
+               visited = new bool[ngears];
+       }
+
+       // let's mesh everything together just for shits and giggles
+       for(int i=0; i<ngears; i++) {
+               for(int j=0; j<ngears; j++) {
+                       meshing[i][j] = abs(i - j) & 1 ? true : false;
+                       if(meshing[i][j]) {
+                               printf("connecting %d - %d\n", i, j);
+                       }
+               }
+       }
+
+       // fix the initial angles so that teeth mesh as best as possible
+       // should work in one pass as long as the gear train is not impossible
+       for(int i=0; i<ngears; i++) {
+               for(int j=1; j<ngears; j++) {
+                       if(meshing[i][j]) {
+                               assert(i != j);
+
+                               float tarc = gears[i]->get_angular_pitch(); // assumed to be the same for meshing gears
+
+                               float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
+                               float frac_j = fmod(gears[j]->init_angle / gears[j]->get_angular_pitch() + 1.0, 1.0);
+                               float delta = frac_j - frac_i;
+
+                               float correction = 0.5 - delta;
+                               gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
+                       }
+               }
+       }
+}
+
+void Machine::update_gear(int idx, float angle)
+{
+       if(visited[idx]) {
+               if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
+                       fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
+                                       gears[idx]->name.c_str(), idx);
+               }
+               return;
+       }
+
+       gears[idx]->angle = angle;
+       visited[idx] = true;
+
+       int ngears = (int)gears.size();
+       for(int i=0; i<ngears; i++) {
+               if(!meshing[idx][i]) continue;
+               assert(idx != i);
+
+               float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
+               update_gear(i, angle * ratio);
+       }
+}
+
+void Machine::update(float dt)
+{
+       int ngears = (int)gears.size();
+
+       memset(visited, 0, ngears * sizeof *visited);
+       for(size_t i=0; i<motors.size(); i++) {
+               int gidx = motors[i].drive;
+               if(gidx < 0) continue;
+
+               update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
+       }
+}
+
+void Machine::draw() const
+{
+       for(size_t i=0; i<gears.size(); i++) {
+               gears[i]->draw();
+       }
+}
+
+
+static float delta_angle(float a, float b)
+{
+       float api = fmod(a + M_PI, 2.0 * M_PI);
+       float bpi = fmod(b + M_PI, 2.0 * M_PI);
+       return std::min(fabs(a - b), fabs(api - bpi));
+}
+
+
+static float signed_delta_angle(float a, float b)
+{
+       float api = fmod(a + M_PI, 2.0 * M_PI);
+       float bpi = fmod(b + M_PI, 2.0 * M_PI);
+
+       if(fabs(a - b) < fabs(api - bpi)) {
+               return a - b;
+       }
+       return api - bpi;
+}