+++ /dev/null
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-#include <float.h>
-#include <assert.h>
-#include "opengl.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)
-{
- if(gearidx.find(g) != gearidx.end()) {
- return; // already have this gear
- }
- gearidx[g] = gears.size();
- 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);
-}
-
-int Machine::get_gear_index(Gear *g) const
-{
- std::map<Gear*, int>::const_iterator it = gearidx.find(g);
- if(it == gearidx.end()) {
- return -1;
- }
- return it->second;
-}
-
-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 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++) {
- ppos[i] = gears[i]->get_position();
- }
-
- for(int i=0; i<ngears; i++) {
- for(int j=i; j<ngears; j++) {
- meshing[i][j] = meshing[j][i] = false;
-
- if(i == j || gears[i]->get_super() == gears[j] || gears[j]->get_super() == gears[i]) {
- // don't attempt meshing if it's the same gear, or they are attached to each other
- 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
- 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(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);
- 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++) {
- /*float rnd = gears[i]->angle + gears[i]->get_angular_pitch() / 2.0;
- float snap = rnd - fmod(rnd, gears[i]->get_angular_pitch());
- gears[i]->set_angle(snap);*/
- gears[i]->set_angular_offset(0);
- }
-
- for(int i=0; i<ngears; i++) {
- for(int j=i; j<ngears; j++) {
- if(meshing[i][j]) {
- assert(i != j);
-
- 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;
- float prev_offs = gears[j]->get_angular_offset();
- gears[j]->set_angular_offset(prev_offs + correction * gears[j]->get_angular_pitch());
- }
- }
- }
-
- /*
- printf("meshing graph\n");
- for(int i=0; i<ngears; i++) {
- putchar(' ');
- for(int j=0; j<ngears; j++) {
- printf("| %d ", meshing[i][j] ? 1 : 0);
- }
- printf("|\n");
- }
- */
-}
-
-void Machine::update_gear(int idx, float angle)
-{
- Gear *gear = gears[idx];
-
- if(visited[idx]) {
- if(delta_angle(angle, gear->angle) > 0.25 / gear->nteeth) {
- fprintf(stderr, "warning: trying to transmit different values to gear %s (%d)\n",
- gear->name.c_str(), idx);
- gear->angle = 0;
- }
- return;
- }
-
- gear->set_angle(angle);
- visited[idx] = true;
-
- // propagate to meshing gears (depth-first)
- int ngears = (int)gears.size();
- for(int i=0; i<ngears; i++) {
- if(!meshing[idx][i]) continue;
- assert(idx != i);
-
- float ratio = -(float)gear->nteeth / (float)gears[i]->nteeth;
- update_gear(i, angle * ratio);
- }
-
- // propagate to rigidly attached gears
- if(gear->supergear) {
- int supidx = gearidx[gear->supergear];
- update_gear(supidx, angle);
- }
-
- int nsub = (int)gear->subgears.size();
- for(int i=0; i<nsub; i++) {
- int subidx = gearidx[gear->subgears[i]];
- update_gear(subidx, angle);
- }
-}
-
-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();
- }
-
- float dcol[] = {0.4, 0.4, 0.4, 1.0};
- float scol[] = {0, 0, 0, 0};
- glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, dcol);
- glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, scol);
-
- glBegin(GL_QUADS);
- glNormal3f(0, 1, 0);
- glVertex3f(-300, -100, 300);
- glVertex3f(300, -100, 300);
- glVertex3f(300, -100, -300);
- glVertex3f(-300, -100, -300);
- glEnd();
-}
-
-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)
-{
- 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));
-}
projects / laserbrain_demo / blobdiff