8 static float delta_angle(float a, float b);
13 meshing_valid = false;
19 int ngears = (int)gears.size();
20 for(int i=0; i<ngears; i++) {
31 void Machine::add_gear(Gear *g)
33 if(gearidx.find(g) != gearidx.end()) {
34 return; // already have this gear
36 gearidx[g] = gears.size();
38 meshing_valid = false;
41 void Machine::add_motor(int gearidx, float speed_hz)
49 void Machine::invalidate_meshing()
51 meshing_valid = false;
54 void Machine::calc_meshing()
56 int ngears = (int)gears.size();
59 meshing = new bool*[ngears];
60 meshing[0] = new bool[ngears * ngears];
62 for(int i=1; i<ngears; i++) {
63 meshing[i] = meshing[i - 1] + ngears;
68 visited = new bool[ngears];
71 // we're going to need the planar position of each gear on its plane, so let's cache it
72 Vec3 *ppos = (Vec3*)alloca(ngears * sizeof *ppos);
73 for(int i=0; i<ngears; i++) {
74 ppos[i] = gears[i]->get_position();
77 for(int i=0; i<ngears; i++) {
78 for(int j=i; j<ngears; j++) {
79 meshing[i][j] = meshing[j][i] = false;
81 if(i == j || gears[i]->get_super() == gears[j] || gears[j]->get_super() == gears[i]) {
82 // don't attempt meshing if it's the same gear, or they are attached to each other
86 if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
87 // co-planar, just check Z range after inverse-transforming to the XY plane
88 if(fabs(ppos[i].z - ppos[j].z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
91 // Z interval match, check distance
92 float dsq = length_sq(ppos[i].xy() - ppos[j].xy());
94 float outer_rad_sum = gears[i]->radius + gears[j]->radius;
95 float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
97 if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
98 //printf("connecting co-planar gears %d - %d\n", i, j);
99 meshing[i][j] = meshing[j][i] = true;
103 /* TODO: not co-planar
104 * - calc line of intersection between the two planes
105 * - find distance of each gear to that line
112 // fix the initial angles so that teeth mesh as best as possible
113 // should work in one pass as long as the gear train is not impossible
114 for(int i=0; i<ngears; i++) {
115 /*float rnd = gears[i]->angle + gears[i]->get_angular_pitch() / 2.0;
116 float snap = rnd - fmod(rnd, gears[i]->get_angular_pitch());
117 gears[i]->set_angle(snap);*/
118 gears[i]->set_angular_offset(0);
121 for(int i=0; i<ngears; i++) {
122 for(int j=i; j<ngears; j++) {
126 Vec2 dir = normalize(ppos[j].xy() - ppos[i].xy());
127 float rel_angle = atan2(dir.y, dir.x);
129 float frac_i = fmod((gears[i]->init_angle + rel_angle) / gears[i]->get_angular_pitch() + 100.0, 1.0);
130 float frac_j = fmod((gears[j]->init_angle - rel_angle) / gears[j]->get_angular_pitch() + 100.0, 1.0);
131 assert(frac_i >= 0.0 && frac_j >= 0.0);
132 float delta = frac_j - frac_i;
134 float correction = 0.5 - delta;
135 float prev_offs = gears[j]->get_angular_offset();
136 gears[j]->set_angular_offset(prev_offs + correction * gears[j]->get_angular_pitch());
142 printf("meshing graph\n");
143 for(int i=0; i<ngears; i++) {
145 for(int j=0; j<ngears; j++) {
146 printf("| %d ", meshing[i][j] ? 1 : 0);
153 void Machine::update_gear(int idx, float angle)
155 Gear *gear = gears[idx];
158 if(delta_angle(angle, gear->angle) > 0.25 / gear->nteeth) {
159 fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
160 gear->name.c_str(), idx);
166 gear->set_angle(angle);
169 // propagate to meshing gears (depth-first)
170 int ngears = (int)gears.size();
171 for(int i=0; i<ngears; i++) {
172 if(!meshing[idx][i]) continue;
175 float ratio = -(float)gear->nteeth / (float)gears[i]->nteeth;
176 update_gear(i, angle * ratio);
179 // propagate to rigidly attached gears
180 if(gear->supergear) {
181 int supidx = gearidx[gear->supergear];
182 update_gear(supidx, angle);
185 int nsub = (int)gear->subgears.size();
186 for(int i=0; i<nsub; i++) {
187 int subidx = gearidx[gear->subgears[i]];
188 update_gear(subidx, angle);
192 void Machine::update(float dt)
194 int ngears = (int)gears.size();
198 meshing_valid = true;
201 memset(visited, 0, ngears * sizeof *visited);
202 for(size_t i=0; i<motors.size(); i++) {
203 int gidx = motors[i].drive;
204 if(gidx < 0) continue;
206 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
210 void Machine::draw() const
212 for(size_t i=0; i<gears.size(); i++) {
217 Gear *Machine::intersect_gear(const Ray &ray, HitPoint *hitp) const
221 nearest.dist = FLT_MAX;
223 for(size_t i=0; i<gears.size(); i++) {
224 Vec3 pos = gears[i]->get_global_position();
225 float rad = gears[i]->radius;
227 Plane plane = Plane(pos, gears[i]->axis);
230 if(plane.intersect(ray, &hit) && hit.dist < nearest.dist &&
231 length_sq(hit.pos - pos) <= rad * rad) {
237 if(hitp) *hitp = nearest;
241 static float delta_angle(float a, float b)
243 float api = fmod(a + M_PI, 2.0 * M_PI);
244 float bpi = fmod(b + M_PI, 2.0 * M_PI);
245 return std::min(fabs(a - b), fabs(api - bpi));