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