开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
RangeGrid.cc
资源名称：scanalyze-1.0.3_source_code.rar　[点击查看]
上传用户：kellyonhid
上传日期：2013-10-12
资源大小：932k
文件大小：20k
源码类别：

3D图形编程

开发平台：

Visual C++

RangeGrid.cc：源码内容
  1. #include <stdio.h>
  2. #include "ply++.h"
  3. #include <stdlib.h>
  4. #include "strings.h"
  5. #include "plvGlobals.h"
  6. #include "RangeGrid.h"
  7. #include "Mesh.h"
  8. #include <iostream.h>
  9. #ifdef WIN32
  10. #include <io.h>
  11. #endif
  12. #include <malloc.h>
  13. #include <math.h>
  14. #include <fcntl.h>
  15. #ifndef linux
  16. #    include <stdexcept.h>
  17. #endif
  18. #include "cyfile.h"
  19. #include "defines.h"
  20. #include "Xform.h"
  21. #include "Pnt3.h"
  23. struct PlyVertex {
  24.   float x,y,z;
  25.   float confidence;
  26.   float intensity;
  27.   unsigned char red,grn,blu;
  28. };
  30. PlyProperty vert_std_props[] = {
  31.   {"x", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,x), 0, 0, 0, 0},
  32.   {"y", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,y), 0, 0, 0, 0},
  33.   {"z", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,z), 0, 0, 0, 0},
  34.   {"confidence", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,confidence), 0, 0, 0,0},
  35.   {"intensity", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,intensity), 0, 0, 0,0},
  36.   {"diffuse_red", PLY_UCHAR, PLY_UCHAR, offsetof(PlyVertex,red), 0, 0, 0, 0},
  37.   {"diffuse_green", PLY_UCHAR, PLY_UCHAR, offsetof(PlyVertex,grn), 0, 0, 0, 0},
  38.   {"diffuse_blue", PLY_UCHAR, PLY_UCHAR, offsetof(PlyVertex,blu), 0, 0, 0, 0},
  39.   {"std_dev", PLY_FLOAT, PLY_FLOAT, offsetof(PlyVertex,confidence), 0, 0, 0,0},
  40. };
  43. struct RangePnt {
  44.   unsigned char num_pts;
  45.   int *pts;
  46. };
  48. // list of property information for a range data point
  49. PlyProperty range_props[] = {
  50.   {"vertex_indices", PLY_INT, PLY_INT, offsetof(RangePnt,pts),
  51.    1, PLY_UCHAR, PLY_UCHAR, offsetof(RangePnt,num_pts)},
  52. };
  55. static float RANGE_DATA_SIGMA_FACTOR = 4;
  56. static float RANGE_DATA_MIN_INTENSITY = 0.05;
  59. RangeGrid::RangeGrid()
  60. {
  61.   coords = NULL;
  62.   confidence = NULL;
  63.   intensity = NULL;
  64.   matDiff = NULL;
  65.   indices = NULL;
  66.   
  67.   hasColor = FALSE;
  68.   hasConfidence = FALSE;
  69.   hasIntensity = FALSE;
  70.   hasTexture = FALSE;
  71.   
  72.   obj_info = NULL;
  73.   num_obj_info = 0;
  74.   isModelMakerScan = FALSE;
  75. }
  78. RangeGrid::~RangeGrid()
  79. {
  80.   delete [] coords;
  81.   
  82.   delete [] indices;
  83.   
  84.   if (num_obj_info > 0) {
  85.     for (int i = 0; i < num_obj_info; i++)
  86.       delete [] obj_info[i];
  87.     delete [] obj_info;
  88.   }
  89.   
  90.   if (hasConfidence)
  91.     delete [] confidence;
  92.   
  93.   if (hasColor)
  94.     delete [] matDiff;
  95.   
  96.   if (hasIntensity)
  97.     delete [] intensity;
  98. }
  101. int 
  102. is_range_grid_file(const char *filename)
  103. {
  104.   int nelems;
  105.   char **elist;
  107.   PlyFile ply;
  108.   if (ply.open_for_reading(filename, &nelems, &elist) == 0)
  109.     return 0;
  110.   
  111.   for (int i = 0; i < nelems; i++) {
  112.     if (!strcmp(elist[i], "range_grid"))
  113.       return 1;
  114.   }
  115.   return 0;
  116. }
  119. /*****************************************************************
  120. Read range data from a PLY file.
  122. Entry:
  123.   name - name of PLY file to read from
  125. Exit:
  126.   returns pointer to data, or NULL if it couldn't read from file
  127. ******************************************************************/
  129. int
  130. RangeGrid::readRangeGrid(const char *name)
  131. {
  132.   int i,j,k,index, best_index;
  133.   PlyFile ply;
  134.   int num_elems;
  135.   int nprops;
  136.   int nelems;
  137.   char **elist;
  138.   PlyProperty **plist;
  139.   int num_rows,num_cols;
  140.   RangePnt range_pnt;
  141.   PlyVertex vert;
  142.   char *elem_name;
  143.   int get_std_dev = 0;
  144.   int get_confidence = 0;
  145.   int get_intensity = 0;
  146.   int get_color = 0;
  147.   int has_red = 0;
  148.   int has_green = 0;
  149.   int has_blue = 0;
  150.   int is_warped = 0;
  151.   char temp[PATH_MAX];
  152.   int isRightMirrorOpen = 1;
  153.   //int found_mirror = 0;  //unused?
  154.   int foundScanType = FALSE;
  156.   float conf,std;
  157.   float min_std,max_std,max;
  158.   float avg_std = 0;
  160.   if (ply.open_for_reading(name, &nelems, &elist) == 0)
  161.     return 0;
  163.   // parse the obj_info
  165.   char **ply_obj_info;
  166.   int ply_num_obj_info;
  167.   ply_obj_info = ply.get_obj_info (&ply_num_obj_info);
  168.   for (i = 0; i < ply_num_obj_info; i++) {
  169.     if (strstr(ply_obj_info[i], "num_cols"))
  170.       sscanf(ply_obj_info[i], "%s%d", temp, &num_cols);
  171.     if (strstr(ply_obj_info[i], "num_rows"))
  172.       sscanf(ply_obj_info[i], "%s%d", temp, &num_rows);
  173.     if (strstr(ply_obj_info[i], "is_warped"))
  174.       sscanf(ply_obj_info[i], "%s%d", temp, &is_warped);
  175.     if (strstr(ply_obj_info[i], "optimum_std_dev"))
  176.       sscanf(ply_obj_info[i], "%s%f", temp, &avg_std);
  177.     if (strstr(ply_obj_info[i], "is_right_mirror_open")) {
  178.       //found_mirror = TRUE;
  179.       sscanf(ply_obj_info[i], "%s%d", temp, &isRightMirrorOpen);
  180.     }
  181.     if (strstr(ply_obj_info[i], "is_linear_scan")) {
  182.       foundScanType = TRUE;
  183.       sscanf(ply_obj_info[i], "%s%d", temp, &isLinearScan);
  184.     }
  185.   }
  187.   min_std = avg_std / RANGE_DATA_SIGMA_FACTOR;
  188.   max_std = avg_std * RANGE_DATA_SIGMA_FACTOR;
  190.   // set up the range data structure
  191.   nlg = num_cols;
  192.   nlt = num_rows;
  193.   intensity = NULL;
  194.   confidence = NULL;
  195.   matDiff = NULL;
  196.   hasColor = 0;
  197.   hasIntensity = 0;
  198.   hasConfidence = 0;
  199.   multConfidence = 0;
  200.   hasTexture = 0;
  202.   ltMin = 0;
  203.   ltMax = nlt-1;
  204.   lgMin = 0;
  205.   lgMax = nlg-1;
  208.   if (!foundScanType) {
  209.     printf("Assumed to be linear scan...n");
  210.     isLinearScan = TRUE;
  211.   }
  213.   if (!is_warped) {
  214.     viewDir.set(0, 0, -1);
  215.   } else {
  216.     if (isRightMirrorOpen) {
  217.       viewDir.set(-sin(30*M_PI/180), 0, -cos(30*M_PI/180));
  218.     } else {
  219.       viewDir.set(sin(30*M_PI/180), 0, -cos(30*M_PI/180));
  220.     }
  221.   }
  223.   num_obj_info = ply_num_obj_info;
  224.   obj_info = new char*[ply_num_obj_info];
  225.   for (i = 0; i < ply_num_obj_info; i++) {
  226.     obj_info[i] = new char[strlen(ply_obj_info[i])+1];
  227.     strcpy(obj_info[i], ply_obj_info[i]);
  228.   }
  230.   // see if we've got both vertex and range_grid data
  232.   plist = ply.get_element_description("vertex", &num_elems, &nprops);
  233.   if (plist == NULL) {
  234.     fprintf (stderr, "file doesn't contain vertex datan");
  235.     return 0;
  236.   }
  237.   numSamples = num_elems;
  238.   plist = ply.get_element_description("range_grid", &num_elems, &nprops);
  239.   if (plist == NULL) {
  240.     fprintf (stderr, "file doesn't contain range_grid datan");
  241.     return 0;
  242.   }
  244.   // read in the range data
  246.   for (i = 0; i < nelems; i++) {
  247.     elem_name = elist[i];
  248.     plist = ply.get_element_description(elem_name, &num_elems, &nprops);
  250.     if (equal_strings ("vertex", elem_name)) {
  252.       // see if the file contains intensities
  253.       for (j = 0; j < nprops; j++) {
  254.         if (equal_strings ("std_dev", plist[j]->name))
  255.           get_std_dev = 1;
  256.         if (equal_strings ("confidence", plist[j]->name))
  257.           get_confidence = 1;
  258.         if (equal_strings ("intensity", plist[j]->name))
  259.           get_intensity = 1;
  260.         if (equal_strings ("diffuse_red", plist[j]->name))
  261.           has_red = 1;
  262.         if (equal_strings ("diffuse_green", plist[j]->name))
  263.           has_green = 1;
  264.         if (equal_strings ("diffuse_blue", plist[j]->name))
  265.           has_blue = 1;
  266.       }
  268.       if (has_red && has_green && has_blue) {
  269.         get_color = 1;
  270.         hasColor = 1;
  271.       }
  273.       if (get_intensity)
  274.         hasIntensity = 1;
  276.       if (get_std_dev && is_warped) {
  277.         hasConfidence = 1;
  278.         multConfidence = 1;
  279.       }
  280.       else if (get_confidence)
  281.         hasConfidence = 1;
  283.       ply.get_property ("vertex", &vert_std_props[0]);
  284.       ply.get_property ("vertex", &vert_std_props[1]);
  285.       ply.get_property ("vertex", &vert_std_props[2]);
  286.       if (get_confidence)     
  287.         ply.get_property ("vertex", &vert_std_props[3]);
  288.       if (get_intensity)             
  289.         ply.get_property ("vertex", &vert_std_props[4]);
  290.       if (get_color) {    
  291.         ply.get_property ("vertex", &vert_std_props[5]);
  292.         ply.get_property ("vertex", &vert_std_props[6]);
  293.         ply.get_property ("vertex", &vert_std_props[7]);
  294.       }
  295.       if (get_std_dev)
  296.         ply.get_property ("vertex", &vert_std_props[8]);
  297.       coords = new Pnt3[numSamples];
  299.       if (get_confidence || get_std_dev)
  300. confidence = new float[numSamples];
  302.       if (get_intensity)
  303. intensity = new float[numSamples];
  305.       if (get_color)
  306. matDiff = new vec3uc[numSamples];
  308.       for (j = 0; j < num_elems; j++) {
  309.         __STL_TRY {
  310.   ply.get_element ((void *) &vert);
  311. }
  312. __STL_CATCH_ALL {
  313.   
  314.   return NULL;
  315. }
  318.         coords[j].set(&vert.x);
  320.         if (get_intensity) {
  321.           intensity[j] = vert.intensity;
  322. }
  324.         if (get_std_dev&&is_warped) {
  326.           std = vert.confidence;
  328.           if (std < min_std)
  329.             conf = 0;
  330.           else if (std < avg_std)
  331.             conf = (std - min_std) / (avg_std - min_std);
  332.           else if (std > max_std)
  333.             conf = 0;
  334.           else
  335.     conf = (max_std - std) / (max_std - avg_std);
  337.   // Unsafe to use vertex intensity, as aperture 
  338.   // settings may change between scans.  
  339.   // Instead, use std_dev confidence * orientation.
  340.   // conf *= vert.intensity;
  342.   if (get_intensity)
  343.     if (vert.intensity < RANGE_DATA_MIN_INTENSITY) conf = 0.0;
  345.           confidence[j] = conf;
  346.         }
  347.         else if (get_confidence) {
  348.           confidence[j] = vert.confidence;
  349.         }
  351.         if (get_color) {
  352.           matDiff[j][0] = vert.red;
  353.           matDiff[j][1] = vert.grn;
  354.           matDiff[j][2] = vert.blu;
  355.         }
  356.       }
  357.     }
  359.     if (equal_strings ("range_grid", elem_name)) {
  360.       indices = new int[nlt * nlg];
  362.       ply.get_element_setup(elem_name, 1, range_props);
  363.       for (j = 0; j < num_elems; j++) {
  365. __STL_TRY {   
  366.   ply.get_element((void *) &range_pnt);
  367. }
  368. __STL_CATCH_ALL {
  369.   return NULL;
  370. }
  372.         if (range_pnt.num_pts == 0)
  373.           indices[j] = -1;
  374.         else {
  375.   max = -FLT_MAX;
  376.   for (k = 0; k < range_pnt.num_pts; k++) {
  377.     index = range_pnt.pts[k];
  378.     // There will only be more than one point per sample
  379.     // if there are intensities
  380.     if (get_intensity) {
  381.       if (intensity[index] > max) {
  382. max = intensity[index];
  383. best_index = index;
  384.       }
  385.     }
  386.     else {
  387.       best_index = index;
  388.     }
  389.   }
  390.   index = best_index;
  391.   if (get_confidence || get_std_dev) {
  392.     if (confidence[index] > 0.0)  
  393.       indices[j] = index;
  394.     else
  395.       indices[j] = -1;
  396.   } else {
  397.     indices[j] = index;
  398.   }
  400.   if (get_intensity) {
  401.     if (intensity[index] < RANGE_DATA_MIN_INTENSITY) {
  402.       indices[j] = -1;
  403.     }
  404.   }
  406.   delete range_pnt.pts;
  407. }
  408.       }
  409.     }
  410.   }
  411.   return 1;
  412. }
  416. /*****************************************************************
  417. Create a triangle mesh from scan data.
  419. Entry:
  420.   sc         - scan data to make into triangle mesh
  421.   level      - level of mesh (how detailed), in [0,1,2,3]
  422.   table_dist - distance for spatial subdivision hash table
  424. Exit:
  425.   returns pointer to newly-created mesh
  426. ******************************************************************/
  428. Mesh *
  429. RangeGrid::toMesh(int subSamp, int hasTexture)
  430. {
  431.   int i,j;
  432.   int ii,jj;
  433.   int in1,in2,in3,in4,vin1,vin2,vin3,vin4;
  434.   int max_tris;
  436.   Mesh *mesh = new Mesh;
  438.   int _ltMin = ltMax - (ltMax/subSamp)*subSamp;
  439.   
  440.   // create a list saying whether a vertex is going to be used
  442.   int *vert_index = new int[numSamples];
  443.   for (i = 0; i < numSamples; i++)
  444.     vert_index[i] = -1;
  446.   // Extra column of vertices for wrap-around
  447.   int *vert_index_extra = new int[nlt];
  448.   for (i = _ltMin; i <= ltMax; i++)
  449.     vert_index_extra[i] = -1;
  452.   // see which vertices will be used in a triangle
  454.   int count = 0;
  455.   for (j = _ltMin; j <= ltMax; j += subSamp) {
  456.     for (i = lgMin; i <= lgMax; i += subSamp) {
  457.       in1 = indices[i + j * nlg];
  458.       if (in1 >= 0) {
  459. vert_index[in1] = 1;
  460. count++;
  461.       }
  462.     }
  464.     if (!isLinearScan) {
  465.       i = lgMin;
  466.       in1 = indices[i + j * nlg];
  467.       if (in1 >= 0) {
  468. vert_index_extra[j] = 1;
  469. count++;
  470.       }
  471.     }
  472.   }
  474.   printf("Num verts = %dn", count);
  476.   if (hasConfidence)
  477.     mesh->vertConfidence = new float[count];
  479.   if (hasTexture)
  480.     mesh->texture = new vec2f[count];
  482.   if (hasIntensity)
  483.     mesh->vertIntensity = new float[count];
  485.   if (matDiff)
  486.     mesh->vertMatDiff = new vec3uc[count];
  488.   max_tris = count * 6;
  489.   mesh->tris.reserve(max_tris);
  491.   // create the vertices
  493.   count = 0;
  494.   for (j = _ltMin; j <= ltMax; j += subSamp) {
  495.     for (i = lgMin; i <= lgMax; i += subSamp) {
  496.       in1 = indices[i + j * nlg];
  497.       if (in1 >= 0) {
  498. vert_index[in1] = count;
  499. mesh->vtx.push_back(coords[in1]);
  500. if (hasConfidence)
  501.   mesh->vertConfidence[count] = confidence[in1];
  503. if (hasIntensity)
  504.   mesh->vertIntensity[count] = intensity[in1];
  506. if (hasColor) {
  507.   mesh->vertMatDiff[count][0] = matDiff[in1][0];
  508.   mesh->vertMatDiff[count][1] = matDiff[in1][1];
  509.   mesh->vertMatDiff[count][2] = matDiff[in1][2];
  510. }
  512. count++;
  513.       }
  514.     }
  516.     if (!isLinearScan) {
  517.       i = lgMin;
  518.       in1 = indices[i + j * nlg];
  519.       if (in1 >= 0) {
  520. vert_index_extra[j] = count;
  522. mesh->vtx.push_back(coords[in1]);
  523. if (hasConfidence)
  524.   mesh->vertConfidence[count] = confidence[in1];
  526. if (hasIntensity)
  527.   mesh->vertIntensity[count] = intensity[in1];
  529. if (hasColor) {
  530.   mesh->vertMatDiff[count][0] = matDiff[in1][0];
  531.   mesh->vertMatDiff[count][1] = matDiff[in1][1];
  532.   mesh->vertMatDiff[count][2] = matDiff[in1][2];
  533. }
  535. count++;
  536.       }
  537.     }
  538.   }
  540.   // Assign texture coordinates
  542.   if (hasTexture) {
  543.     for (j = _ltMin; j <= ltMax; j += subSamp) {
  544.       for (i = lgMin; i <= lgMax; i += subSamp) {
  545. in1 = indices[i + j * nlg];
  546. if (in1 >= 0) {
  547.   vin1 = vert_index[in1];
  548.   mesh->texture[vin1][0] = float(i-lgMin)/(lgMax-lgMin+1);
  549.   mesh->texture[vin1][1] = float(j)/nlt;
  550. }
  551.       }
  552.       if (!isLinearScan) {
  553. i = lgMin;
  554. in1 = indices[i + j * nlg];
  555. if (in1 >= 0) {
  556.   vin1 = vert_index_extra[j];
  557.   mesh->texture[vin1][0] = 1 + 1.0/(lgMax - lgMin + 1);
  558.   mesh->texture[vin1][1] = float(j)/nlt;
  559.   count++;
  560. }
  561.       }
  562.     }
  563.   }
  566.   // create the triangles
  568.   for (j = _ltMin; j <= ltMax - subSamp; j += subSamp) {
  569.     for (i = lgMin; i <= lgMax - subSamp; i += subSamp) {
  571.       ii = (i + subSamp) % nlg;
  572.       jj = (j + subSamp) % nlt;
  574.       // count the number of good vertices
  575.       // 2 3
  576.       // 1 4
  577.       in1 = indices[ i +  j * nlg];
  578.       in2 = indices[ i + jj * nlg];
  579.       in3 = indices[ii + jj * nlg];
  580.       in4 = indices[ii +  j * nlg];
  581.       count = (in1 >= 0) + (in2 >= 0) + (in3 >= 0) + (in4 >=0);
  582.       if (in1 >= 0) {
  583. vin1 = vert_index[in1];
  584.       }
  585.       if (in2 >= 0) {
  586. vin2 = vert_index[in2];
  587.       }
  588.       if (in3 >= 0) {
  589. vin3 = vert_index[in3];
  590.       }
  591.       if (in4 >= 0) {
  592. vin4 = vert_index[in4];
  593.       }
  595.       if (count == 4) { // all 4 vertices okay, so make 2 tris
  596. // compute lengths of cross-edges
  597. float len1 = dist(mesh->vtx[vin1], mesh->vtx[vin3]);
  598. float len2 = dist(mesh->vtx[vin2], mesh->vtx[vin4]);
  600. if (len1 < len2) {
  601.   mesh->addTri(vin2, vin1, vin3);
  602.   mesh->addTri(vin1, vin4, vin3);
  603. } else {
  604.   mesh->addTri(vin2, vin1, vin4);
  605.   mesh->addTri(vin2, vin4, vin3);
  606. }
  607.       }
  608.       else if (count == 3) { // only 3 vertices okay, so make 1 tri
  609. if        (in1 == -1) {
  610.   mesh->addTri(vin2, vin4, vin3);
  611. } else if (in2 == -1) {
  612.   mesh->addTri(vin1, vin4, vin3);
  613. } else if (in3 == -1) {
  614.   mesh->addTri(vin2, vin1, vin4);
  615. } else { // in4 == -1
  616.   mesh->addTri(vin2, vin1, vin3);
  617. }
  618.       }
  619.     }
  620.   }
  621.   
  623.   // Do the wrap around for cylindrical scans
  624.   if (!isLinearScan) {
  626.     ii = lgMin;
  628.     for (j = _ltMin; j <= ltMax - subSamp; j += subSamp) {
  629.       jj = (j + subSamp) % nlt;
  631.       // count the number of good vertices
  632.       in1 = indices[ i +  j * nlg];
  633.       in2 = indices[ i + jj * nlg];
  634.       in3 = indices[ii + jj * nlg];
  635.       in4 = indices[ii +  j * nlg];
  636.       count = (in1 >= 0) + (in2 >= 0) + (in3 >= 0) + (in4 >=0);
  638.       if (in1 >= 0) {
  639. vin1 = vert_index[in1];
  640.       }
  641.       if (in2 >= 0) {
  642. vin2 = vert_index[in2];
  643.       }
  644.       if (in3 >= 0) {
  645. vin3 = vert_index_extra[jj];
  646.       }
  647.       if (in4 >= 0) {
  648. vin4 = vert_index_extra[j];
  649.       }
  651.       if (count == 4) { // all 4 vertices okay, so make 2 tris
  652. // compute lengths of cross-edges
  653. float len1 = dist(mesh->vtx[vin1], mesh->vtx[vin3]);
  654. float len2 = dist(mesh->vtx[vin2], mesh->vtx[vin4]);
  656. if (len1 < len2) {
  657.   mesh->addTri(vin2, vin1, vin3);
  658.   mesh->addTri(vin1, vin4, vin3);
  659. } else {
  660.   mesh->addTri(vin2, vin1, vin4);
  661.   mesh->addTri(vin2, vin4, vin3);
  662. }
  663.       }
  664.       else if (count == 3) { // only 3 vertices okay, so make 1 tri
  665. if        (in1 == -1) {
  666.   mesh->addTri(vin2, vin4, vin3);
  667. } else if (in2 == -1) {
  668.   mesh->addTri(vin1, vin4, vin3);
  669. } else if (in3 == -1) {
  670.   mesh->addTri(vin2, vin1, vin4);
  671. } else { // in4 == -1
  672.   mesh->addTri(vin2, vin1, vin3);
  673. }
  674.       }
  675.     }
  676.   }
  678.   mesh->hasVertNormals = FALSE;
  680.   mesh->initNormals(UseAreaWeightedNormals);
  682.   if (!isLinearScan) {
  683.     for (j = _ltMin; j <= ltMax; j += subSamp) {
  684.       in1 = indices[lgMin + j * nlg];
  686.       if (in1 >= 0) {
  687. vin1 = vert_index[in1];
  688. vin2 = vert_index_extra[j];
  689. mesh->nrm[3*vin1  ] += mesh->nrm[3*vin2  ];
  690. mesh->nrm[3*vin1+1] += mesh->nrm[3*vin2+1];
  691. mesh->nrm[3*vin1+2] += mesh->nrm[3*vin2+2];
  692. Pnt3 temp (mesh->nrm[3*vin1],
  693.    mesh->nrm[3*vin1+1], mesh->nrm[3*vin1+2]);
  694. temp /= 32767;
  695. temp.normalize();
  696. temp *= 32767;
  697. mesh->nrm[3*vin1  ] = mesh->nrm[3*vin2  ] = temp[0];
  698. mesh->nrm[3*vin1+1] = mesh->nrm[3*vin2+1] = temp[1];
  699. mesh->nrm[3*vin1+2] = mesh->nrm[3*vin2+2] = temp[2];
  700.       }
  701.     }
  702.   }
  704.   // free up the vertex index list
  705.   delete [] vert_index;
  706.   delete [] vert_index_extra;
  708.   if (isModelMakerScan) mesh->remove_stepedges(50, 8);
  709.      else mesh->remove_stepedges(50, 4);
  711.   return mesh;
  712. }
  715. void
  716. RangeGrid::addTriangleCheckNormal(Mesh *mesh, 
  717.   int vin1, int vin2, int vin3, 
  718.   Pnt3 &viewDir, float minDot)
  719. {
  720.   float _dot = fabs(dot(viewDir, 
  721. normal(mesh->Vtx(vin1), mesh->Vtx(vin2),
  722.        mesh->Vtx(vin3))));
  723.   int tooGrazing = _dot < minDot;
  724.   if (tooGrazing)
  725.     return;
  727.   mesh->addTri(vin2, vin1, vin3);
  728. }
  731. void
  732. RangeGrid::addTriangleCheckLength(Mesh *mesh, 
  733.   int vin1, int vin2, int vin3, 
  734.   float maxLength)
  735. {
  736.   const Pnt3 &c1 = mesh->Vtx(vin1);
  737.   const Pnt3 &c2 = mesh->Vtx(vin2);
  738.   const Pnt3 &c3 = mesh->Vtx(vin3);
  740.   if (dist(c1,c2) > maxLength)
  741.     return;
  743.   if (dist(c1,c3) > maxLength)
  744.     return;
  746.   if (dist(c2,c3) > maxLength)
  747.     return;
  749.   mesh->addTri(vin2, vin1, vin3);
  750. }
  753. int
  754. RangeGrid::readCyfile(const char *name)
  755. {
  756.   int xx, yy;
  757.   float x, y, z;
  758.   int count;
  759.   long range;
  761.   int fd = open(name, O_RDONLY, 0);
  763.   GSPEC *gs;
  764.   gs = cyread(NULL, fd);
  766.   float lgincr = 1e-6*gs->lgincr;
  767.   float ltincr = 1e-6*gs->ltincr;
  768.   float theta;
  769.   count = 0;
  770.   for (yy = 0;  yy < gs->nlt; yy++) {
  771.     for (xx = 0;  xx < gs->nlg; xx++) {
  772.       range = getr(gs, yy, xx);
  773.       if (range != VOIDGS(gs) ) {
  774. count++;
  775.       }
  776.     }
  777.   }
  779.   numSamples = count;
  780.   isLinearScan = gs->flags & FLAG_CARTESIAN;
  781.   nlg = gs->nlg;
  782.   nlt = gs->nlt;
  783.   coords = new Pnt3[numSamples];
  784.   indices = new int[nlt * nlg];
  785.   viewDir.set(0, 0, -1);
  787.   count = 0;
  788.   int diff;
  789.   int range1, range2;
  791.   if (!isLinearScan) {
  793.     /* For moving targets (like heads), the begin and
  794.        end points will differ noticably.  This is an attempt
  795.        to counteract that motion by shifting the range data
  796.        by an average amount of discrepancy.  A better job
  797.        would be to find a more complex rigid or even
  798.        non-rigid deformation that would match the points. 
  799.        Even better, find new matches iteratively as in Besl
  800.        and McKay. */
  802.     diff = 0;
  803.     for (yy = 0;  yy < gs->nlt; yy++) {
  804.       range1 = getr(gs, yy, 0);
  805.       range2 = getr(gs, yy, gs->nlg-1);
  806.       if (range1 != VOIDGS(gs) && range2 != VOIDGS(gs) ) {
  807. count++;
  808. diff += (range2 - range1);
  809.       }
  810.     }
  811.     diff = int(float(diff)/count);
  812.   }
  814.   count = 0;
  815.   int lg;
  816.   int yymin = 100000;
  817.   int yymax = -1;
  818.   int xxmax = -1;
  819.   int xxmin = 100000;
  821.   for (yy = 0;  yy < gs->nlt; yy++) {
  822.     for (xx = 0;  xx < gs->nlg; xx++) {
  823.       range = getr(gs, yy, xx);
  824.       if (isLinearScan) {
  825. lg = xx;
  826.       } else {
  827. lg = gs->nlg - xx - 1;
  828.       }
  829.       if (range == VOIDGS(gs) ) {
  830. indices[lg + yy*nlg] = -1;
  831.       }
  832.       else {
  833. yymax = yy > yymax ? yy : yymax;
  834. xxmax = lg > xxmax ? lg : xxmax;
  835. yymin = yy < yymin ? yy : yymin;
  836. xxmin = lg < xxmin ? lg : xxmin;
  837. indices[lg + yy*nlg] = count;
  838. y = yy*ltincr;
  839. if (isLinearScan) {
  840.   x = xx*lgincr - (gs->nlg/2) * lgincr;
  841.   z = range*1e-6;
  842. } else {
  843.   //range -= int(diff*float(xx)/gs->nlg);
  844.   theta = xx*lgincr + M_PI;
  845.   x = -range*1e-6*sin(theta);
  846.   z = range*1e-6*cos(theta);     
  847. }
  848. coords[count].set(x,y,z);
  849. count++;
  850.       }
  851.     }
  852.   }    
  854.   if (!isLinearScan) {
  855.     ltMax = yymax;
  856.     lgMax = xxmax;
  857.     ltMin = yymin;
  858.     lgMin = xxmin;
  859.   } else {
  860.     ltMax = gs->nlt-1;
  861.     lgMax = gs->nlg-1;
  862.     ltMin = 0;
  863.     lgMin = 0;
  864.   }
  866.   // To handle color shifts - ack!
  867.   /*
  868.     lgMin++;
  869.     lgMax--;
  870.   */
  872.   cyfree(gs);
  873.   return 1;
  874. }