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
   9 Machine::Machine()
  10 {
  11         meshing = 0;
  12         visited = 0;
  13 }
  14
  15 Machine::~Machine()
  16 {
  17         int ngears = (int)gears.size();
  18         for(int i=0; i<ngears; i++) {
  19                 delete gears[i];
  20         }
  21
  22         if(meshing) {
  23                 delete [] meshing[0];
  24                 delete [] meshing;
  25         }
  26         delete [] visited;
  27 }
  28
  29 void Machine::add_gear(Gear *g)
  30 {
  31         gears.push_back(g);
  32 }
  33
  34 void Machine::add_motor(int gearidx, float speed_hz)
  35 {
  36         Motor m;
  37         m.drive = gearidx;
  38         m.speed = speed_hz;
  39         motors.push_back(m);
  40 }
  41
  42 void Machine::calc_meshing()
  43 {
  44         int ngears = (int)gears.size();
  45
  46         if(!meshing) {
  47                 meshing = new bool*[ngears];
  48                 meshing[0] = new bool[ngears * ngears];
  49
  50                 for(int i=1; i<ngears; i++) {
  51                         meshing[i] = meshing[i - 1] + ngears;
  52                 }
  53         }
  54
  55         if(!visited) {
  56                 visited = new bool[ngears];
  57         }
  58
  59         // we're going to need the inverse of each gear's matrix, so let's cache it here
  60         Mat4 *inv_xform = (Mat4*)alloca(ngears * sizeof *inv_xform);
  61         for(int i=0; i<ngears; i++) {
  62                 inv_xform[i] = transpose(gears[i]->get_dir_matrix());
  63         }
  64
  65         for(int i=0; i<ngears; i++) {
  66                 for(int j=i; j<ngears; j++) {
  67                         meshing[i][j] = meshing[j][i] = false;
  68
  69                         if(i == j) continue;
  70
  71                         if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
  72                                 // co-planar, just check Z range after inverse-transforming to the XY plane
  73                                 Vec3 pos_i = inv_xform[i] * gears[i]->get_position();
  74                                 Vec3 pos_j = inv_xform[j] * gears[j]->get_position();
  75
  76                                 if(fabs(pos_i.z - pos_j.z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
  77                                         continue;
  78                                 }
  79                                 // Z interval match, check distance
  80                                 float dsq = length_sq(pos_i.xy() - pos_j.xy());
  81
  82                                 float outer_rad_sum = gears[i]->radius + gears[j]->radius;
  83                                 float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
  84
  85                                 if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
  86                                         printf("connecting co-planar gears %d - %d\n", i, j);
  87                                         meshing[i][j] = meshing[j][i] = true;
  88                                 }
  89
  90                         } else {
  91                                 /* TODO: not co-planar
  92                                  * - calc line of intersection between the two planes
  93                                  * - find distance of each gear to that line
  94                                  * - profit...
  95                                  */
  96                         }
  97                 }
  98         }
  99
 100         // fix the initial angles so that teeth mesh as best as possible
 101         // should work in one pass as long as the gear train is not impossible
 102         for(int i=0; i<ngears; i++) {
 103                 for(int j=1; j<ngears; j++) {
 104                         if(meshing[i][j]) {
 105                                 assert(i != j);
 106
 107                                 float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
 108                                 float frac_j = fmod(gears[j]->init_angle / gears[j]->get_angular_pitch() + 1.0, 1.0);
 109                                 float delta = frac_j - frac_i;
 110
 111                                 float correction = 0.5 - delta;
 112                                 gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
 113                         }
 114                 }
 115         }
 116 }
 117
 118 void Machine::update_gear(int idx, float angle)
 119 {
 120         if(visited[idx]) {
 121                 if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
 122                         fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
 123                                         gears[idx]->name.c_str(), idx);
 124                 }
 125                 return;
 126         }
 127
 128         gears[idx]->set_angle(angle);
 129         visited[idx] = true;
 130
 131         int ngears = (int)gears.size();
 132         for(int i=0; i<ngears; i++) {
 133                 if(!meshing[idx][i]) continue;
 134                 assert(idx != i);
 135
 136                 float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
 137                 update_gear(i, angle * ratio);
 138         }
 139 }
 140
 141 void Machine::update(float dt)
 142 {
 143         int ngears = (int)gears.size();
 144
 145         memset(visited, 0, ngears * sizeof *visited);
 146         for(size_t i=0; i<motors.size(); i++) {
 147                 int gidx = motors[i].drive;
 148                 if(gidx < 0) continue;
 149
 150                 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
 151         }
 152 }
 153
 154 void Machine::draw() const
 155 {
 156         for(size_t i=0; i<gears.size(); i++) {
 157                 gears[i]->draw();
 158         }
 159 }
 160
 161
 162 static float delta_angle(float a, float b)
 163 {
 164         float api = fmod(a + M_PI, 2.0 * M_PI);
 165         float bpi = fmod(b + M_PI, 2.0 * M_PI);
 166         return std::min(fabs(a - b), fabs(api - bpi));
 167 }