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

diff --git a/src/machine.cc b/src/machine.cc
index 716fc89..913b0b0 100644
--- a/src/machine.cc
+++ b/src/machine.cc
@@ -1,15 +1,16 @@
 #include <stdlib.h>
 #include <string.h>
 #include <math.h>
+#include <float.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;
+	meshing_valid = false;
 	visited = 0;
 }
 
@@ -21,9 +22,7 @@ Machine::~Machine()
 	}
 
 	if(meshing) {
-		for(int i=0; i<ngears; i++) {
-			delete [] meshing[i];
-		}
+		delete [] meshing[0];
 		delete [] meshing;
 	}
 	delete [] visited;
@@ -32,6 +31,7 @@ Machine::~Machine()
 void Machine::add_gear(Gear *g)
 {
 	gears.push_back(g);
+	meshing_valid = false;
 }
 
 void Machine::add_motor(int gearidx, float speed_hz)
@@ -42,14 +42,21 @@ void Machine::add_motor(int gearidx, float speed_hz)
 	motors.push_back(m);
 }
 
+void Machine::invalidate_meshing()
+{
+	meshing_valid = false;
+}
+
 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];
+		meshing[0] = new bool[ngears * ngears];
+
+		for(int i=1; i<ngears; i++) {
+			meshing[i] = meshing[i - 1] + ngears;
 		}
 	}
 
@@ -57,12 +64,40 @@ void Machine::calc_meshing()
 		visited = new bool[ngears];
 	}
 
-	// let's mesh everything together just for shits and giggles
+	// 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++) {
-		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);
+		ppos[i] = gears[i]->get_planar_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) 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...
+				 */
 			}
 		}
 	}
@@ -70,14 +105,19 @@ void Machine::calc_meshing()
 	// 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++) {
+		gears[i]->init_angle = 0;
+	}
+
+	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
+				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 / 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 frac_j = fmod((gears[j]->init_angle + rel_angle) / gears[j]->get_angular_pitch() + 1.0, 1.0);
 				float delta = frac_j - frac_i;
 
 				float correction = 0.5 - delta;
@@ -85,6 +125,12 @@ void Machine::calc_meshing()
 			}
 		}
 	}
+
+	/*
+	for(int i=0; i<ngears; i++) {
+		printf("init %d: %f\n", i, gears[i]->init_angle);
+	}
+	*/
 }
 
 void Machine::update_gear(int idx, float angle)
@@ -93,11 +139,12 @@ void Machine::update_gear(int idx, float angle)
 		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);
+			gears[idx]->angle = 0;
 		}
 		return;
 	}
 
-	gears[idx]->angle = angle;
+	gears[idx]->set_angle(angle);
 	visited[idx] = true;
 
 	int ngears = (int)gears.size();
@@ -114,6 +161,11 @@ 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;
@@ -130,22 +182,33 @@ void Machine::draw() const
 	}
 }
 
-
-static float delta_angle(float a, float b)
+Gear *Machine::intersect_gear(const Ray &ray, HitPoint *hitp) const
 {
-	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));
-}
+	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);
 
-static float signed_delta_angle(float a, float b)
+		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);
-
-	if(fabs(a - b) < fabs(api - bpi)) {
-		return a - b;
-	}
-	return api - bpi;
+	return std::min(fabs(a - b), fabs(api - bpi));
 }