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

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