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)
34 meshing_valid = false;
37 void Machine::add_motor(int gearidx, float speed_hz)
45 void Machine::invalidate_meshing()
47 meshing_valid = false;
50 void Machine::calc_meshing()
52 int ngears = (int)gears.size();
55 meshing = new bool*[ngears];
56 meshing[0] = new bool[ngears * ngears];
58 for(int i=1; i<ngears; i++) {
59 meshing[i] = meshing[i - 1] + ngears;
64 visited = new bool[ngears];
67 // we're going to need the planar position of each gear on its plane, so let's cache it
68 Vec3 *ppos = (Vec3*)alloca(ngears * sizeof *ppos);
69 for(int i=0; i<ngears; i++) {
70 ppos[i] = gears[i]->get_planar_position();
73 for(int i=0; i<ngears; i++) {
74 for(int j=i; j<ngears; j++) {
75 meshing[i][j] = meshing[j][i] = false;
79 if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
80 // co-planar, just check Z range after inverse-transforming to the XY plane
81 if(fabs(ppos[i].z - ppos[j].z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
84 // Z interval match, check distance
85 float dsq = length_sq(ppos[i].xy() - ppos[j].xy());
87 float outer_rad_sum = gears[i]->radius + gears[j]->radius;
88 float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
90 if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
91 printf("connecting co-planar gears %d - %d\n", i, j);
92 meshing[i][j] = meshing[j][i] = true;
96 /* TODO: not co-planar
97 * - calc line of intersection between the two planes
98 * - find distance of each gear to that line
105 // fix the initial angles so that teeth mesh as best as possible
106 // should work in one pass as long as the gear train is not impossible
107 for(int i=0; i<ngears; i++) {
108 gears[i]->init_angle = 0;
111 for(int i=0; i<ngears; i++) {
112 for(int j=1; j<ngears; j++) {
116 Vec2 dir = normalize(ppos[j].xy() - ppos[i].xy());
117 float rel_angle = atan2(dir.y, dir.x);
119 float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
120 float frac_j = fmod((gears[j]->init_angle + rel_angle) / gears[j]->get_angular_pitch() + 1.0, 1.0);
121 float delta = frac_j - frac_i;
123 float correction = 0.5 - delta;
124 gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
130 for(int i=0; i<ngears; i++) {
131 printf("init %d: %f\n", i, gears[i]->init_angle);
136 void Machine::update_gear(int idx, float angle)
139 if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
140 fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
141 gears[idx]->name.c_str(), idx);
142 gears[idx]->angle = 0;
147 gears[idx]->set_angle(angle);
150 int ngears = (int)gears.size();
151 for(int i=0; i<ngears; i++) {
152 if(!meshing[idx][i]) continue;
155 float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
156 update_gear(i, angle * ratio);
160 void Machine::update(float dt)
162 int ngears = (int)gears.size();
166 meshing_valid = true;
169 memset(visited, 0, ngears * sizeof *visited);
170 for(size_t i=0; i<motors.size(); i++) {
171 int gidx = motors[i].drive;
172 if(gidx < 0) continue;
174 update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
178 void Machine::draw() const
180 for(size_t i=0; i<gears.size(); i++) {
185 Gear *Machine::intersect_gear(const Ray &ray, HitPoint *hitp) const
189 nearest.dist = FLT_MAX;
191 for(size_t i=0; i<gears.size(); i++) {
192 Vec3 pos = gears[i]->get_global_position();
193 float rad = gears[i]->radius;
195 Plane plane = Plane(pos, gears[i]->axis);
198 if(plane.intersect(ray, &hit) && hit.dist < nearest.dist &&
199 length_sq(hit.pos - pos) <= rad * rad) {
205 if(hitp) *hitp = nearest;
209 static float delta_angle(float a, float b)
211 float api = fmod(a + M_PI, 2.0 * M_PI);
212 float bpi = fmod(b + M_PI, 2.0 * M_PI);
213 return std::min(fabs(a - b), fabs(api - bpi));