7 static float delta_angle(float a, float b);
17 int ngears = (int)gears.size();
18 for(int i=0; i<ngears; i++) {
29 void Machine::add_gear(Gear *g)
34 void Machine::add_motor(int gearidx, float speed_hz)
42 void Machine::calc_meshing()
44 int ngears = (int)gears.size();
47 meshing = new bool*[ngears];
48 meshing[0] = new bool[ngears * ngears];
50 for(int i=1; i<ngears; i++) {
51 meshing[i] = meshing[i - 1] + ngears;
56 visited = new bool[ngears];
59 // we're going to need the inverse of each gear's matrix, so let's cache it here
60 Mat4 *inv_xform = (Mat4*)alloca(ngears * sizeof *inv_xform);
61 for(int i=0; i<ngears; i++) {
62 inv_xform[i] = transpose(gears[i]->get_dir_matrix());
65 for(int i=0; i<ngears; i++) {
66 for(int j=i; j<ngears; j++) {
67 meshing[i][j] = meshing[j][i] = false;
71 if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
72 // co-planar, just check Z range after inverse-transforming to the XY plane
73 Vec3 pos_i = inv_xform[i] * gears[i]->get_position();
74 Vec3 pos_j = inv_xform[j] * gears[j]->get_position();
76 if(fabs(pos_i.z - pos_j.z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
79 // Z interval match, check distance
80 float dsq = length_sq(pos_i.xy() - pos_j.xy());
82 float outer_rad_sum = gears[i]->radius + gears[j]->radius;
83 float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
85 if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
86 printf("connecting co-planar gears %d - %d\n", i, j);
87 meshing[i][j] = meshing[j][i] = true;
91 /* TODO: not co-planar
92 * - calc line of intersection between the two planes
93 * - find distance of each gear to that line
100 // fix the initial angles so that teeth mesh as best as possible
101 // should work in one pass as long as the gear train is not impossible
102 for(int i=0; i<ngears; i++) {
103 for(int j=1; j<ngears; j++) {
107 float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
108 float frac_j = fmod(gears[j]->init_angle / gears[j]->get_angular_pitch() + 1.0, 1.0);
109 float delta = frac_j - frac_i;
111 float correction = 0.5 - delta;
112 gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
118 void Machine::update_gear(int idx, float angle)
121 if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
122 fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
123 gears[idx]->name.c_str(), idx);
128 gears[idx]->set_angle(angle);
131 int ngears = (int)gears.size();
132 for(int i=0; i<ngears; i++) {
133 if(!meshing[idx][i]) continue;
136 float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
137 update_gear(i, angle * ratio);
141 void Machine::update(float dt)
143 int ngears = (int)gears.size();
145 memset(visited, 0, ngears * sizeof *visited);
146 for(size_t i=0; i<motors.size(); i++) {
147 int gidx = motors[i].drive;
148 if(gidx < 0) continue;
150 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
154 void Machine::draw() const
156 for(size_t i=0; i<gears.size(); i++) {
162 static float delta_angle(float a, float b)
164 float api = fmod(a + M_PI, 2.0 * M_PI);
165 float bpi = fmod(b + M_PI, 2.0 * M_PI);
166 return std::min(fabs(a - b), fabs(api - bpi));