7 static float delta_angle(float a, float b);
8 static float signed_delta_angle(float a, float b);
18 int ngears = (int)gears.size();
19 for(int i=0; i<ngears; i++) {
24 for(int i=0; i<ngears; i++) {
32 void Machine::add_gear(Gear *g)
37 void Machine::add_motor(int gearidx, float speed_hz)
45 void Machine::calc_meshing()
47 int ngears = (int)gears.size();
50 meshing = new bool*[ngears];
51 for(int i=0; i<ngears; i++) {
52 meshing[i] = new bool[ngears];
57 visited = new bool[ngears];
60 // let's mesh everything together just for shits and giggles
61 for(int i=0; i<ngears; i++) {
62 for(int j=0; j<ngears; j++) {
63 meshing[i][j] = abs(i - j) & 1 ? true : false;
65 printf("connecting %d - %d\n", i, j);
70 // fix the initial angles so that teeth mesh as best as possible
71 // should work in one pass as long as the gear train is not impossible
72 for(int i=0; i<ngears; i++) {
73 for(int j=1; j<ngears; j++) {
77 float tarc = gears[i]->get_angular_pitch(); // assumed to be the same for meshing gears
79 float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
80 float frac_j = fmod(gears[j]->init_angle / gears[j]->get_angular_pitch() + 1.0, 1.0);
81 float delta = frac_j - frac_i;
83 float correction = 0.5 - delta;
84 gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
90 void Machine::update_gear(int idx, float angle)
93 if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
94 fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
95 gears[idx]->name.c_str(), idx);
100 gears[idx]->angle = angle;
103 int ngears = (int)gears.size();
104 for(int i=0; i<ngears; i++) {
105 if(!meshing[idx][i]) continue;
108 float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
109 update_gear(i, angle * ratio);
113 void Machine::update(float dt)
115 int ngears = (int)gears.size();
117 memset(visited, 0, ngears * sizeof *visited);
118 for(size_t i=0; i<motors.size(); i++) {
119 int gidx = motors[i].drive;
120 if(gidx < 0) continue;
122 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
126 void Machine::draw() const
128 for(size_t i=0; i<gears.size(); i++) {
134 static float delta_angle(float a, float b)
136 float api = fmod(a + M_PI, 2.0 * M_PI);
137 float bpi = fmod(b + M_PI, 2.0 * M_PI);
138 return std::min(fabs(a - b), fabs(api - bpi));
142 static float signed_delta_angle(float a, float b)
144 float api = fmod(a + M_PI, 2.0 * M_PI);
145 float bpi = fmod(b + M_PI, 2.0 * M_PI);
147 if(fabs(a - b) < fabs(api - bpi)) {