X-Git-Url: http://git.mutantstargoat.com/user/nuclear/?a=blobdiff_plain;f=src%2Fmachine%2Fmachine.cc;fp=src%2Fmachine%2Fmachine.cc;h=441826e4ca08b21bc5824450e25fc2545f54e3c8;hb=a58455a92c7ecab980cbe1b7e282aeb6bfe7e889;hp=0000000000000000000000000000000000000000;hpb=21d0490039c490c6abcc80ebf2142a3376f2be43;p=laserbrain_demo

diff --git a/src/machine/machine.cc b/src/machine/machine.cc
new file mode 100644
index 0000000..441826e
--- /dev/null
+++ b/src/machine/machine.cc
@@ -0,0 +1,269 @@
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <float.h>
+#include <assert.h>
+#include "opengl.h"
+#include "machine.h"
+
+static float delta_angle(float a, float b);
+
+Machine::Machine()
+{
+	meshing = 0;
+	meshing_valid = false;
+	visited = 0;
+}
+
+Machine::~Machine()
+{
+	int ngears = (int)gears.size();
+	for(int i=0; i<ngears; i++) {
+		delete gears[i];
+	}
+
+	if(meshing) {
+		delete [] meshing[0];
+		delete [] meshing;
+	}
+	delete [] visited;
+}
+
+void Machine::add_gear(Gear *g)
+{
+	if(gearidx.find(g) != gearidx.end()) {
+		return;	// already have this gear
+	}
+	gearidx[g] = gears.size();
+	gears.push_back(g);
+	meshing_valid = false;
+}
+
+void Machine::add_motor(int gearidx, float speed_hz)
+{
+	Motor m;
+	m.drive = gearidx;
+	m.speed = speed_hz;
+	motors.push_back(m);
+}
+
+int Machine::get_gear_index(Gear *g) const
+{
+	std::map<Gear*, int>::const_iterator it = gearidx.find(g);
+	if(it == gearidx.end()) {
+		return -1;
+	}
+	return it->second;
+}
+
+void Machine::invalidate_meshing()
+{
+	meshing_valid = false;
+}
+
+void Machine::calc_meshing()
+{
+	int ngears = (int)gears.size();
+
+	if(!meshing) {
+		meshing = new bool*[ngears];
+		meshing[0] = new bool[ngears * ngears];
+
+		for(int i=1; i<ngears; i++) {
+			meshing[i] = meshing[i - 1] + ngears;
+		}
+	}
+
+	if(!visited) {
+		visited = new bool[ngears];
+	}
+
+	// we're going to need the planar position of each gear on its plane, so let's cache it
+	Vec3 *ppos = (Vec3*)alloca(ngears * sizeof *ppos);
+	for(int i=0; i<ngears; i++) {
+		ppos[i] = gears[i]->get_position();
+	}
+
+	for(int i=0; i<ngears; i++) {
+		for(int j=i; j<ngears; j++) {
+			meshing[i][j] = meshing[j][i] = false;
+
+			if(i == j || gears[i]->get_super() == gears[j] || gears[j]->get_super() == gears[i]) {
+				// don't attempt meshing if it's the same gear, or they are attached to each other
+				continue;
+			}
+
+			if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
+				// co-planar, just check Z range after inverse-transforming to the XY plane
+				if(fabs(ppos[i].z - ppos[j].z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
+					continue;
+				}
+				// Z interval match, check distance
+				float dsq = length_sq(ppos[i].xy() - ppos[j].xy());
+
+				float outer_rad_sum = gears[i]->radius + gears[j]->radius;
+				float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
+
+				if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
+					//printf("connecting co-planar gears %d - %d\n", i, j);
+					meshing[i][j] = meshing[j][i] = true;
+				}
+
+			} else {
+				/* TODO: not co-planar
+				 * - calc line of intersection between the two planes
+				 * - find distance of each gear to that line
+				 * - profit...
+				 */
+			}
+		}
+	}
+
+	// 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++) {
+		/*float rnd = gears[i]->angle + gears[i]->get_angular_pitch() / 2.0;
+		float snap = rnd - fmod(rnd, gears[i]->get_angular_pitch());
+		gears[i]->set_angle(snap);*/
+		gears[i]->set_angular_offset(0);
+	}
+
+	for(int i=0; i<ngears; i++) {
+		for(int j=i; j<ngears; j++) {
+			if(meshing[i][j]) {
+				assert(i != j);
+
+				Vec2 dir = normalize(ppos[j].xy() - ppos[i].xy());
+				float rel_angle = atan2(dir.y, dir.x);
+
+				float frac_i = fmod((gears[i]->init_angle + rel_angle) / gears[i]->get_angular_pitch() + 100.0, 1.0);
+				float frac_j = fmod((gears[j]->init_angle - rel_angle) / gears[j]->get_angular_pitch() + 100.0, 1.0);
+				assert(frac_i >= 0.0 && frac_j >= 0.0);
+				float delta = frac_j - frac_i;
+
+				float correction = 0.5 - delta;
+				float prev_offs = gears[j]->get_angular_offset();
+				gears[j]->set_angular_offset(prev_offs + correction * gears[j]->get_angular_pitch());
+			}
+		}
+	}
+
+	/*
+	printf("meshing graph\n");
+	for(int i=0; i<ngears; i++) {
+		putchar(' ');
+		for(int j=0; j<ngears; j++) {
+			printf("| %d ", meshing[i][j] ? 1 : 0);
+		}
+		printf("|\n");
+	}
+	*/
+}
+
+void Machine::update_gear(int idx, float angle)
+{
+	Gear *gear = gears[idx];
+
+	if(visited[idx]) {
+		if(delta_angle(angle, gear->angle) > 0.25 / gear->nteeth) {
+			fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
+					gear->name.c_str(), idx);
+			gear->angle = 0;
+		}
+		return;
+	}
+
+	gear->set_angle(angle);
+	visited[idx] = true;
+
+	// propagate to meshing gears (depth-first)
+	int ngears = (int)gears.size();
+	for(int i=0; i<ngears; i++) {
+		if(!meshing[idx][i]) continue;
+		assert(idx != i);
+
+		float ratio = -(float)gear->nteeth / (float)gears[i]->nteeth;
+		update_gear(i, angle * ratio);
+	}
+
+	// propagate to rigidly attached gears
+	if(gear->supergear) {
+		int supidx = gearidx[gear->supergear];
+		update_gear(supidx, angle);
+	}
+
+	int nsub = (int)gear->subgears.size();
+	for(int i=0; i<nsub; i++) {
+		int subidx = gearidx[gear->subgears[i]];
+		update_gear(subidx, angle);
+	}
+}
+
+void Machine::update(float dt)
+{
+	int ngears = (int)gears.size();
+
+	if(!meshing_valid) {
+		calc_meshing();
+		meshing_valid = true;
+	}
+
+	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();
+	}
+
+	float dcol[] = {0.4, 0.4, 0.4, 1.0};
+	float scol[] = {0, 0, 0, 0};
+	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, dcol);
+	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, scol);
+
+	glBegin(GL_QUADS);
+	glNormal3f(0, 1, 0);
+	glVertex3f(-300, -100, 300);
+	glVertex3f(300, -100, 300);
+	glVertex3f(300, -100, -300);
+	glVertex3f(-300, -100, -300);
+	glEnd();
+}
+
+Gear *Machine::intersect_gear(const Ray &ray, HitPoint *hitp) const
+{
+	Gear *res = 0;
+	HitPoint nearest;
+	nearest.dist = FLT_MAX;
+
+	for(size_t i=0; i<gears.size(); i++) {
+		Vec3 pos = gears[i]->get_global_position();
+		float rad = gears[i]->radius;
+
+		Plane plane = Plane(pos, gears[i]->axis);
+
+		HitPoint hit;
+		if(plane.intersect(ray, &hit) && hit.dist < nearest.dist &&
+				length_sq(hit.pos - pos) <= rad * rad) {
+			nearest = hit;
+			res = gears[i];
+		}
+	}
+
+	if(hitp) *hitp = nearest;
+	return res;
+}
+
+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));
+}