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

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

3D图形编程

开发平台：

Visual C++

CyberScan.cc：源码内容
  1. //############################################################
  2. // 
  3. // CyberScan.cc
  4. //
  5. // Kari Pulli
  6. // Wed Jul  8 13:15:25 PDT 1998
  7. //
  8. // Store range scan information from a Cyberware custom made
  9. // range scanner for the Digital Michelangelo project.
  10. //
  11. //############################################################
  13. #include <stdio.h>
  14. #include <string.h>
  15. #include "DirEntries.h"
  16. #include <fstream.h>
  17. #ifdef WIN32
  18. # include <float.h>
  19. #endif
  20. #ifdef sgi
  21. # include <ieeefp.h>
  22. #endif
  23. #ifdef linux
  24. #       define MAXFLOAT FLT_MAX
  25. #endif
  26. #include "CyberScan.h"
  27. #include "plvGlobals.h"
  28. #include "plvDraw.h"
  29. #include "KDindtree.h"
  30. #include "Random.h"
  31. #include "TriMeshUtils.h"
  32. #include "defines.h"
  33. #include "FileNameUtils.h"
  34. #include "Bbox.h"
  35. #include "Progress.h"
  36. #include "BailDetector.h"
  37. #include "ColorUtils.h"
  38. #include "algo.h"
  39. #include "MeshTransport.h"
  40. #include "VertexFilter.h"
  42. #define SUBSAMPS 7
  44. static ColorSet _cset;
  46. KDindtree*
  47. CyberScan::get_current_kdtree()
  48. {
  49.   int iTree = current_resolution_index();
  50.   assert (iTree < kdtree.size());
  51.   if (kdtree[iTree] != NULL)
  52.     return kdtree[iTree];
  54.   regLevelData* level = getCurrentRegLevel();
  55.   kdtree[iTree] = CreateKDindtree(level->pnts->begin(), 
  56.   level->nrms->begin(),
  57.   level->pnts->size());
  59.   return kdtree[iTree];
  60. }
  63. void
  64. CyberScan::read_postprocess(const char *filename)
  65. {
  66.   for (int i = 0; i < SUBSAMPS; i++) {
  67.     char name[PATH_MAX];
  68.     strcpy (name, filename);
  69.     char* ext = strrchr (name, '.');
  70.     if (ext == NULL)
  71.       ext = name + strlen (name);
  72.     sprintf (ext, "_samp%d.ply", 1<<i);
  74.     int nFacesEstimated = 0;
  75.     for (int j = 0; j < sweeps.size(); j++) {
  76.       nFacesEstimated += sweeps[j]->resolutions[i].abs_resolution;
  77.     }
  79.     insert_resolution(nFacesEstimated, name, false, false);
  80.     kdtree.push_back (NULL);
  81.     reglevels.push_back (NULL);
  82.   }
  84.   // by default, start at lowest res
  86.   for (i=0; i<sweeps.size(); i++) 
  87.     sweeps[i]->select_coarsest();
  88.   select_coarsest();
  90.   // fill holes if requested
  92.   if (!strcmp(Tcl_GetVar(g_tclInterp, "subsamplePreserveHoles",
  93.                  TCL_GLOBAL_ONLY), "filter")) {
  94. for (i=0; i<sweeps.size(); i++) {
  95.   sweeps[i]->sd.fill_holes(2, 30);
  96.   sweeps[i]->sd.fill_holes(1, 20);
  97. }
  98.   }
  99. }
  102. CyberScan::CyberScan()
  103. {
  104.   bDirty = false;
  105. }
  108. CyberScan::~CyberScan ()
  109. {
  110.   while (reglevels.size()) {
  111.     delete reglevels.back();
  112.     reglevels.pop_back();
  113.   }
  115.   while (sweeps.size()) {
  116.     delete sweeps.back();
  117.     sweeps.pop_back();
  118.   }
  120.   while (kdtree.size()) {
  121.     delete kdtree.back();
  122.     kdtree.pop_back();
  123.   }
  124. }
  127. MeshTransport*
  128. CyberScan::mesh(bool perVertex, bool stripped,
  129. ColorSource color, int colorSize)
  130. {
  131.   if (stripped && !perVertex) {
  132.     cerr << "No t-strips without per-vertex properties";
  133.     return NULL;
  134.   }
  136.   /* kberg - trying to get per face flat shading working
  137.   if (!perVertex) {
  138.     cerr << "No per face normals for CyberScan" << endl;
  139.     return NULL;
  140.   } 
  141.   */
  142.   
  143.   int i = current_resolution_index();
  144.   load_resolution (i);
  146.   MeshTransport *mt = new MeshTransport;
  147.   for (int iTurn = 0; iTurn < sweeps.size(); iTurn++) {
  148.     MeshTransport* turn = sweeps[iTurn]->mesh (perVertex, stripped, color, 
  149.        colorSize);
  150.     if (turn) {
  151.       mt->appendMT (turn, sweeps[iTurn]->getXform());
  152.       delete turn;
  153.     }
  154.   }
  156.   return mt;
  157. }
  160. int
  161. CyberScan::num_vertices(void)
  162. {
  163.   return getCurrentRegLevel()->pnts->size();
  164. }
  167. regLevelData*
  168. CyberScan::getHighestRegLevel (void)
  169. {
  170.   if (!resolutions[0].in_memory)
  171.     load_resolution (0);
  173.   return getRegLevelFor (0);
  174. }
  177. regLevelData*
  178. CyberScan::getCurrentRegLevel (void)
  179. {
  180.   return getRegLevelFor (current_resolution_index());
  181. }
  184. regLevelData*
  185. CyberScan::getRegLevelFor (int iRes)
  186. {
  187.   assert (iRes < reglevels.size());
  188.   load_resolution (iRes);
  189.   assert (resolutions[iRes].in_memory);
  191.   if (!reglevels[iRes]) {
  192.     regLevelData* rl = reglevels[iRes] = new regLevelData;
  194.     if (sweeps.size() == 1) {
  195.       // don't actually need merge, so don't copy -- just share
  196.       rl->pnts = &sweeps[0]->levels[iRes]->pnts;
  197.       rl->nrms = &sweeps[0]->levels[iRes]->nrms;
  198.       rl->bdry = &sweeps[0]->levels[iRes]->bdry;
  199.       rl->bFree = false;
  200.     } else {
  201.       // need to create merged copy
  202.       // preallocate vectors
  203.       int nPts = 0;
  204.       for (int i = 0; i < sweeps.size(); i++) {
  205. nPts += sweeps[i]->levels[iRes]->pnts.size();
  206.       }
  207.       cout << "Merging sweep data (" << nPts
  208.    << " pts) for KDtree... " << flush;
  209.       rl->pnts = new vector<Pnt3>;  rl->pnts->reserve (nPts);
  210.       rl->nrms = new vector<short>; rl->nrms->reserve (nPts);
  211.       rl->bdry = new vector<char>;  rl->bdry->reserve (nPts);
  212.       rl->bFree = true;
  213.       
  214.       for (i = 0; i < sweeps.size(); i++) {
  215. levelData *ld = sweeps[i]->levels[iRes];
  216. long ps = rl->pnts->size();
  217. long ns = rl->nrms->size();
  219. rl->pnts->insert (rl->pnts->end(), 
  220.   ld->pnts.begin(), ld->pnts.end());
  221. rl->nrms->insert (rl->nrms->end(), 
  222.   ld->nrms.begin(), ld->nrms.end());
  223. rl->bdry->insert (rl->bdry->end(), 
  224.   ld->bdry.begin(), ld->bdry.end());
  226. Xform<float> sxf = sweeps[i]->getXform();
  227. if (!sxf.isIdentity()) {
  228.   // need to apply sweep's transform to the new data
  229.   for_each (rl->pnts->begin() + ps, rl->pnts->end(), sxf);
  230.   sxf.removeTranslation();
  231.   for (short* nb = rl->nrms->begin() + ns;
  232.        nb < rl->nrms->end(); nb += 3) {
  233.     Pnt3 n (nb[0], nb[1], nb[2]);
  234.     sxf (n);
  235.     nb[0] = n[0]; nb[1] = n[1]; nb[2] = n[2];
  236.   }
  237. }
  238.       }
  239.     }
  240.   }
  242.   return reglevels[iRes];
  243. }
  246. static Random rnd;
  248. void 
  249. CyberScan::subsample_points(float rate, vector<Pnt3> &p,
  250.     vector<Pnt3> &n)
  251. { 
  252.   regLevelData* level = getCurrentRegLevel();
  254.   int end = level->pnts->size();
  255.   p.clear(); p.reserve(end * rate * 1.1);
  256.   n.clear(); n.reserve(end * rate * 1.1);
  257.   for (int i = 0; i < end; i++) {
  258.     if (rnd() <= rate) {
  259.       p.push_back (level->pnts->operator[](i));    // save point
  260.       pushNormalAsPnt3 (n, level->nrms->begin(), i);
  261.     }
  262.   }
  263. }
  266. RigidScan* 
  267. CyberScan::filtered_copy (const VertexFilter &filter)
  268. {
  269.   BailDetector bail;
  270.   CyberScan *newScan = new CyberScan;
  271.   newScan->bDirty = true;
  273.   cout << "Clip " << sweeps.size() << " sweeps: " << flush;
  274.   for (int i = 0; i < sweeps.size(); i++) {
  275.     RigidScan* newSweep = sweeps[i]->filtered_copy(filter);
  276.     if (newSweep) {
  277.       CyberSweep *cs = dynamic_cast<CyberSweep*> (newSweep);
  278.       assert(cs);
  279.       newScan->sweeps.push_back(cs);
  280.     }
  281.     cout << "." << flush;
  283.     if (bail()) {
  284.       cerr << "Warning: clip interrupted; results are partial" << endl;
  285.       break;
  286.     }
  287.   }
  288.   cout << " " << newScan->sweeps.size() << " made it." << endl;
  290.   if (newScan->sweeps.size()) {
  291.     newScan->read_postprocess(get_name().c_str());
  292.     newScan->setXform (getXform());
  293.   }
  295.   return newScan;
  296. }
  299. RigidScan* 
  300. CyberScan::get_piece (int sweepNumber, int frameStart, int frameFinish)
  301. {
  302.   if (sweepNumber >= sweeps.size()) {
  303.     return NULL;
  304.   }
  306.   RigidScan* newSweep = 
  307.     sweeps[sweepNumber]->get_piece(frameStart, frameFinish);
  309.   if (newSweep == NULL)
  310.     return NULL;
  312.   CyberSweep *cs = dynamic_cast<CyberSweep*> (newSweep);
  313.   assert(cs);
  315.   CyberScan *newScan = new CyberScan;
  316.   newScan->sweeps.push_back(cs);
  317.   newScan->read_postprocess(get_name().c_str());
  318.   newScan->setXform (getXform());
  320.   return newScan;
  321. }
  324. bool 
  325. CyberScan::filter_inplace (const VertexFilter &filter)
  326. {
  327.   return false;
  328. }
  331. bool 
  332. CyberScan::filter_vertices (const VertexFilter &filter, 
  333.     vector<Pnt3>& p)
  334. {
  335.   for (int i=0; i<sweeps.size(); i++) {
  336.     CyberSweep *cs = sweeps[i];
  337.     int level = cs->current_resolution_index();
  338.     vector<Pnt3> &pnts = cs->levels[level]->pnts;
  339.     vector<Pnt3>::const_iterator it;
  340.     for (it = pnts.begin(); it != pnts.end(); it++) {
  341.       if (filter.accept(*it)) p.push_back(*it);
  342.     }
  343.   }
  344.   return true;
  345. }
  348. bool
  349. CyberScan::closest_point(const Pnt3 &p, const Pnt3 &n, 
  350.  Pnt3 &cp, Pnt3 &cn,
  351.  float thr, bool bdry_ok)
  352. {
  353.   KDindtree* tree = get_current_kdtree();
  354.   if (!tree) return false;
  356.   int ind, ans;
  358.   regLevelData* level = getCurrentRegLevel();
  359.   ans = tree->search(level->pnts->begin(), level->nrms->begin(), 
  360.      p, n, ind, thr);
  361.   if (ans) {
  362.     if (bdry_ok == 0) {
  363.       // disallow closest points that are on the mesh boundary
  364.       if (level->bdry->operator[](ind)) return 0;
  365.     }
  366.     cp = level->pnts->operator[](ind);
  367.     ind *= 3;
  368.     cn.set(level->nrms->operator[](ind  )/32767.0, 
  369.    level->nrms->operator[](ind+1)/32767.0, 
  370.    level->nrms->operator[](ind+2)/32767.0);
  371.   }
  372.   return ans;
  373. }
  376. void
  377. CyberScan::computeBBox ()
  378. {
  379.   bbox.clear();
  381.   for (int i = 0; i < sweeps.size(); i++) {
  382.     sweeps[i]->computeBBox();
  383.     if (sweeps[i]->bbox.valid())
  384.       bbox.add (sweeps[i]->bbox.worldBox (sweeps[i]->getXform()));
  385.   }
  387.   if (bbox.valid()) {
  388.     rot_ctr = bbox.center();
  389.   } else {
  390.     rot_ctr = Pnt3();
  391.   }
  392. }
  395. void
  396. CyberSweep::computeBBox ()
  397. {
  398.   bbox.clear();
  400.   // find highest built resolution
  401.   levelData* level = NULL;
  402.   for (int i = 0; i < levels.size(); i++) {
  403.     if (levels[i] != NULL && levels[i]->pnts.size() > 0) {
  404.       level = levels[i];
  405.       break;
  406.     }
  407.   }
  409.   // and add its points to bbox
  410.   if (level != NULL) {
  411.     for (Pnt3* p = level->pnts.begin(); p < level->pnts.end(); p++) {
  412.       bbox.add(*p);
  413.     }
  414.   }
  415. }
  418. void
  419. CyberScan::flipNormals (void)
  420. {
  421.   for (int i = 0; i < sweeps.size(); i++)
  422.     sweeps[i]->flipNormals();
  423. }
  426. void
  427. CyberSweep::flipNormals (void)
  428. {
  429.   for (int i = 0; i < levels.size(); i++) {
  430.     if (!resolutions[i].in_memory)
  431.       continue;
  433.     for (int j = 0; j < levels[i]->nrms.size(); j++) 
  434.       levels[i]->nrms[j] = -levels[i]->nrms[j];
  435.     flip_tris(levels[i]->tstrips, true);
  436.   }
  437. }
  440. bool 
  441. CyberScan::read(const crope &fname)
  442. {
  443.   cout << "cs::read()" << endl;
  444.   set_name (fname);
  445.   const char* filename = fname.c_str();
  446.   assert(has_ending(".sd") || has_ending(".sd.gz"));
  448.   // is filename a directory containing lots of little .sd files?
  449.   bool bReadDir = false;
  451.   SHOW(filename);
  453.   DirEntries de(filename, ".sd");
  455.   if (de.isdir)  {    // If it's a directory
  457.     bReadDir = true;
  459.     {
  460.       Progress progress (de.size(), "Read CyberSweeps");
  462.       for (; !de.done(); de.next()) {
  463.         CyberSweep *ct = new CyberSweep;
  464.         if (ct->read(de.path().c_str()))
  465.        sweeps.push_back(ct);
  466.         else
  467.   delete ct;
  468.         progress.updateInc();
  469.       }
  470.     }
  472.     DirEntries gzde(filename, ".sd.gz");
  474.     {
  475.       Progress progress (gzde.size(), "Read CyberSweeps");
  477.       for (; !gzde.done(); gzde.next()) {
  478.         CyberSweep *ct = new CyberSweep;
  479.         if (ct->read(gzde.path().c_str()))
  480.        sweeps.push_back(ct);
  481.         else
  482.   delete ct;
  483.         progress.updateInc();
  484.       }
  485.     }
  487.   } else {                           // If it's a single file
  488.     CyberSweep *ct = new CyberSweep;
  489.     if (ct->read(filename))
  490.       sweeps.push_back(ct);
  491.     else
  492.       delete ct;
  493.   }
  495.   // If we didn't successfully read any .sd files, return error
  496.   
  497.   if (sweeps.size() == 0)
  498.     return false;
  500.   set_name(fname);
  502.   // read registration xform?
  503.   if (TbObj::readXform (get_basename())) {
  504.     // if we read a single file instead of a directory,
  505.     // sweep will have read the same .xf as we do; 
  506.     // don't want it doubly applied so nuke it from sweep.
  507.     if (!bReadDir) {
  508.       assert (sweeps.size() == 1);
  509.       sweeps[0]->setXform (Xform<float>());
  510.       clear_undo (sweeps[0]);
  511.     }
  512.   }
  514.   clear_undo (this);
  516.   read_postprocess(filename);
  517.   return true;
  518. }
  521. bool
  522. CyberScan::is_modified (void)
  523. {
  524.   return bDirty;
  525. }
  528. bool 
  529. CyberScan::write(const crope &fname)
  530. {
  531.    char sweepfn[PATH_MAX];
  533.    if (fname.empty()) {
  534.       // try to save to default name; quit if there isn't one
  535.       if (name.empty()) return false;
  536.    }
  537.    else {
  538.       if (name != fname)  {
  539.          cout << "Saving to filename " << fname << endl;
  540.          set_name(fname);
  541.       }
  542.    }
  543.    
  544.    if (portable_mkdir(name.c_str(), 0775) == 0)  {
  546.       Progress progress (sweeps.size(), "Writing CyberScan");
  548.       for (int i=0; i<sweeps.size(); i++)  {
  549.          sprintf(sweepfn, "%s/%d.sd", name.c_str(), i);
  550.  if (!sweeps[i]->write(sweepfn))
  551.              return false;
  552.          progress.updateInc();
  553.       }
  554.    }
  555.    else
  556.       return false;
  557.   
  558.    bDirty = false;
  559.    return true;
  560. }
  563. bool
  564. CyberScan::load_resolution (int iRes)
  565. {
  566.   if (resolutions[iRes].in_memory)
  567.     return true;
  569.   int nSweeps = sweeps.size();
  570.   if (g_verbose) cout << name << ": create mesh (~" 
  571.        << resolutions[iRes].abs_resolution << ") from "
  572.        << nSweeps << " sweeps: " << flush;
  574.   int newres = 0;
  575.   BailDetector bail;
  576.   for (int i = 0; i < nSweeps; i++) {
  577.     //cout << (nSweeps - i) << " left: " << flush;
  578.     sweeps[i]->load_resolution (iRes);
  579.     newres += sweeps[i]->resolutions[iRes].abs_resolution;
  580.     if (g_verbose) cout << "." << flush;
  582.     if (bail()) {
  583.       return false;
  584.     }
  585.   }
  587.   if(g_verbose)  cout << " done." << endl;
  589.   resolutions[iRes].abs_resolution = newres;
  590.   resolutions[iRes].in_memory = true;
  591.   computeBBox();
  592.   return true;
  593. }
  596. int
  597. CyberScan::create_resolution_absolute(int budget, Decimator dec)
  598. {
  599.   cerr << "Can't decimate CyberScan meshes for now" << endl;
  600.   return 0;
  601. }
  604. bool
  605. CyberScan::release_resolution(int nPolys)
  606. {
  607.   int iRes = findLevelForRes (nPolys);
  609.   if (iRes == -1) {
  610.     cerr << "Cyber scan resolution doesn't exist!" << endl;
  611.     return false;
  612.   }
  614.   for (int i = 0; i < sweeps.size(); i++) {
  615.     delete sweeps[i]->levels[iRes];
  616.     sweeps[i]->levels[iRes] = new levelData;
  617.     sweeps[i]->resolutions[iRes].in_memory = false;
  618.   }
  620.   resolutions[iRes].in_memory = false;
  621.   return true;
  622. }
  625. /*
  626. void
  627. check(vector<int> &tstrips, int n) 
  628. {
  629.   int ns = tstrips.size();
  630.   for (int i=0; i<ns; i++) {
  631.     assert(tstrips[i] >=-1);
  632.     assert(tstrips[i] < n);
  633.   }
  634. }
  635. */
  639. extern DrawObjects draw_other_things;
  642. CyberSweep::CyberSweep(void)
  643. {
  644.   _cset.chooseNewColor(falseColor);
  645.   DrawObj::disable();
  646.   draw_other_things.add(this);
  647. }
  649. CyberSweep::~CyberSweep(void)
  650. {
  651.   while (kdtree.size()) {
  652.     delete (kdtree.back());
  653.     kdtree.pop_back();
  654.   }
  656.   while (levels.size()) {
  657.     delete (levels.back());
  658.     levels.pop_back();
  659.   }
  661.   draw_other_things.remove(this);
  662. }
  665. void
  666. CyberSweep::init_leveldata(void)
  667. {
  668.   levels.clear(); levels.reserve(SUBSAMPS);
  669.   kdtree.clear(); kdtree.reserve(SUBSAMPS);
  670.   for (int i = 0; i < SUBSAMPS; i++) {
  671.     levels.push_back (new levelData);
  672.     kdtree.push_back (NULL);
  673.   }
  674. }
  677. void
  678. CyberSweep::insert_possible_resolutions(void)
  679. {
  680.   for (int i = 0; i < SUBSAMPS; i++) {
  681.     int nFacesEstimated = 4 * sd.count_valid_pnts() / (1<<(2*i));
  683.     if (i > 0) {
  684.       insert_resolution(nFacesEstimated, "", false, false);
  685.       int iPos = findLevelForRes(nFacesEstimated);
  686.     } else {
  687.       insert_resolution(nFacesEstimated / 2, "", false, false);
  688.     }
  689.   }
  690. }
  693. bool
  694. CyberSweep::read(const crope &fname)
  695. {
  696.   if (!sd.read(fname))
  697.     return false;
  699.   set_name(fname);
  700.   init_leveldata();
  702.   insert_possible_resolutions();
  704.   if (g_verbose) cout << "done." << endl;
  705.   /*
  706.   Bbox bb = sd.get_bbox();
  707.   SHOW(bb);
  708. #if 1
  709.   int n = 8;
  710.   Pnt3 diff = bb.max() - bb.min(), p, bp;
  711.   for (int i=0; i<n; i++) {
  712.     for (int j=0; j<n; j++) {
  713.       for (int k=0; k<n; k++) {
  714. p = bb.min() + Pnt3(diff[0]*i, diff[1]*j, diff[2]*k)/n;
  715. if (sd.xf.back_project(p, bp)) {
  716.   // now go back to the data, find the closest
  717.   // data point
  718.   int row;
  719.   unsigned short y,z;
  720.   if (sd.find_data(bp[0], bp[1], row, y, z)) {
  721.     //SHOW(bp[1]);
  722.     //SHOW (y);
  723.     sd.set_xf(row);
  724.     //xf.set_screw(bp[0],bp[0]);
  725.     start.push_back(p);
  726.     end.push_back(sd.xf.apply_xform(y,z));
  727.     start.push_back(p);
  728.     //end.push_back(pa);
  729.     end.push_back(sd.xf.laser_ctr);
  730.     //start.push_back(.9*end.back()+.1*p);
  731.     //SHOW(start.back());
  732.     //SHOW(end.back());
  733.   } else {
  734.     cout << "find_data returns false" << endl;
  735.     //SHOW(bp);
  736.     //cout << row << " " << y << " " << z << endl;
  737.   }
  738. } else {
  739.   cout << "back_project returns false" << endl;
  740. }
  741.       }
  742.     }
  743.   }
  744. #else
  745.   Pnt3 p;
  746.   for (int i=0; i<sd.num_frames(); i+=40) {
  747.     for (int j=40; j<480; j+=40) {
  748.       sd.set_xf(i);
  749.       if (sd.get_point(i,j,p)) {
  750. start.push_back(p);
  751. end.push_back(sd.xf.laser_ctr);
  752.       }
  753.     }
  754.   }
  755. #endif
  756.   DrawObj::enable();
  757.   */
  758.   if (TbObj::readXform (get_basename()))
  759.     cout << "Found .xf for sweep " << fname << endl;
  760.   //else
  761.   //cout << "No .xf for sweep " << fname << endl;
  763.   return true;
  764. }
  767. bool
  768. CyberSweep::write(const crope &fname)
  769. {
  770.   return sd.write(fname);
  771. }
  774. // called by CyberScan::load_resolution(),
  775. // which again is inherited from ResolutionCtrl
  776. bool
  777. CyberSweep::load_resolution (int iRes)
  778. {
  779.   if (resolutions[iRes].in_memory)
  780.     return true;
  782.   //cout << "tstrip... " << flush;
  784.   if (iRes == 0) {
  786.     sd.get_pnts_and_intensities(levels[0]->pnts,
  787. levels[0]->intensity);
  789.     // Changed the following line into a two step process, to avoid
  790.     // Compiler complaint. -jed
  791.     //levels[0]->bdry.assign (levels[0]->pnts.size(), 0);
  792.     vector<char> tmp(levels[0]->pnts.size(), 0);
  793.     levels[0]->bdry = tmp;
  794.     
  796.     sd.make_tstrip(levels[0]->tstrips, levels[0]->bdry);
  798.   } else {
  800.     levelData* level = levels[iRes];
  802.     int step = 1 << iRes;
  804.     sd.subsampled_tstrip(step, 
  805.  Tcl_GetVar (g_tclInterp,
  806.      "subsamplePreserveHoles",
  807.      TCL_GLOBAL_ONLY),
  808.  level->tstrips,
  809.  level->bdry,
  810.  level->pnts,
  811.  level->intensity,
  812.  level->confidence,
  813.  level->map_sampled_to_unsampled);
  814.   }
  816.   // now fix step edges, normals
  817.   levelData* level = levels[iRes];
  819.   if (level->pnts.size()) {
  820.     if (strcmp (Tcl_GetVar (g_tclInterp, "removeStepedges",
  821.     TCL_GLOBAL_ONLY), "0") != 0) {
  822.       //cout << "stepedges... " << flush;
  823.       remove_stepedges(level->pnts, level->tstrips, 4, 50, true);
  824.     }
  826.     //cout << "normals... " << flush;
  827.     getVertexNormals(level->pnts, level->tstrips,
  828.      true, level->nrms, false);
  829.     
  830.     resolutions[iRes].abs_resolution = 
  831.       count_tris (levels[iRes]->tstrips);
  832.   } else {
  833.     resolutions[iRes].abs_resolution = 0;
  834.   }
  836.   resolutions[iRes].in_memory = true;
  837.   return true;
  838. }
  841. void 
  842. CyberSweep::drawthis(void)
  843. {
  844.   glDisable(GL_LIGHTING);
  845.   glBegin(GL_LINES);
  846.   for (int i=0; i<start.size(); i++) {
  847.     glColor3f(1,0,0); 
  848.     glVertex3fv(&start[i][0]);
  849.     glColor3f(0,1,0);
  850.     glVertex3fv(&end[i][0]);
  851.   }
  852.   glEnd();
  853. }
  855. /* kberg - 10 July 2001
  856.  * adding per face normals - modeled partly after strips_to_tris
  857.  * in TriMeshUtils.cc, except rather than storing the vertices, it 
  858.  * calculates a normal based upon those 3 vertices then adds it to
  859.  * the faceNormals vector.
  860.  */
  861. void CyberSweep::simulateFaceNormals(vector<short> &faceNormals, int currentRes)
  862. {
  863.   if (!levels[currentRes]->tstrips.size())
  864.     return;
  865.   
  866.   assert(levels[currentRes]->tstrips.back() == -1);
  867.   
  868.   faceNormals.clear();
  869.   faceNormals.reserve(levels[currentRes]->tstrips.size() * 3); // estimate
  870.   
  871.   vector<int>::const_iterator vert;
  872.   for (vert = levels[currentRes]->tstrips.begin(); vert != levels[currentRes]->tstrips.end(); vert++) {
  873.     while (*vert == -1) { // handle 0-length strips
  874.       ++vert;
  875.       if (vert == levels[currentRes]->tstrips.end()) break;
  876.     }
  878.     if (vert == levels[currentRes]->tstrips.end()) break;
  879.     
  880.     vert += 2; // looking backwards at the triangles
  881.     
  882.     int dir = 0;
  883.     while (*vert != -1) {
  884.       Pnt3 &v0 = levels[currentRes]->pnts[vert[-2 + dir]];
  885.       Pnt3 &v1 = levels[currentRes]->pnts[vert[-1 - dir]];
  886.       Pnt3 &v2 = levels[currentRes]->pnts[vert[0]];
  887.       
  888.       //now calculate the normal based on these 3 points
  889.       Pnt3 norm = normal(v0, v1, v2);
  890.       pushNormalAsShorts(faceNormals, norm);
  892.       vert++;
  893.       dir ^= 1;
  894.     }
  895.   }
  896. }
  898. MeshTransport*
  899. CyberSweep::mesh (bool perVertex, bool stripped,
  900.   ColorSource color, int colorSize)
  901. {
  902.   int i = current_resolution_index();
  903.   assert (resolutions[i].in_memory);
  905.   if (!levels[i]->pnts.size())
  906.     return NULL;
  908.   MeshTransport* mt = new MeshTransport;
  909.   mt->setVtx(&levels[i]->pnts, MeshTransport::share);
  910.   mt->setBbox(localBbox());
  911.   
  912.   /* kberg - 10 July 2001 - per face normals */
  913.   if (perVertex)
  914.     mt->setNrm(&levels[i]->nrms, MeshTransport::share);
  915.   else {
  916.     /* used GenericScan::mesh(...) as an example of how to simulate this. 
  917.        for some reason, by going into this mode, only part of a statue
  918.        is displayed when intensity is selected.  It should have something to do
  919.        with how DisplayMesh::renderMeshSingle(...) handles color.
  920.     */
  921.     vector<short> *faceNrm = new vector<short>;
  922.     simulateFaceNormals(*faceNrm, i);
  923.     mt->setNrm(faceNrm, MeshTransport::steal);
  924.   }
  926.   if (stripped) {
  927.     mt->setTris(&levels[i]->tstrips, MeshTransport::share);
  928.   } else {
  929.     vector<int>* tris = new vector<int>;
  930.     strips_to_tris (levels[i]->tstrips, *tris,
  931.     resolutions[i].abs_resolution);
  932.     mt->setTris(tris, MeshTransport::steal);
  933.   }
  934.   
  935.   switch (color) {
  936.   case colorNone:
  937.     break;
  938.     
  939.   case colorTrue:
  940.     {
  941.       vector<uchar>* colors = new vector<uchar>;
  942.       pushColor (*colors, colorSize, falseColor);
  943.       mt->setColor (colors, MeshTransport::steal);
  944.     }
  945.     break;
  947.   case colorIntensity:
  948.     {
  949.       vector<uchar>* colors = new vector<uchar>;
  950.       if (g_bNoIntensity) {
  951. // BUGBUG: what to do here?
  952.       } else {
  953. uchar* end = levels[i]->intensity.end();
  954. for (uchar* c = levels[i]->intensity.begin(); c < end; c++)
  955.   pushColor (*colors, colorSize, *c);
  956.       }
  957.       mt->setColor (colors, MeshTransport::steal);
  958.     }
  959.     break;
  961.   case colorConf:
  962.     if (levels[i]->confidence.size())
  963.     {
  964.       vector<uchar>* colors = new vector<uchar>;
  965.       colors->reserve (colorSize * levels[i]->confidence.size());
  966.       uchar* end = levels[i]->confidence.end();
  967.       for (uchar* c = levels[i]->confidence.begin(); c < end; c++)
  968. pushConf (*colors, colorSize, *c);
  969.       mt->setColor (colors, MeshTransport::steal);
  970.     }
  971.     break;
  973.   case colorBoundary:
  974.     {
  975.       vector<uchar>* colors = new vector<uchar>;
  976.       colors->reserve (colorSize * levels[i]->bdry.size());
  977.       char* end = levels[i]->bdry.end();
  978.       for (char* c = levels[i]->bdry.begin(); c < end; c++)
  979. pushConf (*colors, colorSize, (uchar)(*c ? 0 : 255));
  980.       mt->setColor (colors, MeshTransport::steal);
  981.     }
  982.     break;
  983.     
  984.   default:
  985.     //cerr << "Color: TODO" << endl;
  986.     break;
  987.   }
  989.   return mt;
  990. }
  993. vector<CyberSweep*>
  994. CyberScan::get_sweep_list (void)
  995. {
  996.   return sweeps;
  997. }
  1000. // double indexing: horizontal translation, turns
  1001. vector< vector<CyberSweep*> >
  1002. CyberScan::get_ordered_sweeps (void)
  1003. {
  1004.   vector< vector<CyberSweep*> > v;
  1006.   // for each sweep
  1007.   for (int i=0; i < sweeps.size(); i++) {
  1008.     CyberSweep* sweep = sweeps[i];
  1010.     // insert a vector in v based on translation
  1011.     float t = sweep->sd.scanner_trans;
  1012.     for (int iTrans = 0; iTrans < v.size(); iTrans++) {
  1013.       float tc = v[iTrans][0]->sd.scanner_trans;
  1014.       if (t <= tc) {
  1015. if (t < tc) v.insert(&v[iTrans], vector<CyberSweep*>());
  1016. break;
  1017.       }
  1018.     }
  1019.     if (iTrans == v.size()) v.push_back( vector<CyberSweep*>() );
  1020.     // get a reference to the shell
  1021.     vector<CyberSweep*>& vShell = v[iTrans];
  1023.     // find a place to insert sweep (within shell) 
  1024.     // based on other screw
  1025.     for (int iSweep = 0; iSweep < vShell.size(); iSweep++) {
  1026.       if (sweep->sd.other_screw < vShell[iSweep]->sd.other_screw)
  1027. break;
  1028.     }
  1029.     // insert into shell
  1030.     vShell.insert (&vShell[iSweep], sweep);
  1031.   }
  1033.   return v;
  1034. }
  1037. // this command dumps into the file fname
  1038. // nPnts uniformly subsampled from all the sweeps
  1039. // such that there are nPnts output
  1040. // for each point, there are 5 floats: y,z,scanscrew,otherscrew,xform
  1041. // file is initialized with 1 for vertical scan, 0 for horizontal scan
  1042. void
  1043. CyberScan::dump_pts_laser_subsampled(std::string fname,
  1044.      int nPnts)
  1045. {
  1046.   // open the fstream
  1047.   ofstream out(fname.c_str());
  1048.   assert(out);
  1050.   // vertical scan?
  1051.   int v = sweeps[0]->sd.xf.vertical_scan;
  1052.   out.write((char*)&v, sizeof(int));
  1054.   // for each sweep, calculate how many points it should
  1055.   // store, have it store them
  1056.   int n_valid_pts = 0;
  1057.   // first, how many valid points are there?
  1058.   for (int i=0; i < sweeps.size(); i++) {
  1059.     n_valid_pts += sweeps[i]->sd.valid_pts();
  1060.   }  
  1061.   // now, iterate
  1062.   SHOW(nPnts);
  1063.   float fact = float(nPnts) / float(n_valid_pts);
  1064.   for (i=0; i < sweeps.size(); i++) {
  1065.     int n = sweeps[i]->sd.valid_pts();
  1066.     n_valid_pts -= n;
  1067.     if (i+1 == sweeps.size()) n = nPnts;
  1068.     else                      n *= fact;
  1069.     nPnts -= sweeps[i]->sd.dump_pts_laser_subsampled(out, n);
  1070.     fact = float(nPnts) / float(n_valid_pts);
  1071.   }  
  1072.   SHOW(nPnts);
  1073. }
  1076. // this function is intended to be used for getting the raw values
  1077. // for a given 3D point (given in local coordinates)
  1078. // used in conjunction with autocalibration
  1079. // assume the mesh has just been registered with ICP
  1080. bool
  1081. CyberScan::get_raw_data(const Pnt3 &p, sd_raw_pnt &data)
  1082. {
  1083.   // to simplify things, only works at finest resolution
  1084.   //int lvl = 0;
  1085.   int lvl = current_resolution_index();
  1086.   //if (current_resolution_index() != lvl) return false;
  1087.   // and the KDtree has to exist
  1088.   if (kdtree[lvl] == NULL) return false;
  1089.   // and the reglevelinfo
  1090.   if (reglevels[lvl] == NULL) return false;
  1092.   // ok. assume p is in the scan coordinates, by the way
  1093.   
  1094.   // find the index of the point closest to p in kdtree
  1095.   int ind;
  1096.   float thr = .1;
  1097.   if (!kdtree[lvl]->search(reglevels[lvl]->pnts->begin(),
  1098.    p, ind, thr)) {
  1099.     cerr << "Couldn't find the point " << p << endl;
  1100.     return false;
  1101.   }
  1103.   // ok, found the point index
  1104.   // now, find which sweep it is and find the relative index there
  1105.   for (int i=0; i<sweeps.size(); i++) {
  1106.     int n = sweeps[i]->levels[lvl]->pnts.size();
  1107.     if (ind >= n) ind -= n;
  1108.     else          break;
  1109.   }
  1110.   if (i==sweeps.size()) {
  1111.     cerr << "Problem in CyberScan::get_raw_data()" << endl;
  1112.     return false;
  1113.   }
  1115.   // now we have the index to the sweep and an index to the
  1116.   // point there
  1117.   Pnt3  pp;
  1118.   if (lvl) {
  1119.     ind = sweeps[i]->levels[lvl]->map_sampled_to_unsampled[ind];
  1120.     pp = sweeps[i]->sd.raw_for_ith(ind, data);
  1121.   } else {
  1122.     pp = sweeps[i]->sd.raw_for_ith_valid(ind, data);
  1123.   }
  1124.   
  1125.   if (dist(pp,p) > .1) {
  1126.     SHOW(p);
  1127.     SHOW(pp);
  1128.     SHOW(dist(p,pp));
  1129.   }
  1130.   
  1131.   return true;
  1132. }
  1135. // returns confidence!
  1136. float 
  1137. CyberScan::closest_point_on_mesh(const Pnt3 &p, Pnt3 &cl_pnt, 
  1138.  OccSt &status_p)
  1139. {
  1140.   // cheat...
  1141.   return closest_along_line_of_sight(p, cl_pnt, status_p);
  1142. }
  1143.  
  1145. // returns confidence!
  1146. float 
  1147. CyberScan::closest_along_line_of_sight(const Pnt3 &p, Pnt3 &cp, 
  1148.        OccSt &status_p)
  1149. {
  1150.   // first need to move p into the local coordinates!!
  1151.   Pnt3 pp = p;
  1152.   xformInvPnt(pp);
  1154.   // simplified assumptions... recheck later
  1155.   // e.g., assume no per sweep xforms...
  1156.   status_p = INDETERMINATE;
  1157.   float mindist = 1e33;
  1158.   OccSt tmp;
  1159.   Pnt3  tmpcp;
  1160.   for (int i=0; i<sweeps.size(); i++) {
  1161.     sweeps[i]->closest_along_line_of_sight(pp,tmpcp,tmp);
  1162.     if (status_p == INDETERMINATE && tmp == NOT_IN_FRUSTUM) {
  1163.       status_p = tmp;
  1164.       continue;
  1165.     }
  1166.     // should make a weighted sum of distances, see VolCarve.cc
  1167.     float d = dist2(pp, tmpcp);
  1168.     if (d < mindist) {
  1169.       mindist  = d;
  1170.       status_p = tmp;
  1171.       cp       = tmpcp;
  1172.     }
  1173.   }
  1175.   // move closest point back into world coordinates!
  1176.   xformPnt(cp);
  1178.   // return confidence
  1179.   return (status_p == INSIDE || status_p == OUTSIDE ? 1.0 : 0.0);
  1180. }
  1181.  
  1183. OccSt
  1184. CyberScan::carve_cube  (const Pnt3 &ctr, float side)
  1185. {
  1186.   // first of all, we are going to carve a sphere s.t.
  1187.   // the ctr is reexpressed in local coordinates
  1189.   Pnt3 lctr(ctr);
  1190.   xformInvPnt(lctr);
  1191.   float r = 1.72 * .5 * side;
  1193.   // find a parent from the carve stack for this sphere
  1194.   while (carveStack.size() &&
  1195.  !carveStack.back().can_be_child(ctr, side)) {
  1196.     carveStack.pop_back();
  1197.   }
  1198.   if (carveStack.size() == 0) {
  1199.     // initialize with all
  1200.     carveStack.push_back(carveStackEntry(lctr, r, sweeps));
  1201.   }
  1202.   
  1203.   // now the back() of the stack should have the sweeps that
  1204.   // we're interested in
  1205.   vector<CyberSweep*> &sw = carveStack.back().sweeps;
  1206.   
  1207.   // add a new entry for this cube
  1208.   carveStackEntry cse(lctr, r);
  1210.   // go through all the sweeps in the list
  1211.   OccSt res = INDETERMINATE;
  1213.   for (int i=0; i<sw.size(); i++) {
  1214.     OccSt tmp = sw[i]->carve_sphere(lctr, r);
  1215.     // add the current to the stack if needed
  1216.     if (tmp == BOUNDARY ||
  1217. tmp == SILHOUETTE ||
  1218. tmp == INDETERMINATE) {
  1219.       cse.sweeps.push_back(sw[i]);
  1220.     }
  1221.     if (int(tmp) < int(res)) res = tmp;
  1222.     if (res == OUTSIDE) return res;
  1223.   }
  1224.   carveStack.push_back(cse);
  1225.   return res;
  1226. }
  1231. bool
  1232. CyberScan::switchToResLevel (int iRes)
  1233. {
  1234.   if (!RigidScan::switchToResLevel (iRes))
  1235.     return false;
  1237.   // keep all sweeps synched to current res
  1238.   for (int i = 0; i < sweeps.size(); i++)
  1239.     sweeps[i]->switchToResLevel (iRes);
  1241.   return true;
  1242. }
  1245. crope
  1246. CyberSweep::get_description (void) const
  1247. {
  1248.   static char name[100];
  1249.   sprintf(name, "trans-%.2d-rot-%.2d",
  1250.   (int)sd.scanner_trans,
  1251.   (int)sd.other_screw);
  1252.   return crope (name);
  1253. }
  1256. KDindtree*
  1257. CyberSweep::get_current_kdtree()
  1258. {
  1259.   int iTree = current_resolution_index();
  1260.   assert (iTree < kdtree.size());
  1261.   if (kdtree[iTree] != NULL)
  1262.     return kdtree[iTree];
  1264.   levelData* level = levels[current_resolution_index()];
  1265.   kdtree[iTree] = CreateKDindtree(level->pnts.begin(), 
  1266.   level->nrms.begin(),
  1267.   level->pnts.size());
  1269.   return kdtree[iTree];
  1270. }
  1273. void 
  1274. CyberSweep::subsample_points(float rate, vector<Pnt3> &p,
  1275.      vector<Pnt3> &n)
  1276. { 
  1277.   levelData* level = levels[current_resolution_index()];
  1279.   int end = level->pnts.size();
  1280.   p.clear(); p.reserve(end * rate * 1.1);
  1281.   n.clear(); n.reserve(end * rate * 1.1);
  1282.   for (int i = 0; i < end; i++) {
  1283.     if (rnd() <= rate) {
  1284.       p.push_back(level->pnts[i]);    // save point
  1285.       pushNormalAsPnt3(n, level->nrms.begin(), i);
  1286.     }
  1287.   }
  1288. }
  1291. RigidScan *
  1292. CyberSweep::filtered_copy(const VertexFilter &filter)
  1293. {
  1294.   CyberSweep* newSweep = NULL;
  1296.   // need to multiply filter by this sweep's xform
  1297.   VertexFilter* sweepFilter = filter.transformedClone ((float*)getXform());
  1298.   if (!sweepFilter) {
  1299.     cerr << "CyberScan cannot clip because VertexFilter::transformedClone"
  1300.  << " is not supported" << endl;
  1301.     return NULL;
  1302.   }
  1304.   if (sweepFilter->accept (bbox)) {
  1305.     newSweep = new CyberSweep;
  1306.     // copy the data, first shallow copy
  1307.     //*newSweep = *this;
  1308.     // except resolutions, which get rebuilt
  1309.     // newSweep->resolutions.clear();
  1310.     // then do a deeper, filtered copy
  1311.     newSweep->init_leveldata();
  1312.     sd.filtered_copy(*sweepFilter, newSweep->sd);
  1313.     if (sd.count_valid_pnts() == 0) {
  1314.       delete newSweep;
  1315.       newSweep = NULL;
  1316.     } else {
  1317.       newSweep->insert_possible_resolutions();
  1318.       newSweep->setXform (getXform());
  1319.     }
  1320.   }
  1322.   delete sweepFilter;
  1323.   return newSweep;
  1324. }
  1327. bool
  1328. CyberSweep::closest_point(const Pnt3 &p, const Pnt3 &n, 
  1329.   Pnt3 &cp, Pnt3 &cn,
  1330.   float thr, bool bdry_ok)
  1331. {
  1332.   KDindtree* tree = get_current_kdtree();
  1333.   if (!tree) return false;
  1335.   int ind, ans;
  1337.   levelData* level = levels[current_resolution_index()];
  1338.   ans = tree->search(&level->pnts[0], &level->nrms[0], 
  1339.      p, n, ind, thr);
  1340.   if (ans) {
  1341.     if (bdry_ok == 0) {
  1342.       // disallow closest points that are on the mesh boundary
  1343.       if (level->bdry[ind]) return 0;
  1344.     }
  1345.     cp = level->pnts[ind];
  1346.     ind *= 3;
  1347.     cn.set(level->nrms[ind  ]/32767.0, 
  1348.    level->nrms[ind+1]/32767.0, 
  1349.    level->nrms[ind+2]/32767.0);
  1350.   }
  1351.   return ans;
  1352. }
  1355. crope
  1356. CyberScan::getInfo (void)
  1357. {
  1358.   long verts = 0;
  1359.   long tris = 0;
  1360.   for (int i = 0; i < sweeps.size(); i++) {
  1361.     verts += sweeps[i]->sd.count_valid_pnts();
  1362.     tris =+ sweeps[i]->resolutions[0].abs_resolution;
  1363.   }
  1365.   char infos[512];
  1366.   sprintf(infos, "CyberScan: verts %ld stripped tris %ldnn",
  1367.   verts, tris);
  1369.   crope info (infos);
  1371.   if (sweeps.size() == 1) {
  1372.     info += crope ("Single sweep at ") + sweeps[0]->get_description();
  1373.     info += crope ("nFrom file: ") + sweeps[0]->get_name();
  1374.   } else {
  1375.     float turnMin = FLT_MAX; float transMin = FLT_MAX;
  1376.     float turnMax = -FLT_MAX; float transMax = -FLT_MAX;
  1377.     int lastShell = -1000;
  1378.     int nShells = 0;
  1379.     
  1380.     for (int i = 0; i < sweeps.size(); i++) {
  1381.       turnMin = MIN (turnMin, sweeps[i]->sd.other_screw);
  1382.       turnMax = MAX (turnMin, sweeps[i]->sd.other_screw);
  1383.       transMin = MIN (transMin, sweeps[i]->sd.scanner_trans);
  1384.       transMax = MAX (transMin, sweeps[i]->sd.scanner_trans);
  1385.       
  1386.       if ((int)sweeps[i]->sd.scanner_trans != lastShell) {
  1387. lastShell = sweeps[i]->sd.scanner_trans;
  1388. ++nShells;
  1389.       }
  1390.     }
  1391.       
  1392.     sprintf (infos, "%ld sweeps ranging:n"
  1393.      "Other-screw (nod/turn): %.2f to %.2fn"
  1394.      "Translation: %.2f to %.2f in %d shells",
  1395.      sweeps.size(), turnMin, turnMax, transMin, transMax, nShells);
  1396.     info += crope (infos);
  1397.   }
  1399.   info += crope ("nn") + RigidScan::getInfo();
  1401.   return info;
  1402. }
  1405. unsigned int
  1406. CyberScan::get_scanner_config(void)
  1407. {
  1408.    return sweeps[0]->sd.scanner_config;
  1409. }
  1412. float
  1413. CyberScan::get_scanner_vertical(void)
  1414. {
  1415.    return sweeps[0]->sd.scanner_vert;
  1416. }
  1419. //
  1420. // This function converts a point from world coordinates (X,Y,Z) to
  1421. // sweep coordinates (screw length, y, and z for a specific sweep).
  1422. //
  1423. bool
  1424. CyberScan::worldCoordToSweepCoord(const Pnt3 &wc, int *sweepIndex, Pnt3 &sc)
  1425. {
  1426.    Pnt3 csc, swc, bpp, fpp;
  1427.    bool gotOneFlag = false;
  1428.    float theDist, minDist = MAXFLOAT;
  1429.    CyberXform cxf;
  1431.    // First transform the world coord to the CyberScan coordinate
  1432.    // system by multiplying by the inverse of the CyberScan's Xform.
  1434.    getXform().apply_inv(wc, csc);
  1435.    
  1436.    // Next, iterate over all the sweeps, backprojecting the
  1437.    // transformed world coordinate to the sweep's coordinates.
  1438.    // If there are any valid backprojected points, we will return
  1439.    // the one with the minimum distance from the original point.
  1441.    for (int j=0; j<sweeps.size(); ++j)
  1442.    {
  1443.       // Apply the inverse of the individual sweep xform
  1444.       (sweeps[j]->getXform()).apply_inv(csc, swc);
  1446.       cxf.setup(sweeps[j]->sd.scanner_config,  // Need to add vertical screw
  1447. sweeps[j]->sd.scanner_trans,   // value to "setup" call when
  1448. sweeps[j]->sd.other_screw);    // we start using it in SDfile.
  1450.       //puts ("checking sweep");
  1451.  
  1452.       if (cxf.back_project(swc, bpp, true)) {
  1453. cout <<"back project success" << j << "  " << bpp << "n" << flush;
  1454.  sweepCoordToWorldCoord(j, bpp, fpp);
  1455.  theDist = dist2(wc, fpp);
  1456.  if (theDist < minDist) {
  1457.     sc = bpp;
  1458.     *sweepIndex = j;
  1459.     minDist = theDist;
  1460.     gotOneFlag = true;
  1461.          }
  1462.       }
  1463.    }
  1465.    return gotOneFlag;
  1466. }
  1469. //
  1470. // This function converts to world coordinates (X,Y,Z) from
  1471. // sweep coordinates (screw length, y, and z for a specific sweep).
  1472. //
  1473. void
  1474. CyberScan::sweepCoordToWorldCoord(int sweepIndex, const Pnt3 &sc, Pnt3 &wc)
  1475. {
  1476.    CyberXform cxf;
  1478.    //out << "In s2s"  << flush;
  1480.    // First, use the sweep configuration information and convert
  1481.    // to the sweep coordinate system.
  1483.    cxf.setup(sweeps[sweepIndex]->sd.scanner_config,  // Need to add vertical
  1484.              sweeps[sweepIndex]->sd.scanner_trans,   // screw value to "setup"
  1485.              sweeps[sweepIndex]->sd.other_screw);    // call at some point!!!
  1486.    cxf.set_screw(sc[0]);
  1487.    cxf.apply_xform((short) sc[1], (short) sc[2], wc);
  1489.    // Next, apply the transformation for the individual sweep.
  1491.    (sweeps[sweepIndex]->getXform())(wc);
  1493.    // Finally, apply the CyberScan's transformation.
  1495.    getXform()(wc);
  1497.    //out << "Out 2s"  << flush;
  1498. }
  1501. RigidScan *
  1502. CyberSweep::get_piece(int firstFrame, int lastFrame)
  1503. {
  1504.   // Does firstFrame have to be even???
  1505.   if (IS_ODD(firstFrame)) {
  1506.     cout << "CyberSweep::get_piece() Not tested with odd start frames!!" << endl;
  1507.   }
  1509.   if (firstFrame <  0 ||
  1510.       lastFrame  >= sd.num_frames() ||
  1511.       lastFrame  <  firstFrame) {
  1512.     return NULL;
  1513.   }
  1515.   CyberSweep *newSweep = new CyberSweep;
  1516.   newSweep->init_leveldata();
  1518.   sd.get_piece(firstFrame, lastFrame, newSweep->sd);
  1520.   if (newSweep->sd.count_valid_pnts() == 0) {
  1521.     delete newSweep;
  1522.     return NULL;
  1523.   }
  1525.   newSweep->insert_possible_resolutions();
  1527.   return newSweep;
  1528. }
  1531. // return confidence!
  1532. float 
  1533. CyberSweep::closest_along_line_of_sight(const Pnt3 &p, Pnt3 &cp, 
  1534. OccSt &status_p)
  1535. {
  1536.   Pnt3 bp;
  1537.   if (!sd.xf.back_project(p, bp)) {
  1538.     status_p = NOT_IN_FRUSTUM; 
  1539.     return 0.0;
  1540.   } 
  1541.   int row;
  1542.   unsigned short y,z; 
  1543.   if (!sd.find_data(bp[0], bp[1], row, y, z)) {
  1544.     status_p = NOT_IN_FRUSTUM;
  1545.     return 0.0;
  1546.   }
  1547.   sd.set_xf(row, false); 
  1548.   sd.xf.apply_xform(y,z,cp);
  1549.   status_p = (z > bp[2]) ? INSIDE : OUTSIDE;
  1550.   return 1.0;
  1551. }
  1552.  
  1554. OccSt
  1555. CyberSweep::carve_sphere(const Pnt3 &ctr, float r)
  1556. {
  1557.   return OccSt(sd.sphere_status(ctr, r));
  1558. }