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         gears.push_back(g);
  34         meshing_valid = false;
  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::invalidate_meshing()
  46 {
  47         meshing_valid = false;
  48 }
  49
  50 void Machine::calc_meshing()
  51 {
  52         int ngears = (int)gears.size();
  53
  54         if(!meshing) {
  55                 meshing = new bool*[ngears];
  56                 meshing[0] = new bool[ngears * ngears];
  57
  58                 for(int i=1; i<ngears; i++) {
  59                         meshing[i] = meshing[i - 1] + ngears;
  60                 }
  61         }
  62
  63         if(!visited) {
  64                 visited = new bool[ngears];
  65         }
  66
  67         // we're going to need the planar position of each gear on its plane, so let's cache it
  68         Vec3 *ppos = (Vec3*)alloca(ngears * sizeof *ppos);
  69         for(int i=0; i<ngears; i++) {
  70                 ppos[i] = gears[i]->get_planar_position();
  71         }
  72
  73         for(int i=0; i<ngears; i++) {
  74                 for(int j=i; j<ngears; j++) {
  75                         meshing[i][j] = meshing[j][i] = false;
  76
  77                         if(i == j) continue;
  78
  79                         if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
  80                                 // co-planar, just check Z range after inverse-transforming to the XY plane
  81                                 if(fabs(ppos[i].z - ppos[j].z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
  82                                         continue;
  83                                 }
  84                                 // Z interval match, check distance
  85                                 float dsq = length_sq(ppos[i].xy() - ppos[j].xy());
  86
  87                                 float outer_rad_sum = gears[i]->radius + gears[j]->radius;
  88                                 float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
  89
  90                                 if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
  91                                         printf("connecting co-planar gears %d - %d\n", i, j);
  92                                         meshing[i][j] = meshing[j][i] = true;
  93                                 }
  94
  95                         } else {
  96                                 /* TODO: not co-planar
  97                                  * - calc line of intersection between the two planes
  98                                  * - find distance of each gear to that line
  99                                  * - profit...
 100                                  */
 101                         }
 102                 }
 103         }
 104
 105         // fix the initial angles so that teeth mesh as best as possible
 106         // should work in one pass as long as the gear train is not impossible
 107         for(int i=0; i<ngears; i++) {
 108                 gears[i]->init_angle = 0;
 109         }
 110
 111         for(int i=0; i<ngears; i++) {
 112                 for(int j=1; j<ngears; j++) {
 113                         if(meshing[i][j]) {
 114                                 assert(i != j);
 115
 116                                 Vec2 dir = normalize(ppos[j].xy() - ppos[i].xy());
 117                                 float rel_angle = atan2(dir.y, dir.x);
 118
 119                                 float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
 120                                 float frac_j = fmod((gears[j]->init_angle + rel_angle) / gears[j]->get_angular_pitch() + 1.0, 1.0);
 121                                 float delta = frac_j - frac_i;
 122
 123                                 float correction = 0.5 - delta;
 124                                 gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
 125                         }
 126                 }
 127         }
 128
 129         /*
 130         for(int i=0; i<ngears; i++) {
 131                 printf("init %d: %f\n", i, gears[i]->init_angle);
 132         }
 133         */
 134 }
 135
 136 void Machine::update_gear(int idx, float angle)
 137 {
 138         if(visited[idx]) {
 139                 if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
 140                         fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
 141                                         gears[idx]->name.c_str(), idx);
 142                         gears[idx]->angle = 0;
 143                 }
 144                 return;
 145         }
 146
 147         gears[idx]->set_angle(angle);
 148         visited[idx] = true;
 149
 150         int ngears = (int)gears.size();
 151         for(int i=0; i<ngears; i++) {
 152                 if(!meshing[idx][i]) continue;
 153                 assert(idx != i);
 154
 155                 float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
 156                 update_gear(i, angle * ratio);
 157         }
 158 }
 159
 160 void Machine::update(float dt)
 161 {
 162         int ngears = (int)gears.size();
 163
 164         if(!meshing_valid) {
 165                 calc_meshing();
 166                 meshing_valid = true;
 167         }
 168
 169         memset(visited, 0, ngears * sizeof *visited);
 170         for(size_t i=0; i<motors.size(); i++) {
 171                 int gidx = motors[i].drive;
 172                 if(gidx < 0) continue;
 173
 174                 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
 175         }
 176 }
 177
 178 void Machine::draw() const
 179 {
 180         for(size_t i=0; i<gears.size(); i++) {
 181                 gears[i]->draw();
 182         }
 183 }
 184
 185 Gear *Machine::intersect_gear(const Ray &ray, HitPoint *hitp) const
 186 {
 187         Gear *res = 0;
 188         HitPoint nearest;
 189         nearest.dist = FLT_MAX;
 190
 191         for(size_t i=0; i<gears.size(); i++) {
 192                 Vec3 pos = gears[i]->get_global_position();
 193                 float rad = gears[i]->radius;
 194
 195                 Plane plane = Plane(pos, gears[i]->axis);
 196
 197                 HitPoint hit;
 198                 if(plane.intersect(ray, &hit) && hit.dist < nearest.dist &&
 199                                 length_sq(hit.pos - pos) <= rad * rad) {
 200                         nearest = hit;
 201                         res = gears[i];
 202                 }
 203         }
 204
 205         if(hitp) *hitp = nearest;
 206         return res;
 207 }
 208
 209 static float delta_angle(float a, float b)
 210 {
 211         float api = fmod(a + M_PI, 2.0 * M_PI);
 212         float bpi = fmod(b + M_PI, 2.0 * M_PI);
 213         return std::min(fabs(a - b), fabs(api - bpi));
 214 }