lazy meshing

[laserbrain_demo] / src / machine.cc

diff --git a/src/machine.cc b/src/machine.cc
index 716fc89..274362a 100644 (file)
--- a/src/machine.cc
+++ b/src/machine.cc
@@ -5,11 +5,11 @@
 #include "machine.h"
 
 static float delta_angle(float a, float b);
-static float signed_delta_angle(float a, float b);
 
 Machine::Machine()
 {
        meshing = 0;
+       meshing_valid = false;
        visited = 0;
 }
 
@@ -21,9 +21,7 @@ Machine::~Machine()
        }
 
        if(meshing) {
-               for(int i=0; i<ngears; i++) {
-                       delete [] meshing[i];
-               }
+               delete [] meshing[0];
                delete [] meshing;
        }
        delete [] visited;
@@ -32,6 +30,7 @@ Machine::~Machine()
 void Machine::add_gear(Gear *g)
 {
        gears.push_back(g);
+       meshing_valid = false;
 }
 
 void Machine::add_motor(int gearidx, float speed_hz)
@@ -42,14 +41,21 @@ 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();
 
        if(!meshing) {
                meshing = new bool*[ngears];
-               for(int i=0; i<ngears; i++) {
-                       meshing[i] = new bool[ngears];
+               meshing[0] = new bool[ngears * ngears];
+
+               for(int i=1; i<ngears; i++) {
+                       meshing[i] = meshing[i - 1] + ngears;
                }
        }
 
@@ -57,12 +63,43 @@ void Machine::calc_meshing()
                visited = new bool[ngears];
        }
 
-       // let's mesh everything together just for shits and giggles
+       // 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++) {
-               for(int j=0; j<ngears; j++) {
-                       meshing[i][j] = abs(i - j) & 1 ? true : false;
-                       if(meshing[i][j]) {
-                               printf("connecting %d - %d\n", i, j);
+               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...
+                                */
                        }
                }
        }
@@ -74,8 +111,6 @@ void Machine::calc_meshing()
                        if(meshing[i][j]) {
                                assert(i != j);
 
-                               float tarc = gears[i]->get_angular_pitch(); // assumed to be the same for meshing gears
-
                                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;
@@ -97,7 +132,7 @@ void Machine::update_gear(int idx, float angle)
                return;
        }
 
-       gears[idx]->angle = angle;
+       gears[idx]->set_angle(angle);
        visited[idx] = true;
 
        int ngears = (int)gears.size();
@@ -114,6 +149,11 @@ 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;
@@ -137,15 +177,3 @@ static float delta_angle(float a, float b)
        float bpi = fmod(b + M_PI, 2.0 * M_PI);
        return std::min(fabs(a - b), fabs(api - bpi));
 }
-
-
-static float signed_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);
-
-       if(fabs(a - b) < fabs(api - bpi)) {
-               return a - b;
-       }
-       return api - bpi;
-}