#include <stdlib.h>
#include <string.h>
#include <math.h>
+#include <float.h>
#include <assert.h>
#include "machine.h"
Machine::Machine()
{
meshing = 0;
+ meshing_valid = false;
visited = 0;
}
void Machine::add_gear(Gear *g)
{
gears.push_back(g);
+ meshing_valid = false;
}
void Machine::add_motor(int gearidx, float speed_hz)
motors.push_back(m);
}
+void Machine::invalidate_meshing()
+{
+ meshing_valid = false;
+}
+
void Machine::calc_meshing()
{
int ngears = (int)gears.size();
visited = new bool[ngears];
}
- // we're going to need the inverse of each gear's matrix, so let's cache it here
- Mat4 *inv_xform = (Mat4*)alloca(ngears * sizeof *inv_xform);
+ // we're going to need the planar position of each gear on its plane, so let's cache it
+ Vec3 *ppos = (Vec3*)alloca(ngears * sizeof *ppos);
for(int i=0; i<ngears; i++) {
- inv_xform[i] = transpose(gears[i]->get_dir_matrix());
+ ppos[i] = gears[i]->get_planar_position();
}
for(int i=0; i<ngears; i++) {
if(1.0 - fabs(dot(gears[i]->axis, gears[j]->axis)) < 1e-5) {
// co-planar, just check Z range after inverse-transforming to the XY plane
- Vec3 pos_i = inv_xform[i] * gears[i]->get_position();
- Vec3 pos_j = inv_xform[j] * gears[j]->get_position();
-
- if(fabs(pos_i.z - pos_j.z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
+ if(fabs(ppos[i].z - ppos[j].z) > (gears[i]->thickness + gears[j]->thickness) / 2.0) {
continue;
}
// Z interval match, check distance
- float dsq = length_sq(pos_i.xy() - pos_j.xy());
+ float dsq = length_sq(ppos[i].xy() - ppos[j].xy());
float outer_rad_sum = gears[i]->radius + gears[j]->radius;
float inner_rad_sum = outer_rad_sum - gears[i]->teeth_length - gears[j]->teeth_length;
if(dsq <= outer_rad_sum * outer_rad_sum && dsq >= inner_rad_sum * inner_rad_sum) {
- printf("connecting co-planar gears %d - %d\n", i, j);
+ //printf("connecting co-planar gears %d - %d\n", i, j);
meshing[i][j] = meshing[j][i] = true;
}
// fix the initial angles so that teeth mesh as best as possible
// should work in one pass as long as the gear train is not impossible
for(int i=0; i<ngears; i++) {
+ gears[i]->init_angle = 0;
+ }
+
+ for(int i=0; i<ngears; i++) {
for(int j=1; j<ngears; j++) {
if(meshing[i][j]) {
assert(i != j);
- float frac_i = fmod(gears[i]->init_angle / gears[i]->get_angular_pitch() + 1.0, 1.0);
- float frac_j = fmod(gears[j]->init_angle / gears[j]->get_angular_pitch() + 1.0, 1.0);
+ Vec2 dir = normalize(ppos[j].xy() - ppos[i].xy());
+ float rel_angle = atan2(dir.y, dir.x);
+
+ float frac_i = fmod((gears[i]->init_angle + rel_angle) / gears[i]->get_angular_pitch() + 100.0, 1.0);
+ float frac_j = fmod((gears[j]->init_angle - rel_angle) / gears[j]->get_angular_pitch() + 100.0, 1.0);
+ assert(frac_i >= 0.0 && frac_j >= 0.0);
float delta = frac_j - frac_i;
float correction = 0.5 - delta;
}
}
}
+
+ /*
+ for(int i=0; i<ngears; i++) {
+ printf("init %d: %f\n", i, gears[i]->init_angle);
+ }
+ */
}
void Machine::update_gear(int idx, float angle)
if(delta_angle(angle, gears[idx]->angle) > 0.25 / gears[idx]->nteeth) {
fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
gears[idx]->name.c_str(), idx);
+ gears[idx]->angle = 0;
}
return;
}
{
int ngears = (int)gears.size();
+ if(!meshing_valid) {
+ calc_meshing();
+ meshing_valid = true;
+ }
+
memset(visited, 0, ngears * sizeof *visited);
for(size_t i=0; i<motors.size(); i++) {
int gidx = motors[i].drive;
}
}
+Gear *Machine::intersect_gear(const Ray &ray, HitPoint *hitp) const
+{
+ Gear *res = 0;
+ HitPoint nearest;
+ nearest.dist = FLT_MAX;
+
+ for(size_t i=0; i<gears.size(); i++) {
+ Vec3 pos = gears[i]->get_global_position();
+ float rad = gears[i]->radius;
+
+ Plane plane = Plane(pos, gears[i]->axis);
+
+ HitPoint hit;
+ if(plane.intersect(ray, &hit) && hit.dist < nearest.dist &&
+ length_sq(hit.pos - pos) <= rad * rad) {
+ nearest = hit;
+ res = gears[i];
+ }
+ }
+
+ if(hitp) *hitp = nearest;
+ return res;
+}
static float delta_angle(float a, float b)
{