Pileus Git - aweather/blob - src/gis/roam.c

   1 /*
   2  * Copyright (C) 2009 Andy Spencer <spenceal@rose-hulman.edu>
   3  *
   4  * This program is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <glib.h>
  19 #include <math.h>
  20 #include <string.h>
  21 #include "gpqueue.h"
  22 #include <GL/gl.h>
  23 #include <GL/glu.h>
  24
  25 #include "roam.h"
  26
  27 /**
  28  * TODO:
  29  *   - Remove/free unused memory
  30  *   - Optimize for memory consumption
  31  *   - Profile for computation speed
  32  *   - Implement good error algorithm
  33  *   - Target polygon count/detail
  34  */
  35
  36 /* For GPQueue comparators */
  37 static gint tri_cmp(RoamTriangle *a, RoamTriangle *b, gpointer data)
  38 {
  39         if      (a->error < b->error) return  1;
  40         else if (a->error > b->error) return -1;
  41         else                          return  0;
  42 }
  43 static gint dia_cmp(RoamDiamond *a, RoamDiamond *b, gpointer data)
  44 {
  45         if      (a->error < b->error) return -1;
  46         else if (a->error > b->error) return  1;
  47         else                          return  0;
  48 }
  49
  50 /*************
  51  * RoamPoint *
  52  *************/
  53 RoamPoint *roam_point_new(double x, double y, double z)
  54 {
  55         RoamPoint *self = g_new0(RoamPoint, 1);
  56         self->x = x;
  57         self->y = y;
  58         self->z = z;
  59         return self;
  60 }
  61 void roam_point_add_triangle(RoamPoint *self, RoamTriangle *triangle)
  62 {
  63         for (int i = 0; i < 3; i++) {
  64                 self->norm[i] *= self->refs;
  65                 self->norm[i] += triangle->norm[i];
  66         }
  67         self->refs++;
  68         for (int i = 0; i < 3; i++)
  69                 self->norm[i] /= self->refs;
  70 }
  71 void roam_point_remove_triangle(RoamPoint *self, RoamTriangle *triangle)
  72 {
  73         for (int i = 0; i < 3; i++) {
  74                 self->norm[i] *= self->refs;
  75                 self->norm[i] -= triangle->norm[i];
  76         }
  77         self->refs--;
  78         for (int i = 0; i < 3; i++)
  79                 self->norm[i] /= self->refs;
  80 }
  81
  82 /****************
  83  * RoamTriangle *
  84  ****************/
  85 RoamTriangle *roam_triangle_new(RoamPoint *l, RoamPoint *m, RoamPoint *r)
  86 {
  87         RoamTriangle *self = g_new0(RoamTriangle, 1);
  88
  89         self->p.l = l;
  90         self->p.m = m;
  91         self->p.r = r;
  92
  93         self->error = 0;
  94
  95         /* Update normal */
  96         double pa[3];
  97         double pb[3];
  98         pa[0] = self->p.l->x - self->p.m->x;
  99         pa[1] = self->p.l->y - self->p.m->y;
 100         pa[2] = self->p.l->z - self->p.m->z;
 101
 102         pb[0] = self->p.r->x - self->p.m->x;
 103         pb[1] = self->p.r->y - self->p.m->y;
 104         pb[2] = self->p.r->z - self->p.m->z;
 105
 106         self->norm[0] = pa[1] * pb[2] - pa[2] * pb[1];
 107         self->norm[1] = pa[2] * pb[0] - pa[0] * pb[2];
 108         self->norm[2] = pa[0] * pb[1] - pa[1] * pb[0];
 109
 110         double total = sqrt(self->norm[0] * self->norm[0] +
 111                            self->norm[1] * self->norm[1] +
 112                            self->norm[2] * self->norm[2]);
 113
 114         self->norm[0] /= total;
 115         self->norm[1] /= total;
 116         self->norm[2] /= total;
 117
 118         //g_message("roam_triangle_new: %p", self);
 119         return self;
 120 }
 121
 122 void roam_triangle_add(RoamTriangle *self,
 123                 RoamTriangle *left, RoamTriangle *base, RoamTriangle *right,
 124                 RoamSphere *sphere)
 125 {
 126         self->t.l = left;
 127         self->t.b = base;
 128         self->t.r = right;
 129
 130         roam_point_add_triangle(self->p.l, self);
 131         roam_point_add_triangle(self->p.m, self);
 132         roam_point_add_triangle(self->p.r, self);
 133
 134         roam_triangle_update_error(self, sphere, NULL);
 135
 136         self->handle = g_pqueue_push(sphere->triangles, self);
 137 }
 138
 139 void roam_triangle_remove(RoamTriangle *self, RoamSphere *sphere)
 140 {
 141         /* Update vertex normals */
 142         roam_point_remove_triangle(self->p.l, self);
 143         roam_point_remove_triangle(self->p.m, self);
 144         roam_point_remove_triangle(self->p.r, self);
 145
 146         g_pqueue_remove(sphere->triangles, self->handle);
 147 }
 148
 149 void roam_triangle_sync_neighbors(RoamTriangle *new, RoamTriangle *old, RoamTriangle *neigh)
 150 {
 151         if      (neigh->t.l == old) neigh->t.l = new;
 152         else if (neigh->t.b == old) neigh->t.b = new;
 153         else if (neigh->t.r == old) neigh->t.r = new;
 154         else g_assert_not_reached();
 155 }
 156
 157 gboolean roam_triangle_update_error_visible(RoamPoint *point, double *model, double *proj)
 158 {
 159         double x, y, z;
 160         int view[4] = {0,0,1,1};
 161         if (!gluProject(point->x, point->y, point->z, model, proj, view, &x, &y, &z)) {
 162                 g_warning("RoamTriangle: update_error_visible - gluProject failed");
 163                 return TRUE;
 164         }
 165         return !(x < 0 || x > 1 ||
 166                  y < 0 || y > 1 ||
 167                  z < 0 || z > 1);
 168 }
 169
 170 void roam_triangle_update_error(RoamTriangle *self, RoamSphere *sphere, GPQueue *triangles)
 171 {
 172         RoamPoint cur = {
 173                 (self->p.l->x + self->p.r->x)/2,
 174                 (self->p.l->y + self->p.r->y)/2,
 175                 (self->p.l->z + self->p.r->z)/2,
 176         };
 177         RoamPoint good = cur;
 178         roam_sphere_update_point(sphere, &good);
 179
 180         //g_message("cur = (%+5.2f %+5.2f %+5.2f)  good = (%+5.2f %+5.2f %+5.2f)",
 181         //      cur[0], cur[1], cur[2], good[0], good[1], good[2]);
 182
 183         double model[16], proj[16];
 184         int view[4];
 185         glGetError();
 186         glGetDoublev(GL_MODELVIEW_MATRIX, model);
 187         glGetDoublev(GL_PROJECTION_MATRIX, proj);
 188         glGetIntegerv(GL_VIEWPORT, view);
 189         if (glGetError() != GL_NO_ERROR) {
 190                 g_warning("RoamTriangle: update_error - glGet failed");
 191                 return;
 192         }
 193
 194         double scur[3], sgood[3];
 195         gluProject( cur.x, cur.y, cur.z, model,proj,view,  &scur[0], &scur[1], &scur[2]);
 196         gluProject(good.x,good.y,good.z, model,proj,view, &sgood[0],&sgood[1],&sgood[2]);
 197
 198         /* Not exactly correct, could be out on both sides (middle in) */
 199         if (!roam_triangle_update_error_visible(self->p.l, model, proj) &&
 200             !roam_triangle_update_error_visible(self->p.m, model, proj) &&
 201             !roam_triangle_update_error_visible(self->p.r, model, proj) &&
 202             !roam_triangle_update_error_visible(&good,     model, proj)) {
 203                 self->error = -1;
 204         } else {
 205                 self->error = sqrt((scur[0]-sgood[0])*(scur[0]-sgood[0]) +
 206                                    (scur[1]-sgood[1])*(scur[1]-sgood[1]));
 207
 208                 /* Multiply by size of triangle (todo, do this in screen space) */
 209                 double sa[3], sb[3], sc[3];
 210                 double *a = (double*)self->p.l;
 211                 double *b = (double*)self->p.m;
 212                 double *c = (double*)self->p.r;
 213                 gluProject(a[0],a[1],a[2], model,proj,view, &sa[0],&sa[1],&sa[2]);
 214                 gluProject(b[0],b[1],b[2], model,proj,view, &sb[0],&sb[1],&sb[2]);
 215                 gluProject(c[0],c[1],c[2], model,proj,view, &sc[0],&sc[1],&sc[2]);
 216                 double size = -( sa[0]*(sb[1]-sc[1]) + sb[0]*(sc[1]-sa[1]) + sc[0]*(sa[1]-sb[1]) ) / 2.0;
 217                 //g_message("%f * %f = %f", self->error, size, self->error * size);
 218                 /* Size < 0 == backface */
 219                 self->error *= size;
 220         }
 221
 222         if (triangles)
 223                 g_pqueue_priority_changed(triangles, self->handle);
 224 }
 225
 226 void roam_triangle_split(RoamTriangle *self, RoamSphere *sphere)
 227 {
 228         //g_message("roam_triangle_split: %p, e=%f\n", self, self->error);
 229
 230         sphere->polys += 2;
 231
 232         if (self != self->t.b->t.b)
 233                 roam_triangle_split(self->t.b, sphere);
 234
 235         RoamTriangle *base = self->t.b;
 236
 237         /* Calculate midpoint */
 238         RoamPoint *mid = roam_point_new(
 239                 (self->p.l->x + self->p.r->x)/2,
 240                 (self->p.l->y + self->p.r->y)/2,
 241                 (self->p.l->z + self->p.r->z)/2
 242         );
 243         roam_sphere_update_point(sphere, mid);
 244
 245         /* Add new triangles */
 246         RoamTriangle *sl = roam_triangle_new(self->p.m, mid, self->p.l); // Self Left
 247         RoamTriangle *sr = roam_triangle_new(self->p.r, mid, self->p.m); // Self Right
 248         RoamTriangle *bl = roam_triangle_new(base->p.m, mid, base->p.l); // Base Left
 249         RoamTriangle *br = roam_triangle_new(base->p.r, mid, base->p.m); // Base Right
 250
 251         /*                tri,l,  base,      r,  sphere */
 252         roam_triangle_add(sl, sr, self->t.l, br, sphere);
 253         roam_triangle_add(sr, bl, self->t.r, sl, sphere);
 254         roam_triangle_add(bl, br, base->t.l, sr, sphere);
 255         roam_triangle_add(br, sl, base->t.r, bl, sphere);
 256
 257         roam_triangle_sync_neighbors(sl, self, self->t.l);
 258         roam_triangle_sync_neighbors(sr, self, self->t.r);
 259         roam_triangle_sync_neighbors(bl, base, base->t.l);
 260         roam_triangle_sync_neighbors(br, base, base->t.r);
 261
 262         /* Remove old triangles */
 263         roam_triangle_remove(self, sphere);
 264         roam_triangle_remove(base, sphere);
 265
 266         /* Add/Remove diamonds */
 267         RoamDiamond *diamond = roam_diamond_new(self, base, sl, sr, bl, br);
 268         roam_diamond_add(diamond, sphere);
 269         roam_diamond_remove(self->diamond, sphere);
 270         roam_diamond_remove(base->diamond, sphere);
 271 }
 272
 273 void roam_triangle_draw_normal(RoamTriangle *self)
 274 {
 275         double center[] = {
 276                 (self->p.l->x + self->p.m->x + self->p.r->x)/3.0,
 277                 (self->p.l->y + self->p.m->y + self->p.r->y)/3.0,
 278                 (self->p.l->z + self->p.m->z + self->p.r->z)/3.0,
 279         };
 280         double end[] = {
 281                 center[0]+self->norm[0]/2,
 282                 center[1]+self->norm[1]/2,
 283                 center[2]+self->norm[2]/2,
 284         };
 285         glBegin(GL_LINES);
 286         glVertex3dv(center);
 287         glVertex3dv(end);
 288         glEnd();
 289 }
 290
 291 /***************
 292  * RoamDiamond *
 293  ***************/
 294 RoamDiamond *roam_diamond_new(
 295                 RoamTriangle *parent0, RoamTriangle *parent1,
 296                 RoamTriangle *kid0, RoamTriangle *kid1,
 297                 RoamTriangle *kid2, RoamTriangle *kid3)
 298 {
 299         RoamDiamond *self = g_new0(RoamDiamond, 1);
 300
 301         self->kid[0] = kid0;
 302         self->kid[1] = kid1;
 303         self->kid[2] = kid2;
 304         self->kid[3] = kid3;
 305
 306         kid0->diamond = self;
 307         kid1->diamond = self;
 308         kid2->diamond = self;
 309         kid3->diamond = self;
 310
 311         self->parent[0] = parent0;
 312         self->parent[1] = parent1;
 313
 314         return self;
 315 }
 316 void roam_diamond_add(RoamDiamond *self, RoamSphere *sphere)
 317 {
 318         self->active = TRUE;
 319         roam_diamond_update_error(self, sphere, NULL);
 320         self->handle = g_pqueue_push(sphere->diamonds, self);
 321 }
 322 void roam_diamond_remove(RoamDiamond *self, RoamSphere *sphere)
 323 {
 324         if (self && self->active) {
 325                 self->active = FALSE;
 326                 g_pqueue_remove(sphere->diamonds, self->handle);
 327         }
 328 }
 329 void roam_diamond_merge(RoamDiamond *self, RoamSphere *sphere)
 330 {
 331         //g_message("roam_diamond_merge: %p, e=%f\n", self, self->error);
 332
 333         sphere->polys -= 2;
 334
 335         /* TODO: pick the best split */
 336         RoamTriangle **kid = self->kid;
 337
 338         /* Create triangles */
 339         RoamTriangle *tri  = self->parent[0];
 340         RoamTriangle *base = self->parent[1];
 341
 342         roam_triangle_add(tri,  kid[0]->t.b, base, kid[1]->t.b, sphere);
 343         roam_triangle_add(base, kid[2]->t.b, tri,  kid[3]->t.b, sphere);
 344
 345         roam_triangle_sync_neighbors(tri,  kid[0], kid[0]->t.b);
 346         roam_triangle_sync_neighbors(tri,  kid[1], kid[1]->t.b);
 347         roam_triangle_sync_neighbors(base, kid[2], kid[2]->t.b);
 348         roam_triangle_sync_neighbors(base, kid[3], kid[3]->t.b);
 349
 350         /* Remove triangles */
 351         roam_triangle_remove(kid[0], sphere);
 352         roam_triangle_remove(kid[1], sphere);
 353         roam_triangle_remove(kid[2], sphere);
 354         roam_triangle_remove(kid[3], sphere);
 355
 356         /* Add/Remove triangles */
 357         if (tri->t.l->t.l == tri->t.r->t.r &&
 358             tri->t.l->t.l != tri && tri->diamond)
 359                 roam_diamond_add(tri->diamond, sphere);
 360
 361         if (base->t.l->t.l == base->t.r->t.r &&
 362             base->t.l->t.l != base && base->diamond)
 363                 roam_diamond_add(base->diamond, sphere);
 364
 365         /* Remove and free diamond and child triangles */
 366         roam_diamond_remove(self, sphere);
 367         g_assert(self->kid[0]->p.m == self->kid[1]->p.m &&
 368                  self->kid[1]->p.m == self->kid[2]->p.m &&
 369                  self->kid[2]->p.m == self->kid[3]->p.m);
 370         g_assert(self->kid[0]->p.m->refs == 0);
 371         g_free(self->kid[0]->p.m);
 372         g_free(self->kid[0]);
 373         g_free(self->kid[1]);
 374         g_free(self->kid[2]);
 375         g_free(self->kid[3]);
 376         g_free(self);
 377 }
 378 void roam_diamond_update_error(RoamDiamond *self, RoamSphere *sphere, GPQueue *diamonds)
 379 {
 380         roam_triangle_update_error(self->parent[0], sphere, NULL);
 381         roam_triangle_update_error(self->parent[1], sphere, NULL);
 382         self->error = MAX(self->parent[0]->error, self->parent[1]->error);
 383         if (diamonds)
 384                 g_pqueue_priority_changed(diamonds, self->handle);
 385 }
 386
 387 /**************
 388  * RoamSphere *
 389  **************/
 390 RoamSphere *roam_sphere_new(RoamTriFunc tri_func, RoamHeightFunc height_func, gpointer user_data)
 391 {
 392         RoamSphere *self = g_new0(RoamSphere, 1);
 393         self->tri_func    = tri_func;
 394         self->height_func = height_func;
 395         self->user_data   = user_data;
 396         self->polys       = 12;
 397         self->triangles   = g_pqueue_new((GCompareDataFunc)tri_cmp, NULL);
 398         self->diamonds    = g_pqueue_new((GCompareDataFunc)dia_cmp, NULL);
 399
 400         RoamPoint *vertexes[] = {
 401                 roam_point_new( 1, 1, 1), // 0
 402                 roam_point_new( 1, 1,-1), // 1
 403                 roam_point_new( 1,-1, 1), // 2
 404                 roam_point_new( 1,-1,-1), // 3
 405                 roam_point_new(-1, 1, 1), // 4
 406                 roam_point_new(-1, 1,-1), // 5
 407                 roam_point_new(-1,-1, 1), // 6
 408                 roam_point_new(-1,-1,-1), // 7
 409         };
 410         int _triangles[][2][3] = {
 411                 /*lv mv rv   ln, bn, rn */
 412                 {{3,2,0}, {10, 1, 7}}, // 0
 413                 {{0,1,3}, { 9, 0, 2}}, // 1
 414                 {{7,3,1}, {11, 3, 1}}, // 2
 415                 {{1,5,7}, { 8, 2, 4}}, // 3
 416                 {{6,7,5}, {11, 5, 3}}, // 4
 417                 {{5,4,6}, { 8, 4, 6}}, // 5
 418                 {{2,6,4}, {10, 7, 5}}, // 6
 419                 {{4,0,2}, { 9, 6, 0}}, // 7
 420                 {{4,5,1}, { 5, 9, 3}}, // 8
 421                 {{1,0,4}, { 1, 8, 7}}, // 9
 422                 {{6,2,3}, { 6,11, 0}}, // 10
 423                 {{3,7,6}, { 2,10, 4}}, // 11
 424         };
 425         RoamTriangle *triangles[12];
 426
 427         for (int i = 0; i < 8; i++)
 428                 roam_sphere_update_point(self, vertexes[i]);
 429         for (int i = 0; i < 12; i++)
 430                 triangles[i] = roam_triangle_new(
 431                         vertexes[_triangles[i][0][0]],
 432                         vertexes[_triangles[i][0][1]],
 433                         vertexes[_triangles[i][0][2]]);
 434         for (int i = 0; i < 12; i++)
 435                 roam_triangle_add(triangles[i],
 436                         triangles[_triangles[i][1][0]],
 437                         triangles[_triangles[i][1][1]],
 438                         triangles[_triangles[i][1][2]],
 439                         self);
 440
 441         return self;
 442 }
 443 static void roam_sphere_update_errors_cb(gpointer obj, GPtrArray *ptrs)
 444 {
 445         g_ptr_array_add(ptrs, obj);
 446 }
 447 void roam_sphere_update_errors(RoamSphere *self)
 448 {
 449         g_debug("RoamSphere: update_errors - polys=%d", self->polys);
 450         GPtrArray *ptrs = g_ptr_array_new();
 451         g_pqueue_foreach(self->triangles, (GFunc)roam_sphere_update_errors_cb, ptrs);
 452         for (int i = 0; i < ptrs->len; i++)
 453                 roam_triangle_update_error(ptrs->pdata[i], self, self->triangles);
 454         g_ptr_array_free(ptrs, TRUE);
 455
 456         ptrs = g_ptr_array_new();
 457         g_pqueue_foreach(self->diamonds, (GFunc)roam_sphere_update_errors_cb, ptrs);
 458         for (int i = 0; i < ptrs->len; i++)
 459                 roam_diamond_update_error(ptrs->pdata[i], self, self->diamonds);
 460         g_ptr_array_free(ptrs, TRUE);
 461 }
 462 void roam_sphere_update_point(RoamSphere *self, RoamPoint *point)
 463 {
 464         self->height_func(point, self->user_data);
 465 }
 466 void roam_sphere_split_one(RoamSphere *self)
 467 {
 468         RoamTriangle *to_split = g_pqueue_peek(self->triangles);
 469         if (!to_split) return;
 470         roam_triangle_split(to_split, self);
 471 }
 472 void roam_sphere_merge_one(RoamSphere *self)
 473 {
 474         RoamDiamond *to_merge = g_pqueue_peek(self->diamonds);
 475         if (!to_merge) return;
 476         roam_diamond_merge(to_merge, self);
 477 }
 478 gint roam_sphere_split_merge(RoamSphere *self)
 479 {
 480         gint iters = 0, max_iters = 500;
 481         gint target = 2000;
 482         while (self->polys < target && iters++ < max_iters)
 483                 roam_sphere_split_one(self);
 484         while (self->polys > target && iters++ < max_iters)
 485                 roam_sphere_merge_one(self);
 486         while (((RoamTriangle*)g_pqueue_peek(self->triangles))->error >
 487                ((RoamDiamond *)g_pqueue_peek(self->diamonds ))->error &&
 488                iters++ < max_iters) {
 489                 roam_sphere_split_one(self);
 490                 if (((RoamTriangle*)g_pqueue_peek(self->triangles))->error >
 491                     ((RoamDiamond *)g_pqueue_peek(self->diamonds ))->error)
 492                         roam_sphere_merge_one(self);
 493         }
 494         return iters;
 495 }
 496 void roam_sphere_draw(RoamSphere *self)
 497 {
 498         g_pqueue_foreach(self->triangles, (GFunc)self->tri_func, self->user_data);
 499 }
 500 void roam_sphere_draw_normals(RoamSphere *self)
 501 {
 502         g_pqueue_foreach(self->triangles, (GFunc)roam_triangle_draw_normal, NULL);
 503 }
 504 static void roam_sphere_free_tri(RoamTriangle *tri)
 505 {
 506         if (--tri->p.l->refs == 0) g_free(tri->p.l);
 507         if (--tri->p.m->refs == 0) g_free(tri->p.m);
 508         if (--tri->p.r->refs == 0) g_free(tri->p.r);
 509         g_free(tri);
 510 }
 511 void roam_sphere_free(RoamSphere *self)
 512 {
 513         /* Slow method, but it should work */
 514         while (self->polys > 12)
 515                 roam_sphere_merge_one(self);
 516         /* TODO: free points */
 517         g_pqueue_foreach(self->triangles, (GFunc)roam_sphere_free_tri, NULL);
 518         g_pqueue_free(self->triangles);
 519         g_pqueue_free(self->diamonds);
 520         g_free(self);
 521 }