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

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

3D图形编程

开发平台：

Visual C++

CyraResLevel.cc：源码内容
  1. //############################################################
  2. // 
  3. // CyraResLevel.cc
  4. //
  5. // Lucas Pereira
  6. // Thu Jul 16 15:20:47 1998
  7. //
  8. // Part of CyraScan.cc,
  9. //    Store range scan information from a Cyra Cyrax Beta 
  10. //    time-of-flight laser range scanner.
  11. //
  12. //############################################################
  14. #include "CyraResLevel.h"
  15. #include <tcl.h>
  16. #include <stdio.h>
  17. #include <iostream.h>
  18. #include <fstream.h>
  19. #include <stack.h>
  20. #include <stdlib.h>
  21. #include "defines.h"
  22. #include "TriMeshUtils.h"
  23. #include "KDindtree.h"
  24. #include "ColorUtils.h"
  25. #include "plvScene.h"
  26. #include "MeshTransport.h"
  27. #include "VertexFilter.h"
  29. #ifdef WIN32
  30. #  define random rand
  31. #endif
  33. // The maximum depth distance across which it will tesselate.
  34. // (Bigger than this, it assumes a noncontinuous surface)
  35. #define DEFAULT_CYRA_TESS_DEPTH 80;
  36. float CyraTessDepth = DEFAULT_CYRA_TESS_DEPTH;
  38. // Default to filtering on...
  39. #define DEFAULT_CYRA_FILTER_SPIKES TRUE;
  40. bool CyraFilterSpikes = DEFAULT_CYRA_FILTER_SPIKES;
  42. // Default to fill holes  on...
  43. #define DEFAULT_CYRA_FILL_HOLES TRUE;
  44. bool CyraFillHoles = DEFAULT_CYRA_FILL_HOLES;
  49. //////////////////////////////////////////////////////////////////////
  50. // CyraSample   (One single data point)
  51. // Helper functions...
  52. //////////////////////////////////////////////////////////////////////
  54. void 
  55. CyraSample::zero(void) {
  56.   vtx[0] = vtx[1] = vtx[2] = 0;
  57.   nrm[0] = nrm[1] = nrm[2] = 0;
  58.   intensity = 0;
  59.   confidence = 0;
  60. }
  61.   
  62. // interp 2 samples
  63. void 
  64. CyraSample::interp(CyraSample &a, CyraSample &b) {
  65.   for (int i = 0; i < 3; i++) {
  66.     vtx[i] = 0.5 * (a.vtx[i] + b.vtx[i]);
  67.     nrm[i] = 0.5 * (a.nrm[i] + b.nrm[i]);
  68.   }
  69.   intensity = 0.5 * (a.intensity + b.intensity);
  70.   confidence = 0.5 * (a.confidence + b.confidence);
  71. }
  73. // interp 3 samples, weighted 1/4, 1/2, 1/4
  74. void 
  75. CyraSample::interp(CyraSample &a, CyraSample &b, CyraSample &c) {
  76.   for (int i = 0; i < 3; i++) {
  77.     vtx[i] = 0.25 * (a.vtx[i] + b.vtx[i]+b.vtx[i] + c.vtx[i]);
  78.     nrm[i] = 0.25 * (a.nrm[i] + b.nrm[i]+b.nrm[i] + c.nrm[i]);
  79.   }
  80.   intensity = 0.25 * (a.intensity + b.intensity+b.intensity + c.intensity);
  81.   confidence = 0.25 * (a.confidence + b.confidence+b.confidence + 
  82.        c.confidence);
  83. }
  84.  
  85. // Interpolate arbitrary number of samples, weighted evenly...
  86. void 
  87. CyraSample::interp(int nsamps, CyraSample *samps[]) {
  88.   assert(nsamps > 0);
  89.   this->zero();
  90.   float wt = 1.0 / nsamps;
  91.   // add up the values of every sample...
  92.   for (int samnum = 0; samnum < nsamps; samnum++) {
  93.     for (int i=0; i < 3; i++) {
  94.       vtx[i] += samps[samnum]->vtx[i];
  95.       nrm[i] += samps[samnum]->nrm[i];
  96.     }
  97.     intensity += samps[samnum]->intensity;
  98.     confidence += samps[samnum]->confidence;
  99.   }
  100.   // divide by the appropriate weight...
  101.   for (int i=0; i < 3; i++) {
  102.     vtx[i] /= wt;
  103.     nrm[i] /= wt;
  104.   }
  105.   intensity /= wt;
  106.   confidence /= wt;
  107. }
  110.  
  111. bool 
  112. CyraSample::isValid(void) {
  113.   return((vtx[2] > 0) ? TRUE : FALSE);
  114. }
  117. //////////////////////////////////////////////////////////////////////
  118. // CyraResLevel  (one resolution level for a cyra scan)
  119. //////////////////////////////////////////////////////////////////////
  122.  
  123. CyraResLevel::CyraResLevel(void) {
  124.   // Clear arrays
  125.   width = 0;
  126.   height = 0;
  127.   numpoints = 0;
  128.   numtris = 0;
  129.   points.clear();
  130.   tris.clear();
  131.   origin.set(0,0,0);
  133.   isDirty_cache = true;
  134.   cachedPoints.clear();
  135.   cachedNorms.clear();
  136.   cachedBoundary.clear();
  137.   kdtree = NULL;
  139. }
  141. CyraResLevel::~CyraResLevel(void) {
  142.   // do nothing yet
  143.   if (kdtree) delete kdtree;
  144. }
  147. // PUSH_TRI --  basically a macro used by the mesh-generation function
  148. // to look up the 3 vertices in vert_inds, and do tstrips if necessary
  149. inline void PUSH_TRI(int &ov2, int &ov3, vector<int> &tri_inds,
  150.       const bool stripped,  
  151.       const vector<int> &vert_inds,
  152.       const int v1, const int v2, const int v3)
  153. { 
  154.   // will next tri be odd or even? (even tris get flipped)
  155.   static bool oddtri = true; 
  157.   if (stripped) { 
  158.     if (oddtri && (ov2 == v2 && ov3 == v1)) { 
  159.       // odd-numbered triangle
  160.       tri_inds.push_back(vert_inds[v3]); 
  161.       oddtri = false;
  162.     } else if (!oddtri && (ov2 == v1 && ov3 == v3)) { 
  163.       // even-numbered triangle -- listed backwards 
  164.       tri_inds.push_back(vert_inds[v2]); 
  165.       oddtri = true;
  166.     } else { 
  167.       tri_inds.push_back(-1); 
  168.       tri_inds.push_back(vert_inds[v1]); 
  169.       tri_inds.push_back(vert_inds[v2]); 
  170.       tri_inds.push_back(vert_inds[v3]); 
  171.       oddtri = false;
  172.     } 
  173.     ov2 = v2; 
  174.     ov3 = v3; 
  175.   } else {  
  176.     tri_inds.push_back(vert_inds[v1]); 
  177.     tri_inds.push_back(vert_inds[v2]); 
  178.     tri_inds.push_back(vert_inds[v3]); 
  179.   } 
  180. }
  185.      
  186. // Add the geometry for this reslevel to the list
  187. MeshTransport*
  188. CyraResLevel::mesh(bool perVertex, bool stripped,
  189.    ColorSource color, int colorSize)
  190. {
  191.   // Offset (index) for the next vertex;
  192.   int vertoffset = 0;
  194.   ////// Vertices /////
  195.   // The valid vertices need contiguous index numbers.  So fill an
  196.   // array vert_inds -- index numbers with a 1-1 correspondence
  197.   // to the array of vertices.
  198.   vector<int> vert_inds;
  199.   vert_inds.reserve(points.size());
  200.   // Set the vert_inds array
  201.   for (int i = 0; i < points.size(); i++) {
  202.     vert_inds.push_back((points[i].confidence > 0) ? 
  203. vertoffset++ : -1);
  204.   }
  206.   vector<Pnt3>* vtx = new vector<Pnt3>;
  207.   vector<short>* nrm = new vector<short>;
  208.   vector<int>* tri_inds = new vector<int>;
  210.   // Add each valid vertex to vtx, nrm(?)
  211.   FOR_EACH_VERT(vtx->push_back(v->vtx));
  212.   if (perVertex) {
  213.     FOR_EACH_VERT(nrm->insert (nrm->end(), v->nrm, v->nrm + 3));
  214.   } else {
  215.     FOR_EACH_TRI(pushNormalAsShorts
  216.  (*nrm, ((Pnt3)cross(points[tv3].vtx-points[tv2].vtx, 
  217.      points[tv1].vtx-points[tv2].vtx))
  218.   .normalize()));
  219.   }
  221.   ////// Triangles /////
  222.   // Now fill in the triangle array
  223.   // Initialize the "old v2, v3" values used in tstripping
  224.   int ov2 = -1; int ov3 = -1;
  226.   // call the FOR_EACH_TRI macro, which defines tv1, tv2, tv3.
  227.   FOR_EACH_TRI(PUSH_TRI(ov2, ov3, *tri_inds, stripped, 
  228. vert_inds, tv1, tv2, tv3));
  230.   // Terminate the last tstrip
  231.   if (stripped) {
  232.     tri_inds->push_back(-1);
  233.   }
  235.   MeshTransport* mt = new MeshTransport;
  236.   mt->setVtx (vtx, MeshTransport::steal);
  237.   mt->setNrm (nrm, MeshTransport::steal);
  238.   mt->setTris (tri_inds, MeshTransport::steal);
  240.   if (color != RigidScan::colorNone) {
  241.     vector<uchar>* colors = new vector<uchar>;
  242.     int nColors = perVertex ? vtx->size() : num_tris();
  243.     colors->reserve (colorSize * nColors);
  245.     if (colorMesh (*colors, perVertex, color, colorSize))
  246.       mt->setColor (colors, MeshTransport::steal);
  247.     else
  248.       delete colors;
  249.   }
  251.   return mt;
  252. }
  255. // Fill in the colors array.
  256. bool 
  257. CyraResLevel::colorMesh(vector<uchar>   &colors,
  258. bool             perVertex,
  259. ColorSource      source,
  260. int              colorsize)
  261. {
  262.   switch (source) {
  263.   case RigidScan::colorConf:
  264.     // ========= Color by Confidence =========
  265.     if (perVertex) {
  266.       cerr << "adding per-vertex conf color..." << endl;
  267.       // per-vertex confidence
  268.       FOR_EACH_VERT(pushColor(colors, colorsize, (float)
  269.       (v->confidence / 
  270.        CYRA_DEFAULT_CONFIDENCE)));
  271.     } else {
  272.       cerr << "adding per-face conf color..." << endl;
  273.       // per-face confidence (take min of 3 confidences)
  274.       FOR_EACH_TRI(pushColor(colors, colorsize, (float)
  275.      (MIN(points[tv1].confidence,
  276.        MIN(points[tv2].confidence,
  277.    points[tv3].confidence)) / 
  278.    CYRA_DEFAULT_CONFIDENCE)));
  279.     }
  280.     break;
  282.   case RigidScan::colorIntensity:
  283.     // ========= Color by Intensity =========
  284.     if (perVertex) {
  285.       // per-vertex intensity
  286.       FOR_EACH_VERT(pushColor(colors, colorsize, (float)
  287.       ((v->intensity+2048.0) / 4096)));
  288.     } else {
  289.       // per-face intensity (take min of 3 intensitys)
  290.       FOR_EACH_TRI(pushColor(colors, colorsize, (float)
  291.      ((MIN(points[tv1].intensity,
  292.    MIN(points[tv2].intensity,
  293.        points[tv3].intensity)) + 2048.0) /
  294.       4096)));
  295.     }
  297.   case RigidScan::colorTrue:
  298.     // ========== Color TrueColor ==========
  299.     if (perVertex) {
  300.       // per-vertex TrueColor
  301.       // BUGBUG: For now, just color using intensity, because
  302.       // I don't yet store colors for CyraScans.
  303.       FOR_EACH_VERT(pushColor(colors, colorsize, (float)
  304.       ((v->intensity+2048.0) / 4096)));
  305.     } else {
  306.       // per-face TrueColor (take avg of 3 TrueColors)
  307.       FOR_EACH_TRI(pushColor(colors, colorsize, (float)
  308.      ((MIN(points[tv1].intensity,
  309.    MIN(points[tv2].intensity,
  310.        points[tv3].intensity)) + 2048.0) /
  311.       4096)));
  312.     }
  314.   case RigidScan::colorBoundary:
  315.     {
  316.       create_kdtree();  // BUGBUG, this is a little heavyhanded approach
  317.       colors.reserve (colorsize * cachedBoundary.size());
  318.       bool* end = cachedBoundary.end();
  319.       for (bool* c = cachedBoundary.begin(); c < end; c++)
  320. pushConf (colors, colorsize, (uchar)(*c ? 0 : 255));
  321.     }
  322.     break;
  324.   default:
  325.     cerr << "Unhandled Color Mode.  colorsource = " << source << endl;
  326.     return false;
  327.   }
  329.   return true;
  330. }
  333. bool
  334. CyraResLevel::ReadPts(const crope &inname) 
  335. {
  336.   // open the input .pts file
  337.   const char* filename = inname.c_str();
  338.   FILE *inFile = fopen(filename, "r");
  339.   if (inFile==NULL) return FALSE;
  340.   bool isOldPtsFormat = false; // old or new cyra .pts?
  342.   // Read the 3-line .pts header
  343.   fscanf(inFile, "%dn", &width);
  344.   fscanf(inFile, "%dn", &height);
  345.   float a,b,c;
  346.   fscanf(inFile, "%f %f %fn", &a, &b, &c);
  347.   origin.set(a,b,c);
  349.   cerr << "Reading " << width << "x" << height << " Cyra scan...";
  351.   // Read in the points, since this is the only data actually in
  352.   // a .pts file....
  353.   CyraSample samp;
  355.   // Figure out whether it's the old or new cyra format
  356.   // old: X new: X
  357.   //      Y       Y
  358.   //      0 0 0 0 0 0
  359.   //    x1 y1 z1 c1 nx1 ny1 nz1 1 0 0
  360.   //    x2 y2 z2 c2 nx2 ny2 nz2 0 1 0
  361.   //    x3 y3 z3 c3 nx3 ny3 nz3 0 0 1
  362.   //    ...                   1 0 0 0
  363.   //                         0 1 0 0 
  364.   //                           0 0 1 0
  365.   //                         0 0 0 1
  366.   // x1 y1 z1 c1
  367.   // x2 y2 z2 c2
  368.   // ...
  369.   char buf[2000];
  370.   int n1, n2, n3;
  371.   // Read first line after 0 0 0 
  372.   fgets(buf, 2000, inFile);
  373.   int nitems = sscanf(buf, "%g %g %g %d %d %d %dn", 
  374.       &(samp.vtx[0]), &(samp.vtx[1]), &(samp.vtx[2]), 
  375.       &(samp.intensity),
  376.       &n1, &n2, &n3);
  377.   if (nitems == 7) {
  378.     // was the x1 y1 z1 c1 nx1 ny1 nz1 line... old format
  379.     isOldPtsFormat = true;
  380.     cerr << " (old format) ";
  381.   } else if (nitems == 3) {
  382.     // was the new format
  383.     isOldPtsFormat = false;
  384.     cerr << " (new format) ";
  385.     // NOTE:  Skip (ignore) the 3+4 matrix lines
  386.     for (int j=0; j < 7; j++) {
  387.       fgets(buf, 2000,inFile);
  388.     }
  389.   }
  390.   // Now, our assertion is that first line of data is read into
  391.   // buf...
  394.   // First reserve space, so we don't waste time doing multi mallocs...
  395.   points.reserve(width * height);
  396.   for (int x=0; x < width; x++) {
  397.     for (int y=0; y < height; y++) {
  398.       // Read position, intensity, normals
  399.       Pnt3 filenrm;
  401.       // Whether isOldPtsFormat or not, just ignore the normals that
  402.       // are in the .pts file, for now...  Since the normals Cyra generates
  403.       // are generally bogus anyways.
  404.       int nread = sscanf(buf, "%f %f %f %dn", 
  405.  &(samp.vtx[0]), &(samp.vtx[1]), &(samp.vtx[2]), 
  406.  &(samp.intensity));
  407.       fgets(buf, 2000, inFile);
  408.       if (nread != 4) {
  409. int lineno = ((isOldPtsFormat)?4:11) + x*height + y;
  410. cerr << "Error: Trouble reading Cyra input line " << lineno 
  411.      << "." << endl;
  412. return false;
  413.       }
  414.      
  415.       samp.nrm[0] = 0; samp.nrm[1] = 0; samp.nrm[2] = 32767;
  416.       // Convert to millimeters
  417.       samp.vtx *= 1000.0;
  419.       // Compute confidence 
  420.       if (samp.vtx[0] == 0. && samp.vtx[1] == 0. &&
  421.   samp.vtx[2] == 0. && samp.intensity == 0) {
  422. // then point is missing data
  423. samp.confidence = 0.0;
  424.       } else {
  425. samp.confidence = CYRA_DEFAULT_CONFIDENCE;
  426. // count number of valid points
  427. numpoints++;
  428.  
  429. // unweight specular highlights -- linearly
  430. // make the weight falloff to 0 between intensities
  431. // -800 and 2048
  432. if (samp.intensity > -800) {
  433.   float unweightfactor = (2048.0 - samp.intensity) / 
  434.     (2048.0 - (-800));
  435.   samp.confidence *= unweightfactor;
  436. }
  437. // also make the weight falloff between -1600 and -2048
  438. if (samp.intensity < -1600) {
  439.   float unweightfactor = (2048.0 + samp.intensity) / 
  440.     (2048.0 - 1600.0);
  441.   samp.confidence *= unweightfactor;
  442. }
  443.       }
  444.     
  445.       // Now put the vertex into the array
  446.       points.push_back(samp);
  447.     }
  448.   }
  449.   cerr << "done." << endl;
  451.   // Now actually figure out the "correct" tesselation
  453.   // first reserve space, so we don't waste time doing multi mallocs...
  454.   tris.reserve((width-1) * (height-1));
  456.   // Grab the CyraTessDepth from TCL...
  457.   char* CTDstring = Tcl_GetVar (g_tclInterp, "CyraTessDepth", TCL_GLOBAL_ONLY);
  458.   if (CTDstring == NULL) {
  459.     CyraTessDepth = DEFAULT_CYRA_TESS_DEPTH;
  460.   } else {
  461.     // String defined, get it...?
  462.     sscanf(CTDstring, "%f", &CyraTessDepth);
  463.     // If 0, then set to default
  464.     if (CyraTessDepth == 0) {
  465.       CyraTessDepth = DEFAULT_CYRA_TESS_DEPTH;
  466.     }
  467.   }
  469.   // Grab the CyraFilterSpikes from TCL...
  470.   char* CFSstring = Tcl_GetVar (g_tclInterp, "CyraFilterSpikes", TCL_GLOBAL_ONLY);
  471.   if (CFSstring == NULL) {
  472.     CyraFilterSpikes = DEFAULT_CYRA_FILTER_SPIKES;
  473.   } else {
  474.     // String defined, get it...?
  475.     int CFSval;
  476.     sscanf(CFSstring, "%d", &CFSval);
  477.     CyraFilterSpikes = (CFSval == 0) ? FALSE : TRUE;
  478.   }
  480.   // Grab the CyraFillHoles from TCL...
  481.   char* CFHstring = Tcl_GetVar (g_tclInterp, "CyraFillHoles", TCL_GLOBAL_ONLY);
  482.   if (CFHstring == NULL) {
  483.     CyraFillHoles = DEFAULT_CYRA_FILTER_SPIKES;
  484.   } else {
  485.     // String defined, get it...?
  486.     int CFHval;
  487.     sscanf(CFHstring, "%d", &CFHval);
  488.     CyraFillHoles = (CFHval == 0) ? FALSE : TRUE;
  489.   }
  491.   ////////////// Filter out spikes?  ///////////
  492.   if (CyraFilterSpikes) {
  493.     cerr << "Filtering out high-intensity spikes....";
  494.     for (x=0; x < width-1; x++) {
  495.       for (int y=0; y < height-1; y++) {
  496. CyraSample *v1 = &(point(x,y));
  498. // Look for random single spikes of noise...
  499. float myZ  = v1->vtx[2];
  500. int myI = v1->intensity;
  501. float neighborZ = 0;
  502. float neighborI = 0;
  503. float neighborWt = 0;
  504. int neighborsDeeper = 0;
  505. int neighborsCloser = 0;
  507.       // over 3x3 neighborhood...
  508.       // within 300mm depth...
  509. for (int nex = MAX(0,x-1); nex <= MIN(width-1, x+1); nex++) {
  510.   for (int ney = MAX(0,y-1); ney <= MIN(width-1, y+1); ney++) {
  511.     CyraSample *ne = &point(nex, ney);
  512.     if (ne->vtx[2] < myZ) neighborsDeeper++;
  513.     else neighborsCloser++;
  514.     if (ne->vtx[2] > myZ - 150 && ne->vtx[2] < myZ + 150 &&
  515. (nex != x || ney != y) && ne->intensity < myI) {
  516.       float neWt = myI - ne->intensity;
  517.       neighborZ += ne->vtx[2] * neWt;
  518.       neighborI += ne->intensity *neWt;
  519.       neighborWt += neWt;
  520.     }
  521.   }
  522. }
  524. if (neighborWt != 0) {
  525.   neighborZ /= neighborWt;
  526.   neighborI /= neighborWt;
  527.       
  528.   if (neighborsDeeper <=2 && neighborsCloser >= 5 &&
  529.     myI > neighborI + 20) {
  530.     // This point seems to be farther away, and brighter, than
  531.     // most of it's neighbors... We think it's the funny artifact...
  533.     //cerr << "before dd: " << (myZ-neighborZ) << " " << (myI-neighborI) 
  534.     //     << " depths: " << myZ << " , " << neighborZ << " ; intens: " 
  535.     //     << myI << " , " << neighborI << endl;
  536.     float posScale = neighborZ / myZ;
  537.     v1->vtx[0] *= posScale;
  538.     v1->vtx[1] *= posScale;
  539.     v1->vtx[2] *= posScale;
  540.     v1->intensity = neighborI;
  541.   }
  542. }
  543.       }  // end of filtering spikes...
  544.     }
  545.     cerr << "Done! (filtering)n";
  546.   }
  548.   ////////////// Fill holes? ///////////
  549.   if (CyraFillHoles) {
  550.     cerr << "Filling Tiny Holes....";
  551.     for (x=0; x < width-1; x++) {
  552.       for (int y=0; y < height-1; y++) {
  553. CyraSample *v1 = &(point(x,y));
  555. float myZ  = v1->vtx[2];
  556. float neighborsZ = 0;
  557. float neighborsWt = 0;
  558. int xstart = MAX(0, x-1);
  559. int ystart = MAX(0, y-1);
  560. int xend = MIN(width-1, x+1);
  561. int yend = MIN(height-1, y+1);
  563. // over 3x3 neighborhood...
  564. // find the mean...
  565. for (int nex = xstart; nex <= xend; nex++) {
  566.   for (int ney = ystart; ney <= yend; ney++) {
  567.     CyraSample *ne = &point(nex, ney);
  568.     if (nex != x || ney != y) {
  569.       neighborsZ += ne->vtx[2];
  570.       neighborsWt += 1.0;
  571.     }
  572.   }
  573. }
  574. neighborsZ /= neighborsWt;
  576. // Find the neighbor closest to the mean...
  577. // over 3x3 neighborhood...
  578. float closestZ = point(xstart, ystart).vtx[2];
  579. float closestDZ2 = (neighborsZ-closestZ)*(neighborsZ-closestZ);
  580. for (nex = xstart; nex <= xend; nex++) {
  581.   for (int ney = ystart; ney <= yend; ney++) {
  582.     CyraSample *ne = &point(nex, ney);
  583.     float neDZ2 = (neighborsZ-ne->vtx[2])*(neighborsZ-ne->vtx[2]);
  584.     if (neDZ2 < closestDZ2) {
  585.       closestDZ2 = neDZ2;
  586.       closestZ = ne->vtx[2];
  587.     }
  588.   }
  589. }
  591. // Set neighborsZ to be closestZ
  592. // (this way, if we have one outlier, we'll still grab from the
  593. // z depth of the main cluster...)
  594. neighborsZ = closestZ;
  596. // Make sure the neighbors are clustered closely together...
  597. // say, within 150mm of each other...?
  598. int neighborsClose = 0;
  599. float neighborX = 0;
  600. float neighborY = 0;
  601. float neighborZ = 0;
  602. float neighborI = 0;
  603. float neighborConf = 0;
  605. for (nex = xstart; nex <= xend; nex++) {
  606.   for (int ney = ystart; ney <= yend; ney++) {
  607.     CyraSample *ne = &point(nex, ney);
  608.     if ((nex != x || ney != y) &&
  609. (ne->vtx[2] < neighborsZ + CyraTessDepth && 
  610.  ne->vtx[2] > neighborsZ - CyraTessDepth)) {
  611.       neighborsClose++;
  612.       neighborX += ne->vtx[0];
  613.       neighborY += ne->vtx[1];
  614.       neighborZ += ne->vtx[2];
  615.       neighborI += ne->intensity;
  616.       neighborConf += ne->confidence;
  617.     }
  618.   }
  619. }
  621. //cerr <<" "<<neighborsClose<< ", nz " <<neighborsZ<< " , myz " <<myZ<< endl;
  623. // Only fill holes of data off by more than 150mm...
  624. // e.g. make sure this point is far from the neighborhood,
  625. if (neighborsWt < 7 || neighborsClose < 7 || 
  626.     (neighborsZ < myZ + CyraTessDepth && 
  627.      neighborsZ > myZ - CyraTessDepth)) continue;
  629. // otherwise, modify the puppy....
  630. v1->vtx[0] = neighborX / neighborsClose;
  631. v1->vtx[1] = neighborY / neighborsClose;
  632. v1->vtx[2] = neighborZ / neighborsClose;
  633. v1->intensity = neighborI / neighborsClose;
  634. v1->confidence = neighborConf / neighborsClose;
  635.       }  // end of filling holes...
  636.     }
  637.     cerr << "Done! (filling holes)n";
  638.   }
  640.   
  641.   ////////////// Generate the Tesselation ///////////
  642.   cerr << "Generating tesselation...";
  643.   for (x=0; x < width-1; x++) {
  644.     for (int y=0; y < height-1; y++) {
  645.       // get pointers to the four vertices surrounding this
  646.       // square:
  647.       // 2 4 
  648.       // 1 3
  649.       CyraSample *v1 = &(point(x,y));
  650.       CyraSample *v2 = &(point(x,y+1));
  651.       CyraSample *v3 = &(point(x+1,y));
  652.       CyraSample *v4 = &(point(x+1,y+1));
  654.       CyraTess tess = TESS0;
  655.       // Set mask bit to be true if a vertex:
  656.       //   a) exists (has confidence)
  657.       //   b) is not an occlusion edge
  658.       unsigned int mask = 
  659. ((v4->confidence && !grazing(v3->vtx, v4->vtx, v2->vtx))? 8 : 0) +
  660. ((v3->confidence && !grazing(v1->vtx, v3->vtx, v4->vtx))? 4 : 0) +
  661. ((v2->confidence && !grazing(v4->vtx, v2->vtx, v1->vtx))? 2 : 0) +
  662. ((v1->confidence && !grazing(v2->vtx, v1->vtx, v3->vtx))? 1 : 0);
  664.       switch (mask) {
  665.       case 15:
  666. // verts: 1 2 3 4 
  667. tess = TESS14;
  668. numtris += 2;
  669. break;
  670.       case 14:
  671. // verts: 2 3 4 
  672. tess = TESS4;
  673. numtris++;
  674. break;
  675.       case 13:
  676. // verts: 1 3 4
  677. tess = TESS3;
  678. numtris++;
  679. break;
  680.       case 11:
  681. // verts 1 2 4
  682. tess = TESS2;
  683. numtris++;
  684. break;
  685.       case 7:
  686. // verts 1 2 3
  687. tess = TESS1;
  688. numtris++;
  689. break;
  690.       default:
  691. // two or less vertices
  692. tess = TESS0;
  693. break;
  694.       }
  696.       // Now put the tesselation into the array
  697.       tris.push_back(tess);
  699.     }
  700.   }
  702.   cerr << "Done! (tesselating)" << endl;
  703.   CalcNormals();
  705.   cerr << "Loaded " << numpoints << " vertices, " <<
  706.     numtris << " triangles." << endl;
  707.   return true;
  708. }
  711. bool
  712. CyraResLevel::WritePts(const crope &outname) 
  713. {
  714.   CyraSample samp;
  716.   // open the output .pts file
  717.   const char* filename = outname.c_str();
  718.   FILE *outFile = fopen(filename, "w");
  719.   if (outFile==NULL) {
  720.     cerr << "Error: couldn't open output file " << filename << endl;
  721.     return FALSE;
  722.   }
  724.   // compute bounds, so we can crop to the smallest rectangle
  725.   // that contains all the data.
  726.   int lox   = width-1;
  727.   int hix   = 0;
  728.   int loy   = height-1;
  729.   int hiy   = 0;
  730.   for (int x=0; x < width; x++) {
  731.     for (int y=0; y < height; y++) {
  732.       samp = point(x,y);
  733.       if (samp.confidence != 0) {
  734.   lox = (x<lox) ? x : lox;
  735.   hix = (x>hix) ? x : hix;
  736. loy = (y<loy) ? y : loy;
  737. hiy = (y>hiy) ? y : hiy;
  738.       }
  739.     }
  740.   }
  741.   if (lox > hix || loy > hiy) {
  742.     cerr << "Error:  no nonzero-points found.  Aborting..." << endl;
  743.     return FALSE;
  744.   }
  746.   // Write the 3-line .pts header
  747.   fprintf(outFile, "%dn", hix - lox + 1);
  748.   fprintf(outFile, "%dn", hiy - loy + 1);
  749.   fprintf(outFile, "%f %f %fn", origin[0], origin[1], origin[2]);
  750.   cerr << "Writing " << (hix-lox+1) << "x" << (hiy-loy+1) << " Cyra scan...";
  752.   // Write out the points, since this is the only data actually in
  753.   // a .pts file....
  754.   for (x=lox; x <= hix; x++) {
  755.     for (int y=loy; y <= hiy; y++) {
  756.       samp = point(x,y);
  757.       if (samp.confidence == 0) {
  758. fprintf(outFile, "0 0 0 0 0 0 0n");
  759.       } else {
  760. // divide by 1000, since .pts files are always in meters
  761. fprintf(outFile, "%.5f %.5f %.5f %d 0 0 1n", 
  762. samp.vtx[0]/1000.0, samp.vtx[1]/1000.0, samp.vtx[2]/1000.0,
  763. samp.intensity);
  764.       }
  765.     }
  766.   }
  767.   fclose(outFile);
  768.   cerr << "Done." << endl;
  769.   return TRUE;
  770. }
  772. // Recompute the normals
  773. void
  774. CyraResLevel::CalcNormals(void)
  775. {
  776.   // Compute Normals
  777.   Pnt3 hedge, vedge, norm;
  778.   Pnt3 v1, v2, v3, v4;
  779.   CyraTess tess;
  780.   
  781.   for (int x=0; x < width; x++) {
  782.     for (int y=0; y < height; y++) {
  783.       if (point(x,y).confidence > 0) {
  784. hedge.set(0,0,0);
  785. vedge.set(0,0,0);
  786. // Lower left corner
  787. if (x >0 && y > 0) {
  788.   v1 = point(x-1, y-1).vtx;
  789.   v2 = point(x-1, y  ).vtx;
  790.   v3 = point(x  , y-1).vtx;
  791.   v4 = point(x  , y  ).vtx;
  793.   tess = tri(x-1, y-1);
  794.   switch (tess) {
  795.   case TESS2:
  796.     hedge += 0.5*(v4-v2);
  797.     vedge += 0.5*(v2-v1);
  798.     break;
  799.   case TESS3:
  800.     hedge += 0.5*(v3-v1);
  801.     vedge += 0.5*(v4-v3);
  802.     break;
  803.   case TESS4:
  804.   case TESS14:
  805.     hedge += 1.0*(v4-v2);
  806.     vedge += 1.0*(v4-v3);
  807.     break;
  808.   case TESS23:
  809.     hedge += 0.5*(v4-v2);
  810.     hedge += 0.5*(v3-v1);
  811.     vedge += 0.5*(v2-v1);
  812.     vedge += 0.5*(v4-v3);
  813.     break;
  814.   }
  815. }
  817. // Upper left corner
  818. if (x > 0 && y < height-1) {
  819.   v1 = point(x-1,y  ).vtx;
  820.   v2 = point(x-1,y+1).vtx;
  821.   v3 = point(x  ,y  ).vtx;
  822.   v4 = point(x  ,y+1).vtx;
  824.   tess = tri(x-1,y);
  825.   switch (tess) {
  826.   case TESS1:
  827.     hedge += 0.5*(v3-v1);
  828.     vedge += 0.5*(v2-v1);
  829.     break;
  830.   case TESS3:
  831.   case TESS23:
  832.     hedge += 1.0*(v3-v1);
  833.     vedge += 1.0*(v4-v3);
  834.     break;
  835.   case TESS4:
  836.     hedge += 0.5*(v4-v2);
  837.     vedge += 0.5*(v4-v3);
  838.     break;
  839.   case TESS14:
  840.     hedge += 0.5*(v4-v2);
  841.     hedge += 0.5*(v3-v1);
  842.     vedge += 0.5*(v2-v1);
  843.     vedge += 0.5*(v4-v3);
  844.     break;
  845.   }
  846. }
  848. // Lower right corner
  849. if (x < width-1 && y > 0) {
  850.   v1 = point(x  ,y-1).vtx;
  851.   v2 = point(x  ,y  ).vtx;
  852.   v3 = point(x+1,y-1).vtx;
  853.   v4 = point(x+1,y  ).vtx;
  855.   tess = tri(x,y-1);
  856.   switch (tess) {
  857.   case TESS1:
  858.     hedge += 0.5*(v3-v1);
  859.     vedge += 0.5*(v2-v1);
  860.     break;
  861.   case TESS2:
  862.   case TESS23:
  863.     hedge += 1.0*(v4-v2);
  864.     vedge += 1.0*(v2-v1);
  865.     break;
  866.   case TESS4:
  867.     hedge += 0.5*(v4-v2);
  868.     vedge += 0.5*(v4-v3);
  869.     break;
  870.   case TESS14:
  871.     hedge += 0.5*(v4-v2);
  872.     hedge += 0.5*(v3-v1);
  873.     vedge += 0.5*(v2-v1);
  874.     vedge += 0.5*(v4-v3);
  875.     break;
  876.   }
  877. }
  879. // Upper right corner
  880. if (x < width-1 && y < height-1) {
  881.   v1 = point(x  ,y  ).vtx;
  882.   v2 = point(x  ,y+1).vtx;
  883.   v3 = point(x+1,y  ).vtx;
  884.   v4 = point(x+1,y+1).vtx;
  886.   tess = tri(x,y);
  887.   switch (tess) {
  888.   case TESS1:
  889.   case TESS14:
  890.     hedge += 1.0*(v3-v1);
  891.     vedge += 1.0*(v2-v1);
  892.     break;
  893.   case TESS2:
  894.     hedge += 0.5*(v4-v2);
  895.     vedge += 0.5*(v2-v1);
  896.     break;
  897.   case TESS3:
  898.     hedge += 0.5*(v3-v1);
  899.     vedge += 0.5*(v4-v3);
  900.     break;
  901.   case TESS23:
  902.     hedge += 0.5*(v4-v2);
  903.     hedge += 0.5*(v3-v1);
  904.     vedge += 0.5*(v2-v1);
  905.     vedge += 0.5*(v4-v3);
  906.     break;
  907.   }
  908. }
  910. // Now compute cross product, save normal
  911. norm = cross(hedge, vedge);
  912. norm.normalize();
  913. norm *= 32767;
  914. point(x, y).nrm[0] = norm[0];
  915. point(x, y).nrm[1] = norm[1];
  916. point(x, y).nrm[2] = norm[2];
  917.       }
  918.     }
  919.   }
  920. }
  924. // detects grazing tris
  925. bool
  926. CyraResLevel::grazing(Pnt3 v1, Pnt3 v2, Pnt3 v3)
  927. {
  928. #if 0
  929.   // First compute (negative) norm, which should point pretty 
  930.   // much away from the origin.  Compare this to the vector
  931.   // from the origin....
  932.   Pnt3 edge1 = v3 - v2;
  933.   Pnt3 edge2 = v2 - v1;
  934.   Pnt3 norm = cross(edge1, edge2);
  935.   norm.normalize();
  936.   // Treat v2 as a vector from 0,0,0
  937.   v2.normalize();
  938.   float cosdev = dot(norm, v2);
  940.   //if (cosdev < .259) {
  941.     // More than 75-degrees away from perpendicular
  942.   if (cosdev < .017) {
  943.     // more than 89-degrees away from perpendicular
  944.     return true;
  945.   } else {
  946.     return false;
  947.   }
  948.   // #else
  950.   // #ifdef USE_Z_AS_DEPTH
  951.   // Check to see if depth difference is greater than our
  952.   // typical occlusion distance (CyraTessDepthmm)
  953.   if (v1[2] > v2[2] + CyraTessDepth ||
  954.       v1[2] > v3[2] + CyraTessDepth ||
  955.       v2[2] > v1[2] + CyraTessDepth ||
  956.       v2[2] > v3[2] + CyraTessDepth ||
  957.       v3[2] > v1[2] + CyraTessDepth ||
  958.       v3[2] > v2[2] + CyraTessDepth) {
  959.     return true;
  960.   } else {
  961.     return false;
  962.   }
  963. #else
  964.   // Don't use Z as depth... compute distance from the center...
  965.   float v1dist = sqrtf(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]);
  966.   float v2dist = sqrtf(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]);
  967.   float v3dist = sqrtf(v3[0]*v3[0] + v3[1]*v3[1] + v3[2]*v3[2]);
  969.   if (v1dist > v2dist + CyraTessDepth ||
  970.       v1dist > v3dist + CyraTessDepth ||
  971.       v2dist > v1dist + CyraTessDepth ||
  972.       v2dist > v3dist + CyraTessDepth ||
  973.       v3dist > v1dist + CyraTessDepth ||
  974.       v3dist > v2dist + CyraTessDepth) {
  975.     return true;
  976.   } else {
  977.     return false;
  978.   }
  983.   //#endif  // USE_Z_AS_DEPTH
  984. #endif  // 0  
  985. }
  989. int 
  990. CyraResLevel::num_vertices(void)
  991. {
  992.   return numpoints;
  993. }
  995. int 
  996. CyraResLevel::num_tris(void)
  997. {
  998.   return numtris;
  999. }
  1001. void 
  1002. CyraResLevel::growBBox(Bbox &bbox)
  1003. {
  1004.   FOR_EACH_VERT(bbox.add(v->vtx));
  1005. }
  1007. void flip3shorts (short* p)
  1008. {
  1009.   p[0] *= -1;
  1010.   p[1] *= -1;
  1011.   p[2] *= -1;
  1012. }
  1014. void
  1015. CyraResLevel::flipNormals(void)
  1016. {
  1017.   // Ok, to flip the normals, we:
  1018.   //     1. Reverse the order of the vertical scanlines (in memory)
  1019.   //     2. Reverse the order of the Triangle tesselation flags (in mem)
  1020.   //     3. Reverse the sign of the per-vertex normals
  1022.   ////// 1. Reverse the order of the vertical scanlines (in memory)
  1023.   CyraSample samp;
  1024.   for (int xx=0; xx < width/2; xx++) {
  1025.     for (int yy=0; yy < height; yy++) {
  1026.       samp = point(xx,yy);
  1027.       point(xx,yy) = point(width-xx-1, yy);
  1028.       point(width-xx-1, yy) = samp;
  1029.     }
  1030.   }
  1032.   ////// 2. Reverse the order of the Triangle tesselation flags (in mem)
  1033.   // (This also requires changing TESS1 <-> TESS3, TESS2 <-> TESS4)
  1034.   CyraTess tess;
  1035.   // Note:  only goes to width-1, but want to do middle col, too, so
  1036.   // the width-1+1 cancel out...
  1037.   for (xx=0; xx < (width)/2; xx++) {
  1038.     for (int yy=0; yy < height-1; yy++) {
  1039.       tess = tri(xx,yy);
  1040.       // Set scanline xx to be mirror of width-xx-2
  1041.       switch(tri(width-xx-2, yy)) {
  1042.       case TESS0:
  1043. tri(xx,yy) = TESS0; break;
  1044.       case TESS1:
  1045. tri(xx,yy) = TESS3; break;
  1046.       case TESS2:
  1047. tri(xx,yy) = TESS4; break;
  1048.       case TESS3:
  1049. tri(xx,yy) = TESS1; break;
  1050.       case TESS4:
  1051. tri(xx,yy) = TESS2; break;
  1052.       case TESS14:
  1053.       case TESS23:
  1054. tri(xx,yy) = TESS14; break;
  1055.       }
  1056.       // Set scanline width-xx-2 to be mirror of xx
  1057.       switch(tess) {
  1058.       case TESS0:
  1059. tri(width-xx-2,yy) = TESS0; break;
  1060.       case TESS1:
  1061. tri(width-xx-2,yy) = TESS3; break;
  1062.       case TESS2:
  1063. tri(width-xx-2,yy) = TESS4; break;
  1064.       case TESS3:
  1065. tri(width-xx-2,yy) = TESS1; break;
  1066.       case TESS4:
  1067. tri(width-xx-2,yy) = TESS2; break;
  1068.       case TESS14:
  1069.       case TESS23:
  1070. tri(width-xx-2,yy) = TESS14; break;
  1071.       }
  1072.     }
  1073.   }
  1075.   ////// 3. Reverse the sign of the per-vertex normals
  1076.   FOR_EACH_VERT(flip3shorts (v->nrm));
  1077. }
  1079. bool 
  1080. CyraResLevel::filtered_copy(CyraResLevel &original, 
  1081.     const VertexFilter& filter)
  1082. {
  1083.   width  = original.width;
  1084.   height = original.height;
  1085.   numpoints = 0; 
  1086.   numtris   = 0;   
  1087.   points.clear();
  1088.   tris.clear();
  1089.   origin = original.origin;
  1091.   CyraSample zero;
  1092.   zero.vtx.set(0,0,0);
  1093.   zero.nrm[0] = 0; zero.nrm[1] = 0; zero.nrm[2] = 32767;
  1094.   zero.confidence = 0;
  1095.   zero.intensity  = 0;
  1097.   // Copy the vertices
  1098.   for (int i=0; i < original.points.size(); i++) {
  1099.     if (filter.accept(original.points[i].vtx)) {
  1100.       points.push_back(original.points[i]);
  1101.       numpoints++;
  1102.     } else {
  1103.       points.push_back(zero);
  1104.     }
  1105.   }
  1107.   // Copy the triangles
  1108.   for (int xx=0; xx < width-1; xx++) {
  1109.     for (int yy=0; yy < height-1; yy++) {
  1110.       bool conf00 = (point(xx,yy).confidence > 0);
  1111.       bool conf01 = (point(xx,yy+1).confidence > 0);
  1112.       bool conf10 = (point(xx+1,yy).confidence > 0);
  1113.       bool conf11 = (point(xx+1,yy+1).confidence > 0);
  1114.       // Compute new tess, minus possible vertices
  1115.       CyraTess tess;
  1116.       switch (original.tri(xx, yy)) {
  1117.       case TESS0:
  1118. tess = TESS0;
  1119. break;
  1120.       case TESS1:
  1121. if (conf00 && conf10 && conf01) tess = TESS1;
  1122. else tess = TESS0;
  1123. break;
  1124.       case TESS2:
  1125. if (conf00 && conf01 && conf11) tess = TESS2;
  1126. else tess = TESS0;
  1127. break;
  1128.       case TESS3:
  1129. if (conf00 && conf10 && conf11) tess = TESS3;
  1130. else tess = TESS0;
  1131. break;
  1132.       case TESS4:
  1133. if (conf01 && conf10 && conf11) tess = TESS4;
  1134. else tess = TESS0;
  1135. break;
  1136.       case TESS14:
  1137.       case TESS23:
  1138. if (conf00 && conf01 && conf10 && conf11) tess = TESS14;
  1139. else if (conf00 && conf01 && conf10) tess = TESS1;
  1140. else if (conf00 && conf01 && conf11) tess = TESS2;
  1141. else if (conf00 && conf10 && conf11) tess = TESS3;
  1142. else if (conf01 && conf10 && conf11) tess = TESS4;
  1143. else tess = TESS0;
  1144. break;
  1145.       default:
  1146. cerr << "Unhandled tesselation flag " << original.tri(xx,yy) 
  1147.      << " in clipping..." << endl;
  1148.       }
  1149.       // Push Tri flag, increment numtris
  1150.       tris.push_back(tess);
  1151.       if (tess == TESS14 || tess == TESS23) numtris+=2;
  1152.       else if (tess == TESS0) numtris += 0;
  1153.       else numtris ++;
  1154.     }
  1155.   }
  1157.   return true;
  1158. }
  1161. bool 
  1162. CyraResLevel::filter_inplace(const VertexFilter& filter)
  1163. {
  1164.   numpoints = 0; 
  1165.   numtris   = 0;   
  1167.   CyraSample zero;
  1168.   zero.vtx.set(0,0,0);
  1169.   zero.nrm[0] = zero.nrm[1] = 0; zero.nrm[2] = 32767;
  1170.   zero.confidence = 0;
  1171.   zero.intensity  = 0;
  1173.   // Filter the vertices
  1174.   FOR_EACH_VERT(*v = (filter.accept(v->vtx) && ++numpoints)? *v : zero);
  1176.   // Copy the triangles
  1177.   for (int xx=0; xx < width-1; xx++) {
  1178.     for (int yy=0; yy < height-1; yy++) {
  1179.       bool conf00 = (point(xx,yy).confidence > 0);
  1180.       bool conf01 = (point(xx,yy+1).confidence > 0);
  1181.       bool conf10 = (point(xx+1,yy).confidence > 0);
  1182.       bool conf11 = (point(xx+1,yy+1).confidence > 0);
  1183.       // Compute new tess, minus possible vertices
  1184.       CyraTess tess;
  1185.       switch (tri(xx, yy)) {
  1186.       case TESS0:
  1187. tess = TESS0;
  1188. break;
  1189.       case TESS1:
  1190. if (conf00 && conf10 && conf01) tess = TESS1;
  1191. else tess = TESS0;
  1192. break;
  1193.       case TESS2:
  1194. if (conf00 && conf01 && conf11) tess = TESS2;
  1195. else tess = TESS0;
  1196. break;
  1197.       case TESS3:
  1198. if (conf00 && conf10 && conf11) tess = TESS3;
  1199. else tess = TESS0;
  1200. break;
  1201.       case TESS4:
  1202. if (conf01 && conf10 && conf11) tess = TESS4;
  1203. else tess = TESS0;
  1204. break;
  1205.       case TESS14:
  1206.       case TESS23:
  1207. if (conf00 && conf01 && conf10 && conf11) tess = TESS14;
  1208. else if (conf00 && conf01 && conf10) tess = TESS1;
  1209. else if (conf00 && conf01 && conf11) tess = TESS2;
  1210. else if (conf00 && conf10 && conf11) tess = TESS3;
  1211. else if (conf01 && conf10 && conf11) tess = TESS4;
  1212. else tess = TESS0;
  1213. break;
  1214.       default:
  1215. cerr << "Unhandled tesselation flag " << tri(xx,yy) 
  1216.      << " in clipping..." << endl;
  1217.       }
  1218.       // Push Tri flag, increment numtris
  1219.       tri(xx,yy) = tess;
  1220.       if (tess == TESS14 || tess == TESS23) numtris+=2;
  1221.       else if (tess == TESS0) numtris += 0;
  1222.       else numtris ++;
  1223.     }
  1224.   }
  1226.   isDirty_cache = true;
  1227.   return true;
  1228. }
  1231. void
  1232. CyraResLevel::create_kdtree(void)
  1233. {
  1234.   // If everything built, no-op.
  1235.   if ((!isDirty_cache) && (kdtree)) return;
  1237.   // otherwise, rebuild
  1238.   if (kdtree) delete kdtree;
  1239.   cachedPoints.clear();
  1240.   cachedNorms.clear();
  1241.   cachedBoundary.clear();
  1243.   // re-assemble cachedPoints, norms, boundary
  1244.   cachedPoints.reserve(num_vertices());
  1245.   cachedNorms.reserve(num_vertices() * 3);
  1246.   cachedBoundary.reserve(num_vertices());
  1247.   FOR_EACH_VERT(cachedPoints.push_back(v->vtx));
  1248.   FOR_EACH_VERT(cachedNorms.insert (cachedNorms.end(), v->nrm, v->nrm + 3));
  1249.   FOR_EACH_VERT(cachedBoundary.push_back
  1250. ((bool) (vx == 0 || vx == width-1 ||
  1251.  vy == 0 || vy == height-1 ||
  1252.  (tri(vx-1,vy-1) != TESS14 && 
  1253.   tri(vx-1,vy-1) != TESS23 && 
  1254.   tri(vx-1,vy-1) != TESS4) ||
  1255.  (tri(vx-1,vy) != TESS14 && 
  1256.   tri(vx-1,vy) != TESS23 && 
  1257.   tri(vx-1,vy) != TESS3) ||
  1258.  (tri(vx,vy-1) != TESS14 && 
  1259.   tri(vx,vy-1) != TESS23 && 
  1260.   tri(vx,vy-1) != TESS2) ||
  1261.  (tri(vx,vy) != TESS14 && 
  1262.   tri(vx,vy) != TESS23 && 
  1263.   tri(vx,vy) != TESS1))));
  1265.   kdtree = CreateKDindtree (cachedPoints.begin(), cachedNorms.begin(),
  1266.     cachedPoints.size());
  1268.   isDirty_cache = false;
  1269. }
  1271. // for ICP...
  1272. void
  1273. CyraResLevel::subsample_points(float rate, vector<Pnt3> &p, vector<Pnt3> &n)
  1274. {
  1275.   int nv = num_vertices();
  1276.   int totalNum = (int)(rate * nv);
  1278.   if (totalNum > nv) return;
  1279.   
  1280.   p.clear(); p.reserve(totalNum);
  1281.   n.clear(); n.reserve(totalNum);
  1282.   
  1283.   int num = totalNum;
  1284.   int end = nv;
  1285.   for (int i = 0; i < end; i++) {
  1286.     if (random()%nv < num) {
  1287.       p.push_back(points[i].vtx);    // save point
  1288.       pushNormalAsPnt3 (n, points[i].nrm, 0);
  1289.       num--;
  1290.     }
  1291.     nv--;
  1292.   }
  1293.   assert(num == 0);
  1294.   if (p.size() != totalNum) {
  1295.     printf("Selected wrong number of points in the CyraResLevel subsample proc.n");
  1296.   }
  1297. }  
  1299. bool 
  1300. CyraResLevel::closest_point(const Pnt3 &p, const Pnt3 &n, 
  1301. Pnt3 &cp, Pnt3 &cn,
  1302. float thr, bool bdry_ok)
  1303. {
  1304.   create_kdtree();
  1305.   int ind, ans;
  1306.   ans = kdtree->search(&cachedPoints[0], 
  1307.        &cachedNorms[0], p, n, ind, thr);
  1308.   if (ans) {
  1309.     if (!bdry_ok) {
  1310.       // disallow closest points that are on the mesh boundary
  1311.       if (cachedBoundary[ind]) return 0;
  1312.     }
  1313.     cp = cachedPoints[ind];
  1314.     short *sp = &cachedNorms[ind*3];
  1315.     cn.set(sp[0]/32767.0, 
  1316.    sp[1]/32767.0, 
  1317.    sp[2]/32767.0);
  1318.   }
  1319.   return ans;
  1320. }