7 static float delta_angle(float a, float b);
12 meshing_valid = false;
18 int ngears = (int)gears.size();
19 for(int i=0; i<ngears; i++) {
30 void Machine::add_gear(Gear *g)
33 meshing_valid = false;
36 void Machine::add_motor(int gearidx, float speed_hz)
44 void Machine::invalidate_meshing()
46 meshing_valid = false;
49 void Machine::calc_meshing()
51 int ngears = (int)gears.size();
54 meshing = new bool*[ngears];
55 meshing[0] = new bool[ngears * ngears];
57 for(int i=1; i<ngears; i++) {
58 meshing[i] = meshing[i - 1] + ngears;
63 visited = new bool[ngears];
66 // we're going to need the inverse of each gear's matrix, so let's cache it here
67 Mat4 *inv_xform = (Mat4*)alloca(ngears * sizeof *inv_xform);
68 for(int i=0; i<ngears; i++) {
69 inv_xform[i] = transpose(gears[i]->get_dir_matrix());
72 for(int i=0; i<ngears; i++) {
73 for(int j=i; j<ngears; j++) {
74 meshing[i][j] = meshing[j][i] = false;
78 if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
79 // co-planar, just check Z range after inverse-transforming to the XY plane
80 Vec3 pos_i = inv_xform[i] * gears[i]->get_position();
81 Vec3 pos_j = inv_xform[j] * gears[j]->get_position();
83 if(fabs(pos_i.z - pos_j.z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
86 // Z interval match, check distance
87 float dsq = length_sq(pos_i.xy() - pos_j.xy());
89 float outer_rad_sum = gears[i]->radius + gears[j]->radius;
90 float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
92 if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
93 printf("connecting co-planar gears %d - %d\n", i, j);
94 meshing[i][j] = meshing[j][i] = true;
98 /* TODO: not co-planar
99 * - calc line of intersection between the two planes
100 * - find distance of each gear to that line
107 // fix the initial angles so that teeth mesh as best as possible
108 // should work in one pass as long as the gear train is not impossible
109 for(int i=0; i<ngears; i++) {
110 for(int j=1; j<ngears; j++) {
114 float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
115 float frac_j = fmod(gears[j]->init_angle / gears[j]->get_angular_pitch() + 1.0, 1.0);
116 float delta = frac_j - frac_i;
118 float correction = 0.5 - delta;
119 gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
125 void Machine::update_gear(int idx, float angle)
128 if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
129 fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
130 gears[idx]->name.c_str(), idx);
135 gears[idx]->set_angle(angle);
138 int ngears = (int)gears.size();
139 for(int i=0; i<ngears; i++) {
140 if(!meshing[idx][i]) continue;
143 float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
144 update_gear(i, angle * ratio);
148 void Machine::update(float dt)
150 int ngears = (int)gears.size();
154 meshing_valid = true;
157 memset(visited, 0, ngears * sizeof *visited);
158 for(size_t i=0; i<motors.size(); i++) {
159 int gidx = motors[i].drive;
160 if(gidx < 0) continue;
162 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
166 void Machine::draw() const
168 for(size_t i=0; i<gears.size(); i++) {
174 static float delta_angle(float a, float b)
176 float api = fmod(a + M_PI, 2.0 * M_PI);
177 float bpi = fmod(b + M_PI, 2.0 * M_PI);
178 return std::min(fabs(a - b), fabs(api - bpi));