git.mutantstargoat.com Git - antikythera/blob - src/machine.cc

   1 #include <stdlib.h>
   2 #include <string.h>
   3 #include <math.h>
   4 #include <assert.h>
   5 #include "machine.h"
   6
   7 static float delta_angle(float a, float b);
   8 static float signed_delta_angle(float a, float b);
   9
  10 Machine::Machine()
  11 {
  12         meshing = 0;
  13         visited = 0;
  14 }
  15
  16 Machine::~Machine()
  17 {
  18         int ngears = (int)gears.size();
  19         for(int i=0; i<ngears; i++) {
  20                 delete gears[i];
  21         }
  22
  23         if(meshing) {
  24                 for(int i=0; i<ngears; i++) {
  25                         delete [] meshing[i];
  26                 }
  27                 delete [] meshing;
  28         }
  29         delete [] visited;
  30 }
  31
  32 void Machine::add_gear(Gear *g)
  33 {
  34         gears.push_back(g);
  35 }
  36
  37 void Machine::add_motor(int gearidx, float speed_hz)
  38 {
  39         Motor m;
  40         m.drive = gearidx;
  41         m.speed = speed_hz;
  42         motors.push_back(m);
  43 }
  44
  45 void Machine::calc_meshing()
  46 {
  47         int ngears = (int)gears.size();
  48
  49         if(!meshing) {
  50                 meshing = new bool*[ngears];
  51                 for(int i=0; i<ngears; i++) {
  52                         meshing[i] = new bool[ngears];
  53                 }
  54         }
  55
  56         if(!visited) {
  57                 visited = new bool[ngears];
  58         }
  59
  60         // let's mesh everything together just for shits and giggles
  61         for(int i=0; i<ngears; i++) {
  62                 for(int j=0; j<ngears; j++) {
  63                         meshing[i][j] = abs(i - j) & 1 ? true : false;
  64                         if(meshing[i][j]) {
  65                                 printf("connecting %d - %d\n", i, j);
  66                         }
  67                 }
  68         }
  69
  70         // fix the initial angles so that teeth mesh as best as possible
  71         // should work in one pass as long as the gear train is not impossible
  72         for(int i=0; i<ngears; i++) {
  73                 for(int j=1; j<ngears; j++) {
  74                         if(meshing[i][j]) {
  75                                 assert(i != j);
  76
  77                                 float tarc = gears[i]->get_angular_pitch(); // assumed to be the same for meshing gears
  78
  79                                 float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
  80                                 float frac_j = fmod(gears[j]->init_angle / gears[j]->get_angular_pitch() + 1.0, 1.0);
  81                                 float delta = frac_j - frac_i;
  82
  83                                 float correction = 0.5 - delta;
  84                                 gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
  85                         }
  86                 }
  87         }
  88 }
  89
  90 void Machine::update_gear(int idx, float angle)
  91 {
  92         if(visited[idx]) {
  93                 if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
  94                         fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
  95                                         gears[idx]->name.c_str(), idx);
  96                 }
  97                 return;
  98         }
  99
 100         gears[idx]->angle = angle;
 101         visited[idx] = true;
 102
 103         int ngears = (int)gears.size();
 104         for(int i=0; i<ngears; i++) {
 105                 if(!meshing[idx][i]) continue;
 106                 assert(idx != i);
 107
 108                 float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
 109                 update_gear(i, angle * ratio);
 110         }
 111 }
 112
 113 void Machine::update(float dt)
 114 {
 115         int ngears = (int)gears.size();
 116
 117         memset(visited, 0, ngears * sizeof *visited);
 118         for(size_t i=0; i<motors.size(); i++) {
 119                 int gidx = motors[i].drive;
 120                 if(gidx < 0) continue;
 121
 122                 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
 123         }
 124 }
 125
 126 void Machine::draw() const
 127 {
 128         for(size_t i=0; i<gears.size(); i++) {
 129                 gears[i]->draw();
 130         }
 131 }
 132
 133
 134 static float delta_angle(float a, float b)
 135 {
 136         float api = fmod(a + M_PI, 2.0 * M_PI);
 137         float bpi = fmod(b + M_PI, 2.0 * M_PI);
 138         return std::min(fabs(a - b), fabs(api - bpi));
 139 }
 140
 141
 142 static float signed_delta_angle(float a, float b)
 143 {
 144         float api = fmod(a + M_PI, 2.0 * M_PI);
 145         float bpi = fmod(b + M_PI, 2.0 * M_PI);
 146
 147         if(fabs(a - b) < fabs(api - bpi)) {
 148                 return a - b;
 149         }
 150         return api - bpi;
 151 }