source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/third_party/vrippack-0.31/src/vrip/rangePly.cc @ 37

Last change on this file since 37 was 37, checked in by (none), 14 years ago

Added original make3d
File size: 54.8 KB
Line 
1/*
2
3Brian Curless
4
5Computer Graphics Laboratory
6Stanford University
7
8---------------------------------------------------------------------
9
10Copyright (1997) The Board of Trustees of the Leland Stanford Junior
11University. Except for commercial resale, lease, license or other
12commercial transactions, permission is hereby given to use, copy,
13modify this software for academic purposes only.  No part of this
14software or any derivatives thereof may be used in the production of
15computer models for resale or for use in a commercial
16product. STANFORD MAKES NO REPRESENTATIONS OR WARRANTIES OF ANY KIND
17CONCERNING THIS SOFTWARE.  No support is implied or provided.
18
19*/
20
21
22#include <stdio.h>
23#include <ply.h>
24#include <stdlib.h>
25#include <strings.h>
26
27#include "rangePly.h"
28#include "vripGlobals.h"
29#include "defines.h"
30#include "plyio.h"
31
32#ifndef RAD
33#define RAD(x) ((x)*M_PI/180)
34#endif
35
36#ifdef linux
37#include <float.h>
38#endif
39
40struct PlyVertex {
41  float x,y,z;
42  float confidence;
43  float intensity;
44  unsigned char red,grn,blu;
45};
46
47PlyProperty vert_std_props[] = {
48  {"x", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,x), 0, 0, 0, 0},
49  {"y", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,y), 0, 0, 0, 0},
50  {"z", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,z), 0, 0, 0, 0},
51  {"confidence", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,confidence), 0, 0, 0,0},
52  {"intensity", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,intensity), 0, 0, 0,0},
53  {"diffuse_red", PLY_UCHAR, PLY_UCHAR, offsetof(PlyVertex,red), 0, 0, 0, 0},
54  {"diffuse_green", PLY_UCHAR, PLY_UCHAR, offsetof(PlyVertex,grn), 0, 0, 0, 0},
55  {"diffuse_blue", PLY_UCHAR, PLY_UCHAR, offsetof(PlyVertex,blu), 0, 0, 0, 0},
56  {"std_dev", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,confidence), 0, 0, 0,0},
57};
58
59
60struct RangePnt {
61  unsigned char num_pts;
62  int *pts;
63};
64
65/* list of property information for a range data point */
66PlyProperty range_props[] = {
67  {"vertex_indices", PLY_INT, PLY_INT, offsetof(RangePnt,pts),
68   1, PLY_UCHAR, PLY_UCHAR, offsetof(RangePnt,num_pts)},
69};
70
71
72
73
74static float RANGE_DATA_SIGMA_FACTOR = 4;
75static float RANGE_DATA_MIN_INTENSITY = 0.05;
76
77
78
79static void trimMesh(Mesh *mesh);
80static void addTriangleUseNormal(Mesh *mesh, int vin1, int vin2, int vin3, 
81                                 float minDot, Vec3f &viewDir, int connectAll);
82static void addTriangleUseLength(Mesh *mesh, int vin1, int vin2, int vin3, 
83                                 float maxLength, int connectAll);
84
85
86RangeGrid::~RangeGrid()
87{
88    delete [] coords;
89
90    delete [] indices;
91
92    if (num_obj_info > 0) {
93        for (int i = 0; i < num_obj_info; i++)
94            delete [] obj_info[i];
95        delete [] obj_info;
96    }
97
98    if (hasConfidence)
99        delete [] confidence;
100
101    if (hasColor)
102        delete [] matDiff;
103
104    if (hasIntensity)
105        delete [] intensity;
106
107}
108
109
110void
111set_range_data_sigma_factor(float factor)
112{
113    RANGE_DATA_SIGMA_FACTOR = factor;
114}
115
116
117float
118get_range_data_sigma_factor()
119{
120    return RANGE_DATA_SIGMA_FACTOR;
121}
122
123
124
125void
126set_range_data_min_intensity(float intensity)
127{
128    RANGE_DATA_MIN_INTENSITY = intensity;
129}
130
131
132float
133get_range_data_min_intensity()
134{
135    return RANGE_DATA_MIN_INTENSITY;
136}
137
138
139
140int 
141is_range_grid_file(const char *filename)
142{
143  int i;
144  PlyFile *ply;
145  int nelems;
146  char **elist;
147  int file_type;
148  float version;
149
150  ply = ply_open_for_reading(filename, &nelems, &elist, &file_type, &version);
151  if (ply == NULL)
152    return 0;
153  ply_close(ply);
154 
155  for (i = 0; i < nelems; i++) {
156      if (!strcmp(elist[i], "range_grid"))
157          return 1;
158  }
159
160  return 0;
161}
162
163
164/******************************************************************************
165Read range data from a PLY file.
166
167Entry:
168  name - name of PLY file to read from
169
170Exit:
171  returns pointer to data, or NULL if it couldn't read from file
172******************************************************************************/
173
174RangeGrid *
175readRangeGrid(const char *name)
176{
177  int i,j,k,index, best_index;
178  PlyFile *ply;
179  RangeGrid *rangeGrid = NULL;
180  char **obj_info;
181  int num_obj_info;
182  int num_elems;
183  int nprops;
184  int nelems;
185  char **elist;
186  int file_type;
187  float version;
188  PlyProperty **plist;
189  int num_rows,num_cols;
190  RangePnt range_pnt;
191  PlyVertex vert;
192  char *elem_name;
193  int get_std_dev = 0;
194  int get_confidence = 0;
195  int get_intensity = 0;
196  int get_color = 0;
197  int has_red = 0;
198  int has_green = 0;
199  int has_blue = 0;
200  int is_warped = 0;
201  char temp[PATH_MAX];
202  int isRightMirrorOpen = 1;
203  int found_mirror = 0;
204  float conf,std;
205  float min_std,max_std,max;
206  float avg_std = 0;
207  float lgincr;
208
209  if (name == NULL) {
210     ply = ply_read (stdin, &nelems, &elist);
211  } else {
212     ply = ply_open_for_reading(name, &nelems, &elist, &file_type, &version);
213  }
214
215  if (ply == NULL) {
216    return (NULL);
217  }
218
219  int isRangeGrid = 0;
220  for (i = 0; i < nelems; i++) {
221     if (!strcmp(elist[i], "range_grid")) {
222        isRangeGrid = 1;
223        break;
224     }
225  }
226
227  if (!isRangeGrid)
228     return NULL;
229
230  /* parse the obj_info */
231
232  obj_info = ply_get_obj_info (ply, &num_obj_info);
233  for (i = 0; i < num_obj_info; i++) {
234    if (strstr(obj_info[i], "num_cols"))
235        sscanf(obj_info[i], "%s%d", temp, &num_cols);
236    if (strstr(obj_info[i], "num_rows"))
237        sscanf(obj_info[i], "%s%d", temp, &num_rows);
238    if (strstr(obj_info[i], "is_warped"))
239        sscanf(obj_info[i], "%s%d", temp, &is_warped);
240    if (strstr(obj_info[i], "optimum_std_dev"))
241        sscanf(obj_info[i], "%s%f", temp, &avg_std);
242    if (strstr(obj_info[i], "echo_lgincr"))
243        sscanf(obj_info[i], "%s%f", temp, &lgincr);
244    if (strstr(obj_info[i], "is_right_mirror_open")) {
245        found_mirror = TRUE;
246        sscanf(obj_info[i], "%s%d", temp, &isRightMirrorOpen);
247    }
248  }
249
250  EdgeLength = lgincr;
251
252  if (is_warped && !found_mirror && Warn)
253      printf("Couldn't tell which mirror was open.  Right mirror assumed.\n");
254
255  min_std = avg_std / RANGE_DATA_SIGMA_FACTOR;
256  max_std = avg_std * RANGE_DATA_SIGMA_FACTOR;
257
258  /* set up the range data structure */
259  rangeGrid = new RangeGrid;
260  rangeGrid->nlg = num_rows;
261  rangeGrid->nlt = num_cols;
262  rangeGrid->intensity = NULL;
263  rangeGrid->confidence = NULL;
264  rangeGrid->matDiff = NULL;
265  rangeGrid->hasColor = 0;
266  rangeGrid->hasIntensity = 0;
267  rangeGrid->hasConfidence = 0;
268  rangeGrid->multConfidence = 0;
269
270  if (!is_warped) {
271      rangeGrid->viewDir.setValue(0, 0, -1);
272  } else {
273      if (isRightMirrorOpen) {
274          rangeGrid->viewDir.setValue(-sin(30*M_PI/180), 0, -cos(30*M_PI/180));
275      } else {
276          rangeGrid->viewDir.setValue(sin(30*M_PI/180), 0, -cos(30*M_PI/180));
277      }
278  }
279
280  rangeGrid->num_obj_info = num_obj_info;
281  rangeGrid->obj_info = new char*[num_obj_info];
282  for (i = 0; i < num_obj_info; i++) {
283      rangeGrid->obj_info[i] = new char[strlen(obj_info[i])+1];
284      strcpy(rangeGrid->obj_info[i], obj_info[i]);
285  }
286
287
288  rangeGrid->isWarped = is_warped;
289  rangeGrid->isRightMirrorOpen = isRightMirrorOpen;
290
291  /* see if we've got both vertex and range_grid data */
292
293  plist = ply_get_element_description (ply, "vertex", &num_elems, &nprops);
294  if (plist == NULL) {
295    fprintf (stderr, "file doesn't contain vertex data\n");
296    return (NULL);
297  }
298  rangeGrid->numOrigSamples = num_elems;
299  rangeGrid->numSamples = num_elems;
300  plist = ply_get_element_description (ply, "range_grid", &num_elems, &nprops);
301  if (plist == NULL) {
302    fprintf (stderr, "file doesn't contain range_grid data\n");
303    return (NULL);
304  }
305
306  /* read in the range data */
307
308  for (i = 0; i < nelems; i++) {
309
310    elem_name = elist[i];
311    plist = ply_get_element_description (ply, elem_name, &num_elems, &nprops);
312
313    if (equal_strings ("vertex", elem_name)) {
314
315      /* see if the file contains intensities */
316      for (j = 0; j < nprops; j++) {
317        if (equal_strings ("std_dev", plist[j]->name))
318          get_std_dev = 1;
319        if (equal_strings ("confidence", plist[j]->name))
320          get_confidence = 1;
321        if (equal_strings ("intensity", plist[j]->name))
322          get_intensity = 1;
323        if (equal_strings ("diffuse_red", plist[j]->name))
324          has_red = 1;
325        if (equal_strings ("diffuse_green", plist[j]->name))
326          has_green = 1;
327        if (equal_strings ("diffuse_blue", plist[j]->name))
328          has_blue = 1;
329      }
330
331      if (has_red && has_green && has_blue) {
332        get_color = 1;
333        rangeGrid->hasColor = 1;
334      }
335
336      if (get_intensity)
337        rangeGrid->hasIntensity = 1;
338
339      if (get_std_dev && is_warped) {
340        rangeGrid->hasConfidence = 1;
341        rangeGrid->multConfidence = 1;
342      }
343      else if (get_confidence)
344        rangeGrid->hasConfidence = 1;
345
346      ply_get_property (ply, "vertex", &vert_std_props[0]);
347      ply_get_property (ply, "vertex", &vert_std_props[1]);
348      ply_get_property (ply, "vertex", &vert_std_props[2]);
349      if (get_confidence)     
350        ply_get_property (ply, "vertex", &vert_std_props[3]);
351      if (get_intensity)             
352        ply_get_property (ply, "vertex", &vert_std_props[4]);
353      if (get_color) {   
354        ply_get_property (ply, "vertex", &vert_std_props[5]);
355        ply_get_property (ply, "vertex", &vert_std_props[6]);
356        ply_get_property (ply, "vertex", &vert_std_props[7]);
357      }
358      if (get_std_dev)
359        ply_get_property (ply, "vertex", &vert_std_props[8]);
360
361      rangeGrid->coords = new Vec3f[rangeGrid->numSamples];
362
363      if (get_confidence || get_std_dev)
364          rangeGrid->confidence = new float[rangeGrid->numSamples];
365
366      if (get_intensity)
367          rangeGrid->intensity = new float[rangeGrid->numSamples];
368
369      if (get_color)
370          rangeGrid->matDiff = new vec3uc[rangeGrid->numSamples];
371
372      for (j = 0; j < num_elems; j++) {
373        ply_get_element (ply, (void *) &vert);
374        rangeGrid->coords[j].x = vert.x;
375        rangeGrid->coords[j].y = vert.y;
376        rangeGrid->coords[j].z = vert.z;
377
378        if (get_intensity) {
379          rangeGrid->intensity[j] = vert.intensity;
380        }
381
382        if (get_std_dev&&is_warped) {
383
384          std = vert.confidence;
385
386          if (std < min_std)
387            conf = 0;
388          else if (std < avg_std)
389            conf = (std - min_std) / (avg_std - min_std);
390          else if (std > max_std)
391            conf = 0;
392          else
393              conf = (max_std - std) / (max_std - avg_std);
394
395/*
396       Unsafe to use vertex intensity, as aperture settings may change
397       between scans.  Instead, use std_dev confidence * orientation.
398
399          conf *= vert.intensity;
400*/         
401
402          if (get_intensity)
403              if (vert.intensity < RANGE_DATA_MIN_INTENSITY) conf = 0.0;
404
405          rangeGrid->confidence[j] = conf;
406        }
407        else if (get_confidence) {
408          rangeGrid->confidence[j] = vert.confidence;
409        }
410
411        if (get_color) {
412          rangeGrid->matDiff[j][0] = vert.red;
413          rangeGrid->matDiff[j][1] = vert.grn;
414          rangeGrid->matDiff[j][2] = vert.blu;
415        }
416      }
417    }
418
419    if (equal_strings ("range_grid", elem_name)) {
420      rangeGrid->indices = new int[rangeGrid->nlt * rangeGrid->nlg];
421      ply_get_element_setup (ply, elem_name, 1, range_props);
422      for (j = 0; j < num_elems; j++) {
423        ply_get_element (ply, (void *) &range_pnt);
424        if (range_pnt.num_pts == 0)
425          rangeGrid->indices[j] = -1;
426        else {
427            max = -FLT_MAX;
428            for (k = 0; k < range_pnt.num_pts; k++) {
429                index = range_pnt.pts[k];
430                // There will only be more than one point per sample
431                // if there are intensities
432                if (get_intensity) {
433                    if (rangeGrid->intensity[index] > max) {
434                        max = rangeGrid->intensity[index];
435                        best_index = index;
436                    }
437                }
438                else {
439                    best_index = index;
440                }
441            }
442            index = best_index;
443            if (get_confidence || get_std_dev) {
444                if (rangeGrid->confidence[index] > 0.0) 
445                    rangeGrid->indices[j] = index;
446                else
447                    rangeGrid->indices[j] = -1;
448            } else {
449                rangeGrid->indices[j] = index;
450            }
451
452            if (get_intensity) {
453                if (rangeGrid->intensity[index] < RANGE_DATA_MIN_INTENSITY) {
454                    rangeGrid->indices[j] = -1;
455                }
456            }
457
458            free (range_pnt.pts);
459        }
460      }
461    }
462  }
463
464  ply_close (ply);
465
466  return (rangeGrid);
467}
468
469
470/******************************************************************************
471Read range data from a PLY file.
472
473Entry:
474  name - name of PLY file to read from
475
476Exit:
477  returns pointer to data, or NULL if it couldn't read from file
478******************************************************************************/
479
480RangeGrid *
481readRangeGridFillGaps(const char *name)
482{
483  int i,j,k,index, best_index;
484  PlyFile *ply;
485  RangeGrid *rangeGrid = NULL;
486  char **obj_info;
487  int num_obj_info;
488  int num_elems;
489  int nprops;
490  int nelems;
491  char **elist;
492  int file_type;
493  float version;
494  PlyProperty **plist;
495  int num_rows,num_cols;
496  RangePnt range_pnt;
497  PlyVertex vert;
498  char *elem_name;
499  int get_std_dev = 0;
500  int get_confidence = 0;
501  int get_intensity = 0;
502  int get_color = 0;
503  int has_red = 0;
504  int has_green = 0;
505  int has_blue = 0;
506  int is_warped = 0;
507  char temp[PATH_MAX];
508  int isRightMirrorOpen = 1;
509  int found_mirror = 0;
510  float conf,std;
511  float min_std,max_std,max;
512  float avg_std = 0;
513  float angle, alongSensor1, alongSensor2, x;
514  float lgincr;
515
516  ply = ply_open_for_reading(name, &nelems, &elist, &file_type, &version);
517  if (ply == NULL)
518    return (NULL);
519
520  /* parse the obj_info */
521
522  obj_info = ply_get_obj_info (ply, &num_obj_info);
523  for (i = 0; i < num_obj_info; i++) {
524    if (strstr(obj_info[i], "num_cols"))
525        sscanf(obj_info[i], "%s%d", temp, &num_cols);
526    if (strstr(obj_info[i], "num_rows"))
527        sscanf(obj_info[i], "%s%d", temp, &num_rows);
528    if (strstr(obj_info[i], "is_warped"))
529        sscanf(obj_info[i], "%s%d", temp, &is_warped);
530    if (strstr(obj_info[i], "optimum_std_dev"))
531        sscanf(obj_info[i], "%s%f", temp, &avg_std);
532    if (strstr(obj_info[i], "echo_lgincr"))
533        sscanf(obj_info[i], "%s%f", temp, &lgincr);
534    if (strstr(obj_info[i], "is_right_mirror_open")) {
535        found_mirror = TRUE;
536        sscanf(obj_info[i], "%s%d", temp, &isRightMirrorOpen);
537    }
538  }
539
540  EdgeLength = lgincr;
541
542  if (is_warped && !found_mirror && Warn)
543      printf("Couldn't tell which mirror was open.  Right mirror assumed.\n");
544
545  min_std = avg_std / RANGE_DATA_SIGMA_FACTOR;
546  max_std = avg_std * RANGE_DATA_SIGMA_FACTOR;
547
548  /* set up the range data structure */
549  rangeGrid = new RangeGrid;
550  rangeGrid->nlg = num_rows;
551  rangeGrid->nlt = num_cols;
552  rangeGrid->intensity = NULL;
553  rangeGrid->confidence = NULL;
554  rangeGrid->matDiff = NULL;
555  rangeGrid->hasColor = 0;
556  rangeGrid->hasIntensity = 0;
557  rangeGrid->hasConfidence = 0;
558  rangeGrid->multConfidence = 0;
559
560  if (!is_warped) {
561      rangeGrid->viewDir.setValue(0, 0, -1);
562  } else {
563      if (isRightMirrorOpen) {
564          rangeGrid->viewDir.setValue(-sin(30*M_PI/180), 0, -cos(30*M_PI/180));
565      } else {
566          rangeGrid->viewDir.setValue(sin(30*M_PI/180), 0, -cos(30*M_PI/180));
567      }
568  }
569
570  rangeGrid->num_obj_info = num_obj_info;
571  rangeGrid->obj_info = new char*[num_obj_info];
572  for (i = 0; i < num_obj_info; i++) {
573      rangeGrid->obj_info[i] = new char[strlen(obj_info[i])+1];
574      strcpy(rangeGrid->obj_info[i], obj_info[i]);
575  }
576
577
578  rangeGrid->isWarped = is_warped;
579  rangeGrid->isRightMirrorOpen = isRightMirrorOpen;
580
581  /* see if we've got both vertex and range_grid data */
582
583  plist = ply_get_element_description (ply, "vertex", &num_elems, &nprops);
584  if (plist == NULL) {
585    fprintf (stderr, "file doesn't contain vertex data\n");
586    return (NULL);
587  }
588  rangeGrid->numOrigSamples = num_elems;
589  rangeGrid->numSamples = num_elems;
590  rangeGrid->maxSamples = rangeGrid->nlt * rangeGrid->nlg;
591  plist = ply_get_element_description (ply, "range_grid", &num_elems, &nprops);
592  if (plist == NULL) {
593    fprintf (stderr, "file doesn't contain range_grid data\n");
594    return (NULL);
595  }
596
597  /* read in the range data */
598
599  for (i = 0; i < nelems; i++) {
600
601    elem_name = elist[i];
602    plist = ply_get_element_description (ply, elem_name, &num_elems, &nprops);
603
604    if (equal_strings ("vertex", elem_name)) {
605
606      /* see if the file contains intensities */
607      for (j = 0; j < nprops; j++) {
608        if (equal_strings ("std_dev", plist[j]->name))
609          get_std_dev = 1;
610        if (equal_strings ("confidence", plist[j]->name))
611          get_confidence = 1;
612        if (equal_strings ("intensity", plist[j]->name))
613          get_intensity = 1;
614        if (equal_strings ("diffuse_red", plist[j]->name))
615          has_red = 1;
616        if (equal_strings ("diffuse_green", plist[j]->name))
617          has_green = 1;
618        if (equal_strings ("diffuse_blue", plist[j]->name))
619          has_blue = 1;
620      }
621
622      if (has_red && has_green && has_blue) {
623        get_color = 1;
624        rangeGrid->hasColor = 1;
625      }
626
627      if (get_intensity)
628        rangeGrid->hasIntensity = 1;
629
630      if (get_std_dev && is_warped) {
631        rangeGrid->hasConfidence = 1;
632        rangeGrid->multConfidence = 1;
633      }
634      else if (get_confidence)
635        rangeGrid->hasConfidence = 1;
636
637      ply_get_property (ply, "vertex", &vert_std_props[0]);
638      ply_get_property (ply, "vertex", &vert_std_props[1]);
639      ply_get_property (ply, "vertex", &vert_std_props[2]);
640      if (get_confidence)     
641        ply_get_property (ply, "vertex", &vert_std_props[3]);
642      if (get_intensity)             
643        ply_get_property (ply, "vertex", &vert_std_props[4]);
644      if (get_color) {   
645        ply_get_property (ply, "vertex", &vert_std_props[5]);
646        ply_get_property (ply, "vertex", &vert_std_props[6]);
647        ply_get_property (ply, "vertex", &vert_std_props[7]);
648      }
649      if (get_std_dev)
650        ply_get_property (ply, "vertex", &vert_std_props[8]);
651
652      rangeGrid->coords = new Vec3f[rangeGrid->maxSamples];
653
654      if (get_confidence || get_std_dev)
655          rangeGrid->confidence = new float[rangeGrid->maxSamples];
656
657      if (get_intensity)
658          rangeGrid->intensity = new float[rangeGrid->maxSamples];
659
660      if (get_color)
661          rangeGrid->matDiff = new vec3uc[rangeGrid->maxSamples];
662
663      for (j = 0; j < num_elems; j++) {
664        ply_get_element (ply, (void *) &vert);
665        rangeGrid->coords[j].x = vert.x;
666        rangeGrid->coords[j].y = vert.y;
667        rangeGrid->coords[j].z = vert.z;
668
669        if (get_intensity) {
670          rangeGrid->intensity[j] = vert.intensity;
671        }
672
673        if (get_std_dev&&is_warped) {
674
675          std = vert.confidence;
676
677          if (std < min_std)
678            conf = 0;
679          else if (std < avg_std)
680            conf = (std - min_std) / (avg_std - min_std);
681          else if (std > max_std)
682            conf = 0;
683          else
684              conf = (max_std - std) / (max_std - avg_std);
685
686/*
687       Unsafe to use vertex intensity, as aperture settings may change
688       between scans.  Instead, use std_dev confidence * orientation.
689
690          conf *= vert.intensity;
691*/         
692
693          if (get_intensity)
694              if (vert.intensity < RANGE_DATA_MIN_INTENSITY) conf = 0.0;
695
696          rangeGrid->confidence[j] = conf;
697        }
698        else if (get_confidence) {
699          rangeGrid->confidence[j] = vert.confidence;
700        }
701
702        if (get_color) {
703          rangeGrid->matDiff[j][0] = vert.red;
704          rangeGrid->matDiff[j][1] = vert.grn;
705          rangeGrid->matDiff[j][2] = vert.blu;
706        }
707      }
708    }
709
710    if (equal_strings ("range_grid", elem_name)) {
711      rangeGrid->indices = new int[rangeGrid->nlt * rangeGrid->nlg];
712      if (rangeGrid->indices == NULL) {
713          fprintf (stderr, "could not allocate space\n");
714          return (NULL);
715      }
716
717      ply_get_element_setup (ply, elem_name, 1, range_props);
718      for (j = 0; j < num_elems; j++) {
719        ply_get_element (ply, (void *) &range_pnt);
720        if (range_pnt.num_pts == 0)
721          rangeGrid->indices[j] = -1;
722        else {
723            max = -FLT_MAX;
724            for (k = 0; k < range_pnt.num_pts; k++) {
725                index = range_pnt.pts[k];
726                // There will only be more than one point per sample
727                // if there are intensities
728                if (get_intensity) {
729                    if (rangeGrid->intensity[index] > max) {
730                        max = rangeGrid->intensity[index];
731                        best_index = index;
732                    }
733                }
734                else {
735                    best_index = index;
736                }
737            }
738            index = best_index;
739            if (get_confidence || get_std_dev) {
740                if (rangeGrid->confidence[index] > 0.0) 
741                    rangeGrid->indices[j] = index;
742                else
743                    rangeGrid->indices[j] = -1;
744            } else {
745                rangeGrid->indices[j] = index;
746            }
747
748            if (get_intensity) {
749                if (rangeGrid->intensity[index] < RANGE_DATA_MIN_INTENSITY) {
750                    rangeGrid->indices[j] = -1;
751                }
752            }
753
754            free (range_pnt.pts);
755        }
756      }
757    }
758  }
759
760  Vec3f vlast, v, dv, vnew;
761  float z;
762  int xx, yy, in, lastOut, lastIn, prevIndex;
763  int xlim, ylim, doFill, oldIndex;
764
765  // The actual filling stuff begins here.
766  // Bugs: 1. I don't account for interlace when interpolating in the
767  //   in the y-direction
768  //    2. I fill by direct connection without regard to
769  //    maximal slopes
770  //    3. I check which sample is closest by z-comparison - should use
771  //       the laser perspective
772
773  // Mark the regions that are off the edge of the mesh as
774  // "unfillable"
775
776#if 1
777
778  // X-pass
779  for (yy = 0; yy < rangeGrid->nlg; yy++) {
780
781      xx = 0;
782      index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
783      while (index < 0) {
784          xx++;
785          if (xx == rangeGrid->nlt)
786              break;
787          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
788      }
789
790      xlim = xx;
791      for (xx = 0; xx < xlim; xx++) {
792          rangeGrid->indices[xx+yy*rangeGrid->nlt] = INT_MIN;
793      }
794
795      if (xlim == rangeGrid->nlt)
796          continue;
797
798      xx = rangeGrid->nlt-1;
799      index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
800      while (index < 0) {
801          xx--;
802          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
803      }
804
805      xlim = xx+1;
806      for (xx = xlim; xx < rangeGrid->nlt; xx++) {
807          rangeGrid->indices[xx+yy*rangeGrid->nlt] = INT_MIN;
808      }
809  }
810
811
812  // Y-pass
813  for (xx = 0; xx < rangeGrid->nlt; xx++) {
814
815      yy = 0;
816      index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
817      while (index < 0) {
818          yy++;
819          if (yy == rangeGrid->nlg)
820              break;
821          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
822      }
823
824      ylim = yy;
825      for (yy = 0; yy < ylim; yy++) {
826          rangeGrid->indices[xx+yy*rangeGrid->nlt] = INT_MIN;
827      }
828
829      if (ylim == rangeGrid->nlg)
830          continue;
831
832      yy = rangeGrid->nlg-1;
833      index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
834      while (index < 0) {
835          yy--;
836          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
837      }
838
839      ylim = yy + 1;
840      for (yy = ylim; yy < rangeGrid->nlg; yy++) {
841          rangeGrid->indices[xx+yy*rangeGrid->nlt] = INT_MIN;
842      }
843  }
844#endif
845
846  // Fill the gaps in the x-direction
847
848#if 1
849  for (yy = 0; yy < rangeGrid->nlg; yy++) {
850
851      // Fill in gaps with simple linear interpolation
852      // between depths of adjacent valid pixels
853       
854      in = FALSE;
855      lastOut = -1;
856     
857      for (xx = 0; xx < rangeGrid->nlt; xx++) {
858          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
859          if (index >= 0 && !in) {
860             
861              lastIn = xx;
862              in = TRUE;
863             
864              if (lastOut == -1)
865                  continue;
866
867              // Look for gaps that include boundaries that
868              // shouldn't be filled
869              doFill = TRUE;
870
871              for (i = lastOut+1; i <lastIn; i++) {
872                  oldIndex = rangeGrid->indices[i+yy*rangeGrid->nlt];
873                  if (oldIndex == INT_MIN) {
874                      doFill = FALSE;
875                      break;
876                  }
877              }
878
879              if (!doFill) {
880                  continue;
881              }
882
883              v.setValue(rangeGrid->coords[index]);
884              dv = v;
885              dv -= vlast;
886              dv /= (lastIn - lastOut);
887              vnew = vlast;
888              vnew += dv;
889              for (i = lastOut+1; i <lastIn; i++, vnew+=dv) {
890                  rangeGrid->coords[rangeGrid->numSamples].setValue(vnew);
891                 
892                  if (rangeGrid->hasConfidence)
893                      rangeGrid->confidence[rangeGrid->numSamples] = 0;
894
895                  // Fill with a negative number so that it
896                  // can be recognized later as a hole fill
897                  rangeGrid->indices[i+yy*rangeGrid->nlt] = 
898                      -rangeGrid->numSamples;
899                  rangeGrid->numSamples++;
900              }
901          } 
902          else if (index < 0 && in) {           
903              lastOut = xx-1;
904              prevIndex = rangeGrid->indices[lastOut+yy*rangeGrid->nlt];
905              vlast.setValue(rangeGrid->coords[prevIndex]);
906              in = FALSE;
907          }
908      }
909  }
910#endif
911
912  if (is_warped && isRightMirrorOpen) {
913      angle = 30;
914  } else if (is_warped && !isRightMirrorOpen) {
915      angle = -30;
916  } else {
917      angle = 0;
918  }
919
920  Vec3f dir(sin(RAD(angle)), 0, cos(RAD(angle)));
921
922#if 1
923
924  // Fill the gaps in the y-direction
925
926  for (xx = 0; xx < rangeGrid->nlt; xx++) {
927
928      // Fill in gaps with simple linear interpolation
929      // between depths of adjacent valid pixels
930       
931      in = FALSE;
932      lastOut = -1;
933     
934      for (yy = 0; yy < rangeGrid->nlg; yy++) {
935          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
936          if (index >= 0 && !in) {
937             
938              lastIn = yy;
939              in = TRUE;
940             
941              if (lastOut == -1)
942                  continue;
943
944              doFill = TRUE;
945              for (i = lastOut+1; i <lastIn; i++, vnew+=dv) {
946                  oldIndex = rangeGrid->indices[xx+i*rangeGrid->nlt];
947                  if (oldIndex == INT_MIN) {
948                      doFill = FALSE;
949                      break;
950                  }
951              }
952
953              if (!doFill)
954                  continue;
955
956              v.setValue(rangeGrid->coords[index]);
957              dv = v;
958              dv -= vlast;
959              dv /= (lastIn - lastOut);
960              vnew = vlast;
961              vnew += dv;
962              for (i = lastOut+1; i <lastIn; i++, vnew+=dv) {
963                  oldIndex =  rangeGrid->indices[xx+i*rangeGrid->nlt];
964                  if (oldIndex == -1) {
965                      rangeGrid->coords[rangeGrid->numSamples].setValue(vnew);
966
967                      if (rangeGrid->hasConfidence)
968                          rangeGrid->confidence[rangeGrid->numSamples] = 0;
969
970                      rangeGrid->indices[xx+i*rangeGrid->nlt] = 
971                          rangeGrid->numSamples;
972                      rangeGrid->numSamples++;
973                  } else {
974                      oldIndex = -oldIndex;
975                      z = rangeGrid->coords[oldIndex].z;
976
977                      // Should probably check along line of sight to
978                      // laser, rather than along a simple orthographic
979                      // projection in the z-direction...
980
981                      x = rangeGrid->coords[oldIndex].x;
982
983                      alongSensor1 = x*dir.x + z*dir.z;
984                      alongSensor2 = vnew.x*dir.x + vnew.z*dir.z;
985
986                      if (alongSensor1 < alongSensor2) {
987                          rangeGrid->coords[oldIndex].setValue(vnew);
988
989/*
990                      if (z < vnew.z) {
991                          rangeGrid->coords[oldIndex].setValue(vnew);
992                          */
993
994                      }
995                  }
996              }
997          } 
998          else if (index < 0 && in) {           
999              lastOut = yy-1;
1000              prevIndex = rangeGrid->indices[xx+lastOut*rangeGrid->nlt];
1001              vlast.setValue(rangeGrid->coords[prevIndex]);
1002              in = FALSE;
1003          }
1004      }
1005  }
1006
1007#endif
1008
1009#if 1
1010  // Clean up indices to be -1 or valid (i.e., index >= 0 )
1011
1012  for (yy = 0; yy < rangeGrid->nlg; yy++) {
1013      for (xx = 0; xx < rangeGrid->nlt; xx++) {
1014          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1015          if (index == INT_MIN) {
1016              rangeGrid->indices[xx+yy*rangeGrid->nlt] = -1;
1017          } else if (index < -1) {
1018              rangeGrid->indices[xx+yy*rangeGrid->nlt] = -index;
1019          }
1020      }
1021  }
1022#endif
1023
1024  ply_close (ply);
1025
1026  return (rangeGrid);
1027}
1028
1029
1030/******************************************************************************
1031Read range data from a PLY file.
1032
1033Entry:
1034  name - name of PLY file to read from
1035
1036Exit:
1037  returns pointer to data, or NULL if it couldn't read from file
1038******************************************************************************/
1039
1040RangeGrid *
1041readRangeGridExtendEdges(const char *name)
1042{
1043  int i,j,k,index, best_index;
1044  PlyFile *ply;
1045  RangeGrid *rangeGrid = NULL;
1046  char **obj_info;
1047  int num_obj_info;
1048  int num_elems;
1049  int nprops;
1050  int nelems;
1051  char **elist;
1052  int file_type;
1053  float version;
1054  PlyProperty **plist;
1055  int num_rows,num_cols;
1056  RangePnt range_pnt;
1057  PlyVertex vert;
1058  char *elem_name;
1059  int get_std_dev = 0;
1060  int get_confidence = 0;
1061  int get_intensity = 0;
1062  int get_color = 0;
1063  int has_red = 0;
1064  int has_green = 0;
1065  int has_blue = 0;
1066  int is_warped = 0;
1067  char temp[PATH_MAX];
1068  int isRightMirrorOpen = 1;
1069  int found_mirror = 0;
1070  float conf,std;
1071  float min_std,max_std,max;
1072  float avg_std = 0;
1073  float angle, alongSensor1, alongSensor2, x;
1074  float lgincr;
1075
1076  ply = ply_open_for_reading(name, &nelems, &elist, &file_type, &version);
1077  if (ply == NULL)
1078    return (NULL);
1079
1080  /* parse the obj_info */
1081
1082  obj_info = ply_get_obj_info (ply, &num_obj_info);
1083  for (i = 0; i < num_obj_info; i++) {
1084    if (strstr(obj_info[i], "num_cols"))
1085        sscanf(obj_info[i], "%s%d", temp, &num_cols);
1086    if (strstr(obj_info[i], "num_rows"))
1087        sscanf(obj_info[i], "%s%d", temp, &num_rows);
1088    if (strstr(obj_info[i], "is_warped"))
1089        sscanf(obj_info[i], "%s%d", temp, &is_warped);
1090    if (strstr(obj_info[i], "optimum_std_dev"))
1091        sscanf(obj_info[i], "%s%f", temp, &avg_std);
1092    if (strstr(obj_info[i], "echo_lgincr"))
1093        sscanf(obj_info[i], "%s%f", temp, &lgincr);
1094    if (strstr(obj_info[i], "is_right_mirror_open")) {
1095        found_mirror = TRUE;
1096        sscanf(obj_info[i], "%s%d", temp, &isRightMirrorOpen);
1097    }
1098  }
1099
1100  EdgeLength = lgincr;
1101
1102  if (is_warped && !found_mirror && Warn)
1103      printf("Couldn't tell which mirror was open.  Right mirror assumed.\n");
1104
1105  min_std = avg_std / RANGE_DATA_SIGMA_FACTOR;
1106  max_std = avg_std * RANGE_DATA_SIGMA_FACTOR;
1107
1108  /* set up the range data structure */
1109  rangeGrid = new RangeGrid;
1110  rangeGrid->nlg = num_rows;
1111  rangeGrid->nlt = num_cols;
1112  rangeGrid->intensity = NULL;
1113  rangeGrid->confidence = NULL;
1114  rangeGrid->matDiff = NULL;
1115  rangeGrid->hasColor = 0;
1116  rangeGrid->hasIntensity = 0;
1117  rangeGrid->hasConfidence = 0;
1118  rangeGrid->multConfidence = 0;
1119
1120  if (!is_warped) {
1121      rangeGrid->viewDir.setValue(0, 0, -1);
1122  } else {
1123      if (isRightMirrorOpen) {
1124          rangeGrid->viewDir.setValue(-sin(30*M_PI/180), 0, -cos(30*M_PI/180));
1125      } else {
1126          rangeGrid->viewDir.setValue(sin(30*M_PI/180), 0, -cos(30*M_PI/180));
1127      }
1128  }
1129
1130  rangeGrid->num_obj_info = num_obj_info;
1131  rangeGrid->obj_info = new char*[num_obj_info];
1132  for (i = 0; i < num_obj_info; i++) {
1133      rangeGrid->obj_info[i] = new char[strlen(obj_info[i])+1];
1134      strcpy(rangeGrid->obj_info[i], obj_info[i]);
1135  }
1136
1137
1138  rangeGrid->isWarped = is_warped;
1139  rangeGrid->isRightMirrorOpen = isRightMirrorOpen;
1140
1141  /* see if we've got both vertex and range_grid data */
1142
1143  plist = ply_get_element_description (ply, "vertex", &num_elems, &nprops);
1144  if (plist == NULL) {
1145    fprintf (stderr, "file doesn't contain vertex data\n");
1146    return (NULL);
1147  }
1148  rangeGrid->numOrigSamples = num_elems;
1149  rangeGrid->numSamples = num_elems;
1150  rangeGrid->maxSamples = rangeGrid->nlt * rangeGrid->nlg;
1151  plist = ply_get_element_description (ply, "range_grid", &num_elems, &nprops);
1152  if (plist == NULL) {
1153    fprintf (stderr, "file doesn't contain range_grid data\n");
1154    return (NULL);
1155  }
1156
1157  /* read in the range data */
1158
1159  for (i = 0; i < nelems; i++) {
1160
1161    elem_name = elist[i];
1162    plist = ply_get_element_description (ply, elem_name, &num_elems, &nprops);
1163
1164    if (equal_strings ("vertex", elem_name)) {
1165
1166      /* see if the file contains intensities */
1167      for (j = 0; j < nprops; j++) {
1168        if (equal_strings ("std_dev", plist[j]->name))
1169          get_std_dev = 1;
1170        if (equal_strings ("confidence", plist[j]->name))
1171          get_confidence = 1;
1172        if (equal_strings ("intensity", plist[j]->name))
1173          get_intensity = 1;
1174        if (equal_strings ("diffuse_red", plist[j]->name))
1175          has_red = 1;
1176        if (equal_strings ("diffuse_green", plist[j]->name))
1177          has_green = 1;
1178        if (equal_strings ("diffuse_blue", plist[j]->name))
1179          has_blue = 1;
1180      }
1181
1182      if (has_red && has_green && has_blue) {
1183        get_color = 1;
1184        rangeGrid->hasColor = 1;
1185      }
1186
1187      if (get_intensity)
1188        rangeGrid->hasIntensity = 1;
1189
1190      if (get_std_dev && is_warped) {
1191        rangeGrid->hasConfidence = 1;
1192        rangeGrid->multConfidence = 1;
1193      }
1194      else if (get_confidence)
1195        rangeGrid->hasConfidence = 1;
1196
1197      ply_get_property (ply, "vertex", &vert_std_props[0]);
1198      ply_get_property (ply, "vertex", &vert_std_props[1]);
1199      ply_get_property (ply, "vertex", &vert_std_props[2]);
1200      if (get_confidence)     
1201        ply_get_property (ply, "vertex", &vert_std_props[3]);
1202      if (get_intensity)             
1203        ply_get_property (ply, "vertex", &vert_std_props[4]);
1204      if (get_color) {   
1205        ply_get_property (ply, "vertex", &vert_std_props[5]);
1206        ply_get_property (ply, "vertex", &vert_std_props[6]);
1207        ply_get_property (ply, "vertex", &vert_std_props[7]);
1208      }
1209      if (get_std_dev)
1210        ply_get_property (ply, "vertex", &vert_std_props[8]);
1211
1212      rangeGrid->coords = new Vec3f[rangeGrid->maxSamples];
1213
1214      if (get_confidence || get_std_dev)
1215          rangeGrid->confidence = new float[rangeGrid->maxSamples];
1216
1217      if (get_intensity)
1218          rangeGrid->intensity = new float[rangeGrid->maxSamples];
1219
1220      if (get_color)
1221          rangeGrid->matDiff = new vec3uc[rangeGrid->maxSamples];
1222
1223      for (j = 0; j < num_elems; j++) {
1224        ply_get_element (ply, (void *) &vert);
1225        rangeGrid->coords[j].x = vert.x;
1226        rangeGrid->coords[j].y = vert.y;
1227        rangeGrid->coords[j].z = vert.z;
1228
1229        if (get_intensity) {
1230          rangeGrid->intensity[j] = vert.intensity;
1231        }
1232
1233        if (get_std_dev&&is_warped) {
1234
1235          std = vert.confidence;
1236
1237          if (std < min_std)
1238            conf = 0;
1239          else if (std < avg_std)
1240            conf = (std - min_std) / (avg_std - min_std);
1241          else if (std > max_std)
1242            conf = 0;
1243          else
1244              conf = (max_std - std) / (max_std - avg_std);
1245
1246/*
1247       Unsafe to use vertex intensity, as aperture settings may change
1248       between scans.  Instead, use std_dev confidence * orientation.
1249
1250          conf *= vert.intensity;
1251*/         
1252
1253          if (get_intensity)
1254              if (vert.intensity < RANGE_DATA_MIN_INTENSITY) conf = 0.0;
1255
1256          rangeGrid->confidence[j] = conf;
1257        }
1258        else if (get_confidence) {
1259          rangeGrid->confidence[j] = vert.confidence;
1260        }
1261
1262        if (get_color) {
1263          rangeGrid->matDiff[j][0] = vert.red;
1264          rangeGrid->matDiff[j][1] = vert.grn;
1265          rangeGrid->matDiff[j][2] = vert.blu;
1266        }
1267      }
1268    }
1269
1270    if (equal_strings ("range_grid", elem_name)) {
1271      rangeGrid->indices = new int[rangeGrid->nlt * rangeGrid->nlg];
1272      ply_get_element_setup (ply, elem_name, 1, range_props);
1273      for (j = 0; j < num_elems; j++) {
1274        ply_get_element (ply, (void *) &range_pnt);
1275        if (range_pnt.num_pts == 0)
1276          rangeGrid->indices[j] = -1;
1277        else {
1278            max = -FLT_MAX;
1279            for (k = 0; k < range_pnt.num_pts; k++) {
1280                index = range_pnt.pts[k];
1281                // There will only be more than one point per sample
1282                // if there are intensities
1283                if (get_intensity) {
1284                    if (rangeGrid->intensity[index] > max) {
1285                        max = rangeGrid->intensity[index];
1286                        best_index = index;
1287                    }
1288                }
1289                else {
1290                    best_index = index;
1291                }
1292            }
1293            index = best_index;
1294            if (get_confidence || get_std_dev) {
1295                if (rangeGrid->confidence[index] > 0.0) 
1296                    rangeGrid->indices[j] = index;
1297                else
1298                    rangeGrid->indices[j] = -1;
1299            } else {
1300                rangeGrid->indices[j] = index;
1301            }
1302
1303            if (get_intensity) {
1304                if (rangeGrid->intensity[index] < RANGE_DATA_MIN_INTENSITY) {
1305                    rangeGrid->indices[j] = -1;
1306                }
1307            }
1308
1309            free (range_pnt.pts);
1310        }
1311      }
1312    }
1313  }
1314
1315
1316  Vec3f vlast, v, dv, vnew;
1317  float z;
1318  int xx, yy, in, lastOut, lastIn, prevIndex;
1319  int xlim, ylim, doFill, oldIndex;
1320
1321
1322  // X-pass
1323  for (yy = 0; yy < rangeGrid->nlg; yy++) {
1324
1325      xx = 0;
1326      index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1327      while (index < 0) {
1328          xx++;
1329          if (xx == rangeGrid->nlt)
1330              break;
1331          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1332      }
1333
1334      xlim = xx;
1335
1336      if (xlim == rangeGrid->nlt)
1337          continue;
1338
1339      if (xlim > EdgeExtensionSamples - 1) {
1340          v.setValue(rangeGrid->coords[index]);
1341          dv.setValue(-lgincr, 0, -lgincr*cos(M_PI*EdgeExtensionAngle/180));
1342          vnew.setValue(v);
1343          vnew += dv;
1344          for (xx = xlim-1; xx > xlim-EdgeExtensionSamples-1; xx--) {
1345              rangeGrid->coords[rangeGrid->numSamples].setValue(vnew);
1346
1347              if (rangeGrid->hasConfidence)
1348                  rangeGrid->confidence[rangeGrid->numSamples] = 0;
1349
1350              // Fill with a negative number so that it
1351              // can be recognized later as a hole fill
1352              rangeGrid->indices[xx+yy*rangeGrid->nlt] = 
1353                  -rangeGrid->numSamples;
1354              rangeGrid->numSamples++;
1355
1356              vnew += dv;
1357          }
1358      }
1359
1360      xx = rangeGrid->nlt-1;
1361      index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1362      while (index < 0) {
1363          xx--;
1364          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1365      }
1366
1367
1368      xlim = xx+1;
1369      if (xlim < rangeGrid->nlt-EdgeExtensionSamples) {
1370          v.setValue(rangeGrid->coords[index]);
1371          dv.setValue(lgincr, 0, -lgincr*tan(M_PI*EdgeExtensionAngle/180));
1372
1373          vnew.setValue(v);
1374          vnew += dv;
1375          for (xx = xlim; xx < xlim+EdgeExtensionSamples; xx++) {
1376              rangeGrid->coords[rangeGrid->numSamples].setValue(vnew);
1377
1378              if (rangeGrid->hasConfidence)
1379                  rangeGrid->confidence[rangeGrid->numSamples] = 0;
1380
1381              // Fill with a negative number so that it
1382              // can be recognized later as a hole fill
1383              rangeGrid->indices[xx+yy*rangeGrid->nlt] = 
1384                  -rangeGrid->numSamples;
1385              rangeGrid->numSamples++;
1386
1387              vnew += dv;
1388          }
1389      }
1390  }
1391
1392
1393  if (is_warped && isRightMirrorOpen) {
1394      angle = 30;
1395  } else if (is_warped && !isRightMirrorOpen) {
1396      angle = -30;
1397  } else {
1398      angle = 0;
1399  }
1400
1401  Vec3f dir(sin(RAD(angle)), 0, cos(RAD(angle)));
1402
1403  // Y-pass
1404  for (xx = 0; xx < rangeGrid->nlt; xx++) {
1405
1406      yy = 0;
1407      index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1408      while (index < 0) {
1409          yy++;
1410          if (yy == rangeGrid->nlg)
1411              break;
1412          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1413      }
1414
1415      ylim = yy;
1416
1417      if (ylim == rangeGrid->nlg)
1418          continue;
1419
1420      if (ylim > EdgeExtensionSamples - 1) {
1421          v.setValue(rangeGrid->coords[index]);
1422          dv.setValue(0, -lgincr, -lgincr*tan(M_PI*EdgeExtensionAngle/180));
1423          vnew.setValue(v);
1424          vnew += dv;
1425          for (yy = ylim-1; yy > ylim-EdgeExtensionSamples-1; yy--) {
1426              oldIndex =  rangeGrid->indices[xx+yy*rangeGrid->nlt];
1427              if (oldIndex == -1) {
1428                  rangeGrid->coords[rangeGrid->numSamples].setValue(vnew);
1429
1430                  if (rangeGrid->hasConfidence)
1431                      rangeGrid->confidence[rangeGrid->numSamples] = 0;
1432
1433                  rangeGrid->indices[xx+yy*rangeGrid->nlt] = 
1434                      rangeGrid->numSamples;
1435                  rangeGrid->numSamples++;
1436              } else {
1437                  oldIndex = -oldIndex;
1438                  z = rangeGrid->coords[oldIndex].z;
1439
1440                  // Should probably check along line of sight to
1441                  // laser, rather than along a simple orthographic
1442                  // projection in the z-direction...
1443
1444                  x = rangeGrid->coords[oldIndex].x;
1445
1446                  alongSensor1 = x*dir.x + z*dir.z;
1447                  alongSensor2 = vnew.x*dir.x + vnew.z*dir.z;
1448
1449                  if (alongSensor1 < alongSensor2) {
1450                      rangeGrid->coords[oldIndex].setValue(vnew);
1451                  }
1452              }
1453
1454              vnew += dv;
1455          }
1456      }
1457
1458      yy = rangeGrid->nlg-1;
1459      index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1460      while (index < 0) {
1461          yy--;
1462          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1463      }
1464
1465
1466      ylim = yy+1;
1467      if (ylim < rangeGrid->nlg-EdgeExtensionSamples-1) {
1468          v.setValue(rangeGrid->coords[index]);
1469          dv.setValue(0, lgincr, -lgincr*cos(M_PI*EdgeExtensionAngle/180));
1470          vnew.setValue(v);
1471          vnew += dv;
1472          for (yy = ylim; yy < ylim+EdgeExtensionSamples-1; yy++) {
1473              oldIndex =  rangeGrid->indices[xx+yy*rangeGrid->nlt];
1474              if (oldIndex == -1) {
1475                  rangeGrid->coords[rangeGrid->numSamples].setValue(vnew);
1476
1477                  if (rangeGrid->hasConfidence)
1478                      rangeGrid->confidence[rangeGrid->numSamples] = 0;
1479
1480                  rangeGrid->indices[xx+yy*rangeGrid->nlt] = 
1481                      rangeGrid->numSamples;
1482                  rangeGrid->numSamples++;
1483              } else {
1484                  oldIndex = -oldIndex;
1485                  z = rangeGrid->coords[oldIndex].z;
1486
1487                  // Should probably check along line of sight to
1488                  // laser, rather than along a simple orthographic
1489                  // projection in the z-direction...
1490
1491                  x = rangeGrid->coords[oldIndex].x;
1492
1493                  alongSensor1 = x*dir.x + z*dir.z;
1494                  alongSensor2 = vnew.x*dir.x + vnew.z*dir.z;
1495
1496                  if (alongSensor1 < alongSensor2) {
1497                      rangeGrid->coords[oldIndex].setValue(vnew);
1498                  }
1499              }
1500
1501              vnew += dv;
1502          }
1503      }
1504  }
1505
1506
1507  for (yy = 0; yy < rangeGrid->nlg; yy++) {
1508      for (xx = 0; xx < rangeGrid->nlt; xx++) {
1509          index = rangeGrid->indices[xx+yy*rangeGrid->nlt];
1510          if (index == INT_MIN) {
1511              rangeGrid->indices[xx+yy*rangeGrid->nlt] = -1;
1512          } else if (index < -1) {
1513              rangeGrid->indices[xx+yy*rangeGrid->nlt] = -index;
1514          }
1515      }
1516  }
1517
1518  ply_close (ply);
1519
1520  return (rangeGrid);
1521}
1522
1523
1524
1525/******************************************************************************
1526Create a triangle mesh from scan data.
1527
1528Entry:
1529
1530
1531Exit:
1532  returns pointer to newly-created mesh
1533******************************************************************************/
1534
1535Mesh *
1536meshFromGrid(RangeGrid *rangeGrid, int subSamp, int connectAll)
1537{
1538  int i,j;
1539  Vec3f v1, v2, vect1, vect2, vect3, vect4;
1540  int ii,jj;
1541  int in1,in2,in3,in4,vin1,vin2,vin3,vin4;
1542  Vertex *vt1,*vt2,*vt3,*vt4;
1543  int count;
1544  int max_lt,max_lg;
1545  Mesh *mesh;
1546  int nlt,nlg,max_verts,max_tris;
1547  int *vert_index;
1548
1549  mesh = new Mesh;
1550
1551  if (Warn)
1552      printf("meshFromGrid(): Not computing useful max_length!!\n");
1553
1554  mesh->isWarped = rangeGrid->isWarped;
1555  mesh->isRightMirrorOpen = rangeGrid->isRightMirrorOpen;
1556
1557  /* allocate space for new triangles and vertices */
1558
1559  mesh->numVerts = 0;
1560  nlt = rangeGrid->nlt;
1561  nlg = rangeGrid->nlg;
1562  // not used: // max_lt = (nlt - 1) / subSamp + 1;
1563  // not used: // max_lg = (nlg - 1) / subSamp + 1;
1564  // not used: // max_verts = max_lt * max_lg;
1565
1566  /* create a list saying whether a vertex is going to be used */
1567
1568  vert_index = new int[rangeGrid->numSamples];
1569  for (i = 0; i < rangeGrid->numSamples; i++)
1570    vert_index[i] = -1;
1571
1572  /* see which vertices will be used in a triangle */
1573
1574  count = 0;
1575  for (i = 0; i <= nlt - subSamp; i += subSamp)
1576    for (j = 0; j <= nlg - subSamp; j += subSamp) {
1577      in1 = rangeGrid->indices[i + j * nlt];
1578      if (in1 >= 0) {
1579          vert_index[in1] = 1;
1580          count++;
1581      }
1582    }
1583
1584  mesh->numVerts = count;
1585  mesh->verts = new Vertex[mesh->numVerts];
1586
1587  mesh->numTris = 0;
1588  max_tris = mesh->numVerts * 2;
1589  mesh->tris = new Triangle[max_tris];
1590
1591
1592  /* create the vertices */
1593
1594  count = 0;
1595  for (i = 0; i < rangeGrid->numSamples; i++) {
1596    if (vert_index[i] == -1)
1597      continue;
1598
1599    vert_index[i] = count;
1600    mesh->verts[count].coord = rangeGrid->coords[i];
1601
1602    mesh->verts[count].maxVerts = 8;
1603    mesh->verts[count].verts = new Vertex*[8];
1604    mesh->verts[count].edgeLengths = new float[8];
1605    mesh->verts[count].numVerts = 0;
1606
1607    mesh->verts[count].maxTris = 8;
1608    mesh->verts[count].tris = new Triangle*[8];
1609    mesh->verts[count].numTris = 0;
1610
1611    if (rangeGrid->hasConfidence)
1612        mesh->verts[count].confidence = rangeGrid->confidence[i];
1613    else 
1614        mesh->verts[count].confidence = 1;
1615
1616    count++;
1617  }
1618  mesh->numVerts = count;
1619
1620  /* create the triangles */
1621
1622  for (i = 0; i < nlt - subSamp; i += subSamp)
1623    for (j = 0; j < nlg - subSamp; j += subSamp) {
1624
1625      ii = (i + subSamp) % nlt;
1626      jj = (j + subSamp) % nlg;
1627
1628      /* count the number of good vertices */
1629      /*
1630      in1 = rangeGrid->indices[ i +  j * nlt];
1631      in2 = rangeGrid->indices[ i + jj * nlt];
1632      in3 = rangeGrid->indices[ii + jj * nlt];
1633      in4 = rangeGrid->indices[ii +  j * nlt];
1634      */
1635
1636      in1 = rangeGrid->indices[ i +  j * nlt];
1637      in2 = rangeGrid->indices[ii +  j * nlt];
1638      in3 = rangeGrid->indices[ii + jj * nlt];
1639      in4 = rangeGrid->indices[ i + jj * nlt];
1640
1641      count = (in1 >= 0) + (in2 >= 0) + (in3 >= 0) + (in4 >=0);
1642      if (in1 >= 0) {
1643          vin1 = vert_index[in1];
1644          vt1 = &mesh->verts[vin1];
1645          if (in1 > rangeGrid->numOrigSamples) {
1646              vt1->holeFill = TRUE;
1647              vt1->stepsToEdge = -1;
1648          } else {
1649              vt1->holeFill = FALSE;
1650              vt1->stepsToEdge = 0;
1651          }
1652      }
1653      if (in2 >= 0) {
1654          vin2 = vert_index[in2];
1655          vt2 = &mesh->verts[vin2];
1656          if (in2 > rangeGrid->numOrigSamples) {
1657              vt2->holeFill = TRUE;
1658              vt2->stepsToEdge = -1;
1659          } else {
1660              vt2->holeFill = FALSE;
1661              vt2->stepsToEdge = 0;
1662          }
1663      }
1664      if (in3 >= 0) {
1665          vin3 = vert_index[in3];
1666          vt3 = &mesh->verts[vin3];
1667          if (in3 > rangeGrid->numOrigSamples) {
1668              vt3->holeFill = TRUE;
1669              vt3->stepsToEdge = -1;
1670          } else {
1671              vt3->holeFill = FALSE;
1672              vt3->stepsToEdge = 0;
1673          }
1674      }
1675      if (in4 >= 0) {
1676          vin4 = vert_index[in4];
1677          vt4 = &mesh->verts[vin4];
1678          if (in4 > rangeGrid->numOrigSamples) {
1679              vt4->holeFill = TRUE;
1680              vt4->stepsToEdge = -1;
1681          } else {
1682              vt4->holeFill = FALSE;
1683              vt4->stepsToEdge = 0;
1684          }
1685      }
1686
1687      if (UseEdgeLength) {
1688          if (count == 4) {    /* all 4 vertices okay, so make 2 tris */
1689              float len1,len2;
1690
1691              /* compute lengths of cross-edges */
1692              v1 = vt1->coord - vt3->coord;
1693              v2 = vt2->coord - vt4->coord;
1694              len1 = v1.length();
1695              len2 = v2.length();
1696
1697              /* make triangles that minimize the cross-length */
1698
1699              if (len1 < len2) {
1700                  addTriangleUseLength(mesh, vin1, vin2, vin3, 
1701                                       MaxEdgeLength, connectAll);
1702                  addTriangleUseLength(mesh, vin1, vin3, vin4, 
1703                                       MaxEdgeLength, connectAll);
1704              } else {
1705                  addTriangleUseLength(mesh, vin2, vin3, vin4, 
1706                                       MaxEdgeLength, connectAll);
1707                  addTriangleUseLength(mesh, vin2, vin4, vin1, 
1708                                       MaxEdgeLength, connectAll);
1709              }
1710          }
1711          else if (count == 3) {   /* only 3 vertices okay, so make 1 tri */
1712              if (in1 == -1) {
1713                  addTriangleUseLength(mesh, vin2, vin3, vin4, 
1714                                       MaxEdgeLength, connectAll);
1715              }
1716              else if (in2 == -1) {
1717                  addTriangleUseLength(mesh, vin1, vin3, vin4, 
1718                                       MaxEdgeLength, connectAll);
1719              }
1720              else if (in3 == -1) {
1721                  addTriangleUseLength(mesh, vin1, vin2, vin4, 
1722                                       MaxEdgeLength, connectAll);
1723              }
1724              else {    /* in4 == -1 */
1725                  addTriangleUseLength(mesh, vin1, vin2, vin3, 
1726                                       MaxEdgeLength, connectAll);
1727              }
1728          }
1729      }
1730      else {
1731          if (count == 4) {    /* all 4 vertices okay, so make 2 tris */
1732              float len1,len2;
1733
1734              /* compute lengths of cross-edges */
1735              v1 = vt1->coord - vt3->coord;
1736              v2 = vt2->coord - vt4->coord;
1737              len1 = v1.length();
1738              len2 = v2.length();
1739
1740              /* make triangles that minimize the cross-length */
1741
1742              if (len1 < len2) {
1743                  addTriangleUseNormal(mesh, vin1, vin2, vin3, 
1744                                       MinViewDot, rangeGrid->viewDir, 
1745                                       connectAll);
1746                  addTriangleUseNormal(mesh, vin1, vin3, vin4, 
1747                                       MinViewDot, rangeGrid->viewDir, 
1748                                       connectAll);
1749              } else {
1750                  addTriangleUseNormal(mesh, vin2, vin3, vin4, 
1751                                       MinViewDot, rangeGrid->viewDir, 
1752                                       connectAll);
1753                  addTriangleUseNormal(mesh, vin2, vin4, vin1, 
1754                                       MinViewDot, rangeGrid->viewDir, 
1755                                       connectAll);
1756              }
1757          }
1758          else if (count == 3) {   /* only 3 vertices okay, so make 1 tri */
1759              if (in1 == -1) {
1760                  addTriangleUseNormal(mesh, vin2, vin3, vin4, 
1761                                       MinViewDot, rangeGrid->viewDir, 
1762                                       connectAll);
1763              }
1764              else if (in2 == -1) {
1765                  addTriangleUseNormal(mesh, vin1, vin3, vin4, 
1766                                       MinViewDot, rangeGrid->viewDir, 
1767                                       connectAll);
1768              }
1769              else if (in3 == -1) {
1770                  addTriangleUseNormal(mesh, vin1, vin2, vin4, 
1771                                       MinViewDot, rangeGrid->viewDir, 
1772                                       connectAll);
1773              }
1774              else {    /* in4 == -1 */
1775                  addTriangleUseNormal(mesh, vin1, vin2, vin3, 
1776                                       MinViewDot, rangeGrid->viewDir, 
1777                                       connectAll);
1778              }
1779          }
1780      }
1781    }
1782
1783  /* free up the vertex index list */
1784  delete [] vert_index;
1785
1786  mesh->initNormals(); 
1787
1788  find_mesh_edges(mesh);
1789
1790  return (mesh);
1791}
1792
1793
1794
1795/******************************************************************************
1796Create a triangle mesh from scan data.
1797
1798Entry:
1799
1800
1801Exit:
1802  returns pointer to newly-created mesh
1803******************************************************************************/
1804
1805Mesh *
1806cleanMeshFromRangeMesh(Mesh *inMesh)
1807{
1808  int i,j;
1809  Vec3f v1, v2, vect1, vect2, vect3, vect4;
1810  int ii,jj;
1811  int in1,in2,in3,in4,vin1,vin2,vin3,vin4;
1812  Vertex *vt1,*vt2,*vt3,*vt4;
1813  int count;
1814  int max_lt,max_lg;
1815  Mesh *mesh;
1816  int nlt,nlg,max_verts,max_tris;
1817  mesh = new Mesh;
1818  Vec3f viewDir;
1819
1820  uchar colorThresh = MinColor;
1821
1822  if (Warn)
1823      printf("meshFromGrid(): Not computing useful max_length!!\n");
1824
1825  mesh->numVerts = inMesh->numVerts;
1826  mesh->verts = new Vertex[mesh->numVerts];
1827
1828  mesh->numTris = 0;
1829  max_tris = inMesh->numTris;
1830  mesh->tris = new Triangle[max_tris];
1831
1832  mesh->isWarped = inMesh->isWarped;
1833  mesh->isRightMirrorOpen = inMesh->isRightMirrorOpen;
1834  mesh->hasConfidence = inMesh->hasConfidence;
1835  mesh->hasColor = inMesh->hasColor;
1836
1837  if (!mesh->isWarped) {
1838      viewDir.setValue(0, 0, -1);
1839  } else {
1840      if (mesh->isRightMirrorOpen) {
1841          viewDir.setValue(-sin(30*M_PI/180), 0, -cos(30*M_PI/180));
1842      } else {
1843          viewDir.setValue(sin(30*M_PI/180), 0, -cos(30*M_PI/180));
1844      }
1845  }
1846
1847  /* create the vertices */
1848
1849  for (i = 0; i < inMesh->numVerts; i++) {
1850    mesh->verts[i].coord = inMesh->verts[i].coord;
1851
1852    mesh->verts[i].maxVerts = 8;
1853    mesh->verts[i].verts = new Vertex*[8];
1854    mesh->verts[i].edgeLengths = new float[8];
1855    mesh->verts[i].numVerts = 0;
1856
1857    mesh->verts[i].maxTris = 8;
1858    mesh->verts[i].tris = new Triangle*[8];
1859    mesh->verts[i].numTris = 0;
1860
1861    mesh->verts[i].holeFill = FALSE;
1862    mesh->verts[i].stepsToEdge = -1;
1863   
1864    if (inMesh->hasConfidence)
1865        mesh->verts[i].confidence = inMesh->verts[i].confidence;
1866    else 
1867        mesh->verts[i].confidence = 1;
1868
1869    if (inMesh->hasColor)
1870       mesh->verts[i].red = inMesh->verts[i].red;
1871       mesh->verts[i].green = inMesh->verts[i].green;
1872       mesh->verts[i].blue = inMesh->verts[i].blue;
1873  }
1874
1875  /* create the triangles */
1876  for (i = 0; i < inMesh->numTris; i++) {
1877
1878     if (inMesh->hasColor) {
1879        if (inMesh->verts[inMesh->tris[i].vindex1].red < colorThresh ||
1880            inMesh->verts[inMesh->tris[i].vindex2].red < colorThresh ||
1881            inMesh->verts[inMesh->tris[i].vindex3].red < colorThresh) {
1882           continue;
1883        }
1884     }
1885
1886     if (UseEdgeLength) {
1887        addTriangleUseLength(mesh, inMesh->tris[i].vindex1, 
1888                             inMesh->tris[i].vindex2, 
1889                             inMesh->tris[i].vindex3, 
1890                             MaxEdgeLength, 0);
1891     } else {
1892        addTriangleUseNormal(mesh, inMesh->tris[i].vindex1, 
1893                             inMesh->tris[i].vindex2, 
1894                             inMesh->tris[i].vindex3,
1895                             MinViewDot, viewDir, 0);
1896     }
1897  }
1898
1899  mesh->initNormals(); 
1900
1901  find_mesh_edges(mesh);
1902
1903  return (mesh);
1904}
1905
1906
1907static void
1908addTriangleUseNormal(Mesh *mesh, int vin1, int vin2, int vin3, 
1909                     float minDot, Vec3f &viewDir, int connectAll)
1910{
1911    Vec3f c1, c2, c3;
1912    Vec3f v1, v2, v3;
1913    Triangle *tri;
1914    Vertex *vert1, *vert2, *vert3;
1915    int tooGrazing;
1916    float dot;
1917
1918    c1.setValue(mesh->verts[vin1].coord);
1919    c2.setValue(mesh->verts[vin2].coord);
1920    c3.setValue(mesh->verts[vin3].coord);
1921
1922    v1 = c1 - c2;
1923    v2 = c1 - c3;
1924    v3 = v2.cross(v1);
1925    v3.normalize();
1926    dot = fabs(v3.dot(viewDir));
1927
1928    tooGrazing = dot < minDot;
1929    if (tooGrazing && !connectAll)
1930        return;
1931
1932    // No check on limits here???
1933    tri = &mesh->tris[mesh->numTris];
1934
1935    tri->vindex1 = vin1;
1936    tri->vindex2 = vin2;
1937    tri->vindex3 = vin3;
1938
1939    vert1 = &mesh->verts[vin1];
1940    vert2 = &mesh->verts[vin2];
1941    vert3 = &mesh->verts[vin3];
1942
1943    vert1->tris[vert1->numTris++] = tri;
1944    vert2->tris[vert2->numTris++] = tri;
1945    vert3->tris[vert3->numTris++] = tri;
1946
1947    addNeighbors(vert1,vert2);
1948    addNeighbors(vert1,vert3);
1949    addNeighbors(vert2,vert3);
1950
1951    if (tooGrazing) {
1952        vert1->holeFill = 1;
1953        vert2->holeFill = 1;
1954        vert3->holeFill = 1;
1955        vert1->stepsToEdge = -1;
1956        vert2->stepsToEdge = -1;
1957        vert3->stepsToEdge = -1;
1958    }
1959
1960    mesh->numTris++;
1961}
1962
1963static void
1964addTriangleUseLength(Mesh *mesh, int vin1, int vin2, int vin3, 
1965                     float maxLength, int connectAll)
1966{
1967    Vec3f c1, c2, c3;
1968    Vec3f v;
1969    Triangle *tri;
1970    Vertex *vert1, *vert2, *vert3;
1971    int tooLong;
1972
1973    c1.setValue(mesh->verts[vin1].coord);
1974    c2.setValue(mesh->verts[vin2].coord);
1975    c3.setValue(mesh->verts[vin3].coord);
1976
1977    v = c1 - c2;
1978    tooLong = v.length() > maxLength;
1979    if (tooLong && !connectAll)
1980        return;
1981
1982    v = c1 - c3;
1983    tooLong = tooLong || v.length() > maxLength;
1984    if (tooLong && !connectAll)
1985        return;
1986
1987    v = c2 - c3;
1988    tooLong = tooLong || v.length() > maxLength;
1989    if (tooLong && !connectAll)
1990        return;
1991
1992    // No check on limits here???
1993    tri = &mesh->tris[mesh->numTris];
1994
1995    tri->vindex1 = vin1;
1996    tri->vindex2 = vin2;
1997    tri->vindex3 = vin3;
1998
1999    vert1 = &mesh->verts[vin1];
2000    vert2 = &mesh->verts[vin2];
2001    vert3 = &mesh->verts[vin3];
2002
2003    vert1->tris[vert1->numTris++] = tri;
2004    vert2->tris[vert2->numTris++] = tri;
2005    vert3->tris[vert3->numTris++] = tri;
2006
2007    addNeighbors(vert1,vert2);
2008    addNeighbors(vert1,vert3);
2009    addNeighbors(vert2,vert3);
2010
2011    if (tooLong) {
2012        vert1->holeFill = 1;
2013        vert2->holeFill = 1;
2014        vert3->holeFill = 1;
2015        vert1->stepsToEdge = -1;
2016        vert2->stepsToEdge = -1;
2017        vert3->stepsToEdge = -1;
2018    }
2019
2020    mesh->numTris++;
2021}
2022
2023
2024void
2025addNeighbors(Vertex *v1, Vertex *v2)
2026{
2027    float length;
2028    Vec3f a;
2029    int found = 0;
2030
2031    for (int i = 0; i < v1->numVerts; i++) {
2032        if (v1->verts[i] == v2) {
2033            found = 1;
2034            break;
2035        }
2036    }
2037
2038    if (!found) {
2039        length = 0.0005;
2040        a = v1->coord - v2->coord;
2041        length = a.length();
2042
2043        if (v1->numVerts == v1->maxVerts)
2044           reallocVerts(v1);
2045       
2046        if (v2->numVerts == v2->maxVerts)
2047           reallocVerts(v2);
2048       
2049        v1->edgeLengths[v1->numVerts] = length;
2050        v2->edgeLengths[v2->numVerts] = length;
2051        v1->verts[v1->numVerts++] = v2;
2052        v2->verts[v2->numVerts++] = v1;
2053    }
2054}
2055
2056
2057/******************************************************************************
2058Find which vertices of a mesh are on the edge of the mesh.
2059
2060Entry:
2061  mesh - mesh to find edges of
2062******************************************************************************/
2063
2064void
2065find_mesh_edges(Mesh *mesh)
2066{
2067  int i;
2068
2069  /* examine each vertex of a mesh to see if it's on the edge */
2070
2071  for (i = 0; i < mesh->numVerts; i++)
2072    vertex_edge_test (mesh, &mesh->verts[i]);
2073}
2074
2075
2076/******************************************************************************
2077Mark a vertex to say whether it is on the edge of a mesh or not.
2078
2079Entry:
2080  vert: vertex to mark as on edge or not
2081
2082Exit:
2083  returns 0 if everything was okay, 1 if the mesh is build funny
2084******************************************************************************/
2085
2086int 
2087vertex_edge_test(Mesh *mesh, Vertex *vert)
2088{
2089  int i;
2090  Triangle *tri;
2091  unsigned char on_edge;
2092  static int been_here = 0;
2093  int bad_mesh = 0;
2094
2095  /* initialize list that counts how many times an edge has been marked */
2096  /* (each edge should be marked twice, once for each triangle, */
2097  /*  otherwise the vertex is on an edge) */
2098
2099  for (i = 0; i < vert->numVerts; i++)
2100    vert->verts[i]->count = 0;
2101
2102  on_edge = 0;  /* assume we're not on an edge */
2103
2104  /* go through each triangle of vertex, counting how many times an */
2105  /* adjacent vertex has been used */
2106
2107  for (i = 0; i < vert->numTris; i++) {
2108    tri = vert->tris[i];
2109
2110    /* count the participation of each vertex of the triangle */
2111    mesh->verts[tri->vindex1].count++;
2112    mesh->verts[tri->vindex2].count++;
2113    mesh->verts[tri->vindex3].count++;
2114
2115
2116    // This prevents us from operating on them as vertices
2117    // internal to the mesh
2118    if (mesh->verts[tri->vindex1].holeFill)
2119        on_edge = 1;
2120
2121    if (mesh->verts[tri->vindex2].holeFill)
2122        on_edge = 1;
2123
2124    if (mesh->verts[tri->vindex3].holeFill)
2125        on_edge = 1;
2126  }
2127
2128  /* examine the counts of the neighboring vertices to see if they */
2129  /* prove the vertex to be an edge.  also check for mesh consistancy */
2130
2131  for (i = 0; i < vert->numVerts; i++) {
2132    if (vert->verts[i]->count == 1) {
2133      on_edge = 1;
2134    }
2135    else if (vert->verts[i]->count == 2) {
2136      /* this is okay */
2137    }
2138    else {
2139      /* if we're here, our mesh is built wrong */
2140      bad_mesh = 1;
2141      if (!been_here) {
2142        been_here = 1;
2143        fprintf (stderr, "vertex_edge_test: %d count on an edge\n",
2144                 vert->verts[i]->count);
2145        /*
2146        fprintf (stderr, "(You'll only get this message once, so beware!)\n");
2147        */
2148      }
2149    }
2150  }
2151
2152  vert->on_edge = on_edge;
2153
2154  /* say if the mesh is improperly built */
2155  return (bad_mesh);
2156}
2157
Note: See TracBrowser for help on using the repository browser.