[antikythera] / src / machine.cc

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include "machine.h"

static float delta_angle(float a, float b);

Machine::Machine()
{
        meshing = 0;
        meshing_valid = false;
        visited = 0;
}

Machine::~Machine()
{
        int ngears = (int)gears.size();
        for(int i=0; i<ngears; i++) {
                delete gears[i];
        }

        if(meshing) {
                delete [] meshing[0];
                delete [] meshing;
        }
        delete [] visited;
}

void Machine::add_gear(Gear *g)
{
        gears.push_back(g);
        meshing_valid = false;
}

void Machine::add_motor(int gearidx, float speed_hz)
{
        Motor m;
        m.drive = gearidx;
        m.speed = speed_hz;
        motors.push_back(m);
}

void Machine::invalidate_meshing()
{
        meshing_valid = false;
}

void Machine::calc_meshing()
{
        int ngears = (int)gears.size();

        if(!meshing) {
                meshing = new bool*[ngears];
                meshing[0] = new bool[ngears * ngears];

                for(int i=1; i<ngears; i++) {
                        meshing[i] = meshing[i - 1] + ngears;
                }
        }

        if(!visited) {
                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);
        for(int i=0; i<ngears; i++) {
                inv_xform[i] = transpose(gears[i]->get_dir_matrix());
        }

        for(int i=0; i<ngears; i++) {
                for(int j=i; j<ngears; j++) {
                        meshing[i][j] = meshing[j][i] = false;

                        if(i == j) continue;

                        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) {
                                        continue;
                                }
                                // Z interval match, check distance
                                float dsq = length_sq(pos_i.xy() - pos_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);
                                        meshing[i][j] = meshing[j][i] = true;
                                }

                        } else {
                                /* TODO: not co-planar
                                 * - calc line of intersection between the two planes
                                 * - find distance of each gear to that line
                                 * - profit...
                                 */
                        }
                }
        }

        // 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++) {
                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);
                                float delta = frac_j - frac_i;

                                float correction = 0.5 - delta;
                                gears[j]->init_angle += correction * gears[j]->get_angular_pitch();
                        }
                }
        }
}

void Machine::update_gear(int idx, float angle)
{
        if(visited[idx]) {
                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);
                }
                return;
        }

        gears[idx]->set_angle(angle);
        visited[idx] = true;

        int ngears = (int)gears.size();
        for(int i=0; i<ngears; i++) {
                if(!meshing[idx][i]) continue;
                assert(idx != i);

                float ratio = -(float)gears[idx]->nteeth / (float)gears[i]->nteeth;
                update_gear(i, angle * ratio);
        }
}

void Machine::update(float dt)
{
        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;
                if(gidx < 0) continue;

                update_gear(gidx, gears[gidx]->angle + dt * motors[i].speed);
        }
}

void Machine::draw() const
{
        for(size_t i=0; i<gears.size(); i++) {
                gears[i]->draw();
        }
}


static float delta_angle(float a, float b)
{
        float api = fmod(a + M_PI, 2.0 * M_PI);
        float bpi = fmod(b + M_PI, 2.0 * M_PI);
        return std::min(fabs(a - b), fabs(api - bpi));
}