X-Git-Url: http://git.mutantstargoat.com/user/nuclear/?p=antikythera;a=blobdiff_plain;f=src%2Fmachine.cc;fp=src%2Fmachine.cc;h=716fc890568810b5e20678d65df950d0ede29e16;hp=0000000000000000000000000000000000000000;hb=a88a9ac53952e1bb3768b147a043c19392e3d5d1;hpb=ae60a8cb1a30e204e7f60969fe6245e510cca0ff

diff --git a/src/machine.cc b/src/machine.cc
new file mode 100644
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;
+}