--- /dev/null
+#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;
+}