#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;
}
}
if(meshing) {
- for(int i=0; i<ngears; i++) {
- delete [] meshing[i];
- }
+ 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)
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;
}
}
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_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++) {
- for(int j=1; j<ngears; j++) {
+ /*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);
- 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_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;
- gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
+ 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, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
+ if(delta_angle(angle, gear->angle) > 0.25 / gear->nteeth) {
fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
- gears[idx]->name.c_str(), idx);
+ gear->name.c_str(), idx);
+ gear->angle = 0;
}
return;
}
- gears[idx]->angle = angle;
+ 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)gears[idx]->nteeth / (float)gears[i]->nteeth;
+ 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;
}
}
-
-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);
+
+ HitPoint hit;
+ if(plane.intersect(ray, &hit) && hit.dist < nearest.dist &&
+ length_sq(hit.pos - pos) <= rad * rad) {
+ nearest = hit;
+ res = gears[i];
+ }
+ }
-static float signed_delta_angle(float a, float b)
+ 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));
}
projects / antikythera / blobdiff