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

   1 #include <stdlib.h>
   2 #include <string.h>
   3 #include <math.h>
   4 #include <float.h>
   5 #include <assert.h>
   6 #include "machine.h"
   7
   8 static float delta_angle(float a, float b);
   9
  10 Machine::Machine()
  11 {
  12         meshing = 0;
  13         meshing_valid = false;
  14         visited = 0;
  15 }
  16
  17 Machine::~Machine()
  18 {
  19         int ngears = (int)gears.size();
  20         for(int i=0; i<ngears; i++) {
  21                 delete gears[i];
  22         }
  23
  24         if(meshing) {
  25                 delete [] meshing[0];
  26                 delete [] meshing;
  27         }
  28         delete [] visited;
  29 }
  30
  31 void Machine::add_gear(Gear *g)
  32 {
  33         if(gearidx.find(g) != gearidx.end()) {
  34                 return; // already have this gear
  35         }
  36         gearidx[g] = gears.size();
  37         gears.push_back(g);
  38         meshing_valid = false;
  39 }
  40
  41 void Machine::add_motor(int gearidx, float speed_hz)
  42 {
  43         Motor m;
  44         m.drive = gearidx;
  45         m.speed = speed_hz;
  46         motors.push_back(m);
  47 }
  48
  49 void Machine::invalidate_meshing()
  50 {
  51         meshing_valid = false;
  52 }
  53
  54 void Machine::calc_meshing()
  55 {
  56         int ngears = (int)gears.size();
  57
  58         if(!meshing) {
  59                 meshing = new bool*[ngears];
  60                 meshing[0] = new bool[ngears * ngears];
  61
  62                 for(int i=1; i<ngears; i++) {
  63                         meshing[i] = meshing[i - 1] + ngears;
  64                 }
  65         }
  66
  67         if(!visited) {
  68                 visited = new bool[ngears];
  69         }
  70
  71         // we're going to need the planar position of each gear on its plane, so let's cache it
  72         Vec3 *ppos = (Vec3*)alloca(ngears * sizeof *ppos);
  73         for(int i=0; i<ngears; i++) {
  74                 ppos[i] = gears[i]->get_position();
  75         }
  76
  77         for(int i=0; i<ngears; i++) {
  78                 for(int j=i; j<ngears; j++) {
  79                         meshing[i][j] = meshing[j][i] = false;
  80
  81                         if(i == j || gears[i]->get_super() == gears[j] || gears[j]->get_super() == gears[i]) {
  82                                 // don't attempt meshing if it's the same gear, or they are attached to each other
  83                                 continue;
  84                         }
  85
  86                         if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
  87                                 // co-planar, just check Z range after inverse-transforming to the XY plane
  88                                 if(fabs(ppos[i].z - ppos[j].z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
  89                                         continue;
  90                                 }
  91                                 // Z interval match, check distance
  92                                 float dsq = length_sq(ppos[i].xy() - ppos[j].xy());
  93
  94                                 float outer_rad_sum = gears[i]->radius + gears[j]->radius;
  95                                 float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
  96
  97                                 if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
  98                                         //printf("connecting co-planar gears %d - %d\n", i, j);
  99                                         meshing[i][j] = meshing[j][i] = true;
 100                                 }
 101
 102                         } else {
 103                                 /* TODO: not co-planar
 104                                  * - calc line of intersection between the two planes
 105                                  * - find distance of each gear to that line
 106                                  * - profit...
 107                                  */
 108                         }
 109                 }
 110         }
 111
 112         // fix the initial angles so that teeth mesh as best as possible
 113         // should work in one pass as long as the gear train is not impossible
 114         for(int i=0; i<ngears; i++) {
 115                 /*float rnd = gears[i]->angle + gears[i]->get_angular_pitch() / 2.0;
 116                 float snap = rnd - fmod(rnd, gears[i]->get_angular_pitch());
 117                 gears[i]->set_angle(snap);*/
 118                 gears[i]->set_angular_offset(0);
 119         }
 120
 121         for(int i=0; i<ngears; i++) {
 122                 for(int j=i; j<ngears; j++) {
 123                         if(meshing[i][j]) {
 124                                 assert(i != j);
 125
 126                                 Vec2 dir = normalize(ppos[j].xy() - ppos[i].xy());
 127                                 float rel_angle = atan2(dir.y, dir.x);
 128
 129                                 float frac_i = fmod((gears[i]->init_angle + rel_angle) / gears[i]->get_angular_pitch() + 100.0, 1.0);
 130                                 float frac_j = fmod((gears[j]->init_angle - rel_angle) / gears[j]->get_angular_pitch() + 100.0, 1.0);
 131                                 assert(frac_i >= 0.0 && frac_j >= 0.0);
 132                                 float delta = frac_j - frac_i;
 133
 134                                 float correction = 0.5 - delta;
 135                                 float prev_offs = gears[j]->get_angular_offset();
 136                                 gears[j]->set_angular_offset(prev_offs + correction * gears[j]->get_angular_pitch());
 137                         }
 138                 }
 139         }
 140
 141         /*
 142         printf("meshing graph\n");
 143         for(int i=0; i<ngears; i++) {
 144                 putchar(' ');
 145                 for(int j=0; j<ngears; j++) {
 146                         printf("| %d ", meshing[i][j] ? 1 : 0);
 147                 }
 148                 printf("|\n");
 149         }
 150         */
 151 }
 152
 153 void Machine::update_gear(int idx, float angle)
 154 {
 155         Gear *gear = gears[idx];
 156
 157         if(visited[idx]) {
 158                 if(delta_angle(angle, gear->angle) > 0.25 / gear->nteeth) {
 159                         fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
 160                                         gear->name.c_str(), idx);
 161                         gear->angle = 0;
 162                 }
 163                 return;
 164         }
 165
 166         gear->set_angle(angle);
 167         visited[idx] = true;
 168
 169         // propagate to meshing gears (depth-first)
 170         int ngears = (int)gears.size();
 171         for(int i=0; i<ngears; i++) {
 172                 if(!meshing[idx][i]) continue;
 173                 assert(idx != i);
 174
 175                 float ratio = -(float)gear->nteeth / (float)gears[i]->nteeth;
 176                 update_gear(i, angle * ratio);
 177         }
 178
 179         // propagate to rigidly attached gears
 180         if(gear->supergear) {
 181                 int supidx = gearidx[gear->supergear];
 182                 update_gear(supidx, angle);
 183         }
 184
 185         int nsub = (int)gear->subgears.size();
 186         for(int i=0; i<nsub; i++) {
 187                 int subidx = gearidx[gear->subgears[i]];
 188                 update_gear(subidx, angle);
 189         }
 190 }
 191
 192 void Machine::update(float dt)
 193 {
 194         int ngears = (int)gears.size();
 195
 196         if(!meshing_valid) {
 197                 calc_meshing();
 198                 meshing_valid = true;
 199         }
 200
 201         memset(visited, 0, ngears * sizeof *visited);
 202         for(size_t i=0; i<motors.size(); i++) {
 203                 int gidx = motors[i].drive;
 204                 if(gidx < 0) continue;
 205
 206                 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
 207         }
 208 }
 209
 210 void Machine::draw() const
 211 {
 212         for(size_t i=0; i<gears.size(); i++) {
 213                 gears[i]->draw();
 214         }
 215 }
 216
 217 Gear *Machine::intersect_gear(const Ray &ray, HitPoint *hitp) const
 218 {
 219         Gear *res = 0;
 220         HitPoint nearest;
 221         nearest.dist = FLT_MAX;
 222
 223         for(size_t i=0; i<gears.size(); i++) {
 224                 Vec3 pos = gears[i]->get_global_position();
 225                 float rad = gears[i]->radius;
 226
 227                 Plane plane = Plane(pos, gears[i]->axis);
 228
 229                 HitPoint hit;
 230                 if(plane.intersect(ray, &hit) && hit.dist < nearest.dist &&
 231                                 length_sq(hit.pos - pos) <= rad * rad) {
 232                         nearest = hit;
 233                         res = gears[i];
 234                 }
 235         }
 236
 237         if(hitp) *hitp = nearest;
 238         return res;
 239 }
 240
 241 static float delta_angle(float a, float b)
 242 {
 243         float api = fmod(a + M_PI, 2.0 * M_PI);
 244         float bpi = fmod(b + M_PI, 2.0 * M_PI);
 245         return std::min(fabs(a - b), fabs(api - bpi));
 246 }